Source Code

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