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