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