Source Code
![]() |
This is a reconstructed version of Luminary 130, the initial AGC program released
in November 1969 for the Apollo 13 LM. Three more releases
followed before the flown revision was finally manufactured in February 1970.
This reconstruction was created from a previous transcription of the original listing of Luminary 131, combined with Luminary memo #129 which describes the small difference between the Luminary 130 and 131. The reconstructed source code was verified by matching memory-bank checksums to those listed for Luminary 130 in drawing 2021152G. In this source code, program comments from the original (Luminary 131) code are prefixed with '#', while those added later by the Virtual AGC project are prefixed with '##'. |
014267,000002: ## Copyright: Public domain. 014268,000003: ## Filename: ATTITUDE_MANEUVER_ROUTINE.agc 014269,000004: ## Purpose: A section of the reconstructed source code for Luminary 130. 014270,000005: ## This was the original program released for the Apollo 13 LM, 014271,000006: ## although several more revisions would follow. It has been 014272,000007: ## reconstructed from a listing of Luminary 131, from which it 014273,000008: ## differs on only two lines in P70-P71. The difference is 014274,000009: ## described in detail in Luminary memo #129, which was used 014275,000010: ## to perform the reconstruction. This file is intended to be a 014276,000011: ## faithful reconstruction, except that the code format has been 014277,000012: ## changed to conform to the requirements of the yaYUL assembler 014278,000013: ## rather than the original YUL assembler. 014279,000014: ## Reference: pp. 344-365 014280,000015: ## Contact: Ron Burkey <info@sandroid.org>. 014281,000016: ## Website: www.ibiblio.org/apollo/index.html 014282,000017: ## Mod history: 05/10/03 RSB. Began transcribing. 014283,000018: ## 05/14/05 RSB Corrected website reference above. 014284,000019: ## 2010-08-24 JL Added missing page number comments. 014285,000020: ## 2017-01-06 RSB Page numbers now agree with those on the 014286,000021: ## original harcopy, as opposed to the PDF page 014287,000022: ## numbers in 1701.pdf. 014288,000023: ## 2017-02-23 RSB Proofed comment text using octopus/ProoferComments. 014289,000024: ## 2017-02-28 RSB Fixed lingering typos. 014290,000025: ## 2017-03-07 RSB Comment-text fixes noted in proofing Luminary 116. 014291,000026: ## 2017-03-15 RSB Comment-text fixes identified in 5-way 014292,000027: ## side-by-side diff of Luminary 69/99/116/131/210. 014293,000028: ## 2018-09-04 MAS Copied from Luminary 131 for Luminary 130. 014294,000029:
![]() |
Page 344 |
014296,000031: # BLOCK 2 LGC ATTITUDE MANEUVER ROUTINE - KALCMANU 014297,000032: 014298,000033: # MOD 2 DATE 5/1/67 BY DON KEENE 014299,000034: 014300,000035: # PROGRAM DESCRIPTION 014301,000036: 014302,000037: # KALCMANU IS A ROUTINE WHICH GENERATES COMMANDS FOR THE LM DAP TO CHANGE THE ATTITUDE OF THE SPACECRAFT 014303,000038: # DURING FREE FALL. IT IS DESIGNED TO MANEUVER THE SPACECRAFT FROM ITS INITIAL ORIENTATION TO SOME DESIRED 014304,000039: # ORIENTATION SPECIFIED BY THE PROGRAM WHICH CALLS KALCMANU, AVOIDING GIMBAL LOCK IN THE PROCESS. IN THE 014305,000040: # MOD 2 VERSION, THIS DESIRED ATTITUDE IS SPECIFIED BY A SET OF THREE COMMANDED CDU ANGLES STORED AS 2S COMPLEMENT 014306,000041: # SINGLE PRECISION ANGLES IN THE THREE CONSECUTIVE LOCATIONS, CPHI, CTHETA, CPSI, WHERE 014307,000042: 014308,000043: # CPHI = COMMANDED OUTER GIMBAL ANGLE 014309,000044: # CTHETA = COMMANDED INNER GIMBAL ANGLE 014310,000045: # CPSI = COMMANDED MIDDLE GIMBAL ANGLE 014311,000046: 014312,000047: # WHEN POINTING A SPACECRAFT AXIS (E.I. X, Y, Z, THE AOT, THRUST AXIS, ETC) THE SUBROUTINE VECPOINT MAY BE 014313,000048: # USED TO GENERATE THIS SET OF DESIRED CDU ANGLES (SEE DESCRIPTION IN R60) - 014314,000049: 014315,000050: # WITH THIS INFORMATION KALCMANU DETERMINES THE DIRECTION OF THE SINGLE EQUIVALENT ROTATION (COF ALSO U) AND THE 014316,000051: # MAGNITUDE OF THE ROTATION (AM) TO BRING THE S/C FROM ITS INITIAL ORIENTATION TO ITS FINAL ORIENTATION. 014317,000052: # THIS DIRECTION REMAINS FIXED BOTH IN INERTIAL COORDINATES AND IN COMMANDED S/C AXES THROUGHOUT THE 014318,000053: # - 014319,000054: # MANEUVER. ONCE COF AND AM HAVE BEEN DETERMINED, KALCMANU THEN EXAMINES THE MANEUVER TO SEE IF IT WILL BRING 014320,000055: # - 014321,000056: # THE S/C THROUGH GIMBAL LOCK. IF SO, COF AND AM ARE READJUSTED SO THAT THE S/C WILL JUST SKIM THE GIMBAL 014322,000057: # LOCK ZONE AND ALIGN THE X-AXIS. IN GENERAL A FINAL YAW ABOUT X WILL BE NECESSARY TO COMPLETE THE MANEUVER. 014323,000058: # NEEDLESS TO SAY, NEITHER THE INITIAL NOR THE FINAL ORIENTATION CAN BE IN GIMBAL LOCK. 014324,000059: 014325,000060: # FOR PROPER ATTITUDE CONTROL THE DIGITAL AUTOPILOT MUST BE GIVEN AN ATTITUDE REFERENCE WHICH IT CAN TRACK. 014326,000061: # KALCMANU DOES THIS BY GENERATING A REFERENCE OF DESIRED GIMBAL ANGLES (CDUXD, CDUYD, CDUZD) WHICH ARE UPDATED 014327,000062: # EVERY ONE SECOND DURING THE MANEUVER. TO ACHIEVE A SMOOTHER SEQUENCE OF COMMANDS BETWEEN SUCCESSIVE UPDATES, 014328,000063: # THE PROGRAM ALSO GENERATES A SET OF INCREMENTAL CDU ANGLES (DELDCDU) TO BE ADDED TO CDU DESIRED BY THE DIGITAL 014329,000064: # AUTOPILOT. KALCMANU ALSO CALCULATES THE COMPONENT MANEUVER RATES (OMEGAPD, OMEGAQD, OMEGARD), WHICH CAN 014330,000065: # - 014331,000066: # BE DETERMINED SIMPLY BY MULTIPLYING COF BY SOME SCALAR (ARATE) CORRESPONDING TO THE DESIRED ROTATIONAL RATE. 014332,000067: 014333,000068: # AUTOMATIC MANEUVERS ARE TIMED WITH THE HELP OF WAITLIST SO THAT AFTER A SPECIFIED INTERVAL THE Y AND Z 014334,000069: # DESIRED RATES ARE SET TO ZERO AND THE DESIRED CDU ANGLES (CDUYD, CDUZD) ARE SET EQUAL TO THE FINAL DESIRED CDU 014335,000070: # ANGLES (CTHETA, CPSI). IF ANY YAW REMAINS DUE TO GIMBAL LOCK AVOIDANCE, THE FINAL YAW MANEUVER IS 014336,000071: # CALCULATED AND THE DESIRED YAW RATE SET TO SOME FIXED VALUE (ROLLRATE = + OR - 2 DEGREES PER SEC). 014337,000072: # IN THIS CASE ONLY AN INCREMENTAL CDUX ANGLE (DELFROLL) IS SUPPLIED TO THE DAP. AT THE END OF THE YAW 014338,000073: # MANEUVER OR IN THE EVENT THAT THERE WAS NO FINAL YAW, CDUXD IS SET EQUAL TO CPHI AND THE X-AXIS DESIRED 014339,000074: # RATE SET TO ZERO. THUS, UPON COMPLETION OF THE MANEUVER THE S/C WILL FINISH UP IN A LIMIT CYCLE ABOUT THE 014340,000075: # DESIRED FINAL GIMBAL ANGLES. 014341,000076: 014342,000077: # PROGRAM LOGIC FLOW 014343,000078: 014344,000079: # KALCMANU IS CALLED AS A HIGH PRIORITY JOB WITH ENTRY POINTS AT KALCMAN3 AND VECPOINT. IT FIRST PICKS 014345,000080: # UP THE CURRENT CDU ANGLES TO BE USED AS THE BASIS FOR ALL COMPUTATIONS INVOLVING THE INITIAL S/C ORIENTATION.
![]() |
Page 345 |
014347,000082: # IT THEN DETERMINES THE DIRECTION COSINE MATRICES RELATING BOTH THE INITIAL AND FINAL S/C ORIENTATION TO STABLE 014348,000083: # * * * 014349,000084: # MEMBER AXES (MIS,MFS). IT ALSO COMPUTES THE MATRIX RELATING FINAL S/C AXES TO INITIAL S/C AXES (MFI). THE 014350,000085: # ANGLE OF ROTATION (AM) IS THEN EXTRACTED FROM THIS MATRIX, AND TESTS ARE MADE TO DETERMINE IF 014351,000086: 014352,000087: # A) AM LESS THAN .25 DEGREES (MINANG) 014353,000088: # B) AM GREATER THAN 170 DEGREES (MAXANG) 014354,000089: 014355,000090: # IF AM LESS THAN .25 DEGREES, NO COMPLICATED AUTOMATIC MANEUVERING IS NECESSARY. THEREFORE WE CAN SIMPLY 014356,000091: # SET CDU DESIRED EQUAL TO THE FINAL CDU DESIRED ANGLES AND TERMINATE THE JOB. 014357,000092: 014358,000093: # IF AM IS GREATER THAN .25 DEGREES BUT LESS THAN 170 DEGREES, THE AXES OF THE SINGLE EQUIVALENT ROTATION 014359,000094: # - * 014360,000095: # (COF) IS EXTRACTED FROM THE SKEW SYMMETRIC COMPONENTS OF MFI. * * 014361,000096: # IF AM GREATER THAN 170 DEGREES AN ALTERNATE METHOD EMPLOYING THE SYMMETRIC PART OF MFI (MFISYM) IS USED 014362,000097: # - 014363,000098: # TO DETERMINE COF. 014364,000099: 014365,000100: # THE PROGRAM THEN CHECKS TO SEE IF THE MANEUVER AS COMPUTED WILL BRING THE S/C THROUGH GIMBAL LOCK. IF 014366,000101: # SO, A NEW MANEUVER IS CALCULATED WHICH WILL JUST SKIM THE GIMBAL LOCK ZONE AND ALIGN THE S/C X-AXIS. THIS 014367,000102: # METHOD ASSURES THAT THE ADDITIONAL MANEUVERING TO AVOID GIMBAL LOCK WILL BE KEPT TO A MINIMUM. SINCE A FINAL 014368,000103: # P AXIS YAW WILL BE NECESSARY, A SWITCH IS RESET (STATE SWITCH 31) TO ALLOW FOR THE COMPUTATION OF THIS FINAL 014369,000104: # YAW. 014370,000105: 014371,000106: # AS STATED PREVIOUSLY KALCMANU GENERATES A SEQUENCE OF DESIRED GIMBAL ANGLES WHICH ARE UPDATED EVERY 014372,000107: # - 014373,000108: # SECOND. THIS IS ACCOMPLISHED BY A SMALL ROTATION OF THE DESIRED S/C FRAME ABOUT THE VECTOR COF. THE NEW 014374,000109: # DESIRED REFERENCE MATRIX IS THEN, 014375,000110: # * * * 014376,000111: # MIS = MIS DEL 014377,000112: # N+1 N 014378,000113: # * 014379,000114: # WHERE DEL IS THE MATRIX CORRESPONDING TO THIS SMALL ROTATION. THE NEW CDU ANGLES CAN THEN BE EXTRACTED 014380,000115: # * 014381,000116: # FROM MIS. 014382,000117: 014383,000118: # AT THE BEGINNING OF THE MANEUVER THE AUTOPILOT DESIRED RATES (OMEGAPD, OMEGAQD, OMEGARD) AND THE 014384,000119: # MANEUVER TIMINGS ARE ESTABLISHED. ON THE FIRST PASS AND ON ALL SUBSEQUENT UPDATES THE CDU DESIRED 014385,000120: # ANGLES ARE LOADED WITH THE APPROPRIATE VALUES AND THE INCREMENTAL CDU ANGLES ARE COMPUTED. THE AGC CLOCKS 014386,000121: # (TIME1 AND TIME2) ARE THAN CHECKED TO SEE IF THE MANEUVER WILL TERMINATE BEFORE THE NEXT UPDATE. IF 014387,000122: # NOT, KALCMANU CALLS FOR ANOTHER UPDATE (RUN AS A JOB WITH PRIORITY TBD) IN ONE SECOND. ANY DELAYS IN THIS 014388,000123: # CALLING SEQUENCE ARE AUTOMATICALLY COMPENSATED IN CALLING FOR THE NEXT UPDATE. 014389,000124: 014390,000125: # IF IT IS FOUND THAT THE MANEUVER IS TO TERMINATE BEFORE THE NEXT UPDATE A ROUTINE IS CALLED (AS A WAIT- 014391,000126: # LIST TASK) TO STOP THE MANEUVER AT THE APPROPRIATE TIME AS EXPLAINED ABOVE. 014392,000127:
![]() |
Page 346 |
014394,000129: # CALLING SEQUENCE 014395,000130: 014396,000131: # IN ORDER TO PERFORM A KALCMANU SUPERVISED MANEUVER, THE COMMANDED GIMBAL ANGLES MUST BE PRECOMPUTED AND 014397,000132: # STORED IN LOCATIONS CPHI, CTHETA, CPSI. THE USER:S PROGRAM MUST THEN CLEAR STATE SWITCH NO 33 TO ALLOW THE 014398,000133: # ATTITUDE MANEUVER ROUTINE TO PERFORM ANY FINAL P-AXIS YAW INCURRED BY AVOIDING GIMBAL LOCK. THE MANEUVER IS 014399,000134: # THEN INITIATED BY ESTABLISHING THE FOLLOWING EXECUTIVE JOB 014400,000135: # * 014401,000136: # CAF PRIO XX 014402,000137: # -- 014403,000138: # INHINT 014404,000139: # TC FINDVAC 014405,000140: # 2CADR KALCMAN3 014406,000141: # RELINT 014407,000142: 014408,000143: # THE USER:S PROGRAM MAY EITHER CONTINUE OR WAIT FOR THE TERMINATION OF THE MANEUVER. IF THE USER WISHES TO 014409,000144: # WAIT, HE MAY PUT HIS JOB TO SLEEP WITH THE FOLLOWING INSTRUCTIONS 014410,000145: 014411,000146: # L TC BANKCALL 014412,000147: # L+1 CADR ATTSTALL 014413,000148: # L+2 (BAD RETURN) 014414,000149: # L+3 (GOOD RETURN) 014415,000150: 014416,000151: # UPON COMPLETION OF THE MANEUVER, THE PROGRAM WILL BE AWAKENED AT L+3 IF THE MANEUVER WAS COMPLETED 014417,000152: # SUCCESSFULLY, OR AT L+2 IF THE MANEUVER WAS ABORTED. THIS ABORT WOULD OCCUR IF THE INITIAL OR FINAL ATTITUDE 014418,000153: # WAS IN GIMBAL LOCK. 014419,000154: 014420,000155: # *** NOTA BENE *** IT IS ASSUMED THAT THE DESIRED MANEUVERING RATE (0.5, 2, 5, 10, DEG/SEC) HAS BEEN SELECTED BY 014421,000156: # KEYBOARD ENTRY PRIOR TO THE EXECUTION OF KALCMANU. 014422,000157: 014423,000158: # IT IS ALSO ASSUMED THAT THE AUTOPILOT IS IN THE AUTO MODE. IF THE MODE SWITCH IS CHANGED DURING THE 014424,000159: # MANEUVER, KALCMANU WILL TERMINATE VIA GOODEND WITHIN 1 SECOND SO THAT R60 MAY REQUEST A TRIM OF THE S/C ATTITUDE 014425,000160: # THIS IS THE ONLY MEANS FOR MANUALLY TERMINATING A KALCMANU SUPERVISED MANEUVER. 014426,000161: # SUBROUTINES 014427,000162: 014428,000163: # KALCMANU USES A NUMBER OF INTERPRETIVE SUBROUTINES WHICH MAY BE OF GENERAL INTEREST. SINCE THESE ROUTINES 014429,000164: # WERE PROGRAMMED EXCLUSIVELY FOR KALCMANU, THEY ARE NOT, AS YET, GENERALLY AVAILABLE FOR USE BY OTHER PROGRAMS. 014430,000165: 014431,000166: # MXM3 014432,000167: # ---- 014433,000168: 014434,000169: # THIS SUBROUTINE MULTIPLIES TWO 3X3 MATRICES AND LEAVES THE RESULT IN THE FIRST 18 LOCATIONS OF THE PUSH 014435,000170: # DOWN LIST, I.E., 014436,000171: # ( M M M ) 014437,000172: # ( 0 1 2 ) 014438,000173: # * ( ) * * 014439,000174: # M = ( M M M ) = M1 X M2 014440,000175: # ( 3 4 5 ) 014441,000176: # ( ) 014442,000177: # ( M M M )
![]() |
Page 347 |
014444,000179: # ( 6 7 8 ) 014445,000180: # * 014446,000181: # INDEX REGISTER X1 MUST BE LOADED WITH THE COMPLEMENT OF THE STARTING ADDRESS FOR M1, AND X2 MUST BE 014447,000182: # * 014448,000183: # LOADED WITH THE COMPLEMENT OF THE STARTING ADDRESS FOR M2. THE ROUTINE USES THE FIRST 20 LOCATIONS OF THE PUSH 014449,000184: # DOWN LIST. THE FIRST ELEMENT OF THE MATRIX APPEARS IN PDO. PUSH UP FOR M . 014450,000185: # 8 014451,000186: # TRANSPOS 014452,000187: # -------- 014453,000188: 014454,000189: # THIS ROUTINE TRANSPOSES A 3X3 MATRIX AND LEAVES THE RESULT IN THE PUSH DOWN LIST, I.E., 014455,000190: # 014456,000191: # * * T 014457,000192: # M = M1 014458,000193: 014459,000194: # INDEX REGISTER X1 MUST CONTAIN THE COMPLEMENT OF THE STARTING ADDRESS FOR M1. PUSH UP FOR THE FIRST AND SUB- 014460,000195: # * 014461,000196: # SEQUENT COMPONENTS OF M. THIS SUBROUTINE ALSO USES THE FIRST 20 LOCATIONS OF THE PUSH DOWN LIST. 014462,000197: 014463,000198: # CDU TO DCM 014464,000199: # ---------- 014465,000200: 014466,000201: # THIS SUBROUTINE CONVERTS THREE CDU ANGLES IN T(MPAC) TO A DIRECTION COSINE MATRIX (SCALED BY 2) RELATING 014467,000202: # THE CORRESPONDING S/C ORIENTATIONS TO THE STABLE MEMBER FRAME. THE FORMULAS FOR THIS CONVERSION ARE 014468,000203: 014469,000204: # M = COSY COSZ 014470,000205: # 0 014471,000206: 014472,000207: # M = -COSY SINZ COSX + SINY SINX 014473,000208: # 1 014474,000209: 014475,000210: # M = COSY SINZ SINX + SINY COSX 014476,000211: # 2 014477,000212: 014478,000213: # M = SINZ 014479,000214: # 3 014480,000215: 014481,000216: # M = COSZ COSX 014482,000217: # 4 014483,000218: 014484,000219: # M = -COSZ SINX 014485,000220: # 5 014486,000221: 014487,000222: # M = -SINY COSZ 014488,000223: # 6 014489,000224: # 014490,000225: # M = SINY SINZ COSX + COSY SINX 014491,000226: # 7
![]() |
Page 348 |
014493,000228: # M = -SINY SINZ SINX + COSY COSX 014494,000229: # 8 014495,000230: 014496,000231: # WHERE X = OUTER GIMBAL ANGLE 014497,000232: # Y = INNER GIMBAL ANGLE 014498,000233: # Z = MIDDLE GIMBAL ANGLE 014499,000234: 014500,000235: # THE INTERPRETATION OF THIS MATRIX IS AS FOLLOWS 014501,000236: 014502,000237: # IF A , A , A REPRESENT THE COMPONENTS OF A VECTOR IN S/C AXES THEN THE COMPONENTS OF THE SAME VECTOR IN 014503,000238: # X Y Z 014504,000239: # STABLE MEMBER AXES (B , B , B ) ARE 014505,000240: # X Y Z 014506,000241: 014507,000242: # ( B ) ( A ) 014508,000243: # ( X ) ( X ) 014509,000244: # ( ) ( ) 014510,000245: # ( ) * ( ) 014511,000246: # ( B ) = M ( A ) 014512,000247: # ( Y ) ( Y ) 014513,000248: # ( ) ( ) 014514,000249: # ( B ) ( A ) 014515,000250: # ( Z ) ( Z ) 014516,000251: 014517,000252: # THE SUBROUTINE WILL STORE THIS MATRIX IN SEQUENTIAL LOCATIONS OF ERASABLE MEMORY AS SPECIFIED BY THE CALLING 014518,000253: # * 014519,000254: # PROGRAM. TO DO THIS THE CALLING PROGRAM MUST FIRST LOAD X2 WITH THE COMPLEMENT OF THE STARTING ADDRESS FOR M. 014520,000255: 014521,000256: # INTERNALLY, THE ROUTINE USES THE FIRST 16 LOCATIONS OF THE PUSH DOWN LIST, ALSO STEP REGISTER S1 AND INDEX 014522,000257: # REGISTER X2. 014523,000258: 014524,000259: # DCM TO CDU 014525,000260: # ---------- 014526,000261: # * 014527,000262: # THIS ROUTINE EXTRACTS THE CDU ANGLES FROMA DIRECTION COSINE MATRIX (M SCALED BY 2) RELATING S/C AXIS TO 014528,000263: # * 014529,000264: # STABLE MEMBER AXES. X1 MUST CONTAIN THE COMPLEMENT OF THE STARTING ADDRESS FOR M. THE SUBROUTINE LEAVES THE 014530,000265: # CORRESPONDING GIMBAL ANGLES IN V(MPAC) AS DOUBLE PRECISION 1:S COMPLEMENT ANGLES SCALED BY 2PI. THE FORMULAS 014531,000266: # FOR THIS CONVERSION ARE 014532,000267: 014533,000268: # Z = ARCSIN (M ) 014534,000269: # 3 014535,000270: 014536,000271: # Y = ARCSIN (-M /COSZ) 014537,000272: # 6 014538,000273: 014539,000274: # IF M IS NEGATIVE, Y IS REPLACED BY PI SGN Y - Y 014540,000275: # 0
![]() |
Page 349 |
014542,000277: # X = ARCSIN (-M /COSZ) 014543,000278: # 5 014544,000279: 014545,000280: # IF M IS NEGATIVE X IS REPLACED BY PI SGN X - X 014546,000281: # 4 014547,000282: 014548,000283: # THIS ROUTINE DOES NOT SET THE PUSH DOWN POINTER, BUT USES THE NEXT 8 LOCATIONS OF THE PUSH DOWN LIST AND 014549,000284: # RETURNS THE POINTER TO ITS ORIGINAL SETTING. THIS PROCEDURE ALLOWS THE CALLER TO STORE THE MATRIX AT THE TOP OF 014550,000285: # THE PUSH DOWN LIST. 014551,000286: 014552,000287: # DELCOMP 014553,000288: # ------- 014554,000289: # * 014555,000290: # THIS ROUTINE COMPUTES THE DIRECTION COSINE MATRIX (DEL) RELATING ON 014556,000291: # - 014557,000292: # IS ROTATED WITH RESPECT TO THE FIRST BY AN ANGLE, A, ABOUT A UNIT VECTOR, U. THE FORMULA FOR THIS MATRIX IS 014558,000293: 014559,000294: # * * - -T * 014560,000295: # DEL = I COSA + U U (1 - COSA) + V SINA 014561,000296: # X 014562,000297: 014563,000298: # WHERE * ( 1 0 0 ) 014564,000299: # I = ( 0 1 0 ) 014565,000300: # ( 0 0 1 ) 014566,000301: 014567,000302: # 2 014568,000303: # ( U U U U U ) 014569,000304: # ( X X Y X Z ) 014570,000305: # ( ) 014571,000306: # - -T ( 2 ) 014572,000307: # U U = ( U U U U U ) 014573,000308: # ( Y X Y Y Z ) 014574,000309: # ( ) 014575,000310: # ( 2 ) 014576,000311: # ( U U U U U ) 014577,000312: # ( Z X Z Y Z ) 014578,000313: 014579,000314: 014580,000315: # ( 0 -U U ) 014581,000316: # ( Z Y ) 014582,000317: # * ( ) 014583,000318: # V = ( U 0 -U ) 014584,000319: # X ( Z X ) 014585,000320: # ( ) 014586,000321: # ( -U U 0 ) 014587,000322: # ( Y X ) 014588,000323:
![]() |
Page 350 |
014590,000325: # - 014591,000326: # U = UNIT ROTATION VECTOR RESOLVED INTO S/C AXES 014592,000327: # A = ROTATION ANGLE 014593,000328: 014594,000329: # * 014595,000330: # THE INTERPRETATION OF DEL IS AS FOLLOWS 014596,000331: 014597,000332: # IF A , A , A REPRESENT THE COMPONENT OF A VECTOR INTHE ROTATED FRAME, THEN THE COMPONENTS OF THE SAME 014598,000333: # X Y Z 014599,000334: # VECTOR IN THE ORIGINAL S/C AXES (B , B , B ) ARE 014600,000335: # X Y Z 014601,000336: 014602,000337: # ( B ) ( A ) 014603,000338: # ( X ) ( X ) 014604,000339: # ( ) * ( ) 014605,000340: # ( B ) = DEL ( A ) 014606,000341: # ( Y ) ( Y ) 014607,000342: # ( ) ( ) 014608,000343: # ( B ) ( A ) 014609,000344: # ( Z ) ( Z ) 014610,000345: 014611,000346: # THE ROUTINE WILL STORE THIS MATRIX (SCALED UNITY) IN SEQUENTIAL LOCATIONS OF ERASABLE MEMORY BEGINNING WITH 014612,000347: # - 014613,000348: # THE LOCATION CALLED DEL. IN ORDER TO USE THE ROUTINE, THE CALLING PROGRAM MUST FIRST STORE U (A HALF UNIT 014614,000349: # DOUBLE PRECISION VECTOR) IN THE SET OF ERASABLE LOCATIONS BEGINNING WITH THE ADDRESS CALLED COF. THE ANGLE, A, 014615,000350: # MUST THEN BE LOADED INTO D(MPAC). 014616,000351: # 014617,000352: # INTERNALLY, THE PROGRAM ALSO USES THE FIRST 10 LOCATIONS OF THE PUSH DOWN LIST. 014618,000353: 014619,000354: # READCDUK 014620,000355: # -------- 014621,000356: 014622,000357: # THIS BASIC LANGUAGE SUBROUTINE LOADS T(MPAC) WITH THE THREE CDU ANGLES. 014623,000358: 014624,000359: # SIGNMPAC 014625,000360: # -------- 014626,000361: 014627,000362: # THIS IS A BASIC LANGUAGE SUBROUTINE WHICH LIMITS THE MAGNITUDE OF D(MPAC) TO + OR - DPOSMAX ON OVERFLOW. 014628,000363: 014629,000364: # PROGRAM STORAGE ALLOCATION 014630,000365: 014631,000366: # 1) FIXED MEMORY 1059 WORDS 014632,000367: # 2) ERASABLE MEMORY 98 014633,000368: # 3) STATE SWITCHES 3
![]() |
Page 351 |
014635,000370: # 4) FLAGS 1 014636,000371: 014637,000372: # JOB PRIORITIES 014638,000373: 014639,000374: # 1) KALCMANU TBD 014640,000375: # 2) ONE SECOND UPDATE TBD 014641,000376: 014642,000377: # SUMMARY OF STATE SWITCHES AND FLAGWORDS USED BY KALCMANU. 014643,000378: 014644,000379: # STATE FLAGWRD 2 SETTING MEANING 014645,000380: # SWITCH NO. BIT NO. 014646,000381: 014647,000382: # * 014648,000383: # 31 14 0 MANEUVER WENT THROUGH GIMBAL LOCK 014649,000384: # 1 MANEUVER DID NOT GO THROUGH GIMBAL LOCK 014650,000385: # * 014651,000386: # 32 13 0 CONTINUE UPDATE PROCESS 014652,000387: # 1 START UPDATE PROCESS 014653,000388: 014654,000389: # 33 12 0 PERFORM FINAL P-AXIS YAW IF REQUIRED 014655,000390: # 1 IGNORE ANY FINAL P-AXIS YAW 014656,000391: 014657,000392: # 34 11 0 SIGNAL END OF KALCMANU 014658,000393: # 1 KALCMANU IN PROCESS USER MUST SET SWITCH BEFORE INITIATING 014659,000394: 014660,000395: # * INTERNAL TO KALCMANU 014661,000396: 014662,000397: # SUGGESTIONS FOR PROGRAM INTEGRATION 014663,000398: 014664,000399: # THE FOLLOWING VARIABLES SHOULD BE ASSIGNED TO UNSWITCH ERASABLE 014665,000400: 014666,000401: # CPHI 014667,000402: # CTHETA 014668,000403: # CPSI 014669,000404: # POINTVSM +5 014670,000405: # SCAXIS +5 014671,000406: # DELDCDU 014672,000407: # DELDCDU1 014673,000408: # DELDCDU2 014674,000409: # RATEINDX 014675,000410: 014676,000411: # THE FOLLOWING SUBROUTINES MAY BE PUT IN A DIFFERENT BANK 014677,000412: 014678,000413: # MXM3
![]() |
Page 352 |
014680,000415: # TRANSPOS 014681,000416: # SIGNMPAC 014682,000417: # READCDUK 014683,000418: # CDUTODCM 014684,000419:
![]() |
Page 353 |
014686,000421: 15,2050 BANK 15 014687,000422: 22,2000 SETLOC KALCMON1 014688,000423: 22,2000 BANK 014689,000424: 014690,000425: 22,2004 E6,1675 EBANK= BCDU 014691,000426: 014692,000427: # THE THREE DESIRED CDU ANGLES MUST BE STORED AS SINGLE PRECISION TWOS COMPLEMENT ANGLES IN THE THREE SUCCESSIVE 014693,000428: # LOCATIONS, CPHI, CTHETA, CPSI. 014694,000429: 014695,000430: 22,2004 COUNT* $$/KALC 014696,000431: 22,2004 06042 KALCMAN3 TC INTPRET # PICK UP THE CURRENT CDU ANGLES AND 014697,000432: 22,2005 77634 RTB # COMPUTE THE MATRIX FROM INITIAL S/C 014698,000433: 22,2006 44403 READCDUK # AXES TO FINAL S/C AXES 014699,000434: 22,2007 03276 STORE BCDU # STORE INITIAL S/C ANGLES 014700,000435: 22,2010 51535 SLOAD ABS # CHECK THE MAGNITUDE OF THE DESIRED 014701,000436: 22,2011 00324 CPSI # MIDDLE GIMBAL ANGLE 014702,000437: 22,2012 51025 DSU BPL 014703,000438: 22,2013 04403 LOCKANGL # IF GREATER THAN 70 DEG ABORT MANEUVER 014704,000439: 22,2014 44724 TOOBADF 014705,000440: 22,2015 72364 AXC,2 TLOAD 014706,000441: 22,2016 03245 MIS 014707,000442: 22,2017 03276 BCDU 014708,000443: 22,2020 77624 CALL # COMPUTE THE TRANSFORMATION FROM INITIAL 014709,000444: 22,2021 44410 CDUTODCM # S/C AXES TO STABLE MEMBER AXES 014710,000445: 22,2022 72364 AXC,2 TLOAD 014711,000446: 22,2023 02230 MFS # PREPARE TO CALCULATE ARRAY MFS 014712,000447: 22,2024 00322 CPHI 014713,000448: 22,2025 77624 CALL 014714,000449: 22,2026 44410 CDUTODCM 014715,000450: 22,2027 45160 SECAD AXC,1 CALL # MIS AND MFS ARRAYS CALCULATED $2 014716,000451: 22,2030 03245 MIS 014717,000452: 22,2031 44326 TRANSPOS 014718,000453: 22,2032 45575 VLOAD STADR 014719,000454: 22,2033 50460 STOVL TMIS +12D 014720,000455: 22,2034 77626 STADR 014721,000456: 22,2035 50466 STOVL TMIS +6 014722,000457: 22,2036 77626 STADR 014723,000458: 22,2037 74474 STORE TMIS # TMIS = TRANSPOSE(MIS) SCALED BY 2 014724,000459: 22,2040 75160 AXC,1 AXC,2 014725,000460: 22,2041 03302 TMIS 014726,000461: 22,2042 02230 MFS 014727,000462: 22,2043 77624 CALL 014728,000463: 22,2044 44312 MXM3 014729,000464: 22,2045 45575 VLOAD STADR 014730,000465: 22,2046 51532 STOVL MFI +12D 014731,000466: 22,2047 77626 STADR 014732,000467: 22,2050 51540 STOVL MFI +6 014733,000468: 22,2051 77626 STADR 014734,000469: 22,2052 75546 STORE MFI # MFI = TMIS MFS (SCALED BY 4) 014735,000470: 22,2053 45001 SETPD CALL # TRANSPOSE MFI IN PD LIST
![]() |
Page 354 |
014737,000472: 22,2054 00023 18D 014738,000473: 22,2055 44335 TRNSPSPD 014739,000474: 22,2056 45575 VLOAD STADR 014740,000475: 22,2057 50460 STOVL TMFI +12D 014741,000476: 22,2060 77626 STADR 014742,000477: 22,2061 50466 STOVL TMFI +6 014743,000478: 22,2062 77626 STADR 014744,000479: 22,2063 74474 STORE TMFI # TMFI = TRANSPOSE (MFI) SCALED BY 4 014745,000480: 014746,000481: # CALCULATE COFSKEW AND MFISYM 014747,000482: 014748,000483: 22,2064 45345 DLOAD DSU 014749,000484: 22,2065 03305 TMFI +2 014750,000485: 22,2066 02233 MFI +2 014751,000486: 22,2067 45325 PDDL DSU # CALCULATE COF SCALED BY 2/SIN(AM) 014752,000487: 22,2070 02235 MFI +4 014753,000488: 22,2071 03307 TMFI +4 014754,000489: 22,2072 45325 PDDL DSU 014755,000490: 22,2073 03315 TMFI +10D 014756,000491: 22,2074 02243 MFI +10D 014757,000492: 22,2075 77666 VDEF 014758,000493: 22,2076 03325 STORE COFSKEW # EQUALS MFISKEW 014759,000494: 014760,000495: # CALCULATE AM AND PROCEED ACCORDING TO ITS MAGNITUDE 014761,000496: 014762,000497: 22,2077 43345 DLOAD DAD 014763,000498: 22,2100 02231 MFI 014764,000499: 22,2101 02251 MFI +16D 014765,000500: 22,2102 43225 DSU DAD 014766,000501: 22,2103 06514 DP1/4TH 014767,000502: 22,2104 02241 MFI +8D 014768,000503: 22,2105 03333 STORE CAM # CAM = (MFI0+MFI4+MFI8-1)/2 HALF SCALE 014769,000504: 22,2106 77726 ARCCOS 014770,000505: 22,2107 03335 STORE AM # AM=ARCCOS(CAM) (AM SCALED BY 2) 014771,000506: 22,2110 51025 DSU BPL 014772,000507: 22,2111 04363 MINANG 014773,000508: 22,2112 44117 CHECKMAX 014774,000509: 22,2113 77751 TLOAD # MANEUVER LESS THAN .25 DEGREES 014775,000510: 22,2114 00322 CPHI # GO DIRECTLY INTO ATTITUDE HOLD 014776,000511: 22,2115 37235 STCALL CDUXD # ABOUT COMMANDED ANGLES 014777,000512: 22,2116 44742 TOOBADI # STOP RATE AND EXIT 014778,000513: 014779,000514: 22,2117 45345 CHECKMAX DLOAD DSU 014780,000515: 22,2120 03335 AM 014781,000516: 22,2121 04365 MAXANG 014782,000517: 22,2122 77244 BPL VLOAD 014783,000518: 22,2123 44131 ALTCALC # UNIT 014784,000519: 22,2124 03325 COFSKEW # COFSKEW 014785,000520: 22,2125 77656 UNIT 014786,000521: 22,2126 03270 STORE COF # COF IS THE MANEUVER AXIS
![]() |
Page 355 |
014788,000523: 22,2127 77650 GOTO # SEE IF MANEUVER GOES THRU GIMBAL LOCK 014789,000524: 22,2130 44744 LOCSKIRT 014790,000525: 22,2131 53375 ALTCALC VLOAD VAD # IF AM GREATER THAN 170 DEGREES 014791,000526: 22,2132 02231 MFI 014792,000527: 22,2133 03303 TMFI 014793,000528: 22,2134 77762 VSR1 014794,000529: 22,2135 27303 STOVL MFISYM 014795,000530: 22,2136 02237 MFI +6 014796,000531: 22,2137 74455 VAD VSR1 014797,000532: 22,2140 03311 TMFI +6 014798,000533: 22,2141 27311 STOVL MFISYM +6 014799,000534: 22,2142 02245 MFI +12D 014800,000535: 22,2143 74455 VAD VSR1 014801,000536: 22,2144 03317 TMFI +12D 014802,000537: 22,2145 03317 STORE MFISYM +12D # MFISYM=(MFI+TMFI)/2 SCALED BY 4 014803,000538: 014804,000539: # CALCULATE COF 014805,000540: 014806,000541: 22,2146 70545 DLOAD SR1 014807,000542: 22,2147 03333 CAM 014808,000543: 22,2150 45325 PDDL DSU # PDO CAM $4 014809,000544: 22,2151 06522 DPHALF 014810,000545: 22,2152 03333 CAM 014811,000546: 22,2153 65204 BOVB PDDL # PD2 1 - CAM $2 014812,000547: 22,2154 21713 SIGNMPAC 014813,000548: 22,2155 03323 MFISYM +16D 014814,000549: 22,2156 56225 DSU DDV 014815,000550: 22,2157 00001 0 014816,000551: 22,2160 00003 2 014817,000552: 22,2161 65366 SQRT PDDL # COFZ = SQRT(MFISYM8-CAM)/1-CAM) 014818,000553: 22,2162 03313 MFISYM +8D # $ ROOT 2 014819,000554: 22,2163 56225 DSU DDV 014820,000555: 22,2164 00001 0 014821,000556: 22,2165 00003 2 014822,000557: 22,2166 65366 SQRT PDDL # COFY = SQRT(MFISYM4-CAM)/(1-CAM) $ROOT2 014823,000558: 22,2167 03303 MFISYM 014824,000559: 22,2170 56225 DSU DDV 014825,000560: 22,2171 00001 0 014826,000561: 22,2172 00003 2 014827,000562: 22,2173 55566 SQRT VDEF # COFX = SQRT(MFISYM-CAM)/(1-CAM) $ROOT 2 014828,000563: 22,2174 77656 UNIT 014829,000564: 22,2175 03270 STORE COF 014830,000565: 014831,000566: # DETERMINE LARGEST COF AND ADJUST ACCORDINGLY 014832,000567: 014833,000568: 22,2176 45345 COFMAXGO DLOAD DSU 014834,000569: 22,2177 03270 COF 014835,000570: 22,2200 03272 COF +2 014836,000571: 22,2201 71240 BMN DLOAD # COFY G COFX
![]() |
Page 356 |
014838,000573: 22,2202 44211 COMP12 014839,000574: 22,2203 03270 COF 014840,000575: 22,2204 50025 DSU BMN 014841,000576: 22,2205 03274 COF +4 014842,000577: 22,2206 44266 METHOD3 # COFZ G COFX OR COFY 014843,000578: 22,2207 77650 GOTO 014844,000579: 22,2210 44242 METHOD1 # COFX G COFY OR COFZ 014845,000580: 22,2211 45345 COMP12 DLOAD DSU 014846,000581: 22,2212 03272 COF +2 014847,000582: 22,2213 03274 COF +4 014848,000583: 22,2214 77640 BMN 014849,000584: 22,2215 44266 METHOD3 # COFZ G COFY OR COFX 014850,000585: 014851,000586: 22,2216 51145 METHOD2 DLOAD BPL # COFY MAX 014852,000587: 22,2217 03327 COFSKEW +2 # UY 014853,000588: 22,2220 44224 U2POS 014854,000589: 22,2221 57575 VLOAD VCOMP 014855,000590: 22,2222 03270 COF 014856,000591: 22,2223 03270 STORE COF 014857,000592: 22,2224 51145 U2POS DLOAD BPL 014858,000593: 22,2225 03305 MFISYM +2 # UX UY 014859,000594: 22,2226 44232 OKU21 014860,000595: 22,2227 57545 DLOAD DCOMP # SIGN OF UX OPPOSITE TO UY 014861,000596: 22,2230 03270 COF 014862,000597: 22,2231 03270 STORE COF 014863,000598: 22,2232 51145 OKU21 DLOAD BPL 014864,000599: 22,2233 03315 MFISYM +10D # UY UZ 014865,000600: 22,2234 44744 LOCSKIRT 014866,000601: 22,2235 57545 DLOAD DCOMP # SIGN OF UZ OPPOSITE TO UY 014867,000602: 22,2236 03274 COF +4 014868,000603: 22,2237 03274 STORE COF +4 014869,000604: 22,2240 77650 GOTO 014870,000605: 22,2241 44744 LOCSKIRT 014871,000606: 22,2242 51145 METHOD1 DLOAD BPL # COFX MAX 014872,000607: 22,2243 03325 COFSKEW # UX 014873,000608: 22,2244 44250 U1POS 014874,000609: 22,2245 57575 VLOAD VCOMP 014875,000610: 22,2246 03270 COF 014876,000611: 22,2247 03270 STORE COF 014877,000612: 22,2250 51145 U1POS DLOAD BPL 014878,000613: 22,2251 03305 MFISYM +2 # UX UY 014879,000614: 22,2252 44256 OKU12 014880,000615: 22,2253 57545 DLOAD DCOMP 014881,000616: 22,2254 03272 COF +2 # SIGN OF UY OPPOSITE TO UX 014882,000617: 22,2255 03272 STORE COF +2 014883,000618: 22,2256 51145 OKU12 DLOAD BPL 014884,000619: 22,2257 03307 MFISYM +4 # UX UZ 014885,000620: 22,2260 44744 LOCSKIRT 014886,000621: 22,2261 57545 DLOAD DCOMP # SIGN OF UZ OPPOSITE TO UY 014887,000622: 22,2262 03274 COF +4
![]() |
Page 357 |
014889,000624: 22,2263 03274 STORE COF +4 014890,000625: 22,2264 77650 GOTO 014891,000626: 22,2265 44744 LOCSKIRT 014892,000627: 22,2266 51145 METHOD3 DLOAD BPL # COFZ MAX 014893,000628: 22,2267 03331 COFSKEW +4 # UZ 014894,000629: 22,2270 44274 U3POS 014895,000630: 22,2271 57575 VLOAD VCOMP 014896,000631: 22,2272 03270 COF 014897,000632: 22,2273 03270 STORE COF 014898,000633: 22,2274 51145 U3POS DLOAD BPL 014899,000634: 22,2275 03307 MFISYM +4 # UX UZ 014900,000635: 22,2276 44302 OKU31 014901,000636: 22,2277 57545 DLOAD DCOMP 014902,000637: 22,2300 03270 COF # SIGN OF UX OPPOSITE TO UZ 014903,000638: 22,2301 03270 STORE COF 014904,000639: 22,2302 51145 OKU31 DLOAD BPL 014905,000640: 22,2303 03315 MFISYM +10D # UY UZ 014906,000641: 22,2304 44744 LOCSKIRT 014907,000642: 22,2305 57545 DLOAD DCOMP 014908,000643: 22,2306 03272 COF +2 # SIGN OF UY OPPOSITE TO UZ 014909,000644: 22,2307 03272 STORE COF +2 014910,000645: 22,2310 77650 GOTO 014911,000646: 22,2311 44744 LOCSKIRT
![]() |
Page 358 |
014913,000648: # MATRIX OPERATIONS 014914,000649: 014915,000650: 13,2207 BANK 13 014916,000651: 22,2000 SETLOC KALCMON2 014917,000652: 22,2000 BANK 014918,000653: 014919,000654: 22,2312 E6,1675 EBANK= BCDU 014920,000655: 014921,000656: 22,2312 76601 MXM3 SETPD VLOAD* # MXM3 MULTIPLIES 2 3X3 MATRICES 014922,000657: 22,2313 00001 0 # AND LEAVES RESULT IN PD LIST 014923,000658: 22,2314 00001 0,1 # AND MPAC 014924,000659: 22,2315 62703 VXM* PDVL* 014925,000660: 22,2316 77776 0,2 014926,000661: 22,2317 00007 6,1 014927,000662: 22,2320 62703 VXM* PDVL* 014928,000663: 22,2321 77776 0,2 014929,000664: 22,2322 00015 12D,1 014930,000665: 22,2323 41503 VXM* PUSH 014931,000666: 22,2324 77776 0,2 014932,000667: 22,2325 77616 RVQ 014933,000668: 014934,000669: # RETURN WITH M1XM2 IN PD LIST 014935,000670: 014936,000671: 22,2326 76601 TRANSPOS SETPD VLOAD* # TRANSPOS TRANSPOSES A 3X3 MATRIX 014937,000672: 22,2327 00001 0 # AND LEAVES RESULT IN PD LIST 014938,000673: 22,2330 00001 0,1 # MATRIX ADDRESS IN XR1 014939,000674: 22,2331 62713 PDVL* PDVL* 014940,000675: 22,2332 00007 6,1 014941,000676: 22,2333 00015 12D,1 014942,000677: 22,2334 77606 PUSH # MATRIX IN PD 014943,000678: 22,2335 77776 TRNSPSPD EXIT # ENTER WITH MATRIX AT 0 IN PD LIST 014944,000679: 22,2336 50120 INDEX FIXLOC 014945,000680: 22,2337 52013 DXCH 12 014946,000681: 22,2340 50120 INDEX FIXLOC 014947,000682: 22,2341 52017 DXCH 16 014948,000683: 22,2342 50120 INDEX FIXLOC 014949,000684: 22,2343 52013 DXCH 12 014950,000685: 22,2344 50120 INDEX FIXLOC 014951,000686: 22,2345 52015 DXCH 14 014952,000687: 22,2346 50120 INDEX FIXLOC 014953,000688: 22,2347 52005 DXCH 4 014954,000689: 22,2350 50120 INDEX FIXLOC 014955,000690: 22,2351 52015 DXCH 14 014956,000691: 22,2352 50120 INDEX FIXLOC 014957,000692: 22,2353 52003 DXCH 2 014958,000693: 22,2354 50120 INDEX FIXLOC 014959,000694: 22,2355 52007 DXCH 6 014960,000695: 22,2356 50120 INDEX FIXLOC 014961,000696: 22,2357 52003 DXCH 2
![]() |
Page 359 |
014963,000698: 22,2360 06042 TC INTPRET 014964,000699: 22,2361 77616 RVQ 014965,000700: 014966,000701: 15,2050 BANK 15 014967,000702: 22,2000 SETLOC KALCMON1 014968,000703: 22,2000 BANK 014969,000704: 014970,000705: 22,2362 E6,1675 EBANK= BCDU 014971,000706: 014972,000707: 22,2362 00013 13563 MINANG 2DEC 0.00069375 014973,000708: 22,2364 17070 34343 MAXANG 2DEC 0.472222222 014974,000709: 014975,000710: # GIMBAL LOCK CONSTANTS 014976,000711: 014977,000712: # D = MGA CORRESPONDING TO GIMBAL LOCK = 60 DEGREES 014978,000713: # NGL = BUFFER ANGLE (TO AVOID DIVISIONS BY ZERO) = 2 DEGREES 014979,000714: 014980,000715: 22,2366 15666 20443 SD 2DEC .433015 # = SIN(D) $2 014981,000716: 22,2370 33555 01106 K3S1 2DEC .86603 # = SIN(D) $1 014982,000717: 22,2372 67777 77777 K4 2DEC -.25 # = -COS(D) $2 014983,000718: 22,2374 04000 00000 K4SQ 2DEC .125 # = COS(D)COS(D) $2 014984,000719: 22,2376 00216 36323 SNGLCD 2DEC .008725 # = SIN(NGL)COS(D) $2 014985,000720: 22,2400 17773 00057 CNGL 2DEC .499695 # COS(NGL) $2 014986,000721: 22,2402 14344 LOCKANGL DEC .388889 # = 70 DEGREES 014987,000722: 014988,000723: # INTERPRETIVE SUBROUTINE TO READ THE CDU ANGLES 014989,000724: 014990,000725: 22,2403 30034 READCDUK CA CDUZ # LOAD T(MPAC) WITH CDU ANGLES 014991,000726: 22,2404 54156 TS MPAC +2 014992,000727: 22,2405 00006 EXTEND 014993,000728: 22,2406 30033 DCA CDUX # AND CHANGE MODE TO TRIPLE PRECISION 014994,000729: 22,2407 16501 TCF TLOAD +6 014995,000730: 014996,000731: 22,2410 66370 CDUTODCM AXT,1 SSP 014997,000732: 22,2411 00003 OCT 3 014998,000733: 22,2412 00051 S1 014999,000734: 22,2413 00001 OCT 1 # SET XR1, S1, AND PD FOR LOOP 015000,000735: 22,2414 00010 STORE 7 015001,000736: 22,2415 77601 SETPD 015002,000737: 22,2416 00001 0 015003,000738: 22,2417 47133 LOOPSIN SLOAD* RTB 015004,000739: 22,2420 00013 10D,1 015005,000740: 22,2421 21577 CDULOGIC
![]() |
Page 360 |
015007,000742: 22,2422 00013 STORE 10D # LOAD PD WITH 0 SIN(PHI) 015008,000743: 22,2423 65356 SIN PDDL # 2 COS(PHI) 015009,000744: 22,2424 00013 10D # 4 SIN(THETA) 015010,000745: 22,2425 41546 COS PUSH # 6 COS(THETA) 015011,000746: 22,2426 71300 TIX,1 DLOAD # 8 SIN(PSI) 015012,000747: 22,2427 44417 LOOPSIN # 10 COS(PSI) 015013,000748: 22,2430 00007 6 015014,000749: 22,2431 72405 DMP SL1 015015,000750: 22,2432 00013 10D 015016,000751: 22,2433 10001 STORE 0,2 # C0 = COS(THETA)COS(PSI) 015017,000752: 22,2434 41345 DLOAD DMP 015018,000753: 22,2435 00005 4 015019,000754: 22,2436 00001 0 015020,000755: 22,2437 41325 PDDL DMP # (PD6 SIN(THETA)SIN(PHI)) 015021,000756: 22,2440 00007 6 015022,000757: 22,2441 00011 8D 015023,000758: 22,2442 72405 DMP SL1 015024,000759: 22,2443 00003 2 015025,000760: 22,2444 72421 BDSU SL1 015026,000761: 22,2445 00015 12D 015027,000762: 22,2446 10003 STORE 2,2 # C1=-COS(THETA)SIN(PSI)COS(PHI) 015028,000763: 22,2447 41345 DLOAD DMP 015029,000764: 22,2450 00003 2 015030,000765: 22,2451 00005 4 015031,000766: 22,2452 41325 PDDL DMP # (PD7 COS(PHI)SIN(THETA)) SCALED 4 015032,000767: 22,2453 00007 6 015033,000768: 22,2454 00011 8D 015034,000769: 22,2455 72405 DMP SL1 015035,000770: 22,2456 00001 0 015036,000771: 22,2457 72415 DAD SL1 015037,000772: 22,2460 00017 14D 015038,000773: 22,2461 10005 STORE 4,2 # C2=COS(THETA)SIN(PSI)SIN(PHI) 015039,000774: 22,2462 77745 DLOAD 015040,000775: 22,2463 00011 8D 015041,000776: 22,2464 10007 STORE 6,2 # C3=SIN(PSI) 015042,000777: 22,2465 77745 DLOAD 015043,000778: 22,2466 00013 10D 015044,000779: 22,2467 72405 DMP SL1 015045,000780: 22,2470 00003 2 015046,000781: 22,2471 10011 STORE 8D,2 # C4=COS(PSI)COS(PHI) 015047,000782: 22,2472 41345 DLOAD DMP 015048,000783: 22,2473 00013 10D 015049,000784: 22,2474 00001 0 015050,000785: 22,2475 72476 DCOMP SL1 015051,000786: 22,2476 10013 STORE 10D,2 # C5=-COS(PSI)SIN(PHI) 015052,000787: 22,2477 41345 DLOAD DMP 015053,000788: 22,2500 00005 4 015054,000789: 22,2501 00013 10D 015055,000790: 22,2502 72476 DCOMP SL1 015056,000791: 22,2503 10015 STORE 12D,2 # C6=-SIN(THETA)COS(PSI)
![]() |
Page 361 |
015058,000793: 22,2504 77745 DLOAD 015059,000794: 22,2505 72405 DMP SL1 # (PUSH UP 7) 015060,000795: 22,2506 00011 8D 015061,000796: 22,2507 41325 PDDL DMP # (PD7 COS(PHI)SIN(THETA)SIN(PSI)) SCALE 4 015062,000797: 22,2510 00007 6 015063,000798: 22,2511 00001 0 015064,000799: 22,2512 72415 DAD SL1 # (PUSH UP 7) 015065,000800: 22,2513 77626 STADR # C7=COS(PHI)SIN(THETA)SIN(PSI) 015066,000801: 22,2514 67760 STORE 14D,2 # +COS(THETA)SIN(PHI) 015067,000802: 22,2515 77745 DLOAD 015068,000803: 22,2516 72405 DMP SL1 # (PUSH UP 6) 015069,000804: 22,2517 00011 8D 015070,000805: 22,2520 41325 PDDL DMP # (PD6 SIN(THETA)SIN(PHI)SIN(PSI)) SCALE 4 015071,000806: 22,2521 00007 6 015072,000807: 22,2522 00003 2 015073,000808: 22,2523 72425 DSU SL1 # (PUSH UP 6) 015074,000809: 22,2524 77626 STADR 015075,000810: 22,2525 67756 STORE 16D,2 # C8=-SIN(THETA)SIN(PHI)SIN(PSI) 015076,000811: 22,2526 77616 RVQ # +COS(THETA)COS(PHI) 015077,000812: 015078,000813: # CALCULATION OF THE MATRIX DEL...... 015079,000814: 015080,000815: # * * --T * 015081,000816: # DEL = (IDMATRIX)COS(A)+UU (1-COS(A))+UX SIN(A) SCALED 1 015082,000817: # - 015083,000818: # WHERE U IS A UNIT VECTOR (DP SCALED 2) ALONG THE AXIS OF ROTATION. 015084,000819: # A IS THE ANGLE OF ROTATION (DP SCALED 2) 015085,000820: # - 015086,000821: # UPON ENTRY THE STARTING ADDRESS OF U IS COF, AND A IS IN MPAC 015087,000822: 015088,000823: 22,2527 41401 DELCOMP SETPD PUSH # MPAC CONTAINS THE ANGLE A 015089,000824: 22,2530 00001 0 015090,000825: 22,2531 65356 SIN PDDL # PD0 = SIN(A) 015091,000826: 22,2532 41546 COS PUSH # PD2 = COS(A) 015092,000827: 22,2533 65302 SR2 PDDL # PD2 = COS(A) $8 015093,000828: 22,2534 41021 BDSU BOVB 015094,000829: 22,2535 06522 DPHALF 015095,000830: 22,2536 21713 SIGNMPAC 015096,000831: 22,2537 77725 PDDL # PD4 = 1-COS(A) 015097,000832: 015098,000833: # COMPUTE THE DIAGONAL COMPONENTS OF DEL 015099,000834: 015100,000835: 22,2540 03270 COF 015101,000836: 22,2541 41316 DSQ DMP 015102,000837: 22,2542 00005 4 015103,000838: 22,2543 52415 DAD SL3 015104,000839: 22,2544 00003 2 015105,000840: 22,2545 77604 BOVB 015106,000841: 22,2546 21713 SIGNMPAC
![]() |
Page 362 |
015108,000843: 22,2547 16231 STODL KEL # UX UX(1-COS(A)) +COS(A) $1 015109,000844: 22,2550 03272 COF +2 015110,000845: 22,2551 41316 DSQ DMP 015111,000846: 22,2552 00005 4 015112,000847: 22,2553 52415 DAD SL3 015113,000848: 22,2554 00003 2 015114,000849: 22,2555 77604 BOVB 015115,000850: 22,2556 21713 SIGNMPAC 015116,000851: 22,2557 16241 STODL KEL +8D # UY UY(1-COS(A)) +COS(A) $1 015117,000852: 22,2560 03274 COF +4 015118,000853: 22,2561 41316 DSQ DMP 015119,000854: 22,2562 00005 4 015120,000855: 22,2563 52415 DAD SL3 015121,000856: 22,2564 00003 2 015122,000857: 22,2565 77604 BOVB 015123,000858: 22,2566 21713 SIGNMPAC 015124,000859: 22,2567 02251 STORE KEL +16D # UZ UZ(1-COS(A)) +COS(A) $1 015125,000860: 015126,000861: # COMPUTE THE OFF DIAGONAL TERMS OF DEL 015127,000862: 015128,000863: 22,2570 41345 DLOAD DMP 015129,000864: 22,2571 03270 COF 015130,000865: 22,2572 03272 COF +2 015131,000866: 22,2573 72405 DMP SL1 015132,000867: 22,2574 00005 4 015133,000868: 22,2575 41325 PDDL DMP # D6 UX UY (1-COS A) $4 015134,000869: 22,2576 03274 COF +4 015135,000870: 22,2577 00001 0 015136,000871: 22,2600 43206 PUSH DAD # D8 UZ SIN A $4 015137,000872: 22,2601 00007 6 015138,000873: 22,2602 41112 SL2 BOVB 015139,000874: 22,2603 21713 SIGNMPAC 015140,000875: 22,2604 16237 STODL KEL +6 015141,000876: 22,2605 62421 BDSU SL2 015142,000877: 22,2606 77604 BOVB 015143,000878: 22,2607 21713 SIGNMPAC 015144,000879: 22,2610 16233 STODL KEL +2 015145,000880: 22,2611 03270 COF 015146,000881: 22,2612 41205 DMP DMP 015147,000882: 22,2613 03274 COF +4 015148,000883: 22,2614 00005 4 015149,000884: 22,2615 65352 SL1 PDDL # D6 UX UZ (1-COS A) $4 015150,000885: 22,2616 03272 COF +2 015151,000886: 22,2617 41405 DMP PUSH # D8 UY SIN(A) 015152,000887: 22,2620 00001 0 015153,000888: 22,2621 62415 DAD SL2 015154,000889: 22,2622 00007 6 015155,000890: 22,2623 77604 BOVB 015156,000891: 22,2624 21713 SIGNMPAC 015157,000892: 22,2625 16235 STODL KEL +4 # UX UZ (1-COS(A))+UY SIN(A)
![]() |
Page 363 |
015159,000894: 22,2626 62421 BDSU SL2 015160,000895: 22,2627 77604 BOVB 015161,000896: 22,2630 21713 SIGNMPAC 015162,000897: 22,2631 16245 STODL KEL +12D # UX UZ (1-COS(A))-UY SIN(A) 015163,000898: 22,2632 03272 COF +2 015164,000899: 22,2633 41205 DMP DMP 015165,000900: 22,2634 03274 COF +4 015166,000901: 22,2635 00005 4 015167,000902: 22,2636 65352 SL1 PDDL # D6 UY UZ (1-COS(A)) $4 015168,000903: 22,2637 03270 COF 015169,000904: 22,2640 41405 DMP PUSH # D8 UX SIN(A) 015170,000905: 22,2641 00001 0 015171,000906: 22,2642 62415 DAD SL2 015172,000907: 22,2643 00007 6 015173,000908: 22,2644 77604 BOVB 015174,000909: 22,2645 21713 SIGNMPAC 015175,000910: 22,2646 16247 STODL KEL +14D # UY UZ(1-COS(A)) +UX SIN(A) 015176,000911: 22,2647 62421 BDSU SL2 015177,000912: 22,2650 77604 BOVB 015178,000913: 22,2651 21713 SIGNMPAC 015179,000914: 22,2652 02243 STORE KEL +10D # UY UZ (1-COS(A)) -UX SIN(A) 015180,000915: 22,2653 77616 RVQ 015181,000916: 015182,000917: # DIRECTION COSINE MATRIX TO CDU ANGLE ROUTINE 015183,000918: 015184,000919: # X1 CONTAINS THE COMPLEMENT OF THE STARTING ADDRESS FOR MATRIX (SCALED 2) 015185,000920: # LEAVES CDU ANGLES SCALED 2PI IN V(MPAC) 015186,000921: # COS(MGA) WILL BE LEFT IN S1 (SCALED 1) 015187,000922: # THE DIRECTION COSINE MATRIX RELATING S/C AXES TO STABLE MEMBER AXES CAN BE WRITTEN AS*** 015188,000923: 015189,000924: # C =COS(THETA)COS(PSI) 015190,000925: # 0 015191,000926: 015192,000927: # C =-COS(THETA)SIN(PSI)COS(PHI)+SI (THETA)SIN(PHI) 015193,000928: # 1 015194,000929: 015195,000930: # C =COS(THETA)SIN(PSI)SIN(PHI) + S N(THETA)COS(PHI) 015196,000931: # 2 015197,000932: 015198,000933: # C =SIN(PSI) 015199,000934: # 3 015200,000935: 015201,000936: # C =COS(PSI)COS(PHI) 015202,000937: # 4 015203,000938: 015204,000939: # C =-COS(PSI)SIN(PHI) 015205,000940: # 5 015206,000941: 015207,000942: # C =-SIN(THETA)COS(PSI) 015208,000943: # 6 015209,000944: 015210,000945: # C =SIN(THETA)SIN(PSI)COS(PHI)+COS THETA)SIN(PHI) 015211,000946: # 7 015212,000947: 015213,000948: # C =-SIN(THETA)SIN(PSI)SIN(PHI)+CO (THETA)COS(PHI) 015214,000949: # 8
![]() |
Page 364 |
015216,000951: 015217,000952: # WHERE PHI = OGA 015218,000953: # THETA = IGA 015219,000954: # PSI = MGA 015220,000955: 015221,000956: 22,2654 67543 DCMTOCDU DLOAD* ARCSIN 015222,000957: 22,2655 00007 6,1 015223,000958: 22,2656 71406 PUSH COS # PD +0 PSI 015224,000959: 22,2657 41152 SL1 BOVB 015225,000960: 22,2660 21713 SIGNMPAC 015226,000961: 22,2661 00051 STORE S1 015227,000962: 22,2662 57543 DLOAD* DCOMP 015228,000963: 22,2663 00015 12D,1 015229,000964: 22,2664 67471 DDV ARCSIN 015230,000965: 22,2665 00051 S1 015231,000966: 22,2666 51123 PDDL* BPL # PD +2 THETA 015232,000967: 22,2667 00001 0,1 # MUST CHECK THE SIGN OF COS(THETA) 015233,000968: 22,2670 44702 OKTHETA # TO DETERMINE THE PROPER QUADRANT 015234,000969: 22,2671 57545 DLOAD DCOMP 015235,000970: 22,2672 43244 BPL DAD 015236,000971: 22,2673 44677 SUHALFA 015237,000972: 22,2674 06522 DPHALF 015238,000973: 22,2675 77650 GOTO 015239,000974: 22,2676 44701 CALCPHI 015240,000975: 22,2677 77625 SUHALFA DSU 015241,000976: 22,2700 06522 DPHALF 015242,000977: 22,2701 77606 CALCPHI PUSH 015243,000978: 22,2702 57543 OKTHETA DLOAD* DCOMP 015244,000979: 22,2703 00013 10D,1 015245,000980: 22,2704 67471 DDV ARCSIN 015246,000981: 22,2705 00051 S1 015247,000982: 22,2706 51123 PDDL* BPL # PUSH DOWN PHI 015248,000983: 22,2707 00011 8D,1 015249,000984: 22,2710 44722 OKPHI 015250,000985: 22,2711 57545 DLOAD DCOMP # PUSH UP PHI 015251,000986: 22,2712 43244 BPL DAD 015252,000987: 22,2713 44717 SUHALFAP 015253,000988: 22,2714 06522 DPHALF 015254,000989: 22,2715 77650 GOTO 015255,000990: 22,2716 44723 VECOFANG 015256,000991: 22,2717 52025 SUHALFAP DSU GOTO 015257,000992: 22,2720 06522 DPHALF 015258,000993: 22,2721 44723 VECOFANG 015259,000994: 22,2722 77745 OKPHI DLOAD # PUSH UP PHI 015260,000995: 22,2723 43466 VECOFANG VDEF RVQ
![]() |
Page 365 |
015262,000997: # ROUTINES FOR TERMINATING THE AUTOMATIC MANEUVER AND RETURNING TO USER 015263,000998: 015264,000999: 22,2724 77776 TOOBADF EXIT 015265,001000: 22,2725 05567 TC ALARM 015266,001001: 22,2726 00401 OCT 00401 015267,001002: 015268,001003: 22,2727 12732 TCF NOGO # DO NOT ZERO ATTITUDE ERRORS 015269,001004: 015270,001005: 22,2730 04616 TC BANKCALL 015271,001006: 22,2731 40154 CADR ZATTEROR # ZERO ATTITUDE ERRORS 015272,001007: 015273,001008: 22,2732 04616 NOGO TC BANKCALL 015274,001009: 22,2733 40166 CADR STOPRATE # STOP RATES 015275,001010: 015276,001011: 22,2734 34752 CAF TWO 015277,001012: 22,2735 00004 INHINT # ALL RETURNS ARE NOW MADE VIA GOODEND 015278,001013: 22,2736 05203 TC WAITLIST 015279,001014: 22,2737 E6,1675 EBANK= BCDU 015280,001015: 22,2737 03234 44066 2CADR GOODMANU 015281,001016: 015282,001017: 22,2741 15155 TCF ENDOFJOB 015283,001018: 015284,001019: 22,2742 77776 TOOBADI EXIT 015285,001020: 22,2743 12732 TCF NOGO 015286,001021: 015287,001022: End of include-file ATTITUDE_MANEUVER_ROUTINE.agc. Parent file is MAIN.agc