Source Code
|
These source-code files are part of a reconstructed copy of Luminary 178, the
final release of the Apollo 14 Lunar Module (LM) Apollo Guidance Computer
(AGC) software.
The reconstruction began with source code of Zerlina 56, Luminary 210, and
Luminary 131, previously transcribed from digitized copies of those programs.
The code was created by combining the three, using Zerlina 56 as a base, since
it was based off of Luminary 183. The differences between the three were
carefully merged using the Luminary memos as a guide, with all changes made
after Luminary 178 being backed out. The reconstruction was verified by matching
memory-bank checksums to those listed in drawing 2021152N. Note that page numbers
in the reconstructed code match those for the baseline log section mentioned in
each file's changelog; the page numbers for a real Luminary 178 listing would
be different.
Comments from the original source code are prefixed with a single '#' symbol,
whereas comments added later are prefixed by "##" or "###". Report any errors
noted by creating an
issue report at the Virtual AGC
project's GitHub repository.
|
014736,000002: ## Copyright: Public domain.
014737,000003: ## Filename: ATTITUDE_MANEUVER_ROUTINE.agc
014738,000004: ## Purpose: A section of Luminary revision 178.
014739,000005: ## It is part of the reconstructed source code for the final
014740,000006: ## release of the flight software for the Lunar Module's
014741,000007: ## (LM) Apollo Guidance Computer (AGC) for Apollo 14. The
014742,000008: ## code has been recreated from copies of Zerlina 56, Luminary
014743,000009: ## 210, and Luminary 131, as well as many Luminary memos.
014744,000010: ## It has been adapted such that the resulting bugger words
014745,000011: ## exactly match those specified for Luminary 178 in NASA
014746,000012: ## drawing 2021152N, which gives relatively high confidence
014747,000013: ## that the reconstruction is correct.
014748,000014: ## Reference: pp. 347-368
014749,000015: ## Assembler: yaYUL
014750,000016: ## Contact: Ron Burkey <info@sandroid.org>.
014751,000017: ## Website: www.ibiblio.org/apollo/index.html
014752,000018: ## Mod history: 2019-08-14 MAS Created from Zerlina 56.
014753,000019:
|
|
Page 347
|
014755,000021: # BLOCK 2 LGC ATTITUDE MANEUVER ROUTINE-KALCMANU
014756,000022:
014757,000023:
014758,000024: # MOD 2 DATE 5/1/67 BY DON KEENE
014759,000025:
014760,000026: # PROGRAM DESCRIPTION
014761,000027:
014762,000028: # KALCMANU IS A ROUTINE WHICH GENERATES COMMANDS FOR THE LM DAP TO CHANGE THE ATTITUDE OF THE SPACECRAFT
014763,000029: # DURING FREE FALL. IT IS DESIGNED TO MANEUVER THE SPACECRAFT FROM ITS INITIAL ORIENTATION TO SOME DESIRED
014764,000030: # ORIENTATION SPECIFIED BY THE PROGRAM WHICH CALLS KALCMANU, AVOIDING GIMBAL LOCK IN THE PROCESS. IN THE
014765,000031: # MOD 2 VERSION, THIS DESIRED ATTITUDE IS SPECIFIED BY A SET OF THREE COMMANDED CDU ANGLES STORED AS 2S COMPLEMENT
014766,000032: # SINGLE PRECISION ANGLES IN THE THREE CONSECUTIVE LOCATIONS, CPHI, CTHETA, CPSI, WHERE
014767,000033:
014768,000034: # CPHI = COMMANDED OUTER GIMBAL ANGLE
014769,000035: # CTHETA = COMMANDED INNER GIMBAL ANGLE
014770,000036: # CPSI = COMMANDED MIDDLE GIMBAL ANGLE
014771,000037:
014772,000038: # WHEN POINTING A SPACECRAFT AXIS (E.I. X, Y, Z, THE AOT, THRUST AXIS, ETC) THE SUBROUTINE VECPOINT MAY BE
014773,000039: # USED TO GENERATE THIS SET OF DESIRED CDU ANGLES (SEE DESCRIPTION IN R60) -
014774,000040: # WITH THIS INFORMATION KALCMANU DETERMINES THE DIRECTION OF THE SINGLE EQUIVALENT ROTATION (COF ALSO U) AND THE
014775,000041: # MAGNITUDE OF THE ROTATION (AM) TO BRING THE S/C FROM ITS INITIAL ORIENTATION TO ITS FINAL ORIENTATION.
014776,000042: # THIS DIRECTION REMAINS FIXED BOTH IN INERTIAL COORDINATES AND IN COMMANDED S/C AXES THROUGHOUT THE
014777,000043: # -
014778,000044: # MANEUVER. ONCE COF AND AM HAVE BEEN DETERMINED, KALCMANU THEN EXAMINES THE MANEUVER TO SEE IF IT WILL BRING
014779,000045: # -
014780,000046: # THE S/C THROUGH GIMBAL LOCK. IF SO, COF AND AM ARE READJUSTED SO THAT THE S/C WILL JUST SKIM THE GIMBAL
014781,000047: # LOCK ZONE AND ALIGN THE X-AXIS. IN GENERAL A FINAL YAW ABOUT X WILL BE NECESSARY TO COMPLETE THE MANEUVER.
014782,000048: # NEEDLESS TO SAY, NEITHER THE INITIAL NOR THE FINAL ORIENTATION CAN BE IN GIMBAL LOCK.
014783,000049:
014784,000050: # FOR PROPER ATTITUDE CONTROL THE DIGITAL AUTOPILOT MUST BE GIVEN AN ATTITUDE REFERENCE WHICH IT CAN TRACK.
014785,000051: # KALCMANU DOES THIS BY GENERATING A REFERENCE OF DESIRED GIMBAL ANGLES (CDUXD, CDUYD, CDUZD) WHICH ARE UPDATED
014786,000052: # EVERY ONE SECOND DURING THE MANEUVER. TO ACHIEVE A SMOOTHER SEQUENCE OF COMMANDS BETWEEN SUCCESSIVE UPDATES,
014787,000053: # THE PROGRAM ALSO GENERATES A SET OF INCREMENTAL CDU ANGLES (DELDCDU) TO BE ADDED TO CDU DESIRED BY THE DIGITAL
014788,000054: # AUTOPILOT. KALCMANU ALSO CALCULATES THE COMPONENT MANEUVER RATES (OMEGAPD, OMEGAQD, OMEGARD), WHICH CAN
014789,000055: # -
014790,000056: # BE DETERMINED SIMPLY BY MULTIPLYING COF BY SOME SCALAR (ARATE) CORRESPONDING TO THE DESIRED ROTATIONAL RATE.
014791,000057:
014792,000058: # AUTOMATIC MANEUVERS ARE TIMED WITH THE HELP OF WAITLIST SO THAT AFTER A SPECIFIED INTERVAL THE Y AND Z
014793,000059: # DESIRED RATES ARE SET TO ZERO AND THE DESIRED CDU ANGLES (CDUYD, CDUZD) ARE SET EQUAL TO THE FINAL DESIRED CDU
014794,000060: # ANGLES (CTHETA, CPSI). IF ANY YAW REMAINS DUE TO GIMBAL LOCK AVOIDANCE, THE FINAL YAW MANEUVER IS
014795,000061: # CALCULATED AND THE DESIRED YAW RATE SET TO SOME FIXED VALUE (ROLLRATE = + OR - 2 DEGREES PER SEC).
014796,000062: # IN THIS CASE ONLY AN INCREMENTAL CDUX ANGLE (DELFROLL) IS SUPPLIED TO THE DAP. AT THE END OF THE YAW
014797,000063: # MANEUVER OR IN THE EVENT THAT THERE WAS NO FINAL YAW, CDUXD IS SET EQUAL TO CPHI AND THE X-AXIS DESIRED
014798,000064: # RATE SET TO ZERO. THUS, UPON COMPLETION OF THE MANEUVER THE S/C WILL FINISH UP IN A LIMIT CYCLE ABOUT THE
014799,000065: # DESIRED FINAL GIMBAL ANGLES.
014800,000066:
014801,000067:
014802,000068: # PROGRAM LOGIC FLOW
014803,000069:
014804,000070: # KALCMANU IS CALLED AS A HIGH PRIORITY JOB WITH ENTRY POINTS AT KALCMAN3 AND VECPOINT. IT FIRST PICKS
014805,000071: # UP THE CURRENT CDU ANGLES TO BE USED AS THE BASIS FOR ALL COMPUTATIONS INVOLVING THE INITIAL S/C ORIENTATION.
014806,000072:
|
|
Page 348
|
014808,000074: # IT THEN DETERMINES THE DIRECTION COSINE MATRICES RELATING BOTH THE INITIAL AND FINAL S/C ORIENTATION TO STABLE
014809,000075: # * * *
014810,000076: # MEMBER AXES (MIS, MFS). IT ALSO COMPUTES THE MATRIX RELATING FINAL S/C AXES TO INITIAL S/C AXES (MFI). THE
014811,000077: # ANGLE OF ROTATION (AM) IS THEN EXTRACTED FROM THIS MATRIX, AND TESTS ARE MADE TO DETERMINE IF
014812,000078:
014813,000079: # A) AM LESS THAN .25 DEGREES (MINANG)
014814,000080: # B) AM GREATER THAN 170 DEGREES (MAXANG)
014815,000081:
014816,000082: # IF AM LESS THAN .25 DEGREES, NO COMPLICATED AUTOMATIC MANEUVERING IS NECESSARY. THEREFORE WE CAN SIMPLY
014817,000083: # SET CDU DESIRED EQUAL TO THE FINAL CDU DESIRED ANGLES AND TERMINATE THE JOB.
014818,000084:
014819,000085: # IF AM IS GREATER THAN .25 DEGREES BUT LESS THAN 170 DEGREES, THE AXES OF THE SINGLE EQUIVALENT ROTATION
014820,000086: # - *
014821,000087: # (COF) IS EXTRACTED FROM THE SKEW SYMMETRIC COMPONENTS OF MFI. * *
014822,000088: # IF AM GREATER THAN 170 DEGREES AN ALTERNATE METHOD EMPLOYING THE SYMMETRIC PART OF MFI (MFISYM) IS USED
014823,000089: # -
014824,000090: # TO DETERMINE COF.
014825,000091:
014826,000092: # THE PROGRAM THEN CHECKS TO SEE IF THE MANEUVER AS COMPUTED WILL BRING THE S/C THROUGH GIMBAL LOCK. IF
014827,000093: # SO, A NEW MANEUVER IS CALCULATED WHICH WILL JUST SKIM THE GIMBAL LOCK ZONE AND ALIGN THE S/C X-AXIS. THIS
014828,000094: # METHOD ASSURES THAT THE ADDITIONAL MANEUVERING TO AVOID GIMBAL LOCK WILL BE KEPT TO A MINIMUM. SINCE A FINAL
014829,000095: # P AXIS YAW WILL BE NECESSARY, A SWITCH IS RESET (STATE SWITCH 31) TO ALLOW FOR THE COMPUTATION OF THIS FINAL
014830,000096: # YAW.
014831,000097:
014832,000098: # AS STATED PREVIOUSLY KALCMANU GENERATES A SEQUENCE OF DESIRED GIMBAL ANGLES WHICH ARE UPDATED EVERY
014833,000099: # -
014834,000100: # SECOND. THIS IS ACCOMPLISHED BY A SMALL ROTATION OF THE DESIRED S/C FRAME ABOUT THE VECTOR COF. THE NEW
014835,000101: # DESIRED REFERENCE MATRIX IS THEN,
014836,000102:
014837,000103: # * * *
014838,000104: # MIS = MIS DEL
014839,000105: # N+1 N
014840,000106: # *
014841,000107: # WHERE DEL IS THE MATRIX CORRESPONDING TO THIS SMALL ROTATION. THE NEW CDU ANGLES CAN THEN BE EXTRACTED
014842,000108: # *
014843,000109: # FROM MIS.
014844,000110:
014845,000111: # AT THE BEGINNING OF THE MANEUVER THE AUTOPILOT DESIRED RATES (OMEGAPD, OMEGAQD, OMEGARD) AND THE
014846,000112: # MANEUVER TIMINGS ARE ESTABLISHED. ON THE FIRST PASS AND ON ALL SUBSEQUENT UPDATES THE CDU DESIRED
014847,000113: # ANGLES ARE LOADED WITH THE APPROPRIATE VALUES AND THE INCREMENTAL CDU ANGLES ARE COMPUTED. THE AGC CLOCKS
014848,000114: # (TIME1 AND TIME2) ARE THAN CHECKED TO SEE IF THE MANEUVER WILL TERMINATE BEFORE THE NEXT UPDATE. IF
014849,000115: # NOT, KALCMANU CALLS FOR ANOTHER UPDATE (RUN AS A JOB WITH PRIORITY TBD) IN ONE SECOND. ANY DELAYS IN THIS
014850,000116: # CALLING SEQUENCE ARE AUTOMATICALLY COMPENSATED IN CALLING FOR THE NEXT UPDATE.
014851,000117:
014852,000118: # IF IT IS FOUND THAT THE MANEUVER IS TO TERMINATE BEFORE THE NEXT UPDATE A ROUTINE IS CALLED (AS A WAIT-
014853,000119: # LIST TASK) TO STOP THE MANEUVER AT THE APPROPRIATE TIME AS EXPLAINED ABOVE.
014854,000120:
|
|
Page 349
|
014856,000122: # CALLING SEQUENCE
014857,000123:
014858,000124: # IN ORDER TO PERFORM A KALCMANU SUPERVISED MANEUVER, THE COMMANDED GIMBAL ANGLES MUST BE PRECOMPUTED AND
014859,000125: # STORED IN LOCATIONS CPHI, CTHETA, CPSI. THE USER:S PROGRAM MUST THEN CLEAR STATE SWITCH NO 33 TO ALLOW THE
014860,000126: # ATTITUDE MANEUVER ROUTINE TO PERFORM ANY FINAL P-AXIS YAW INCURRED BY AVOIDING GIMBAL LOCK. THE MANEUVER IS
014861,000127: # THEN INITIATED BY ESTABLISHING THE FOLLOWING EXECUTIVE JOB
014862,000128:
014863,000129: # *
014864,000130: # CAF PRIO XX
014865,000131: # --
014866,000132: # INHINT
014867,000133: # TC FINDVAC
014868,000134: # 2CADR KALCMAN3
014869,000135: # RELINT
014870,000136:
014871,000137: # THE USER:S PROGRAM MAY EITHER CONTINUE OR WAIT FOR THE TERMINATION OF THE MANEUVER. IF THE USER WISHES TO
014872,000138: # WAIT, HE MAY PUT HIS JOB TO SLEEP WITH THE FOLLOWING INSTRUCTIONS
014873,000139:
014874,000140: # L TC BANKCALL
014875,000141: # L+1 CADR ATTSTALL
014876,000142: # L+2 (BAD RETURN)
014877,000143: # L+3 (GOOD RETURN)
014878,000144:
014879,000145: # UPON COMPLETION OF THE MANEUVER, THE PROGRAM WILL BE AWAKENED AT L+3 IF THE MANEUVER WAS COMPLETED
014880,000146: # SUCCESSFULLY, OR AT L+2 IF THE MANEUVER WAS ABORTED. THIS ABORT WOULD OCCUR IF THE INITIAL OR FINAL ATTITUDE
014881,000147: # WAS IN GIMBAL LOCK.
014882,000148:
014883,000149: # ***NOTA BENE*** IT IS ASSUMED THAT THE DESIRED MANEUVERING RATE (0.5, 2, 5, 10, DEG/SEC) HAS BEEN SELECTED BY
014884,000150: # KEYBOARD ENTRY PRIOR TO THE EXECUTION OF KALCMANU.
014885,000151: # IT IS ALSO ASSUMED THAT THE AUTOPILOT IS IN THE AUTO MODE. IF THE MODE SWITCH IS CHANGED DURING THE
014886,000152: # MANEUVER, KALCMANU WILL TERMINATE VIA GOODEND WITHIN 1 SECOND SO THAT R60 MAY REQUEST A TRIM OF THE S/C ATTITUDE
014887,000153: # THIS IS THE ONLY MEANS FOR MANUALLY TERMINATING A KALCMANU SUPERVISED MANEUVER.
014888,000154: # SUBROUTINES
014889,000155:
014890,000156: # KALCMANU USES A NUMBER OF INTERPRETIVE SUBROUTINES WHICH MAY BE OF GENERAL INTEREST. SINCE THESE ROUTINES
014891,000157: # WERE PROGRAMMED EXCLUSIVELY FOR KALCMANU, THEY ARE NOT, AS YET, GENERALLY AVAILABLE FOR USE BY OTHER PROGRAMS.
014892,000158:
014893,000159: # MXM3
014894,000160: # ----
014895,000161:
014896,000162: # THIS SUBROUTINE MULTIPLIES TWO 3X3 MATRICES AND LEAVES THE RESULT IN THE FIRST 18 LOCATIONS OF THE PUSH
014897,000163: # DOWN LIST, I.E.,
014898,000164:
014899,000165: # (M M M )
014900,000166: # ( 0 1 2)
014901,000167: # * ( ) * *
014902,000168: # M = (M M M ) = M1 X M2
014903,000169: # ( 3 4 5)
014904,000170: # ( )
014905,000171: # (M M M )
014906,000172:
|
|
Page 350
|
014908,000174: # ( 6 7 8)
014909,000175:
014910,000176: # *
014911,000177: # INDEX REGISTER X1 MUST BE LOADED WITH THE COMPLEMENT OF THE STARTING ADDRESS FOR M1, AND X2 MUST BE
014912,000178: # *
014913,000179: # LOADED WITH THE COMPLEMENT OF THE STARTING ADDRESS FOR M2. THE ROUTINE USES THE FIRST 20 LOCATIONS OF THE PUSH
014914,000180: # DOWN LIST. THE FIRST ELEMENT OF THE MATRIX APPEARS IN PDO. PUSH UP FOR M .
014915,000181: # 8
014916,000182:
014917,000183: # TRANSPOS
014918,000184: # --------
014919,000185:
014920,000186: # THIS ROUTINE TRANSPOSES A 3X3 MATRIX AND LEAVES THE RESULT IN THE PUSH DOWN LIST, I.E.,
014921,000187:
014922,000188: # * * T
014923,000189: # M = M1
014924,000190:
014925,000191: # INDEX REGISTER X1 MUST CONTAIN THE COMPLEMENT OF THE STARTING ADDRESS FOR M1. PUSH UP FOR THE FIRST AND SUB-
014926,000192: # *
014927,000193: # SEQUENT COMPONENTS OF M. THIS SUBROUTINE ALSO USES THE FIRST 20 LOCATIONS OF THE PUSH DOWN LIST.
014928,000194:
014929,000195: # CDU TO DCM
014930,000196: # ----------
014931,000197:
014932,000198: # THIS SUBROUTINE CONVERTS THREE CDU ANGLES IN T(MPAC) TO A DIRECTION COSINE MATRIX (SCALED BY 2) RELATING
014933,000199: # THE CORRESPONDING S/C ORIENTATIONS TO THE STABLE MEMBER FRAME. THE FORMULAS FOR THIS CONVERSION ARE
014934,000200:
014935,000201: # M = COSY COSZ
014936,000202: # 0
014937,000203:
014938,000204: # M = -COSY SINZ COSX + SINY SINX
014939,000205: # 1
014940,000206:
014941,000207: # M = COSY SINZ SINX + SINY COSX
014942,000208: # 2
014943,000209:
014944,000210: # M = SINZ
014945,000211: # 3
014946,000212:
014947,000213: # M = COSZ COSX
014948,000214: # 4
014949,000215:
014950,000216: # M = -COSZ SINX
014951,000217: # 5
014952,000218:
014953,000219: # M = -SINY COSZ
014954,000220: # 6
014955,000221:
014956,000222: # M = SINY SINZ COSX + COSY SINX
014957,000223: # 7
014958,000224:
|
|
Page 351
|
014960,000226: # M = -SINY SINZ SINX + COSY COSX
014961,000227: # 8
014962,000228:
014963,000229: # WHERE X = OUTER GIMBAL ANGLE
014964,000230: # Y = INNER GIMBAL ANGLE
014965,000231: # Z = MIDDLE GIMBAL ANGLE
014966,000232:
014967,000233: # THE INTERPRETATION OF THIS MATRIX IS AS FOLLOWS
014968,000234:
014969,000235: # IF A , A , A REPRESENT THE COMPONENTS OF A VECTOR IN S/C AXES THEN THE COMPONENTS OF THE SAME VECTOR IN
014970,000236: # X Y Z
014971,000237: # STABLE MEMBER AXES (B , B , B ) ARE
014972,000238: # X Y Z
014973,000239:
014974,000240: # (B ) (A )
014975,000241: # ( X) ( X)
014976,000242: # ( ) ( )
014977,000243: # ( ) * ( )
014978,000244: # (B ) = M (A )
014979,000245: # ( Y) ( Y)
014980,000246: # ( ) ( )
014981,000247: # (B ) (A )
014982,000248: # ( Z) ( Z)
014983,000249:
014984,000250: # THE SUBROUTINE WILL STORE THIS MATRIX IN SEQUENTIAL LOCATIONS OF ERASABLE MEMORY AS SPECIFIED BY THE CALLING
014985,000251: # *
014986,000252: # PROGRAM. TO DO THIS THE CALLING PROGRAM MUST FIRST LOAD X2 WITH THE COMPLEMENT OF THE STARTING ADDRESS FOR M.
014987,000253:
014988,000254: # INTERNALLY, THE ROUTINE USES THE FIRST 16 LOCATIONS OF THE PUSH DOWN LIST, ALSO STEP REGISTER S1 AND INDEX
014989,000255: # REGISTER X2.
014990,000256:
014991,000257:
014992,000258: # DCM TO CDU
014993,000259: # ----------
014994,000260: # *
014995,000261: # THIS ROUTINE EXTRACTS THE CDU ANGLES FROM A DIRECTION COSINE MATRIX (M SCALED BY 2) RELATING S/C AXIS TO
014996,000262: # *
014997,000263: # STABLE MEMBER AXES. X1 MUST CONTAIN THE COMPLEMENT OF THE STARTING ADDRESS FOR M. THE SUBROUTINE LEAVES THE
014998,000264: # CORRESPONDING GIMBAL ANGLES IN V(MPAC) AS DOUBLE PRECISION 1:S COMPLEMENT ANGLES SCALED BY 2PI. THE FORMULAS
014999,000265: # FOR THIS CONVERSION ARE
015000,000266:
015001,000267: # Z = ARCSIN (M )
015002,000268: # 3
015003,000269:
015004,000270: # Y = ARCSIN (-M /COSZ)
015005,000271: # 6
015006,000272:
015007,000273: # IF M IS NEGATIVE, Y IS REPLACED BY PI SGN Y - Y
015008,000274: # 0
015009,000275:
|
|
Page 352
|
015011,000277: # X = ARCSIN (-M /COSZ)
015012,000278: # 5
015013,000279:
015014,000280: # IF M IS NEGATIVE X IS REPLACED BY PI SGN X - X
015015,000281: # 4
015016,000282:
015017,000283: # THIS ROUTINE DOES NOT SET THE PUSH DOWN POINTER, BUT USES THE NEXT 8 LOCATIONS OF THE PUSH DOWN LIST AND
015018,000284: # RETURNS THE POINTER TO ITS ORIGINAL SETTING. THIS PROCEDURE ALLOWS THE CALLER TO STORE THE MATRIX AT THE TOP OF
015019,000285: # THE PUSH DOWN LIST.
015020,000286:
015021,000287:
015022,000288: # DELCOMP
015023,000289: # -------
015024,000290:
015025,000291: # *
015026,000292: # THIS ROUTINE COMPUTES THE DIRECTION COSINE MATRIX (DEL) RELATING ON
015027,000293: # -
015028,000294: # IS ROTATED WITH RESPECT TO THE FIRST BY AN ANGLE, A, ABOUT A UNIT VECTOR, U. THE FORMULA FOR THIS MATRIX IS
015029,000295:
015030,000296: # * * --T *
015031,000297: # DEL = I COSA + UU (1-COSA) + V SINA
015032,000298: # X
015033,000299:
015034,000300: # WHERE * (1 0 0)
015035,000301: # I = (0 1 0)
015036,000302: # (0 0 1)
015037,000303:
015038,000304:
015039,000305: # 2
015040,000306: # (U U U U U )
015041,000307: # ( X X Y X Z)
015042,000308: # ( )
015043,000309: # --T ( 2 )
015044,000310: # UU = (U U U U U )
015045,000311: # ( Y X Y Y Z )
015046,000312: # ( )
015047,000313: # ( 2 )
015048,000314: # (U U U U U )
015049,000315: # ( Z X Z Y Z )
015050,000316:
015051,000317:
015052,000318: # (0 -U U )
015053,000319: # ( Z Y )
015054,000320: # * ( )
015055,000321: # V = (U 0 -U )
015056,000322: # X ( Z X)
015057,000323: # ( )
015058,000324: # (-U U 0 )
015059,000325: # ( Y X )
015060,000326:
|
|
Page 353
|
015062,000328: # -
015063,000329: # U = UNIT ROTATION VECTOR RESOLVED INTO S/C AXES
015064,000330: # A = ROTATION ANGLE
015065,000331:
015066,000332: # *
015067,000333: # THE INTERPRETATION OF DEL IS AS FOLLOWS
015068,000334:
015069,000335: # IF A , A , A REPRESENT THE COMPONENT OF A VECTOR INTHE ROTATED FRAME, THEN THE COMPONENTS OF THE SAME
015070,000336: # X Y Z
015071,000337: # VECTOR IN THE ORIGINAL S/C AXES (B , B , B ) ARE
015072,000338: # X Y Z
015073,000339:
015074,000340: # (B ) (A )
015075,000341: # ( X) ( X)
015076,000342: # ( ) * ( )
015077,000343: # (B ) = DEL (A )
015078,000344: # ( Y) ( Y)
015079,000345: # ( ) ( )
015080,000346: # (B ) (A )
015081,000347: # ( Z) ( Z)
015082,000348:
015083,000349: # THE ROUTINE WILL STORE THIS MATRIX (SCALED UNITY) IN SEQUENTIAL LOCATIONS OF ERASABLE MEMORY BEGINNING WITH
015084,000350: # -
015085,000351: # THE LOCATION CALLED DEL. IN ORDER TO USE THE ROUTINE, THE CALLING PROGRAM MUST FIRST STORE U (A HALF UNIT
015086,000352: # DOUBLE PRECISION VECTOR) IN THE SET OF ERASABLE LOCATIONS BEGINNING WITH THE ADDRESS CALLED COF. THE ANGLE, A,
015087,000353: # MUST THEN BE LOADED INTO D(MPAC).
015088,000354:
015089,000355: # INTERNALLY, THE PROGRAM ALSO USES THE FIRST 10 LOCATIONS OF THE PUSH DOWN LIST.
015090,000356:
015091,000357:
015092,000358: # READCDUK
015093,000359: # --------
015094,000360:
015095,000361: # THIS BASIC LANGUAGE SUBROUTINE LOADS T(MPAC) WITH THE THREE CDU ANGLES.
015096,000362:
015097,000363:
015098,000364: # SIGNMPAC
015099,000365: # --------
015100,000366:
015101,000367: # THIS IS A BASIC LANGUAGE SUBROUTINE WHICH LIMITS THE MAGNITUDE OF D(MPAC) TO + OR - DPOSMAX ON OVERFLOW.
015102,000368:
015103,000369:
015104,000370: # PROGRAM STORAGE ALLOCATION
015105,000371:
015106,000372: # 1) FIXED MEMORY 1059 WORDS
015107,000373: # 2) ERASABLE MEMORY 98
015108,000374: # 3) STATE SWITCHES 3
015109,000375:
|
|
Page 354
|
015111,000377: # 4) FLAGS 1
015112,000378:
015113,000379: # JOB PRIORITIES
015114,000380:
015115,000381: # 1) KALCMANU TBD
015116,000382: # 2) ONE SECOND UPDATE TBD
015117,000383:
015118,000384:
015119,000385: # SUMMARY OF STATE SWITCHES AND FLAGWORDS USED BY KALCMANU.
015120,000386:
015121,000387:
015122,000388: # STATE FLAGWRD 2 SETTING MEANING
015123,000389: # SWITCH NO. BIT NO.
015124,000390:
015125,000391: # *
015126,000392: # 31 14 0 MANEUVER WENT THROUGH GIMBAL LOCK
015127,000393: # 1 MANEUVER DID NOT GO THROUGH GIMBAL LOCK
015128,000394:
015129,000395: # *
015130,000396: # 32 13 0 CONTINUE UPDATE PROCESS
015131,000397: # 1 START UPDATE PROCESS
015132,000398:
015133,000399: # 33 12 0 PERFORM FINAL P-AXIS YAW IF REQUIRED
015134,000400: # 1 IGNORE ANY FINAL P-AXIS YAW
015135,000401:
015136,000402: # 34 11 0 SIGNAL END OF KALCMANU
015137,000403: # 1 KALCMANU IN PROCESS USER MUST SET SWITCH BEFORE INITIATING
015138,000404:
015139,000405:
015140,000406: # * INTERNAL TO KALCMANU
015141,000407:
015142,000408:
015143,000409: # SUGGESTIONS FOR PROGRAM INTEGRATION
015144,000410:
015145,000411: # THE FOLLOWING VARIABLES SHOULD BE ASSIGNED TO UNSWITCH ERASABLE
015146,000412:
015147,000413: # CPHI
015148,000414: # CTHETA
015149,000415: # CPSI
015150,000416: # POINTVSM +5
015151,000417: # SCAXIS +5
015152,000418: # DELDCDU
015153,000419: # DELDCDU1
015154,000420: # DELDCDU2
015155,000421: # RATEINDX
015156,000422:
015157,000423: # THE FOLLOWING SUBROUTINES MAY BE PUT IN A DIFFERENT BANK
015158,000424:
015159,000425: # MXM3
|
|
Page 355
|
015161,000427: # TRANSPOS
015162,000428: # SIGNMPAC
015163,000429: # READCDUK
015164,000430: # CDUTODCM
015165,000431:
|
|
Page 356
|
015167,000433: 15,2050 BANK 15
015168,000434: 22,2000 SETLOC KALCMON1
015169,000435: 22,2000 BANK
015170,000436:
015171,000437: 22,2004 E6,1675 EBANK= BCDU
015172,000438:
015173,000439: # THE THREE DESIRED CDU ANGLES MUST BE STORED AS SINGLE PRECISION TWOS COMPLEMENT ANGLES IN THE THREE SUCCESSIVE
015174,000440: # LOCATIONS, CPHI, CTHETA, CPSI.
015175,000441:
015176,000442: 22,2004 COUNT* $$/KALC
015177,000443: 22,2004 06042 KALCMAN3 TC INTPRET # PICK UP THE CURRENT CDU ANGLES AND
015178,000444: 22,2005 77634 RTB # COMPUTE THE MATRIX FROM INITIAL S/C
015179,000445: 22,2006 44403 READCDUK # AXES TO FINAL S/C AXES
015180,000446: 22,2007 03276 STORE BCDU # STORE INITIAL S/C ANGLES
015181,000447: 22,2010 51535 SLOAD ABS # CHECK THE MAGNITUDE OF THE DESIRED
015182,000448: 22,2011 00324 CPSI # MIDDLE GIMBAL ANGLE
015183,000449: 22,2012 51025 DSU BPL
015184,000450: 22,2013 04403 LOCKANGL # IF GREATER THAN 70 DEG ABORT MANEUVER
015185,000451: 22,2014 44724 TOOBADF
015186,000452: 22,2015 72364 AXC,2 TLOAD
015187,000453: 22,2016 03245 MIS
015188,000454: 22,2017 03276 BCDU
015189,000455: 22,2020 77624 CALL # COMPUTE THE TRANSFORMATION FROM INITIAL
015190,000456: 22,2021 44410 CDUTODCM # S/C AXES TO STABLE MEMBER AXES
015191,000457: 22,2022 72364 AXC,2 TLOAD
015192,000458: 22,2023 02230 MFS # PREPARE TO CALCULATE ARRAY MFS
015193,000459: 22,2024 00322 CPHI
015194,000460: 22,2025 77624 CALL
015195,000461: 22,2026 44410 CDUTODCM
015196,000462: 22,2027 45160 SECAD AXC,1 CALL # MIS AND MFS ARRAYS CALCULATED $2
015197,000463: 22,2030 03245 MIS
015198,000464: 22,2031 44326 TRANSPOS
015199,000465: 22,2032 45575 VLOAD STADR
015200,000466: 22,2033 50460 STOVL TMIS +12D
015201,000467: 22,2034 77626 STADR
015202,000468: 22,2035 50466 STOVL TMIS +6
015203,000469: 22,2036 77626 STADR
015204,000470: 22,2037 74474 STORE TMIS # TMIS = TRANSPOSE(MIS) SCALED BY 2
015205,000471: 22,2040 75160 AXC,1 AXC,2
015206,000472: 22,2041 03302 TMIS
015207,000473: 22,2042 02230 MFS
015208,000474: 22,2043 77624 CALL
015209,000475: 22,2044 44312 MXM3
015210,000476: 22,2045 45575 VLOAD STADR
015211,000477: 22,2046 51532 STOVL MFI +12D
015212,000478: 22,2047 77626 STADR
015213,000479: 22,2050 51540 STOVL MFI +6
015214,000480: 22,2051 77626 STADR
015215,000481: 22,2052 75546 STORE MFI # MFI = TMIS MFS (SCALED BY 4)
015216,000482: 22,2053 45001 SETPD CALL # TRANSPOSE MFI IN PD LIST
015217,000483:
|
|
Page 357
|
015219,000485: 22,2054 00023 18D
015220,000486: 22,2055 44335 TRNSPSPD
015221,000487: 22,2056 45575 VLOAD STADR
015222,000488: 22,2057 50460 STOVL TMFI +12D
015223,000489: 22,2060 77626 STADR
015224,000490: 22,2061 50466 STOVL TMFI +6
015225,000491: 22,2062 77626 STADR
015226,000492: 22,2063 74474 STORE TMFI # TMFI = TRANSPOSE (MFI) SCALED BY 4
015227,000493:
015228,000494: # CALCULATE COFSKEW AND MFISYM
015229,000495:
015230,000496: 22,2064 45345 DLOAD DSU
015231,000497: 22,2065 03305 TMFI +2
015232,000498: 22,2066 02233 MFI +2
015233,000499: 22,2067 45325 PDDL DSU # CALCULATE COF SCALED BY 2/SIN(AM)
015234,000500: 22,2070 02235 MFI +4
015235,000501: 22,2071 03307 TMFI +4
015236,000502: 22,2072 45325 PDDL DSU
015237,000503: 22,2073 03315 TMFI +10D
015238,000504: 22,2074 02243 MFI +10D
015239,000505: 22,2075 77666 VDEF
015240,000506: 22,2076 03325 STORE COFSKEW # EQUALS MFISKEW
015241,000507:
015242,000508: # CALCULATE AM AND PROCEED ACCORDING TO ITS MAGNITUDE
015243,000509:
015244,000510: 22,2077 43345 DLOAD DAD
015245,000511: 22,2100 02231 MFI
015246,000512: 22,2101 02251 MFI +16D
015247,000513: 22,2102 43225 DSU DAD
015248,000514: 22,2103 06460 DP1/4TH
015249,000515: 22,2104 02241 MFI +8D
015250,000516: 22,2105 03333 STORE CAM # CAM = (MFI0+MFI4+MFI8-1)/2 HALF SCALE
015251,000517: 22,2106 77726 ARCCOS
015252,000518: 22,2107 03335 STORE AM # AM=ARCCOS(CAM) (AM SCALED BY 2)
015253,000519: 22,2110 51025 DSU BPL
015254,000520: 22,2111 04363 MINANG
015255,000521: 22,2112 44117 CHECKMAX
015256,000522: 22,2113 77751 TLOAD # MANEUVER LESS THAN .25 DEGREES
015257,000523: 22,2114 00322 CPHI # GO DIRECTLY INTO ATTITUDE HOLD
015258,000524: 22,2115 37235 STCALL CDUXD # ABOUT COMMANDED ANGLES
015259,000525: 22,2116 44742 TOOBADI # STOP RATE AND EXIT
015260,000526:
015261,000527: 22,2117 45345 CHECKMAX DLOAD DSU
015262,000528: 22,2120 03335 AM
015263,000529: 22,2121 04365 MAXANG
015264,000530: 22,2122 77244 BPL VLOAD
015265,000531: 22,2123 44131 ALTCALC # UNIT
015266,000532: 22,2124 03325 COFSKEW # COFSKEW
015267,000533: 22,2125 77656 UNIT
015268,000534: 22,2126 03270 STORE COF # COF IS THE MANEUVER AXIS
015269,000535:
|
|
Page 358
|
015271,000537: 22,2127 77650 GOTO # SEE IF MANEUVER GOES THRU GIMBAL LOCK
015272,000538: 22,2130 44744 LOCSKIRT
015273,000539: 22,2131 53375 ALTCALC VLOAD VAD # IF AM GREATER THAN 170 DEGREES
015274,000540: 22,2132 02231 MFI
015275,000541: 22,2133 03303 TMFI
015276,000542: 22,2134 77762 VSR1
015277,000543: 22,2135 27303 STOVL MFISYM
015278,000544: 22,2136 02237 MFI +6
015279,000545: 22,2137 74455 VAD VSR1
015280,000546: 22,2140 03311 TMFI +6
015281,000547: 22,2141 27311 STOVL MFISYM +6
015282,000548: 22,2142 02245 MFI +12D
015283,000549: 22,2143 74455 VAD VSR1
015284,000550: 22,2144 03317 TMFI +12D
015285,000551: 22,2145 03317 STORE MFISYM +12D # MFISYM=(MFI+TMFI)/2 SCALED BY 4
015286,000552:
015287,000553:
015288,000554: # CALCULATE COF
015289,000555:
015290,000556:
015291,000557: 22,2146 70545 DLOAD SR1
015292,000558: 22,2147 03333 CAM
015293,000559: 22,2150 45325 PDDL DSU # PDO CAM $4
015294,000560: 22,2151 06466 DPHALF
015295,000561: 22,2152 03333 CAM
015296,000562: 22,2153 65204 BOVB PDDL # PD2 1 - CAM $2
015297,000563: 22,2154 21713 SIGNMPAC
015298,000564: 22,2155 03323 MFISYM +16D
015299,000565: 22,2156 56225 DSU DDV
015300,000566: 22,2157 00001 0
015301,000567: 22,2160 00003 2
015302,000568: 22,2161 65366 SQRT PDDL # COFZ = SQRT(MFISYM8-CAM)/1-CAM)
015303,000569: 22,2162 03313 MFISYM +8D # $ ROOT 2
015304,000570: 22,2163 56225 DSU DDV
015305,000571: 22,2164 00001 0
015306,000572: 22,2165 00003 2
015307,000573: 22,2166 65366 SQRT PDDL # COFY = SQRT(MFISYM4-CAM)/(1-CAM) $ROOT2
015308,000574: 22,2167 03303 MFISYM
015309,000575: 22,2170 56225 DSU DDV
015310,000576: 22,2171 00001 0
015311,000577: 22,2172 00003 2
015312,000578: 22,2173 55566 SQRT VDEF # COFX = SQRT(MFISYM-CAM)/(1-CAM) $ROOT 2
015313,000579: 22,2174 77656 UNIT
015314,000580: 22,2175 03270 STORE COF
015315,000581:
015316,000582: # DETERMINE LARGEST COF AND ADJUST ACCORDINGLY
015317,000583:
015318,000584: 22,2176 45345 COFMAXGO DLOAD DSU
015319,000585: 22,2177 03270 COF
015320,000586: 22,2200 03272 COF +2
015321,000587: 22,2201 71240 BMN DLOAD # COFY G COFX
015322,000588:
|
|
Page 359
|
015324,000590: 22,2202 44211 COMP12
015325,000591: 22,2203 03270 COF
015326,000592: 22,2204 50025 DSU BMN
015327,000593: 22,2205 03274 COF +4
015328,000594: 22,2206 44266 METHOD3 # COFZ G COFX OR COFY
015329,000595: 22,2207 77650 GOTO
015330,000596: 22,2210 44242 METHOD1 # COFX G COFY OR COFZ
015331,000597: 22,2211 45345 COMP12 DLOAD DSU
015332,000598: 22,2212 03272 COF +2
015333,000599: 22,2213 03274 COF +4
015334,000600: 22,2214 77640 BMN
015335,000601: 22,2215 44266 METHOD3 # COFZ G COFY OR COFX
015336,000602:
015337,000603: 22,2216 51145 METHOD2 DLOAD BPL # COFY MAX
015338,000604: 22,2217 03327 COFSKEW +2 # UY
015339,000605: 22,2220 44224 U2POS
015340,000606: 22,2221 57575 VLOAD VCOMP
015341,000607: 22,2222 03270 COF
015342,000608: 22,2223 03270 STORE COF
015343,000609: 22,2224 51145 U2POS DLOAD BPL
015344,000610: 22,2225 03305 MFISYM +2 # UX UY
015345,000611: 22,2226 44232 OKU21
015346,000612: 22,2227 57545 DLOAD DCOMP # SIGN OF UX OPPOSITE TO UY
015347,000613: 22,2230 03270 COF
015348,000614: 22,2231 03270 STORE COF
015349,000615: 22,2232 51145 OKU21 DLOAD BPL
015350,000616: 22,2233 03315 MFISYM +10D # UY UZ
015351,000617: 22,2234 44744 LOCSKIRT
015352,000618: 22,2235 57545 DLOAD DCOMP # SIGN OF UZ OPPOSITE TO UY
015353,000619: 22,2236 03274 COF +4
015354,000620: 22,2237 03274 STORE COF +4
015355,000621: 22,2240 77650 GOTO
015356,000622: 22,2241 44744 LOCSKIRT
015357,000623: 22,2242 51145 METHOD1 DLOAD BPL # COFX MAX
015358,000624: 22,2243 03325 COFSKEW # UX
015359,000625: 22,2244 44250 U1POS
015360,000626: 22,2245 57575 VLOAD VCOMP
015361,000627: 22,2246 03270 COF
015362,000628: 22,2247 03270 STORE COF
015363,000629: 22,2250 51145 U1POS DLOAD BPL
015364,000630: 22,2251 03305 MFISYM +2 # UX UY
015365,000631: 22,2252 44256 OKU12
015366,000632: 22,2253 57545 DLOAD DCOMP
015367,000633: 22,2254 03272 COF +2 # SIGN OF UY OPPOSITE TO UX
015368,000634: 22,2255 03272 STORE COF +2
015369,000635: 22,2256 51145 OKU12 DLOAD BPL
015370,000636: 22,2257 03307 MFISYM +4 # UX UZ
015371,000637: 22,2260 44744 LOCSKIRT
015372,000638: 22,2261 57545 DLOAD DCOMP # SIGN OF UZ OPPOSITE TO UY
015373,000639: 22,2262 03274 COF +4
015374,000640:
|
|
Page 360
|
015376,000642: 22,2263 03274 STORE COF +4
015377,000643: 22,2264 77650 GOTO
015378,000644: 22,2265 44744 LOCSKIRT
015379,000645: 22,2266 51145 METHOD3 DLOAD BPL # COFZ MAX
015380,000646: 22,2267 03331 COFSKEW +4 # UZ
015381,000647: 22,2270 44274 U3POS
015382,000648: 22,2271 57575 VLOAD VCOMP
015383,000649: 22,2272 03270 COF
015384,000650: 22,2273 03270 STORE COF
015385,000651: 22,2274 51145 U3POS DLOAD BPL
015386,000652: 22,2275 03307 MFISYM +4 # UX UZ
015387,000653: 22,2276 44302 OKU31
015388,000654: 22,2277 57545 DLOAD DCOMP
015389,000655: 22,2300 03270 COF # SIGN OF UX OPPOSITE TO UZ
015390,000656: 22,2301 03270 STORE COF
015391,000657: 22,2302 51145 OKU31 DLOAD BPL
015392,000658: 22,2303 03315 MFISYM +10D # UY UZ
015393,000659: 22,2304 44744 LOCSKIRT
015394,000660: 22,2305 57545 DLOAD DCOMP
015395,000661: 22,2306 03272 COF +2 # SIGN OF UY OPPOSITE TO UZ
015396,000662: 22,2307 03272 STORE COF +2
015397,000663: 22,2310 77650 GOTO
015398,000664: 22,2311 44744 LOCSKIRT
|
|
Page 361
|
015400,000666: # MATRIX OPERATIONS
015401,000667:
015402,000668: 13,2207 BANK 13
015403,000669: 22,2000 SETLOC KALCMON2
015404,000670: 22,2000 BANK
015405,000671:
015406,000672: 22,2312 E6,1675 EBANK= BCDU
015407,000673:
015408,000674: 22,2312 76601 MXM3 SETPD VLOAD* # MXM3 MULTIPLIES 2 3X3 MATRICES
015409,000675: 22,2313 00001 0 # AND LEAVES RESULT IN PD LIST
015410,000676: 22,2314 00001 0,1 # AND MPAC
015411,000677: 22,2315 62703 VXM* PDVL*
015412,000678: 22,2316 77776 0,2
015413,000679: 22,2317 00007 6,1
015414,000680: 22,2320 62703 VXM* PDVL*
015415,000681: 22,2321 77776 0,2
015416,000682: 22,2322 00015 12D,1
015417,000683: 22,2323 41503 VXM* PUSH
015418,000684: 22,2324 77776 0,2
015419,000685: 22,2325 77616 RVQ
015420,000686:
015421,000687:
015422,000688: # RETURN WITH M1XM2 IN PD LIST
015423,000689:
015424,000690: 22,2326 76601 TRANSPOS SETPD VLOAD* # TRANSPOS TRANSPOSES A 3X3 MATRIX
015425,000691: 22,2327 00001 0 # AND LEAVES RESULT IN PD LIST
015426,000692: 22,2330 00001 0,1 # MATRIX ADDRESS IN XR1
015427,000693: 22,2331 62713 PDVL* PDVL*
015428,000694: 22,2332 00007 6,1
015429,000695: 22,2333 00015 12D,1
015430,000696: 22,2334 77606 PUSH # MATRIX IN PD
015431,000697: 22,2335 77776 TRNSPSPD EXIT # ENTER WITH MATRIX AT 0 IN PD LIST
015432,000698: 22,2336 50120 INDEX FIXLOC
015433,000699: 22,2337 52013 DXCH 12
015434,000700: 22,2340 50120 INDEX FIXLOC
015435,000701: 22,2341 52017 DXCH 16
015436,000702: 22,2342 50120 INDEX FIXLOC
015437,000703: 22,2343 52013 DXCH 12
015438,000704: 22,2344 50120 INDEX FIXLOC
015439,000705: 22,2345 52015 DXCH 14
015440,000706: 22,2346 50120 INDEX FIXLOC
015441,000707: 22,2347 52005 DXCH 4
015442,000708: 22,2350 50120 INDEX FIXLOC
015443,000709: 22,2351 52015 DXCH 14
015444,000710: 22,2352 50120 INDEX FIXLOC
015445,000711: 22,2353 52003 DXCH 2
015446,000712: 22,2354 50120 INDEX FIXLOC
015447,000713: 22,2355 52007 DXCH 6
015448,000714: 22,2356 50120 INDEX FIXLOC
015449,000715: 22,2357 52003 DXCH 2
015450,000716:
|
|
Page 362
|
015452,000718: 22,2360 06042 TC INTPRET
015453,000719: 22,2361 77616 RVQ
015454,000720:
015455,000721: 15,2050 BANK 15
015456,000722: 22,2000 SETLOC KALCMON1
015457,000723: 22,2000 BANK
015458,000724:
015459,000725: 22,2362 E6,1675 EBANK= BCDU
015460,000726:
015461,000727: 22,2362 00013 13563 MINANG 2DEC 0.00069375
015462,000728: 22,2364 17070 34343 MAXANG 2DEC 0.472222222
015463,000729: # GIMBAL LOCK CONSTANTS
015464,000730:
015465,000731: # D = MGA CORRESPONDING TO GIMBAL LOCK = 60 DEGREES
015466,000732: # NGL = BUFFER ANGLE (TO AVOID DIVISIONS BY ZERO) = 2 DEGREES
015467,000733:
015468,000734: 22,2366 15666 20443 SD 2DEC .433015 # = SIN(D) $2
015469,000735: 22,2370 33555 01106 K3S1 2DEC .86603 # = SIN(D) $1
015470,000736: 22,2372 67777 77777 K4 2DEC -.25 # = -COS(D) $2
015471,000737: 22,2374 04000 00000 K4SQ 2DEC .125 # = COS(D)COS(D) $2
015472,000738: 22,2376 00216 36323 SNGLCD 2DEC .008725 # = SIN(NGL)COS(D) $2
015473,000739: 22,2400 17773 00057 CNGL 2DEC .499695 # COS(NGL) $2
015474,000740: 22,2402 14344 LOCKANGL DEC .388889 # = 70 DEGREES
015475,000741: # INTERPRETIVE SUBROUTINE TO READ THE CDU ANGLES
015476,000742:
015477,000743: 22,2403 30034 READCDUK CA CDUZ # LOAD T(MPAC) WITH CDU ANGLES
015478,000744: 22,2404 54156 TS MPAC +2
015479,000745: 22,2405 00006 EXTEND
015480,000746: 22,2406 30033 DCA CDUX # AND CHANGE MODE TO TRIPLE PRECISION
015481,000747: 22,2407 16501 TCF TLOAD +6
015482,000748:
015483,000749:
015484,000750: 22,2410 66370 CDUTODCM AXT,1 SSP
015485,000751: 22,2411 00003 OCT 3
015486,000752: 22,2412 00051 S1
015487,000753: 22,2413 00001 OCT 1 # SET XR1, S1, AND PD FOR LOOP
015488,000754: 22,2414 00010 STORE 7
015489,000755: 22,2415 77601 SETPD
015490,000756: 22,2416 00001 0
015491,000757: 22,2417 47133 LOOPSIN SLOAD* RTB
015492,000758: 22,2420 00013 10D,1
015493,000759: 22,2421 21577 CDULOGIC
015494,000760:
|
|
Page 363
|
015496,000762: 22,2422 00013 STORE 10D # LOAD PD WITH 0 SIN(PHI)
015497,000763: 22,2423 65356 SIN PDDL # 2 COS(PHI)
015498,000764: 22,2424 00013 10D # 4 SIN(THETA)
015499,000765: 22,2425 41546 COS PUSH # 6 COS(THETA)
015500,000766: 22,2426 71300 TIX,1 DLOAD # 8 SIN(PSI)
015501,000767: 22,2427 44417 LOOPSIN # 10 COS(PSI)
015502,000768: 22,2430 00007 6
015503,000769: 22,2431 72405 DMP SL1
015504,000770: 22,2432 00013 10D
015505,000771: 22,2433 10001 STORE 0,2 # C0=COS(THETA)COS(PSI)
015506,000772: 22,2434 41345 DLOAD DMP
015507,000773: 22,2435 00005 4
015508,000774: 22,2436 00001 0
015509,000775: 22,2437 41325 PDDL DMP # (PD6 SIN(THETA)SIN(PHI))
015510,000776: 22,2440 00007 6
015511,000777: 22,2441 00011 8D
015512,000778: 22,2442 72405 DMP SL1
015513,000779: 22,2443 00003 2
015514,000780: 22,2444 72421 BDSU SL1
015515,000781: 22,2445 00015 12D
015516,000782: 22,2446 10003 STORE 2,2 # C1=-COS(THETA)SIN(PSI)COS(PHI)
015517,000783: 22,2447 41345 DLOAD DMP
015518,000784: 22,2450 00003 2
015519,000785: 22,2451 00005 4
015520,000786: 22,2452 41325 PDDL DMP # (PD7 COS(PHI)SIN(THETA)) SCALED 4
015521,000787: 22,2453 00007 6
015522,000788: 22,2454 00011 8D
015523,000789: 22,2455 72405 DMP SL1
015524,000790: 22,2456 00001 0
015525,000791: 22,2457 72415 DAD SL1
015526,000792: 22,2460 00017 14D
015527,000793: 22,2461 10005 STORE 4,2 # C2=COS(THETA)SIN(PSI)SIN(PHI)
015528,000794: 22,2462 77745 DLOAD
015529,000795: 22,2463 00011 8D
015530,000796: 22,2464 10007 STORE 6,2 # C3=SIN(PSI)
015531,000797: 22,2465 77745 DLOAD
015532,000798: 22,2466 00013 10D
015533,000799: 22,2467 72405 DMP SL1
015534,000800: 22,2470 00003 2
015535,000801: 22,2471 10011 STORE 8D,2 # C4=COS(PSI)COS(PHI)
015536,000802: 22,2472 41345 DLOAD DMP
015537,000803: 22,2473 00013 10D
015538,000804: 22,2474 00001 0
015539,000805: 22,2475 72476 DCOMP SL1
015540,000806: 22,2476 10013 STORE 10D,2 # C5=-COS(PSI)SIN(PHI)
015541,000807: 22,2477 41345 DLOAD DMP
015542,000808: 22,2500 00005 4
015543,000809: 22,2501 00013 10D
015544,000810: 22,2502 72476 DCOMP SL1
015545,000811: 22,2503 10015 STORE 12D,2 # C6=-SIN(THETA)COS(PSI)
015546,000812:
|
|
Page 364
|
015548,000814: 22,2504 77745 DLOAD
015549,000815: 22,2505 72405 DMP SL1 # (PUSH UP 7)
015550,000816: 22,2506 00011 8D
015551,000817: 22,2507 41325 PDDL DMP # (PD7 COS(PHI)SIN(THETA)SIN(PSI)) SCALE4
015552,000818: 22,2510 00007 6
015553,000819: 22,2511 00001 0
015554,000820: 22,2512 72415 DAD SL1 # (PUSH UP 7)
015555,000821: 22,2513 77626 STADR # C7=COS(PHI)SIN(THETA)SIN(PSI)
015556,000822: 22,2514 67760 STORE 14D,2 # +COS(THETA)SIN(PHI)
015557,000823: 22,2515 77745 DLOAD
015558,000824: 22,2516 72405 DMP SL1 # (PUSH UP 6)
015559,000825: 22,2517 00011 8D
015560,000826: 22,2520 41325 PDDL DMP # (PD6 SIN(THETA)SIN(PHI)SIN(PSI)) SCALE4
015561,000827: 22,2521 00007 6
015562,000828: 22,2522 00003 2
015563,000829: 22,2523 72425 DSU SL1 # (PUSH UP 6)
015564,000830: 22,2524 77626 STADR
015565,000831: 22,2525 67756 STORE 16D,2 # C8=-SIN(THETA)SIN(PHI)SIN(PSI)
015566,000832: 22,2526 77616 RVQ # +COS(THETA)COS(PHI)
015567,000833:
015568,000834: # CALCULATION OF THE MATRIX DEL......
015569,000835:
015570,000836: # * * --T *
015571,000837: # DEL = (IDMATRIX)COS(A)+UU (1-COS(A))+UX SIN(A) SCALED 1
015572,000838:
015573,000839: # -
015574,000840: # WHERE U IS A UNIT VECTOR (DP SCALED 2) ALONG THE AXIS OF ROTATION.
015575,000841: # A IS THE ANGLE OF ROTATION (DP SCALED 2)
015576,000842: # -
015577,000843: # UPON ENTRY THE STARTING ADDRESS OF U IS COF, AND A IS IN MPAC
015578,000844:
015579,000845: 22,2527 41401 DELCOMP SETPD PUSH # MPAC CONTAINS THE ANGLE A
015580,000846: 22,2530 00001 0
015581,000847: 22,2531 65356 SIN PDDL # PD0 = SIN(A)
015582,000848: 22,2532 41546 COS PUSH # PD2 = COS(A)
015583,000849: 22,2533 65302 SR2 PDDL # PD2 = COS(A) $8
015584,000850: 22,2534 41021 BDSU BOVB
015585,000851: 22,2535 06466 DPHALF
015586,000852: 22,2536 21713 SIGNMPAC
015587,000853: 22,2537 77725 PDDL # PD4 = 1-COS(A)
015588,000854:
015589,000855: # COMPUTE THE DIAGONAL COMPONENTS OF DEL
015590,000856:
015591,000857: 22,2540 03270 COF
015592,000858: 22,2541 41316 DSQ DMP
015593,000859: 22,2542 00005 4
015594,000860: 22,2543 52415 DAD SL3
015595,000861: 22,2544 00003 2
015596,000862: 22,2545 77604 BOVB
015597,000863: 22,2546 21713 SIGNMPAC
015598,000864:
|
|
Page 365
|
015600,000866: 22,2547 16231 STODL KEL # UX UX(1-COS(A)) +COS(A) $1
015601,000867: 22,2550 03272 COF +2
015602,000868: 22,2551 41316 DSQ DMP
015603,000869: 22,2552 00005 4
015604,000870: 22,2553 52415 DAD SL3
015605,000871: 22,2554 00003 2
015606,000872: 22,2555 77604 BOVB
015607,000873: 22,2556 21713 SIGNMPAC
015608,000874: 22,2557 16241 STODL KEL +8D # UY UY(1-COS(A)) +COS(A) $1
015609,000875: 22,2560 03274 COF +4
015610,000876: 22,2561 41316 DSQ DMP
015611,000877: 22,2562 00005 4
015612,000878: 22,2563 52415 DAD SL3
015613,000879: 22,2564 00003 2
015614,000880: 22,2565 77604 BOVB
015615,000881: 22,2566 21713 SIGNMPAC
015616,000882: 22,2567 02251 STORE KEL +16D # UZ UZ(1-COS(A)) +COS(A) $1
015617,000883:
015618,000884: # COMPUTE THE OFF DIAGONAL TERMS OF DEL
015619,000885:
015620,000886: 22,2570 41345 DLOAD DMP
015621,000887: 22,2571 03270 COF
015622,000888: 22,2572 03272 COF +2
015623,000889: 22,2573 72405 DMP SL1
015624,000890: 22,2574 00005 4
015625,000891: 22,2575 41325 PDDL DMP # D6 UX UY (1-COS A) $ 4
015626,000892: 22,2576 03274 COF +4
015627,000893: 22,2577 00001 0
015628,000894: 22,2600 43206 PUSH DAD # D8 UZ SIN A $ 4
015629,000895: 22,2601 00007 6
015630,000896: 22,2602 41112 SL2 BOVB
015631,000897: 22,2603 21713 SIGNMPAC
015632,000898: 22,2604 16237 STODL KEL +6
015633,000899: 22,2605 62421 BDSU SL2
015634,000900: 22,2606 77604 BOVB
015635,000901: 22,2607 21713 SIGNMPAC
015636,000902: 22,2610 16233 STODL KEL +2
015637,000903: 22,2611 03270 COF
015638,000904: 22,2612 41205 DMP DMP
015639,000905: 22,2613 03274 COF +4
015640,000906: 22,2614 00005 4
015641,000907: 22,2615 65352 SL1 PDDL # D6 UX UZ (1-COS A) $ 4
015642,000908: 22,2616 03272 COF +2
015643,000909: 22,2617 41405 DMP PUSH # D8 UY SIN(A)
015644,000910: 22,2620 00001 0
015645,000911: 22,2621 62415 DAD SL2
015646,000912: 22,2622 00007 6
015647,000913: 22,2623 77604 BOVB
015648,000914: 22,2624 21713 SIGNMPAC
015649,000915: 22,2625 16235 STODL KEL +4 # UX UZ (1-COS(A))+UY SIN(A)
015650,000916:
|
|
Page 366
|
015652,000918: 22,2626 62421 BDSU SL2
015653,000919: 22,2627 77604 BOVB
015654,000920: 22,2630 21713 SIGNMPAC
015655,000921: 22,2631 16245 STODL KEL +12D # UX UZ (1-COS(A))-UY SIN(A)
015656,000922: 22,2632 03272 COF +2
015657,000923: 22,2633 41205 DMP DMP
015658,000924: 22,2634 03274 COF +4
015659,000925: 22,2635 00005 4
015660,000926: 22,2636 65352 SL1 PDDL # D6 UY UZ (1-COS(A)) $ 4
015661,000927: 22,2637 03270 COF
015662,000928: 22,2640 41405 DMP PUSH # D8 UX SIN(A)
015663,000929: 22,2641 00001 0
015664,000930: 22,2642 62415 DAD SL2
015665,000931: 22,2643 00007 6
015666,000932: 22,2644 77604 BOVB
015667,000933: 22,2645 21713 SIGNMPAC
015668,000934: 22,2646 16247 STODL KEL +14D # UY UZ(1-COS(A)) +UX SIN(A)
015669,000935: 22,2647 62421 BDSU SL2
015670,000936: 22,2650 77604 BOVB
015671,000937: 22,2651 21713 SIGNMPAC
015672,000938: 22,2652 02243 STORE KEL +10D # UY UZ (1-COS(A)) -UX SIN(A)
015673,000939: 22,2653 77616 RVQ
015674,000940:
015675,000941:
015676,000942: # DIRECTION COSINE MATRIX TO CDU ANGLE ROUTINE
015677,000943: # X1 CONTAINS THE COMPLEMENT OF THE STARTING ADDRESS FOR MATRIX (SCALED 2)
015678,000944: # LEAVES CDU ANGLES SCALED 2PI IN V(MPAC)
015679,000945: # COS(MGA) WILL BE LEFT IN S1 (SCALED 1)
015680,000946:
015681,000947: # THE DIRECTION COSINE MATRIX RELATING S/C AXES TO STABLE MEMBER AXES CAN BE WRITTEN AS***
015682,000948:
015683,000949: # C =COS(THETA)COS(PSI)
015684,000950: # 0
015685,000951: # C =-COS(THETA)SIN(PSI)COS(PHI)+SI (THETA)SIN(PHI)
015686,000952: # 1
015687,000953: # C =COS(THETA)SIN(PSI)SIN(PHI) + S N(THETA)COS(PHI)
015688,000954: # 2
015689,000955: # C =SIN(PSI)
015690,000956: # 3
015691,000957: # C =COS(PSI)COS(PHI)
015692,000958: # 4
015693,000959: # C =-COS(PSI)SIN(PHI)
015694,000960: # 5
015695,000961: # C =-SIN(THETA)COS(PSI)
015696,000962: # 6
015697,000963: # C =SIN(THETA)SIN(PSI)COS(PHI)+COS THETA)SIN(PHI)
015698,000964: # 7
015699,000965: # C =-SIN(THETA)SIN(PSI)SIN(PHI)+CO (THETA)COS(PHI)
015700,000966: # 8
015701,000967:
|
|
Page 367
|
015703,000969:
015704,000970: # WHERE PHI = OGA
015705,000971: # THETA = IGA
015706,000972: # PSI = MGA
015707,000973:
015708,000974: 22,2654 67543 DCMTOCDU DLOAD* ARCSIN
015709,000975: 22,2655 00007 6,1
015710,000976: 22,2656 71406 PUSH COS # PD +0 PSI
015711,000977: 22,2657 41152 SL1 BOVB
015712,000978: 22,2660 21713 SIGNMPAC
015713,000979: 22,2661 00051 STORE S1
015714,000980: 22,2662 57543 DLOAD* DCOMP
015715,000981: 22,2663 00015 12D,1
015716,000982: 22,2664 67471 DDV ARCSIN
015717,000983: 22,2665 00051 S1
015718,000984: 22,2666 51123 PDDL* BPL # PD +2 THETA
015719,000985: 22,2667 00001 0,1 # MUST CHECK THE SIGN OF COS(THETA)
015720,000986: 22,2670 44702 OKTHETA # TO DETERMINE THE PROPER QUADRANT
015721,000987: 22,2671 57545 DLOAD DCOMP
015722,000988: 22,2672 43244 BPL DAD
015723,000989: 22,2673 44677 SUHALFA
015724,000990: 22,2674 06466 DPHALF
015725,000991: 22,2675 77650 GOTO
015726,000992: 22,2676 44701 CALCPHI
015727,000993: 22,2677 77625 SUHALFA DSU
015728,000994: 22,2700 06466 DPHALF
015729,000995: 22,2701 77606 CALCPHI PUSH
015730,000996: 22,2702 57543 OKTHETA DLOAD* DCOMP
015731,000997: 22,2703 00013 10D,1
015732,000998: 22,2704 67471 DDV ARCSIN
015733,000999: 22,2705 00051 S1
015734,001000: 22,2706 51123 PDDL* BPL # PUSH DOWN PHI
015735,001001: 22,2707 00011 8D,1
015736,001002: 22,2710 44722 OKPHI
015737,001003: 22,2711 57545 DLOAD DCOMP # PUSH UP PHI
015738,001004: 22,2712 43244 BPL DAD
015739,001005: 22,2713 44717 SUHALFAP
015740,001006: 22,2714 06466 DPHALF
015741,001007: 22,2715 77650 GOTO
015742,001008: 22,2716 44723 VECOFANG
015743,001009: 22,2717 52025 SUHALFAP DSU GOTO
015744,001010: 22,2720 06466 DPHALF
015745,001011: 22,2721 44723 VECOFANG
015746,001012: 22,2722 77745 OKPHI DLOAD # PUSH UP PHI
015747,001013: 22,2723 43466 VECOFANG VDEF RVQ
015748,001014:
|
|
Page 368
|
015750,001016: # ROUTINES FOR TERMINATING THE AUTOMATIC MANEUVER AND RETURNING TO USER
015751,001017:
015752,001018: 22,2724 77776 TOOBADF EXIT
015753,001019: 22,2725 05600 TC ALARM
015754,001020: 22,2726 00401 OCT 00401
015755,001021:
015756,001022: 22,2727 12732 TCF NOGO # DO NOT ZERO ATTITUDE ERRORS
015757,001023:
015758,001024: 22,2730 04616 TC BANKCALL
015759,001025: 22,2731 40154 CADR ZATTEROR # ZERO ATTITUDE ERRORS
015760,001026:
015761,001027: 22,2732 04616 NOGO TC BANKCALL
015762,001028: 22,2733 40166 CADR STOPRATE # STOP RATES
015763,001029:
015764,001030: 22,2734 34752 CAF TWO
015765,001031: 22,2735 00004 INHINT # ALL RETURNS ARE NOW MADE VIA GOODEND
015766,001032: 22,2736 05214 TC WAITLIST
015767,001033: 22,2737 E6,1675 EBANK= BCDU
015768,001034: 22,2737 03234 44066 2CADR GOODMANU
015769,001035:
015770,001036: 22,2741 15155 TCF ENDOFJOB
015771,001037:
015772,001038: 22,2742 77776 TOOBADI EXIT
015773,001039: 22,2743 12732 TCF NOGO
End of include-file ATTITUDE_MANEUVER_ROUTINE.agc. Parent file is MAIN.agc