Source Code
|
These source code files are a reconstruction of Sundance, the Apollo 9 LM (Lunar Module) AGC
(Apollo Guidance Computer) flight software, created from binary dumps of original core rope
program modules, as follows:
Since only binary dumps (rather than listings) of Sundance are available as source material, all comments and labels are approximate. They have been taken from other AGC programs where possible, or, in some places, written from scratch to match what we believe would have been in the original listing. |
012372,000002: ## Copyright: Public domain.
012373,000003: ## Filename: ATTITUDE_MANEUVER_ROUTINE.agc
012374,000004: ## Purpose: A section of a reconstructed, mixed version of Sundance
012375,000005: ## It is part of the reconstructed source code for the Lunar
012376,000006: ## Module's (LM) Apollo Guidance Computer (AGC) for Apollo 9.
012377,000007: ## No original listings of this program are available;
012378,000008: ## instead, this file was created via disassembly of dumps
012379,000009: ## of various revisions of Sundance core rope modules.
012380,000010: ## Assembler: yaYUL
012381,000011: ## Contact: Ron Burkey <info@sandroid.org>.
012382,000012: ## Website: www.ibiblio.org/apollo/index.html
012383,000013: ## Mod history: 2020-06-17 MAS Created from Luminary 69.
012384,000014:
012385,000015: ## Sundance 302
012386,000016:
012387,000017: # BLOCK 2 LGC ATTITUDE MANEUVER ROUTINE-KALCMANU
012388,000018:
012389,000019:
012390,000020: # MOD 2 DATE 5/1/67 BY DON KEENE
012391,000021: # PROGRAM DESCRIPTION
012392,000022:
012393,000023: # KALCMANU IS A ROUTINE WHICH GENERATES COMMANDS FOR THE LM DAP TO CHANGE THE ATTITUDE OF THE SPACECRAFT
012394,000024: # DURING FREE FALL. IT IS DESIGNED TO MANEUVER THE SPACECRAFT FROM ITS INITIAL ORIENTATION TO SOME DESIRED
012395,000025: # ORIENTATION SPECIFIED BY THE PROGRAM WHICH CALLS KALCMANU, AVOIDING GIMBAL LOCK IN THE PROCESS. IN THE
012396,000026: # MOD 2 VERSION, THIS DESIRED ATTITUDE IS SPECIFIED BY A SET OF THREE COMMANDED CDU ANGLES STORED AS 2S COMPLEMENT
012397,000027: # SINGLE PRECISION ANGLES IN THE THREE CONSECUTIVE LOCATIONS, CPHI, CTHETA, CPSI, WHERE
012398,000028:
012399,000029: # CPHI = COMMANDED OUTER GIMBAL ANGLE
012400,000030: # CTHETA = COMMANDED INNER GIMBAL ANGLE
012401,000031: # CPSI = COMMANDED MIDDLE GIMBAL ANGLE
012402,000032:
012403,000033: # WHEN POINTING A SPACECRAFT AXIS (E.I. X, Y, Z, THE AOT, THRUST AXIS, ETC) THE SUBROUTINE VECPOINT MAY BE
012404,000034: # USED TO GENERATE THIS SET OF DESIRED CDU ANGLES (SEE DESCRIPTION IN R60) -
012405,000035: # WITH THIS INFORMATION KALCMANU DETERMINES THE DIRECTION OF THE SINGLE EQUIVALENT ROTATION (COF ALSO U) AND THE
012406,000036: # MAGNITUDE OF THE ROTATION (AM) TO BRING THE S/C FROM ITS INITIAL ORIENTATION TO ITS FINAL ORIENTATION.
012407,000037: # THIS DIRECTION REMAINS FIXED BOTH IN INERTIAL COORDINATES AND IN COMMANDED S/C AXES THROUGHOUT THE
012408,000038: # -
012409,000039: # MANEUVER. ONCE COF AND AM HAVE BEEN DETERMINED, KALCMANU THEN EXAMINES THE MANEUVER TO SEE IF IT WILL BRING
012410,000040: # -
012411,000041: # THE S/C THROUGH GIMBAL LOCK. IF SO, COF AND AM ARE READJUSTED SO THAT THE S/C WILL JUST SKIM THE GIMBAL
012412,000042: # LOCK ZONE AND ALIGN THE X-AXIS. IN GENERAL A FINAL YAW ABOUT X WILL BE NECESSARY TO COMPLETE THE MANEUVER.
012413,000043: # NEEDLESS TO SAY, NEITHER THE INITIAL NOR THE FINAL ORIENTATION CAN BE IN GIMBAL LOCK.
012414,000044:
012415,000045: # FOR PROPER ATTITUDE CONTROL THE DIGITAL AUTOPILOT MUST BE GIVEN AN ATTITUDE REFERENCE WHICH IT CAN TRACK.
012416,000046: # KALCMANU DOES THIS BY GENERATING A REFERENCE OF DESIRED GIMBAL ANGLES (CDUXD, CDUYD, CDUZD) WHICH ARE UPDATED
012417,000047: # EVERY ONE SECOND DURING THE MANEUVER. TO ACHIEVE A SMOOTHER SEQUENCE OF COMMANDS BETWEEN SUCCESSIVE UPDATES,
012418,000048: # THE PROGRAM ALSO GENERATES A SET OF INCREMENTAL CDU ANGLES (DELDCDU) TO BE ADDED TO CDU DESIRED BY THE DIGITAL
012419,000049: # AUTOPILOT. KALCMANU ALSO CALCULATES THE COMPONENT MANEUVER RATES (OMEGAPD, OMEGAQD, OMEGARD), WHICH CAN
012420,000050: # -
012421,000051: # BE DETERMINED SIMPLY BY MULTIPLYING COF BY SOME SCALAR (ARATE) CORRESPONDING TO THE DESIRED ROTATIONAL RATE.
012422,000052:
012423,000053: # AUTOMATIC MANEUVERS ARE TIMED WITH THE HELP OF WAITLIST SO THAT AFTER A SPECIFIED INTERVAL THE Y AND Z
012424,000054: # DESIRED RATES ARE SET TO ZERO AND THE DESIRED CDU ANGLES (CDUYD, CDUZD) ARE SET EQUAL TO THE FINAL DESIRED CDU
012425,000055: # ANGLES (CTHETA, CPSI). IF ANY YAW REMAINS DUE TO GIMBAL LOCK AVOIDANCE, THE FINAL YAW MANEUVER IS
012426,000056: # CALCULATED AND THE DESIRED YAW RATE SET TO SOME FIXED VALUE (ROLLRATE = + OR - 2 DEGREES PER SEC).
012427,000057: # IN THIS CASE ONLY AN INCREMENTAL CDUX ANGLE (DELFROLL) IS SUPPLIED TO THE DAP. AT THE END OF THE YAW
012428,000058: # MANEUVER OR IN THE EVENT THAT THERE WAS NO FINAL YAW, CDUXD IS SET EQUAL TO CPHI AND THE X-AXIS DESIRED
012429,000059: # RATE SET TO ZERO. THUS, UPON COMPLETION OF THE MANEUVER THE S/C WILL FINISH UP IN A LIMIT CYCLE ABOUT THE
012430,000060: # DESIRED FINAL GIMBAL ANGLES.
012431,000061:
012432,000062:
012433,000063: # PROGRAM LOGIC FLOW
012434,000064:
012435,000065: # KALCMANU IS CALLED AS A HIGH PRIORITY JOB WITH ENTRY POINTS AT KALCMAN3 AND VECPOINT. IT FIRST PICKS
012436,000066: # UP THE CURRENT CDU ANGLES TO BE USED AS THE BASIS FOR ALL COMPUTATIONS INVOLVING THE INITIAL S/C ORIENTATION.
012437,000067: # IT THEN DETERMINES THE DIRECTION COSINE MATRICES RELATING BOTH THE INITIAL AND FINAL S/C ORIENTATION TO STABLE
012438,000068: # * * *
012439,000069: # MEMBER AXES (MIS, MFS). IT ALSO COMPUTES THE MATRIX RELATING FINAL S/C AXES TO INITIAL S/C AXES (MFI). THE
012440,000070: # ANGLE OF ROTATION (AM) IS THEN EXTRACTED FROM THIS MATRIX, AND TESTS ARE MADE TO DETERMINE IF
012441,000071:
012442,000072: # A) AM LESS THAN .25 DEGREES (MINANG)
012443,000073: # B) AM GREATER THAN 170 DEGREES (MAXANG)
012444,000074:
012445,000075: # IF AM LESS THAN .25 DEGREES, NO COMPLICATED AUTOMATIC MANEUVERING IS NECESSARY. THEREFORE WE CAN SIMPLY
012446,000076: # SET CDU DESIRED EQUAL TO THE FINAL CDU DESIRED ANGLES AND TERMINATE THE JOB.
012447,000077:
012448,000078: # IF AM IS GREATER THAN .25 DEGREES BUT LESS THAN 170 DEGREES, THE AXES OF THE SINGLE EQUIVALENT ROTATION
012449,000079: # - *
012450,000080: # (COF) IS EXTRACTED FROM THE SKEW SYMMETRIC COMPONENTS OF MFI. * *
012451,000081: # IF AM GREATER THAN 170 DEGREES AN ALTERNATE METHOD EMPLOYING THE SYMMETRIC PART OF MFI (MFISYM) IS USED
012452,000082: # -
012453,000083: # TO DETERMINE COF.
012454,000084:
012455,000085: # THE PROGRAM THEN CHECKS TO SEE IF THE MANEUVER AS COMPUTED WILL BRING THE S/C THROUGH GIMBAL LOCK. IF
012456,000086: # SO, A NEW MANEUVER IS CALCULATED WHICH WILL JUST SKIM THE GIMBAL LOCK ZONE AND ALIGN THE S/C X-AXIS. THIS
012457,000087: # METHOD ASSURES THAT THE ADDITIONAL MANEUVERING TO AVOID GIMBAL LOCK WILL BE KEPT TO A MINIMUM. SINCE A FINAL
012458,000088: # P AXIS YAW WILL BE NECESSARY, A SWITCH IS RESET (STATE SWITCH 31) TO ALLOW FOR THE COMPUTATION OF THIS FINAL
012459,000089: # YAW.
012460,000090:
012461,000091: # AS STATED PREVIOUSLY KALCMANU GENERATES A SEQUENCE OF DESIRED GIMBAL ANGLES WHICH ARE UPDATED EVERY
012462,000092: # -
012463,000093: # SECOND. THIS IS ACCOMPLISHED BY A SMALL ROTATION OF THE DESIRED S/C FRAME ABOUT THE VECTOR COF. THE NEW
012464,000094: # DESIRED REFERENCE MATRIX IS THEN,
012465,000095:
012466,000096: # * * *
012467,000097: # MIS = MIS DEL
012468,000098: # N+1 N
012469,000099:
012470,000100: # *
012471,000101: # WHERE DEL IS THE MATRIX CORRESPONDING TO THIS SMALL ROTATION. THE NEW CDU ANGLES CAN THEN BE EXTRACTED
012472,000102: # *
012473,000103: # FROM MIS.
012474,000104:
012475,000105: # AT THE BEGINNING OF THE MANEUVER THE AUTOPILOT DESIRED RATES (OMEGAPD, OMEGAQD, OMEGARD) AND THE
012476,000106: # MANEUVER TIMINGS ARE ESTABLISHED. ON THE FIRST PASS AND ON ALL SUBSEQUENT UPDATES THE CDU DESIRED
012477,000107: # ANGLES ARE LOADED WITH THE APPROPRIATE VALUES AND THE INCREMENTAL CDU ANGLES ARE COMPUTED. THE AGC CLOCKS
012478,000108: # (TIME1 AND TIME2) ARE THAN CHECKED TO SEE IF THE MANEUVER WILL TERMINATE BEFORE THE NEXT UPDATE. IF
012479,000109: # NOT, KALCMANU CALLS FOR ANOTHER UPDATE (RUN AS A JOB WITH PRIORITY TBD) IN ONE SECOND. ANY DELAYS IN THIS
012480,000110: # CALLING SEQUENCE ARE AUTOMATICALLY COMPENSATED IN CALLING FOR THE NEXT UPDATE.
012481,000111:
012482,000112: # IF IT IS FOUND THAT THE MANEUVER IS TO TERMINATE BEFORE THE NEXT UPDATE A ROUTINE IS CALLED (AS A WAIT-
012483,000113: # LIST TASK) TO STOP THE MANEUVER AT THE APPROPRIATE TIME AS EXPLAINED ABOVE.
012484,000114:
012485,000115: # CALLING SEQUENCE
012486,000116:
012487,000117: # IN ORDER TO PERFORM A KALCMANU SUPERVISED MANEUVER, THE COMMANDED GIMBAL ANGLES MUST BE PRECOMPUTED AND
012488,000118: # STORED IN LOCATIONS CPHI, CTHETA, CPSI. THE USER:S PROGRAM MUST THEN CLEAR STATE SWITCH NO 33 TO ALLOW THE
012489,000119: # ATTITUDE MANEUVER ROUTINE TO PERFORM ANY FINAL P-AXIS YAW INCURRED BY AVOIDING GIMBAL LOCK. THE MANEUVER IS
012490,000120: # THEN INITIATED BY ESTABLISHING THE FOLLOWING EXECUTIVE JOB
012491,000121:
012492,000122: # *
012493,000123: # CAF PRIO XX
012494,000124: # --
012495,000125: # INHINT
012496,000126: # TC FINDVAC
012497,000127: # 2CADR KALCMAN3
012498,000128: # RELINT
012499,000129:
012500,000130: # THE USER:S PROGRAM MAY EITHER CONTINUE OR WAIT FOR THE TERMINATION OF THE MANEUVER. IF THE USER WISHES TO
012501,000131: # WAIT, HE MAY PUT HIS JOB TO SLEEP WITH THE FOLLOWING INSTRUCTIONS
012502,000132:
012503,000133: # L TC BANKCALL
012504,000134: # L+1 CADR ATTSTALL
012505,000135: # L+2 (BAD RETURN)
012506,000136: # L+3 (GOOD RETURN)
012507,000137:
012508,000138: # UPON COMPLETION OF THE MANEUVER, THE PROGRAM WILL BE AWAKENED AT L+3 IF THE MANEUVER WAS COMPLETED
012509,000139: # SUCCESSFULLY, OR AT L+2 IF THE MANEUVER WAS ABORTED. THIS ABORT WOULD OCCUR IF THE INITIAL OR FINAL ATTITUDE
012510,000140: # WAS IN GIMBAL LOCK.
012511,000141:
012512,000142: # ***NOTA BENE*** IT IS ASSUMED THAT THE DESIRED MANEUVERING RATE (0.5, 2, 5, 10, DEG/SEC) HAS BEEN SELECTED BY
012513,000143: # KEYBOARD ENTRY PRIOR TO THE EXECUTION OF KALCMANU.
012514,000144: # IT IS ALSO ASSUMED THAT THE AUTOPILOT IS IN THE AUTO MODE. IF THE MODE SWITCH IS CHANGED DURING THE
012515,000145: # MANEUVER, KALCMANU WILL TERMINATE VIA GOODEND WITHIN 1 SECOND SO THAT R60 MAY REQUEST A TRIM OF THE S/C ATTITUDE
012516,000146: # THIS IS THE ONLY MEANS FOR MANUALLY TERMINATING A KALCMANU SUPERVISED MANEUVER.
012517,000147: # SUBROUTINES
012518,000148:
012519,000149: # KALCMANU USES A NUMBER OF INTERPRETIVE SUBROUTINES WHICH MAY BE OF GENERAL INTEREST. SINCE THESE ROUTINES
012520,000150: # WERE PROGRAMMED EXCLUSIVELY FOR KALCMANU, THEY ARE NOT, AS YET, GENERALLY AVAILABLE FOR USE BY OTHER PROGRAMS.
012521,000151:
012522,000152: # MXM3
012523,000153: # ----
012524,000154:
012525,000155: # THIS SUBROUTINE MULTIPLIES TWO 3X3 MATRICES AND LEAVES THE RESULT IN THE FIRST 18 LOCATIONS OF THE PUSH
012526,000156: # DOWN LIST, I.E.,
012527,000157:
012528,000158: # (M M M )
012529,000159: # ( 0 1 2)
012530,000160: # * ( ) * *
012531,000161: # M = (M M M ) = M1 X M2
012532,000162: # ( 3 4 5)
012533,000163: # ( )
012534,000164: # (M M M )
012535,000165: # ( 6 7 8)
012536,000166:
012537,000167: # *
012538,000168: # INDEX REGISTER X1 MUST BE LOADED WITH THE COMPLEMENT OF THE STARTING ADDRESS FOR M1, AND X2 MUST BE
012539,000169: # *
012540,000170: # LOADED WITH THE COMPLEMENT OF THE STARTING ADDRESS FOR M2. THE ROUTINE USES THE FIRST 20 LOCATIONS OF THE PUSH
012541,000171: # DOWN LIST. THE FIRST ELEMENT OF THE MATRIX APPEARS IN PDO. PUSH UP FOR M .
012542,000172: # 8
012543,000173:
012544,000174: # TRANSPOS
012545,000175: # --------
012546,000176:
012547,000177: # THIS ROUTINE TRANSPOSES A 3X3 MATRIX AND LEAVES THE RESULT IN THE PUSH DOWN LIST, I.E.,
012548,000178: #
012549,000179: # * * T
012550,000180: # M = M1
012551,000181:
012552,000182: # INDEX REGISTER X1 MUST CONTAIN THE COMPLEMENT OF THE STARTING ADDRESS FOR M1. PUSH UP FOR THE FIRST AND SUB-
012553,000183: # *
012554,000184: # SEQUENT COMPONENTS OF M. THIS SUBROUTINE ALSO USES THE FIRST 20 LOCATIONS OF THE PUSH DOWN LIST.
012555,000185:
012556,000186: # CDU TO DCM
012557,000187: # ----------
012558,000188:
012559,000189: # THIS SUBROUTINE CONVERTS THREE CDU ANGLES IN T(MPAC) TO A DIRECTION COSINE MATRIX (SCALED BY 2) RELATING
012560,000190: # THE CORRESPONDING S/C ORIENTATIONS TO THE STABLE MEMBER FRAME. THE FORMULAS FOR THIS CONVERSION ARE
012561,000191:
012562,000192: # M = COSY COSZ
012563,000193: # 0
012564,000194:
012565,000195: # M = -COSY SINZ COSX + SINY SINX
012566,000196: # 1
012567,000197:
012568,000198: # M = COSY SINZ SINX + SINY COSX
012569,000199: # 2
012570,000200:
012571,000201: # M = SINZ
012572,000202: # 3
012573,000203:
012574,000204: # M = COSZ COSX
012575,000205: # 4
012576,000206:
012577,000207: # M = -COSZ SINX
012578,000208: # 5
012579,000209:
012580,000210: # M = -SINY COSZ
012581,000211: # 6
012582,000212: #
012583,000213: # M = SINY SINZ COSX + COSY SINX
012584,000214: # 7
012585,000215: # M = -SINY SINZ SINX + COSY COSX
012586,000216: # 8
012587,000217:
012588,000218: # WHERE X = OUTER GIMBAL ANGLE
012589,000219: # Y = INNER GIMBAL ANGLE
012590,000220: # Z = MIDDLE GIMBAL ANGLE
012591,000221:
012592,000222: # THE INTERPRETATION OF THIS MATRIX IS AS FOLLOWS
012593,000223:
012594,000224: # IF A , A , A REPRESENT THE COMPONENTS OF A VECTOR IN S/C AXES THEN THE COMPONENTS OF THE SAME VECTOR IN
012595,000225: # X Y Z
012596,000226: # STABLE MEMBER AXES (B , B , B ) ARE
012597,000227: # X Y Z
012598,000228:
012599,000229: # (B ) (A )
012600,000230: # ( X) ( X)
012601,000231: # ( ) ( )
012602,000232: # ( ) * ( )
012603,000233: # (B ) = M (A )
012604,000234: # ( Y) ( Y)
012605,000235: # ( ) ( )
012606,000236: # (B ) (A )
012607,000237: # ( Z) ( Z)
012608,000238:
012609,000239: # THE SUBROUTINE WILL STORE THIS MATRIX IN SEQUENTIAL LOCATIONS OF ERASABLE MEMORY AS SPECIFIED BY THE CALLING
012610,000240: # *
012611,000241: # PROGRAM. TO DO THIS THE CALLING PROGRAM MUST FIRST LOAD X2 WITH THE COMPLEMENT OF THE STARTING ADDRESS FOR M.
012612,000242:
012613,000243: # INTERNALLY, THE ROUTINE USES THE FIRST 16 LOCATIONS OF THE PUSH DOWN LIST, ALSO STEP REGISTER S1 AND INDEX
012614,000244: # REGISTER X2.
012615,000245:
012616,000246:
012617,000247: # DCM TO CDU
012618,000248: # ----------
012619,000249: # *
012620,000250: # THIS ROUTINE EXTRACTS THE CDU ANGLES FROM A DIRECTION COSINE MATRIX (M SCALED BY 2) RELATING S/C AXIS TO
012621,000251: # *
012622,000252: # STABLE MEMBER AXES. X1 MUST CONTAIN THE COMPLEMENT OF THE STARTING ADDRESS FOR M. THE SUBROUTINE LEAVES THE
012623,000253: # CORRESPONDING GIMBAL ANGLES IN V(MPAC) AS DOUBLE PRECISION 1:S COMPLEMENT ANGLES SCALED BY 2PI. THE FORMULAS
012624,000254: # FOR THIS CONVERSION ARE
012625,000255:
012626,000256: # Z = ARCSIN (M )
012627,000257: # 3
012628,000258:
012629,000259: # Y = ARCSIN (-M /COSZ)
012630,000260: # 6
012631,000261:
012632,000262: # IF M IS NEGATIVE, Y IS REPLACED BY PI SGN Y - Y
012633,000263: # 0
012634,000264: # X = ARCSIN (-M /COSZ)
012635,000265: # 5
012636,000266:
012637,000267: # IF M IS NEGATIVE X IS REPLACED BY PI SGN X - X
012638,000268: # 4
012639,000269:
012640,000270: # THIS ROUTINE DOES NOT SET THE PUSH DOWN POINTER, BUT USES THE NEXT 8 LOCATIONS OF THE PUSH DOWN LIST AND
012641,000271: # RETURNS THE POINTER TO ITS ORIGINAL SETTING. THIS PROCEDURE ALLOWS THE CALLER TO STORE THE MATRIX AT THE TOP OF
012642,000272: # THE PUSH DOWN LIST.
012643,000273:
012644,000274:
012645,000275: # DELCOMP
012646,000276: # -------
012647,000277:
012648,000278: # *
012649,000279: # THIS ROUTINE COMPUTES THE DIRECTION COSINE MATRIX (DEL) RELATING ON
012650,000280: # -
012651,000281: # IS ROTATED WITH RESPECT TO THE FIRST BY AN ANGLE, A, ABOUT A UNIT VECTOR, U. THE FORMULA FOR THIS MATRIX IS
012652,000282:
012653,000283: # * * --T *
012654,000284: # DEL = I COSA + UU (1-COSA) + V SINA
012655,000285: # X
012656,000286:
012657,000287: # WHERE * (1 0 0)
012658,000288: # I = (0 1 0)
012659,000289: # (0 0 1)
012660,000290:
012661,000291:
012662,000292: # 2
012663,000293: # (U U U U U )
012664,000294: # ( X X Y X Z)
012665,000295: # ( )
012666,000296: # --T ( 2 )
012667,000297: # UU = (U U U U U )
012668,000298: # ( Y X Y Y Z)
012669,000299: # ( )
012670,000300: # ( 2 )
012671,000301: # (U U U U U )
012672,000302: # ( Z X Z Y Z )
012673,000303:
012674,000304:
012675,000305: # (0 -U U )
012676,000306: # ( Z Y )
012677,000307: # * ( )
012678,000308: # V = (U 0 -U )
012679,000309: # X ( Z X)
012680,000310: # ( )
012681,000311: # (-U U 0 )
012682,000312: # ( Y X )
012683,000313:
012684,000314: # -
012685,000315: # U = UNIT ROTATION VECTOR RESOLVED INTO S/C AXES
012686,000316: # A = ROTATION ANGLE
012687,000317:
012688,000318: # *
012689,000319: # THE INTERPRETATION OF DEL IS AS FOLLOWS
012690,000320:
012691,000321: # IF A , A , A REPRESENT THE COMPONENT OF A VECTOR IN THE ROTATED FRAME, THEN THE COMPONENTS OF THE SAME
012692,000322: # X Y Z
012693,000323: # VECTOR IN THE ORIGINAL S/C AXES (B , B , B ) ARE
012694,000324: # X Y Z
012695,000325:
012696,000326: # (B ) (A )
012697,000327: # ( X) ( X)
012698,000328: # ( ) * ( )
012699,000329: # (B ) = DEL (A )
012700,000330: # ( Y) ( Y)
012701,000331: # ( ) ( )
012702,000332: # (B ) (A )
012703,000333: # ( Z) ( Z)
012704,000334:
012705,000335: # THE ROUTINE WILL STORE THIS MATRIX (SCALED UNITY) IN SEQUENTIAL LOCATIONS OF ERASABLE MEMORY BEGINNING WITH
012706,000336: # -
012707,000337: # THE LOCATION CALLED DEL. IN ORDER TO USE THE ROUTINE, THE CALLING PROGRAM MUST FIRST STORE U (A HALF UNIT
012708,000338: # DOUBLE PRECISION VECTOR) IN THE SET OF ERASABLE LOCATIONS BEGINNING WITH THE ADDRESS CALLED COF. THE ANGLE, A,
012709,000339: # MUST THEN BE LOADED INTO D(MPAC).
012710,000340:
012711,000341: # INTERNALLY, THE PROGRAM ALSO USES THE FIRST 10 LOCATIONS OF THE PUSH DOWN LIST.
012712,000342:
012713,000343:
012714,000344:
012715,000345: # READCDUK
012716,000346: # --------
012717,000347:
012718,000348: # THIS BASIC LANGUAGE SUBROUTINE LOADS T(MPAC) WITH THE THREE CDU ANGLES.
012719,000349:
012720,000350:
012721,000351:
012722,000352: # SIGNMPAC
012723,000353: # --------
012724,000354:
012725,000355: # THIS IS A BASIC LANGUAGE SUBROUTINE WHICH LIMITS THE MAGNITUDE OF D(MPAC) TO + OR - DPOSMAX ON OVERFLOW.
012726,000356:
012727,000357:
012728,000358:
012729,000359: # PROGRAM STORAGE ALLOCATION
012730,000360:
012731,000361: # 1) FIXED MEMORY 1059 WORDS
012732,000362: # 2) ERASABLE MEMORY 98
012733,000363: # 3) STATE SWITCHES 3
012734,000364: # 4) FLAGS 1
012735,000365:
012736,000366:
012737,000367: # JOB PRIORITIES
012738,000368:
012739,000369: # 1) KALCMANU TBD
012740,000370: # 2) ONE SECOND UPDATE TBD
012741,000371:
012742,000372:
012743,000373: # SUMMARY OF STATE SWITCHES AND FLAGWORDS USED BY KALCMANU.
012744,000374:
012745,000375: # STATE FLAGWRD 2 SETTING MEANING
012746,000376: # SWITCH NO. BIT NO.
012747,000377:
012748,000378: # *
012749,000379: # 31 14 0 MANEUVER WENT THROUGH GIMBAL LOCK
012750,000380: # 1 MANEUVER DID NOT GO THROUGH GIMBAL LOCK
012751,000381: # *
012752,000382: # 32 13 0 CONTINUE UPDATE PROCESS
012753,000383: # 1 START UPDATE PROCESS
012754,000384:
012755,000385: # 33 12 0 PERFORM FINAL P-AXIS YAW IF REQUIRED
012756,000386: # 1 IGNORE ANY FINAL P-AXIS YAW
012757,000387:
012758,000388: # 34 11 0 SIGNAL END OF KALCMANU
012759,000389: # 1 KALCMANU IN PROCESS USER MUST SET SWITCH BEFORE INITIATING
012760,000390:
012761,000391:
012762,000392: # * INTERNAL TO KALCMANU
012763,000393:
012764,000394:
012765,000395: # SUGGESTIONS FOR PROGRAM INTEGRATION
012766,000396:
012767,000397: # THE FOLLOWING VARIABLES SHOULD BE ASSIGNED TO UNSWITCH ERASABLE
012768,000398:
012769,000399: # CPHI
012770,000400: # CTHETA
012771,000401: # CPSI
012772,000402: # POINTVSM +5
012773,000403: # SCAXIS +5
012774,000404: # DELDCDU
012775,000405: # DELDCDU1
012776,000406: # DELDCDU2
012777,000407: # RATEINDX
012778,000408:
012779,000409: # THE FOLLOWING SUBROUTINES MAY BE PUT IN A DIFFERENT BANK
012780,000410:
012781,000411: # MXM3
012782,000412: # TRANSPOS
012783,000413: # SIGNMPAC
012784,000414: # READCDUK
012785,000415: # CDUTODCM
012786,000416:
012787,000417: 15,2000 BANK 15
012788,000418: 22,2000 SETLOC KALCMON1
012789,000419: 22,2000 BANK
012790,000420:
012791,000421: 22,2000 E6,1673 EBANK= BCDU
012792,000422:
012793,000423: # THE THREE DESIRED CDU ANGLES MUST BE STORED AS SINGLE PRECISION TWOS COMPLEMENT ANGLES IN THE THREE SUCCESSIVE
012794,000424: # LOCATIONS, CPHI, CTHETA, CPSI.
012795,000425:
012796,000426: 22,2000 COUNT* $$/KALC
012797,000427: 22,2000 06014 KALCMAN3 TC INTPRET # PICK UP THE CURRENT CDU ANGLES AND
012798,000428: 22,2001 77634 RTB # COMPUTE THE MATRIX FROM INITIAL S/C
012799,000429: 22,2002 44377 READCDUK # AXES TO FINAL S/C AXES
012800,000430: 22,2003 03274 STORE BCDU # STORE INITIAL S/C ANGLES
012801,000431: 22,2004 51535 SLOAD ABS # CHECK THE MAGNITUDE OF THE DESIRED
012802,000432: 22,2005 01211 CPSI # MIDDLE GIMBAL ANGLE
012803,000433: 22,2006 51025 DSU BPL
012804,000434: 22,2007 04377 LOCKANGL # IF GREATER THAN 70 DEG ABORT MANEUVER
012805,000435: 22,2010 44720 TOOBADF
012806,000436: 22,2011 72364 AXC,2 TLOAD
012807,000437: 22,2012 03243 MIS
012808,000438: 22,2013 03274 BCDU
012809,000439: 22,2014 77624 CALL # COMPUTE THE TRANSFORMATION FROM INITIAL
012810,000440: 22,2015 44404 CDUTODCM # S/C AXES TO STABLE MEMBER AXES
012811,000441: 22,2016 72364 AXC,2 TLOAD
012812,000442: 22,2017 02236 MFS # PREPARE TO CALCULATE ARRAY MFS
012813,000443: 22,2020 01207 CPHI
012814,000444: 22,2021 77624 CALL
012815,000445: 22,2022 44404 CDUTODCM
012816,000446: 22,2023 45160 SECAD AXC,1 CALL # MIS AND MFS ARRAYS CALCULATED $2
012817,000447: 22,2024 03243 MIS
012818,000448: 22,2025 44322 TRANSPOS
012819,000449: 22,2026 45575 VLOAD STADR
012820,000450: 22,2027 50462 STOVL TMIS +12D
012821,000451: 22,2030 77626 STADR
012822,000452: 22,2031 50470 STOVL TMIS +6
012823,000453: 22,2032 77626 STADR
012824,000454: 22,2033 74476 STORE TMIS # TMIS = TRANSPOSE(MIS) SCALED BY 2
012825,000455: 22,2034 75160 AXC,1 AXC,2
012826,000456: 22,2035 03300 TMIS
012827,000457: 22,2036 02236 MFS
012828,000458: 22,2037 77624 CALL
012829,000459: 22,2040 44306 MXM3
012830,000460: 22,2041 45575 VLOAD STADR
012831,000461: 22,2042 51524 STOVL MFI +12D
012832,000462: 22,2043 77626 STADR
012833,000463: 22,2044 51532 STOVL MFI +6
012834,000464: 22,2045 77626 STADR
012835,000465: 22,2046 75540 STORE MFI # MFI = TMIS MFS (SCALED BY 4)
012836,000466: 22,2047 45001 SETPD CALL # TRANSPOSE MFI IN PD LIST
012837,000467: 22,2050 00023 18D
012838,000468: 22,2051 44331 TRNSPSPD
012839,000469: 22,2052 45575 VLOAD STADR
012840,000470: 22,2053 50462 STOVL TMFI +12D
012841,000471: 22,2054 77626 STADR
012842,000472: 22,2055 50470 STOVL TMFI +6
012843,000473: 22,2056 77626 STADR
012844,000474: 22,2057 74476 STORE TMFI # TMFI = TRANSPOSE (MFI) SCALED BY 4
012845,000475: #
012846,000476: # CALCULATE COFSKEW AND MFISYM
012847,000477:
012848,000478: 22,2060 45345 DLOAD DSU
012849,000479: 22,2061 03303 TMFI +2
012850,000480: 22,2062 02241 MFI +2
012851,000481: 22,2063 45325 PDDL DSU # CALCULATE COF SCALED BY 2/SIN(AM)
012852,000482: 22,2064 02243 MFI +4
012853,000483: 22,2065 03305 TMFI +4
012854,000484: 22,2066 45325 PDDL DSU
012855,000485: 22,2067 03313 TMFI +10D
012856,000486: 22,2070 02251 MFI +10D
012857,000487: 22,2071 77666 VDEF
012858,000488: 22,2072 03323 STORE COFSKEW # EQUALS MFISKEW
012859,000489: #
012860,000490: # CALCULATE AM AND PROCEED ACCORDING TO ITS MAGNITUDE
012861,000491:
012862,000492: 22,2073 43345 DLOAD DAD
012863,000493: 22,2074 02237 MFI
012864,000494: 22,2075 02257 MFI +16D
012865,000495: 22,2076 43225 DSU DAD
012866,000496: 22,2077 06223 DP1/4TH
012867,000497: 22,2100 02247 MFI +8D
012868,000498: 22,2101 03331 STORE CAM # CAM = (MFI0+MFI4+MFI8-1)/2 HALF SCALE
012869,000499: 22,2102 77726 ARCCOS
012870,000500: 22,2103 03333 STORE AM # AM=ARCCOS(CAM) (AM SCALED BY 2)
012871,000501: 22,2104 51025 DSU BPL
012872,000502: 22,2105 04357 MINANG
012873,000503: 22,2106 44113 CHECKMAX
012874,000504: 22,2107 77751 TLOAD # MANEUVER LESS THAN .25 DEGREES
012875,000505: 22,2110 01207 CPHI # GO DIRECTLY INTO ATTITUDE HOLD
012876,000506: 22,2111 37233 STCALL CDUXD # ABOUT COMMANDED ANGLES
012877,000507: 22,2112 44736 TOOBADI # STOP RATE AND EXIT
012878,000508:
012879,000509: 22,2113 45345 CHECKMAX DLOAD DSU
012880,000510: 22,2114 03333 AM
012881,000511: 22,2115 04361 MAXANG
012882,000512: 22,2116 77244 BPL VLOAD
012883,000513: 22,2117 44125 ALTCALC # UNIT
012884,000514: 22,2120 03323 COFSKEW # COFSKEW
012885,000515: 22,2121 77656 UNIT
012886,000516: 22,2122 03266 STORE COF # COF IS THE MANEUVER AXIS
012887,000517: 22,2123 77650 GOTO # SEE IF MANEUVER GOES THRU GIMBAL LOCK
012888,000518: 22,2124 44740 LOCSKIRT
012889,000519: 22,2125 53375 ALTCALC VLOAD VAD # IF AM GREATER THAN 170 DEGREES
012890,000520: 22,2126 02237 MFI
012891,000521: 22,2127 03301 TMFI
012892,000522: 22,2130 77762 VSR1
012893,000523: 22,2131 27301 STOVL MFISYM
012894,000524: 22,2132 02245 MFI +6
012895,000525: 22,2133 74455 VAD VSR1
012896,000526: 22,2134 03307 TMFI +6
012897,000527: 22,2135 27307 STOVL MFISYM +6
012898,000528: 22,2136 02253 MFI +12D
012899,000529: 22,2137 74455 VAD VSR1
012900,000530: 22,2140 03315 TMFI +12D
012901,000531: 22,2141 03315 STORE MFISYM +12D # MFISYM=(MFI+TMFI)/2 SCALED BY 4
012902,000532:
012903,000533:
012904,000534: # CALCULATE COF
012905,000535:
012906,000536:
012907,000537: 22,2142 70545 DLOAD SR1
012908,000538: 22,2143 03331 CAM
012909,000539: 22,2144 45325 PDDL DSU # PDO CAM $4
012910,000540: 22,2145 06231 DPHALF
012911,000541: 22,2146 03331 CAM
012912,000542: 22,2147 65204 BOVB PDDL # PD2 1 - CAM $2
012913,000543: 22,2150 21656 SIGNMPAC
012914,000544: 22,2151 03321 MFISYM +16D
012915,000545: 22,2152 56225 DSU DDV
012916,000546: 22,2153 00001 0
012917,000547: 22,2154 00003 2
012918,000548: 22,2155 65366 SQRT PDDL # COFZ = SQRT(MFISYM8-CAM)/1-CAM)
012919,000549: 22,2156 03311 MFISYM +8D # $ ROOT 2
012920,000550: 22,2157 56225 DSU DDV
012921,000551: 22,2160 00001 0
012922,000552: 22,2161 00003 2
012923,000553: 22,2162 65366 SQRT PDDL # COFY = SQRT(MFISYM4-CAM)/(1-CAM) $ROOT2
012924,000554: 22,2163 03301 MFISYM
012925,000555: 22,2164 56225 DSU DDV
012926,000556: 22,2165 00001 0
012927,000557: 22,2166 00003 2
012928,000558: 22,2167 55566 SQRT VDEF # COFX = SQRT(MFISYM-CAM)/(1-CAM) $ROOT 2
012929,000559: 22,2170 77656 UNIT
012930,000560: 22,2171 03266 STORE COF
012931,000561: #
012932,000562: # DETERMINE LARGEST COF AND ADJUST ACCORDINGLY
012933,000563:
012934,000564: 22,2172 45345 COFMAXGO DLOAD DSU
012935,000565: 22,2173 03266 COF
012936,000566: 22,2174 03270 COF +2
012937,000567: 22,2175 71240 BMN DLOAD # COFY G COFX
012938,000568: 22,2176 44205 COMP12
012939,000569: 22,2177 03266 COF
012940,000570: 22,2200 50025 DSU BMN
012941,000571: 22,2201 03272 COF +4
012942,000572: 22,2202 44262 METHOD3 # COFZ G COFX OR COFY
012943,000573: 22,2203 77650 GOTO
012944,000574: 22,2204 44236 METHOD1 # COFX G COFY OR COFZ
012945,000575: 22,2205 45345 COMP12 DLOAD DSU
012946,000576: 22,2206 03270 COF +2
012947,000577: 22,2207 03272 COF +4
012948,000578: 22,2210 77640 BMN
012949,000579: 22,2211 44262 METHOD3 # COFZ G COFY OR COFX
012950,000580:
012951,000581: 22,2212 51145 METHOD2 DLOAD BPL # COFY MAX
012952,000582: 22,2213 03325 COFSKEW +2 # UY
012953,000583: 22,2214 44220 U2POS
012954,000584: 22,2215 57575 VLOAD VCOMP
012955,000585: 22,2216 03266 COF
012956,000586: 22,2217 03266 STORE COF
012957,000587: 22,2220 51145 U2POS DLOAD BPL
012958,000588: 22,2221 03303 MFISYM +2 # UX UY
012959,000589: 22,2222 44226 OKU21
012960,000590: 22,2223 57545 DLOAD DCOMP # SIGN OF UX OPPOSITE TO UY
012961,000591: 22,2224 03266 COF
012962,000592: 22,2225 03266 STORE COF
012963,000593: 22,2226 51145 OKU21 DLOAD BPL
012964,000594: 22,2227 03313 MFISYM +10D # UY UZ
012965,000595: 22,2230 44740 LOCSKIRT
012966,000596: 22,2231 57545 DLOAD DCOMP # SIGN OF UZ OPPOSITE TO UY
012967,000597: 22,2232 03272 COF +4
012968,000598: 22,2233 03272 STORE COF +4
012969,000599: 22,2234 77650 GOTO
012970,000600: 22,2235 44740 LOCSKIRT
012971,000601: 22,2236 51145 METHOD1 DLOAD BPL # COFX MAX
012972,000602: 22,2237 03323 COFSKEW # UX
012973,000603: 22,2240 44244 U1POS
012974,000604: 22,2241 57575 VLOAD VCOMP
012975,000605: 22,2242 03266 COF
012976,000606: 22,2243 03266 STORE COF
012977,000607: 22,2244 51145 U1POS DLOAD BPL
012978,000608: 22,2245 03303 MFISYM +2 # UX UY
012979,000609: 22,2246 44252 OKU12
012980,000610: 22,2247 57545 DLOAD DCOMP
012981,000611: 22,2250 03270 COF +2 # SIGN OF UY OPPOSITE TO UX
012982,000612: 22,2251 03270 STORE COF +2
012983,000613: 22,2252 51145 OKU12 DLOAD BPL
012984,000614: 22,2253 03305 MFISYM +4 # UX UZ
012985,000615: 22,2254 44740 LOCSKIRT
012986,000616: 22,2255 57545 DLOAD DCOMP # SIGN OF UZ OPPOSITE TO UY
012987,000617: 22,2256 03272 COF +4
012988,000618: 22,2257 03272 STORE COF +4
012989,000619: 22,2260 77650 GOTO
012990,000620: 22,2261 44740 LOCSKIRT
012991,000621: 22,2262 51145 METHOD3 DLOAD BPL # COFZ MAX
012992,000622: 22,2263 03327 COFSKEW +4 # UZ
012993,000623: 22,2264 44270 U3POS
012994,000624: 22,2265 57575 VLOAD VCOMP
012995,000625: 22,2266 03266 COF
012996,000626: 22,2267 03266 STORE COF
012997,000627: 22,2270 51145 U3POS DLOAD BPL
012998,000628: 22,2271 03305 MFISYM +4 # UX UZ
012999,000629: 22,2272 44276 OKU31
013000,000630: 22,2273 57545 DLOAD DCOMP
013001,000631: 22,2274 03266 COF # SIGN OF UX OPPOSITE TO UZ
013002,000632: 22,2275 03266 STORE COF
013003,000633: 22,2276 51145 OKU31 DLOAD BPL
013004,000634: 22,2277 03313 MFISYM +10D # UY UZ
013005,000635: 22,2300 44740 LOCSKIRT
013006,000636: 22,2301 57545 DLOAD DCOMP
013007,000637: 22,2302 03270 COF +2 # SIGN OF UY OPPOSITE TO UZ
013008,000638: 22,2303 03270 STORE COF +2
013009,000639: 22,2304 77650 GOTO
013010,000640: 22,2305 44740 LOCSKIRT
013011,000641: # MATRIX OPERATIONS
013012,000642:
013013,000643: 13,2161 BANK 13
013014,000644: 22,2000 SETLOC KALCMON2
013015,000645: 22,2000 BANK
013016,000646:
013017,000647: 22,2306 E6,1673 EBANK= BCDU
013018,000648:
013019,000649: 22,2306 76601 MXM3 SETPD VLOAD* # MXM3 MULTIPLIES 2 3X3 MATRICES
013020,000650: 22,2307 00001 0 # AND LEAVES RESULT IN PD LIST
013021,000651: 22,2310 00001 0,1 # AND MPAC
013022,000652: 22,2311 62703 VXM* PDVL*
013023,000653: 22,2312 77776 0,2
013024,000654: 22,2313 00007 6,1
013025,000655: 22,2314 62703 VXM* PDVL*
013026,000656: 22,2315 77776 0,2
013027,000657: 22,2316 00015 12D,1
013028,000658: 22,2317 41503 VXM* PUSH
013029,000659: 22,2320 77776 0,2
013030,000660: 22,2321 77616 RVQ
013031,000661:
013032,000662:
013033,000663: # RETURN WITH M1XM2 IN PD LIST
013034,000664:
013035,000665: 22,2322 76601 TRANSPOS SETPD VLOAD* # TRANSPOS TRANSPOSES A 3X3 MATRIX
013036,000666: 22,2323 00001 0 # AND LEAVES RESULT IN PD LIST
013037,000667: 22,2324 00001 0,1 # MATRIX ADDRESS IN XR1
013038,000668: 22,2325 62713 PDVL* PDVL*
013039,000669: 22,2326 00007 6,1
013040,000670: 22,2327 00015 12D,1
013041,000671: 22,2330 77606 PUSH # MATRIX IN PD
013042,000672: 22,2331 77776 TRNSPSPD EXIT # ENTER WITH MATRIX AT 0 IN PD LIST
013043,000673: 22,2332 50120 INDEX FIXLOC
013044,000674: 22,2333 52013 DXCH 12
013045,000675: 22,2334 50120 INDEX FIXLOC
013046,000676: 22,2335 52017 DXCH 16
013047,000677: 22,2336 50120 INDEX FIXLOC
013048,000678: 22,2337 52013 DXCH 12
013049,000679: 22,2340 50120 INDEX FIXLOC
013050,000680: 22,2341 52015 DXCH 14
013051,000681: 22,2342 50120 INDEX FIXLOC
013052,000682: 22,2343 52005 DXCH 4
013053,000683: 22,2344 50120 INDEX FIXLOC
013054,000684: 22,2345 52015 DXCH 14
013055,000685: 22,2346 50120 INDEX FIXLOC
013056,000686: 22,2347 52003 DXCH 2
013057,000687: 22,2350 50120 INDEX FIXLOC
013058,000688: 22,2351 52007 DXCH 6
013059,000689: 22,2352 50120 INDEX FIXLOC
013060,000690: 22,2353 52003 DXCH 2
013061,000691: 22,2354 06014 TC INTPRET
013062,000692: 22,2355 77616 RVQ
013063,000693:
013064,000694: 15,2000 BANK 15
013065,000695: 22,2000 SETLOC KALCMON1
013066,000696: 22,2000 BANK
013067,000697:
013068,000698: 22,2356 E6,1673 EBANK= BCDU
013069,000699:
013070,000700: 22,2356 00013 13563 MINANG 2DEC 0.00069375
013071,000701:
013072,000702: 22,2360 17070 34343 MAXANG 2DEC 0.472222222
013073,000703:
013074,000704: # GIMBAL LOCK CONSTANTS
013075,000705:
013076,000706: # D = MGA CORRESPONDING TO GIMBAL LOCK = 60 DEGREES
013077,000707: # NGL = BUFFER ANGLE (TO AVOID DIVISIONS BY ZERO) = 2 DEGREES
013078,000708:
013079,000709: 22,2362 15666 20443 SD 2DEC .433015 # = SIN(D) $2
013080,000710:
013081,000711: 22,2364 33555 01106 K3S1 2DEC .86603 # = SIN(D) $1
013082,000712:
013083,000713: 22,2366 67777 77777 K4 2DEC -.25 # = -COS(D) $2
013084,000714:
013085,000715: 22,2370 04000 00000 K4SQ 2DEC .125 # = COS(D)COS(D) $2
013086,000716:
013087,000717: 22,2372 00216 36323 SNGLCD 2DEC .008725 # = SIN(NGL)COS(D) $2
013088,000718:
013089,000719: 22,2374 17773 00057 CNGL 2DEC .499695 # COS(NGL) $2
013090,000720:
013091,000721: 22,2376 14344 LOCKANGL DEC .388889 # = 70 DEGREES
013092,000722: # INTERPRETIVE SUBROUTINE TO READ THE CDU ANGLES
013093,000723:
013094,000724: 22,2377 30034 READCDUK CA CDUZ # LOAD T(MPAC) WITH CDU ANGLES
013095,000725: 22,2400 54156 TS MPAC +2
013096,000726: 22,2401 00006 EXTEND
013097,000727: 22,2402 30033 DCA CDUX # AND CHANGE MODE TO TRIPLE PRECISION
013098,000728: 22,2403 16452 TCF TLOAD +6
013099,000729:
013100,000730: 22,2404 66370 CDUTODCM AXT,1 SSP
013101,000731: 22,2405 00003 OCT 3
013102,000732: 22,2406 00051 S1
013103,000733: 22,2407 00001 OCT 1 # SET XR1, S1, AND PD FOR LOOP
013104,000734: 22,2410 00010 STORE 7
013105,000735: 22,2411 77601 SETPD
013106,000736: 22,2412 00001 0
013107,000737: 22,2413 47133 LOOPSIN SLOAD* RTB
013108,000738: 22,2414 00013 10D,1
013109,000739: 22,2415 21457 CDULOGIC
013110,000740: 22,2416 00013 STORE 10D # LOAD PD WITH 0 SIN(PHI)
013111,000741: 22,2417 65356 SIN PDDL # 2 COS(PHI)
013112,000742: 22,2420 00013 10D # 4 SIN(THETA)
013113,000743: 22,2421 41546 COS PUSH # 6 COS(THETA)
013114,000744: 22,2422 71300 TIX,1 DLOAD # 8 SIN(PSI)
013115,000745: 22,2423 44413 LOOPSIN # 10 COS(PSI)
013116,000746: 22,2424 00007 6
013117,000747: 22,2425 72405 DMP SL1
013118,000748: 22,2426 00013 10D
013119,000749: 22,2427 10001 STORE 0,2 # C0=COS(THETA)COS(PSI)
013120,000750: 22,2430 41345 DLOAD DMP
013121,000751: 22,2431 00005 4
013122,000752: 22,2432 00001 0
013123,000753: 22,2433 41325 PDDL DMP # (PD6 SIN(THETA)SIN(PHI))
013124,000754: 22,2434 00007 6
013125,000755: 22,2435 00011 8D
013126,000756: 22,2436 72405 DMP SL1
013127,000757: 22,2437 00003 2
013128,000758: 22,2440 72421 BDSU SL1
013129,000759: 22,2441 00015 12D
013130,000760: 22,2442 10003 STORE 2,2 # C1=-COS(THETA)SIN(PSI)COS(PHI)
013131,000761: 22,2443 41345 DLOAD DMP
013132,000762: 22,2444 00003 2
013133,000763: 22,2445 00005 4
013134,000764: 22,2446 41325 PDDL DMP # (PD7 COS(PHI)SIN(THETA)) SCALED 4
013135,000765: 22,2447 00007 6
013136,000766: 22,2450 00011 8D
013137,000767: 22,2451 72405 DMP SL1
013138,000768: 22,2452 00001 0
013139,000769: 22,2453 72415 DAD SL1
013140,000770: 22,2454 00017 14D
013141,000771: 22,2455 10005 STORE 4,2 # C2=COS(THETA)SIN(PSI)SIN(PHI)
013142,000772: 22,2456 77745 DLOAD
013143,000773: 22,2457 00011 8D
013144,000774: 22,2460 10007 STORE 6,2 # C3=SIN(PSI)
013145,000775: 22,2461 77745 DLOAD
013146,000776: 22,2462 00013 10D
013147,000777: 22,2463 72405 DMP SL1
013148,000778: 22,2464 00003 2
013149,000779: 22,2465 10011 STORE 8D,2 # C4=COS(PSI)COS(PHI)
013150,000780: 22,2466 41345 DLOAD DMP
013151,000781: 22,2467 00013 10D
013152,000782: 22,2470 00001 0
013153,000783: 22,2471 72476 DCOMP SL1
013154,000784: 22,2472 10013 STORE 10D,2 # C5=-COS(PSI)SIN(PHI)
013155,000785: 22,2473 41345 DLOAD DMP
013156,000786: 22,2474 00005 4
013157,000787: 22,2475 00013 10D
013158,000788: 22,2476 72476 DCOMP SL1
013159,000789: 22,2477 10015 STORE 12D,2 # C6=-SIN(THETA)COS(PSI)
013160,000790: 22,2500 77745 DLOAD
013161,000791: 22,2501 72405 DMP SL1 # (PUSH UP 7)
013162,000792: 22,2502 00011 8D
013163,000793: 22,2503 41325 PDDL DMP # (PD7 COS(PHI)SIN(THETA)SIN(PSI)) SCALE4
013164,000794: 22,2504 00007 6
013165,000795: 22,2505 00001 0
013166,000796: 22,2506 72415 DAD SL1 # (PUSH UP 7)
013167,000797: 22,2507 77626 STADR # C7=COS(PHI)SIN(THETA)SIN(PSI)
013168,000798: 22,2510 67760 STORE 14D,2 # +COS(THETA)SIN(PHI)
013169,000799: 22,2511 77745 DLOAD
013170,000800: 22,2512 72405 DMP SL1 # (PUSH UP 6)
013171,000801: 22,2513 00011 8D
013172,000802: 22,2514 41325 PDDL DMP # (PD6 SIN(THETA)SIN(PHI)SIN(PSI)) SCALE4
013173,000803: 22,2515 00007 6
013174,000804: 22,2516 00003 2
013175,000805: 22,2517 72425 DSU SL1 # (PUSH UP 6)
013176,000806: 22,2520 77626 STADR
013177,000807: 22,2521 67756 STORE 16D,2 # C8=-SIN(THETA)SIN(PHI)SIN(PSI)
013178,000808: 22,2522 77616 RVQ # +COS(THETA)COS(PHI)
013179,000809:
013180,000810: # CALCULATION OF THE MATRIX DEL......
013181,000811:
013182,000812: # * * --T *
013183,000813: # DEL = (IDMATRIX)COS(A)+UU (1-COS(A))+UX SIN(A) SCALED 1
013184,000814:
013185,000815: # -
013186,000816: # WHERE U IS A UNIT VECTOR (DP SCALED 2) ALONG THE AXIS OF ROTATION.
013187,000817: # A IS THE ANGLE OF ROTATION (DP SCALED 2)
013188,000818: # -
013189,000819: # UPON ENTRY THE STARTING ADDRESS OF U IS COF, AND A IS IN MPAC
013190,000820:
013191,000821: 22,2523 41401 DELCOMP SETPD PUSH # MPAC CONTAINS THE ANGLE A
013192,000822: 22,2524 00001 0
013193,000823: 22,2525 65356 SIN PDDL # PD0 = SIN(A)
013194,000824: 22,2526 41546 COS PUSH # PD2 = COS(A)
013195,000825: 22,2527 65302 SR2 PDDL # PD2 = COS(A) $8
013196,000826: 22,2530 41021 BDSU BOVB
013197,000827: 22,2531 06231 DPHALF
013198,000828: 22,2532 21656 SIGNMPAC
013199,000829: 22,2533 77725 PDDL # PD4 = 1-COS(A)
013200,000830:
013201,000831: # COMPUTE THE DIAGONAL COMPONENTS OF DEL
013202,000832:
013203,000833: 22,2534 03266 COF
013204,000834: 22,2535 41316 DSQ DMP
013205,000835: 22,2536 00005 4
013206,000836: 22,2537 52415 DAD SL3
013207,000837: 22,2540 00003 2
013208,000838: 22,2541 77604 BOVB
013209,000839: 22,2542 21656 SIGNMPAC
013210,000840: 22,2543 16237 STODL KEL # UX UX(1-COS(A)) +COS(A) $1
013211,000841: 22,2544 03270 COF +2
013212,000842: 22,2545 41316 DSQ DMP
013213,000843: 22,2546 00005 4
013214,000844: 22,2547 52415 DAD SL3
013215,000845: 22,2550 00003 2
013216,000846: 22,2551 77604 BOVB
013217,000847: 22,2552 21656 SIGNMPAC
013218,000848: 22,2553 16247 STODL KEL +8D # UY UY(1-COS(A)) +COS(A) $1
013219,000849: 22,2554 03272 COF +4
013220,000850: 22,2555 41316 DSQ DMP
013221,000851: 22,2556 00005 4
013222,000852: 22,2557 52415 DAD SL3
013223,000853: 22,2560 00003 2
013224,000854: 22,2561 77604 BOVB
013225,000855: 22,2562 21656 SIGNMPAC
013226,000856: 22,2563 02257 STORE KEL +16D # UZ UZ(1-COS(A)) +COS(A) $1
013227,000857:
013228,000858: # COMPUTE THE OFF DIAGONAL TERMS OF DEL
013229,000859:
013230,000860: 22,2564 41345 DLOAD DMP
013231,000861: 22,2565 03266 COF
013232,000862: 22,2566 03270 COF +2
013233,000863: 22,2567 72405 DMP SL1
013234,000864: 22,2570 00005 4
013235,000865: 22,2571 41325 PDDL DMP # D6 UX UY (1-COS A) $4
013236,000866: 22,2572 03272 COF +4
013237,000867: 22,2573 00001 0
013238,000868: 22,2574 43206 PUSH DAD # D8 UZ SIN A $4
013239,000869: 22,2575 00007 6
013240,000870: 22,2576 41112 SL2 BOVB
013241,000871: 22,2577 21656 SIGNMPAC
013242,000872: 22,2600 16245 STODL KEL +6
013243,000873: 22,2601 62421 BDSU SL2
013244,000874: 22,2602 77604 BOVB
013245,000875: 22,2603 21656 SIGNMPAC
013246,000876: 22,2604 16241 STODL KEL +2
013247,000877: 22,2605 03266 COF
013248,000878: 22,2606 41205 DMP DMP
013249,000879: 22,2607 03272 COF +4
013250,000880: 22,2610 00005 4
013251,000881: 22,2611 65352 SL1 PDDL # D6 UX UZ (1-COS A) $4
013252,000882: 22,2612 03270 COF +2
013253,000883: 22,2613 41405 DMP PUSH # D8 UY SIN(A)
013254,000884: 22,2614 00001 0
013255,000885: 22,2615 62415 DAD SL2
013256,000886: 22,2616 00007 6
013257,000887: 22,2617 77604 BOVB
013258,000888: 22,2620 21656 SIGNMPAC
013259,000889: 22,2621 16243 STODL KEL +4 # UX UZ (1-COS(A))+UY SIN(A)
013260,000890: 22,2622 62421 BDSU SL2
013261,000891: 22,2623 77604 BOVB
013262,000892: 22,2624 21656 SIGNMPAC
013263,000893: 22,2625 16253 STODL KEL +12D # UX UZ (1-COS(A))-UY SIN(A)
013264,000894: 22,2626 03270 COF +2
013265,000895: 22,2627 41205 DMP DMP
013266,000896: 22,2630 03272 COF +4
013267,000897: 22,2631 00005 4
013268,000898: 22,2632 65352 SL1 PDDL # D6 UY UZ (1-COS(A)) $ 4
013269,000899: 22,2633 03266 COF
013270,000900: 22,2634 41405 DMP PUSH # D8 UX SIN(A)
013271,000901: 22,2635 00001 0
013272,000902: 22,2636 62415 DAD SL2
013273,000903: 22,2637 00007 6
013274,000904: 22,2640 77604 BOVB
013275,000905: 22,2641 21656 SIGNMPAC
013276,000906: 22,2642 16255 STODL KEL +14D # UY UZ(1-COS(A)) +UX SIN(A)
013277,000907: 22,2643 62421 BDSU SL2
013278,000908: 22,2644 77604 BOVB
013279,000909: 22,2645 21656 SIGNMPAC
013280,000910: 22,2646 02251 STORE KEL +10D # UY UZ (1-COS(A)) -UX SIN(A)
013281,000911: 22,2647 77616 RVQ
013282,000912:
013283,000913:
013284,000914: # DIRECTION COSINE MATRIX TO CDU ANGLE ROUTINE
013285,000915: # X1 CONTAINS THE COMPLEMENT OF THE STARTING ADDRESS FOR MATRIX (SCALED 2)
013286,000916: # LEAVES CDU ANGLES SCALED 2PI IN V(MPAC)
013287,000917: # COS(MGA) WILL BE LEFT IN S1 (SCALED 1)
013288,000918:
013289,000919: # THE DIRECTION COSINE MATRIX RELATING S/C AXES TO STABLE MEMBER AXES CAN BE WRITTEN AS***
013290,000920:
013291,000921: # C =COS(THETA)COS(PSI)
013292,000922: # 0
013293,000923: # C =-COS(THETA)SIN(PSI)COS(PHI)+SI (THETA)SIN(PHI)
013294,000924: # 1
013295,000925: # C =COS(THETA)SIN(PSI)SIN(PHI) + S N(THETA)COS(PHI)
013296,000926: # 2
013297,000927: # C =SIN(PSI)
013298,000928: # 3
013299,000929: # C =COS(PSI)COS(PHI)
013300,000930: # 4
013301,000931: # C =-COS(PSI)SIN(PHI)
013302,000932: # 5
013303,000933: # C =-SIN(THETA)COS(PSI)
013304,000934: # 6
013305,000935: # C =SIN(THETA)SIN(PSI)COS(PHI)+COS THETA)SIN(PHI)
013306,000936: # 7
013307,000937: # C =-SIN(THETA)SIN(PSI)SIN(PHI)+CO (THETA)COS(PHI)
013308,000938: # 8
013309,000939: # WHERE PHI = OGA
013310,000940: # THETA = IGA
013311,000941: # PSI = MGA
013312,000942:
013313,000943: 22,2650 67543 DCMTOCDU DLOAD* ARCSIN
013314,000944: 22,2651 00007 6,1
013315,000945: 22,2652 71406 PUSH COS # PD +0 PSI
013316,000946: 22,2653 41152 SL1 BOVB
013317,000947: 22,2654 21656 SIGNMPAC
013318,000948: 22,2655 00051 STORE S1
013319,000949: 22,2656 57543 DLOAD* DCOMP
013320,000950: 22,2657 00015 12D,1
013321,000951: 22,2660 67471 DDV ARCSIN
013322,000952: 22,2661 00051 S1
013323,000953: 22,2662 51123 PDDL* BPL # PD +2 THETA
013324,000954: 22,2663 00001 0,1 # MUST CHECK THE SIGN OF COS(THETA)
013325,000955: 22,2664 44676 OKTHETA # TO DETERMINE THE PROPER QUADRANT
013326,000956: 22,2665 57545 DLOAD DCOMP
013327,000957: 22,2666 43244 BPL DAD
013328,000958: 22,2667 44673 SUHALFA
013329,000959: 22,2670 06231 DPHALF
013330,000960: 22,2671 77650 GOTO
013331,000961: 22,2672 44675 CALCPHI
013332,000962: 22,2673 77625 SUHALFA DSU
013333,000963: 22,2674 06231 DPHALF
013334,000964: 22,2675 77606 CALCPHI PUSH
013335,000965: 22,2676 57543 OKTHETA DLOAD* DCOMP
013336,000966: 22,2677 00013 10D,1
013337,000967: 22,2700 67471 DDV ARCSIN
013338,000968: 22,2701 00051 S1
013339,000969: 22,2702 51123 PDDL* BPL # PUSH DOWN PHI
013340,000970: 22,2703 00011 8D,1
013341,000971: 22,2704 44716 OKPHI
013342,000972: 22,2705 57545 DLOAD DCOMP # PUSH UP PHI
013343,000973: 22,2706 43244 BPL DAD
013344,000974: 22,2707 44713 SUHALFAP
013345,000975: 22,2710 06231 DPHALF
013346,000976: 22,2711 77650 GOTO
013347,000977: 22,2712 44717 VECOFANG
013348,000978: 22,2713 52025 SUHALFAP DSU GOTO
013349,000979: 22,2714 06231 DPHALF
013350,000980: 22,2715 44717 VECOFANG
013351,000981: 22,2716 77745 OKPHI DLOAD # PUSH UP PHI
013352,000982: 22,2717 43466 VECOFANG VDEF RVQ
013353,000983: # ROUTINES FOR TERMINATING THE AUTOMATIC MANEUVER AND RETURNING TO USER
013354,000984:
013355,000985: 22,2720 77776 TOOBADF EXIT
013356,000986: 22,2721 05646 TC ALARM
013357,000987: 22,2722 00401 OCT 00401
013358,000988:
013359,000989: 22,2723 12726 TCF NOGO # DO NOT ZERO ATTITUDE ERRORS
013360,000990:
013361,000991: 22,2724 04655 TC BANKCALL
013362,000992: 22,2725 40232 CADR ZATTEROR # ZERO ATTITUDE ERRORS
013363,000993:
013364,000994: 22,2726 04655 NOGO TC BANKCALL
013365,000995: 22,2727 40244 CADR STOPRATE # STOP RATES
013366,000996:
013367,000997: 22,2730 35025 CAF TWO
013368,000998: 22,2731 00004 INHINT # ALL RETURNS ARE NOW MADE VIA GOODEND
013369,000999: 22,2732 05252 TC WAITLIST
013370,001000: 22,2733 E6,1673 EBANK= BCDU
013371,001001: 22,2733 03230 44066 2CADR GOODMANU
013372,001002:
013373,001003: 22,2735 15225 TCF ENDOFJOB
013374,001004:
013375,001005: 22,2736 77776 TOOBADI EXIT
013376,001006: 22,2737 12726 TCF NOGO
End of include-file ATTITUDE_MANEUVER_ROUTINE.agc. Parent file is MAIN.agc