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