Notations on the program listing read, in part:
GAP: ASSEMBLE REVISION 210 OF AGC PROGRAM LUMINARY BY NASA 2021112-161 17:11 MAR. 19,1971Note that the date is the date of the printout, not the date of the program revision.
Source Code
|
|
These source-code files derive from a printout of Luminary 210 (Apollo 15-17
Lunar Module guidance computer program), from the personal library of
original AGC developer Don Eyles, digitally photographed at archive.org,
financially sponsored by Jim Lawton, and transcribed to source code by a
team of volunteers. This colorized, syntax-highlighted form was created
by assembling that transcribed source code. Note that the full page images
are available on the
Virtual AGC project page at archive.org, while reduced-size images
are presented at the VirtualAGC project website. Report or fix any
transcription errors at
the Virtual AGC project code repository. Notations on the program listing read, in part: GAP: ASSEMBLE REVISION 210 OF AGC PROGRAM LUMINARY BY NASA 2021112-161 17:11 MAR. 19,1971Note that the date is the date of the printout, not the date of the program revision. |
015347,000002: ## Copyright: Public domain.
015348,000003: ## Filename: ATTITUDE_MANEUVER_ROUTINE.agc
015349,000004: ## Purpose: A section of Luminary revision 210.
015350,000005: ## It is part of the source code for the Lunar Module's (LM)
015351,000006: ## Apollo Guidance Computer (AGC) for Apollo 15-17.
015352,000007: ## This file is intended to be a faithful transcription, except
015353,000008: ## that the code format has been changed to conform to the
015354,000009: ## requirements of the yaYUL assembler rather than the
015355,000010: ## original YUL assembler.
015356,000011: ## Reference: pp. 354-375
015357,000012: ## Assembler: yaYUL
015358,000013: ## Contact: Ron Burkey <info@sandroid.org>.
015359,000014: ## Website: www.ibiblio.org/apollo/index.html
015360,000015: ## Mod history: 2016-11-17 JL Created from Luminary131 version.
015361,000016: ## 2016-11-24 HG Transcribed
015362,000017: ## 2016-12-23 RSB Proofed comment text with octopus/ProoferComments
015363,000018: ## and fixed all errors found.
015364,000019: ## 2017-03-15 RSB Comment-text fixes identified in 5-way
015365,000020: ## side-by-side diff of Luminary 69/99/116/131/210.
015366,000021:
|
|
Page 354 |
015368,000023: # BLOCK 2 LGC ATTITUDE MANEUVER ROUTINE-KALCMANU
015369,000024:
015370,000025:
015371,000026: # MOD 2 DATE 5/1/67 BY DON KEENE
015372,000027:
015373,000028: # PROGRAM DESCRIPTION
015374,000029:
015375,000030: # KALCMANU IS A ROUTINE WHICH GENERATES COMMANDS FOR THE LM DAP TO CHANGE THE ATTITUDE OF THE SPACECRAFT
015376,000031: # DURING FREE FALL. IT IS DESIGNED TO MANEUVER THE SPACECRAFT FROM ITS INITIAL ORIENTATION TO SOME DESIRED
015377,000032: # ORIENTATION SPECIFIED BY THE PROGRAM WHICH CALLS KALCMANU, AVOIDING GIMBAL LOCK IN THE PROCESS. IN THE
015378,000033: # MOD 2 VERSION, THIS DESIRED ATTITUDE IS SPECIFIED BY A SET OF THREE COMMANDED CDU ANGLES STORED AS 2S COMPLEMENT
015379,000034: # SINGLE PRECISION ANGLES IN THE THREE CONSECUTIVE LOCATIONS, CPHI, CTHETA, CPSI, WHERE
015380,000035:
015381,000036: # CPHI = COMMANDED OUTER GIMBAL ANGLE
015382,000037: # CTHETA = COMMANDED INNER GIMBAL ANGLE
015383,000038: # CPSI = COMMANDED MIDDLE GIMBAL ANGLE
015384,000039:
015385,000040: # WHEN POINTING A SPACECRAFT AXIS (E.I. X, Y, Z, THE AOT, THRUST AXIS, ETC) THE SUBROUTINE VECPOINT MAY BE
015386,000041: # USED TO GENERATE THIS SET OF DESIRED CDU ANGLES (SEE DESCRIPTION IN R60) -
015387,000042: # WITH THIS INFORMATION KALCMANU DETERMINES THE DIRECTION OF THE SINGLE EQUIVALENT ROTATION (COF ALSO U) AND THE
015388,000043: # MAGNITUDE OF THE ROTATION (AM) TO BRING THE S/C FROM ITS INITIAL ORIENTATION TO ITS FINAL ORIENTATION.
015389,000044: # THIS DIRECTION REMAINS FIXED BOTH IN INERTIAL COORDINATES AND IN COMMANDED S/C AXES THROUGHOUT THE
015390,000045: # -
015391,000046: # MANEUVER. ONCE COF AND AM HAVE BEEN DETERMINED, KALCMANU THEN EXAMINES THE MANEUVER TO SEE IF IT WILL BRING
015392,000047: # -
015393,000048: # THE S/C THROUGH GIMBAL LOCK. IF SO, COF AND AM ARE READJUSTED SO THAT THE S/C WILL JUST SKIM THE GIMBAL
015394,000049: # LOCK ZONE AND ALIGN THE X-AXIS. IN GENERAL A FINAL YAW ABOUT X WILL BE NECESSARY TO COMPLETE THE MANEUVER.
015395,000050: # NEEDLESS TO SAY, NEITHER THE INITIAL NOR THE FINAL ORIENTATION CAN BE IN GIMBAL LOCK.
015396,000051:
015397,000052: # FOR PROPER ATTITUDE CONTROL THE DIGITAL AUTOPILOT MUST BE GIVEN AN ATTITUDE REFERENCE WHICH IT CAN TRACK.
015398,000053: # KALCMANU DOES THIS BY GENERATING A REFERENCE OF DESIRED GIMBAL ANGLES (CDUXD, CDUYD, CDUZD) WHICH ARE UPDATED
015399,000054: # EVERY ONE SECOND DURING THE MANEUVER. TO ACHIEVE A SMOOTHER SEQUENCE OF COMMANDS BETWEEN SUCCESSIVE UPDATES,
015400,000055: # THE PROGRAM ALSO GENERATES A SET OF INCREMENTAL CDU ANGLES (DELDCDU) TO BE ADDED TO CDU DESIRED BY THE DIGITAL
015401,000056: # AUTOPILOT. KALCMANU ALSO CALCULATES THE COMPONENT MANEUVER RATES (OMEGAPD, OMEGAQD, OMEGARD), WHICH CAN
015402,000057: # -
015403,000058: # BE DETERMINED SIMPLY BY MULTIPLYING COF BY SOME SCALAR (ARATE) CORRESPONDING TO THE DESIRED ROTATIONAL RATE.
015404,000059:
015405,000060: # AUTOMATIC MANEUVERS ARE TIMED WITH THE HELP OF WAITLIST SO THAT AFTER A SPECIFIED INTERVAL THE Y AND Z
015406,000061: # DESIRED RATES ARE SET TO ZERO AND THE DESIRED CDU ANGLES (CDUYD, CDUZD) ARE SET EQUAL TO THE FINAL DESIRED CDU
015407,000062: # ANGLES (CTHETA, CPSI). IF ANY YAW REMAINS DUE TO GIMBAL LOCK AVOIDANCE, THE FINAL YAW MANEUVER IS
015408,000063: # CALCULATED AND THE DESIRED YAW RATE SET TO SOME FIXED VALUE (ROLLRATE = + OR - 2 DEGREES PER SEC).
015409,000064: # IN THIS CASE ONLY AN INCREMENTAL CDUX ANGLE (DELFROLL) IS SUPPLIED TO THE DAP. AT THE END OF THE YAW
015410,000065: # MANEUVER OR IN THE EVENT THAT THERE WAS NO FINAL YAW, CDUXD IS SET EQUAL TO CPHI AND THE X-AXIS DESIRED
015411,000066: # RATE SET TO ZERO. THUS, UPON COMPLETION OF THE MANEUVER THE S/C WILL FINISH UP IN A LIMIT CYCLE ABOUT THE
015412,000067: # DESIRED FINAL GIMBAL ANGLES.
015413,000068:
015414,000069:
015415,000070: # PROGRAM LOGIC FLOW
015416,000071:
015417,000072: # KALCMANU IS CALLED AS A HIGH PRIORITY JOB WITH ENTRY POINTS AT KALCMAN3 AND VECPOINT. IT FIRST PICKS
015418,000073: # UP THE CURRENT CDU ANGLES TO BE USED AS THE BASIS FOR ALL COMPUTATIONS INVOLVING THE INITIAL S/C ORIENTATION.
015419,000074:
|
|
Page 355 |
015421,000076: # IT THEN DETERMINES THE DIRECTION COSINE MATRICES RELATING BOTH THE INITIAL AND FINAL S/C ORIENTATION TO STABLE
015422,000077: # * * *
015423,000078: # MEMBER AXES (MIS, MFS). IT ALSO COMPUTES THE MATRIX RELATING FINAL S/C AXES TO INITIAL S/C AXES (MFI). THE
015424,000079: # ANGLE OF ROTATION (AM) IS THEN EXTRACTED FROM THIS MATRIX, AND TESTS ARE MADE TO DETERMINE IF
015425,000080:
015426,000081: # A) AM LESS THAN .25 DEGREES (MINANG)
015427,000082: # B) AM GREATER THAN 170 DEGREES (MAXANG)
015428,000083:
015429,000084: # IF AM LESS THAN .25 DEGREES, NO COMPLICATED AUTOMATIC MANEUVERING IS NECESSARY. THEREFORE WE CAN SIMPLY
015430,000085: # SET CDU DESIRED EQUAL TO THE FINAL CDU DESIRED ANGLES AND TERMINATE THE JOB.
015431,000086:
015432,000087: # IF AM IS GREATER THAN .25 DEGREES BUT LESS THAN 170 DEGREES, THE AXES OF THE SINGLE EQUIVALENT ROTATION
015433,000088: # - *
015434,000089: # (COF) IS EXTRACTED FROM THE SKEW SYMMETRIC COMPONENTS OF MFI. * *
015435,000090: # IF AM GREATER THAN 170 DEGREES AN ALTERNATE METHOD EMPLOYING THE SYMMETRIC PART OF MFI (MFISYM) IS USED
015436,000091: # -
015437,000092: # TO DETERMINE COF.
015438,000093:
015439,000094: # THE PROGRAM THEN CHECKS TO SEE IF THE MANEUVER AS COMPUTED WILL BRING THE S/C THROUGH GIMBAL LOCK. IF
015440,000095: # SO, A NEW MANEUVER IS CALCULATED WHICH WILL JUST SKIM THE GIMBAL LOCK ZONE AND ALIGN THE S/C X-AXIS. THIS
015441,000096: # METHOD ASSURES THAT THE ADDITIONAL MANEUVERING TO AVOID GIMBAL LOCK WILL BE KEPT TO A MINIMUM. SINCE A FINAL
015442,000097: # P AXIS YAW WILL BE NECESSARY, A SWITCH IS RESET (STATE SWITCH 31) TO ALLOW FOR THE COMPUTATION OF THIS FINAL
015443,000098: # YAW.
015444,000099:
015445,000100: # AS STATED PREVIOUSLY KALCMANU GENERATES A SEQUENCE OF DESIRED GIMBAL ANGLES WHICH ARE UPDATED EVERY
015446,000101: # -
015447,000102: # SECOND. THIS IS ACCOMPLISHED BY A SMALL ROTATION OF THE DESIRED S/C FRAME ABOUT THE VECTOR COF. THE NEW
015448,000103: # DESIRED REFERENCE MATRIX IS THEN,
015449,000104:
015450,000105: # * * *
015451,000106: # MIS = MIS DEL
015452,000107: # N+1 N
015453,000108: # *
015454,000109: # WHERE DEL IS THE MATRIX CORRESPONDING TO THIS SMALL ROTATION. THE NEW CDU ANGLES CAN THEN BE EXTRACTED
015455,000110: # *
015456,000111: # FROM MIS.
015457,000112:
015458,000113: # AT THE BEGINNING OF THE MANEUVER THE AUTOPILOT DESIRED RATES (OMEGAPD, OMEGAQD, OMEGARD) AND THE
015459,000114: # MANEUVER TIMINGS ARE ESTABLISHED. ON THE FIRST PASS AND ON ALL SUBSEQUENT UPDATES THE CDU DESIRED
015460,000115: # ANGLES ARE LOADED WITH THE APPROPRIATE VALUES AND THE INCREMENTAL CDU ANGLES ARE COMPUTED. THE AGC CLOCKS
015461,000116: # (TIME1 AND TIME2) ARE THAN CHECKED TO SEE IF THE MANEUVER WILL TERMINATE BEFORE THE NEXT UPDATE. IF
015462,000117: # NOT, KALCMANU CALLS FOR ANOTHER UPDATE (RUN AS A JOB WITH PRIORITY TBD) IN ONE SECOND. ANY DELAYS IN THIS
015463,000118: # CALLING SEQUENCE ARE AUTOMATICALLY COMPENSATED IN CALLING FOR THE NEXT UPDATE.
015464,000119:
015465,000120: # IF IT IS FOUND THAT THE MANEUVER IS TO TERMINATE BEFORE THE NEXT UPDATE A ROUTINE IS CALLED (AS A WAIT-
015466,000121: # LIST TASK) TO STOP THE MANEUVER AT THE APPROPRIATE TIME AS EXPLAINED ABOVE.
015467,000122:
|
|
Page 356 |
015469,000124: # CALLING SEQUENCE
015470,000125:
015471,000126: # IN ORDER TO PERFORM A KALCMANU SUPERVISED MANEUVER, THE COMMANDED GIMBAL ANGLES MUST BE PRECOMPUTED AND
015472,000127: # STORED IN LOCATIONS CPHI, CTHETA, CPSI. THE USER:S PROGRAM MUST THEN CLEAR STATE SWITCH NO 33 TO ALLOW THE
015473,000128: # ATTITUDE MANEUVER ROUTINE TO PERFORM ANY FINAL P-AXIS YAW INCURRED BY AVOIDING GIMBAL LOCK. THE MANEUVER IS
015474,000129: # THEN INITIATED BY ESTABLISHING THE FOLLOWING EXECUTIVE JOB
015475,000130:
015476,000131: # *
015477,000132: # CAF PRIO XX
015478,000133: # --
015479,000134: # INHINT
015480,000135: # TC FINDVAC
015481,000136: # 2CADR KALCMAN3
015482,000137: # RELINT
015483,000138:
015484,000139: # THE USER:S PROGRAM MAY EITHER CONTINUE OR WAIT FOR THE TERMINATION OF THE MANEUVER. IF THE USER WISHES TO
015485,000140: # WAIT, HE MAY PUT HIS JOB TO SLEEP WITH THE FOLLOWING INSTRUCTIONS
015486,000141:
015487,000142: # L TC BANKCALL
015488,000143: # L+1 CADR ATTSTALL
015489,000144: # L+2 (BAD RETURN)
015490,000145: # L+3 (GOOD RETURN)
015491,000146:
015492,000147: # UPON COMPLETION OF THE MANEUVER, THE PROGRAM WILL BE AWAKENED AT L+3 IF THE MANEUVER WAS COMPLETED
015493,000148: # SUCCESSFULLY, OR AT L+2 IF THE MANEUVER WAS ABORTED. THIS ABORT WOULD OCCUR IF THE INITIAL OR FINAL ATTITUDE
015494,000149: # WAS IN GIMBAL LOCK.
015495,000150:
015496,000151: # ***NOTA BENE*** IT IS ASSUMED THAT THE DESIRED MANEUVERING RATE (0.5, 2, 5, 10, DEG/SEC) HAS BEEN SELECTED BY
015497,000152: # KEYBOARD ENTRY PRIOR TO THE EXECUTION OF KALCMANU.
015498,000153: # IT IS ALSO ASSUMED THAT THE AUTOPILOT IS IN THE AUTO MODE. IF THE MODE SWITCH IS CHANGED DURING THE
015499,000154: # MANEUVER, KALCMANU WILL TERMINATE VIA GOODEND WITHIN 1 SECOND SO THAT R60 MAY REQUEST A TRIM OF THE S/C ATTITUDE
015500,000155: # THIS IS THE ONLY MEANS FOR MANUALLY TERMINATING A KALCMANU SUPERVISED MANEUVER.
015501,000156: # SUBROUTINES
015502,000157:
015503,000158: # KALCMANU USES A NUMBER OF INTERPRETIVE SUBROUTINES WHICH MAY BE OF GENERAL INTEREST. SINCE THESE ROUTINES
015504,000159: # WERE PROGRAMMED EXCLUSIVELY FOR KALCMANU, THEY ARE NOT, AS YET, GENERALLY AVAILABLE FOR USE BY OTHER PROGRAMS.
015505,000160:
015506,000161: # MXM3
015507,000162: # ----
015508,000163:
015509,000164: # THIS SUBROUTINE MULTIPLIES TWO 3X3 MATRICES AND LEAVES THE RESULT IN THE FIRST 18 LOCATIONS OF THE PUSH
015510,000165: # DOWN LIST, I.E.,
015511,000166:
015512,000167: # (M M M )
015513,000168: # ( 0 1 2)
015514,000169: # * ( ) * *
015515,000170: # M = (M M M ) = M1 X M2
015516,000171: # ( 3 4 5)
015517,000172: # ( )
015518,000173: # (M M M )
015519,000174:
|
|
Page 357 |
015521,000176: # ( 6 7 8)
015522,000177:
015523,000178: # *
015524,000179: # INDEX REGISTER X1 MUST BE LOADED WITH THE COMPLEMENT OF THE STARTING ADDRESS FOR M1, AND X2 MUST BE
015525,000180: # *
015526,000181: # LOADED WITH THE COMPLEMENT OF THE STARTING ADDRESS FOR M2. THE ROUTINE USES THE FIRST 20 LOCATIONS OF THE PUSH
015527,000182: # DOWN LIST. THE FIRST ELEMENT OF THE MATRIX APPEARS IN PDO. PUSH UP FOR M .
015528,000183: # 8
015529,000184:
015530,000185: # TRANSPOS
015531,000186: # --------
015532,000187:
015533,000188: # THIS ROUTINE TRANSPOSES A 3X3 MATRIX AND LEAVES THE RESULT IN THE PUSH DOWN LIST, I.E.,
015534,000189:
015535,000190: # * * T
015536,000191: # M = M1
015537,000192:
015538,000193: # INDEX REGISTER X1 MUST CONTAIN THE COMPLEMENT OF THE STARTING ADDRESS FOR M1. PUSH UP FOR THE FIRST AND SUB-
015539,000194: # *
015540,000195: # SEQUENT COMPONENTS OF M. THIS SUBROUTINE ALSO USES THE FIRST 20 LOCATIONS OF THE PUSH DOWN LIST.
015541,000196:
015542,000197: # CDU TO DCM
015543,000198: # ----------
015544,000199:
015545,000200: # THIS SUBROUTINE CONVERTS THREE CDU ANGLES IN T(MPAC) TO A DIRECTION COSINE MATRIX (SCALED BY 2) RELATING
015546,000201: # THE CORRESPONDING S/C ORIENTATIONS TO THE STABLE MEMBER FRAME. THE FORMULAS FOR THIS CONVERSION ARE
015547,000202:
015548,000203: # M = COSY COSZ
015549,000204: # 0
015550,000205:
015551,000206: # M = -COSY SINZ COSX + SINY SINX
015552,000207: # 1
015553,000208:
015554,000209: # M = COSY SINZ SINX + SINY COSX
015555,000210: # 2
015556,000211:
015557,000212: # M = SINZ
015558,000213: # 3
015559,000214:
015560,000215: # M = COSZ COSX
015561,000216: # 4
015562,000217:
015563,000218: # M = -COSZ SINX
015564,000219: # 5
015565,000220:
015566,000221: # M = -SINY COSZ
015567,000222: # 6
015568,000223:
015569,000224: # M = SINY SINZ COSX + COSY SINX
015570,000225: # 7
015571,000226:
|
|
Page 358 |
015573,000228: # M = -SINY SINZ SINX + COSY COSX
015574,000229: # 8
015575,000230:
015576,000231: # WHERE X = OUTER GIMBAL ANGLE
015577,000232: # Y = INNER GIMBAL ANGLE
015578,000233: # Z = MIDDLE GIMBAL ANGLE
015579,000234:
015580,000235: # THE INTERPRETATION OF THIS MATRIX IS AS FOLLOWS
015581,000236:
015582,000237: # IF A , A , A REPRESENT THE COMPONENTS OF A VECTOR IN S/C AXES THEN THE COMPONENTS OF THE SAME VECTOR IN
015583,000238: # X Y Z
015584,000239: # STABLE MEMBER AXES (B , B , B ) ARE
015585,000240: # X Y Z
015586,000241:
015587,000242: # (B ) (A )
015588,000243: # ( X) ( X)
015589,000244: # ( ) ( )
015590,000245: # ( ) * ( )
015591,000246: # (B ) = M (A )
015592,000247: # ( Y) ( Y)
015593,000248: # ( ) ( )
015594,000249: # (B ) (A )
015595,000250: # (Z ) ( Z)
015596,000251:
015597,000252: # THE SUBROUTINE WILL STORE THIS MATRIX IN SEQUENTIAL LOCATIONS OF ERASABLE MEMORY AS SPECIFIED BY THE CALLING
015598,000253: # *
015599,000254: # PROGRAM. TO DO THIS THE CALLING PROGRAM MUST FIRST LOAD X2 WITH THE COMPLEMENT OF THE STARTING ADDRESS FOR M.
015600,000255:
015601,000256: # INTERNALLY, THE ROUTINE USES THE FIRST 16 LOCATIONS OF THE PUSH DOWN LIST, ALSO STEP REGISTER S1 AND INDEX
015602,000257: # REGISTER X2.
015603,000258:
015604,000259:
015605,000260: # DCM TO CDU
015606,000261: # ----------
015607,000262: # *
015608,000263: # THIS ROUTINE EXTRACTS THE CDU ANGLES FROM A DIRECTION COSINE MATRIX (M SCALED BY 2) RELATING S/C AXIS TO
015609,000264: # *
015610,000265: # STABLE MEMBER AXES. X1 MUST CONTAIN THE COMPLEMENT OF THE STARTING ADDRESS FOR M. THE SUBROUTINE LEAVES THE
015611,000266: # CORRESPONDING GIMBAL ANGLES IN V(MPAC) AS DOUBLE PRECISION 1:S COMPLEMENT ANGLES SCALED BY 2PI. THE FORMULAS
015612,000267: # FOR THIS CONVERSION ARE
015613,000268:
015614,000269: # Z = ARCSIN (M )
015615,000270: # 3
015616,000271:
015617,000272: # Y = ARCSIN (-M /COSZ)
015618,000273: # 6
015619,000274:
015620,000275: # IF M IS NEGATIVE, Y IS REPLACED BY PI SGN Y - Y
015621,000276: # 0
015622,000277:
|
|
Page 359 |
015624,000279: # X = ARCSIN (-M /COSZ)
015625,000280: # 5
015626,000281:
015627,000282: # IF M IS NEGATIVE X IS REPLACED BY PI SGN X - X
015628,000283: # 4
015629,000284:
015630,000285: # THIS ROUTINE DOES NOT SET THE PUSH DOWN POINTER, BUT USES THE NEXT 8 LOCATIONS OF THE PUSH DOWN LIST AND
015631,000286: # RETURNS THE POINTER TO ITS ORIGINAL SETTING. THIS PROCEDURE ALLOWS THE CALLER TO STORE THE MATRIX AT THE TOP OF
015632,000287: # THE PUSH DOWN LIST.
015633,000288:
015634,000289:
015635,000290: # DELCOMP
015636,000291: # -------
015637,000292:
015638,000293: # *
015639,000294: # THIS ROUTINE COMPUTES THE DIRECTION COSINE MATRIX (DEL) RELATING ON
015640,000295: # -
015641,000296: # IS ROTATED WITH RESPECT TO THE FIRST BY AN ANGLE, A, ABOUT A UNIT VECTOR, U. THE FORMULA FOR THIS MATRIX IS
015642,000297:
015643,000298: # * * --T *
015644,000299: # DEL = I COSA + UU (1-COSA) + V SINA
015645,000300: # X
015646,000301:
015647,000302: # WHERE * (1 0 0)
015648,000303: # I = (0 1 0)
015649,000304: # (0 0 1)
015650,000305:
015651,000306:
015652,000307: # 2
015653,000308: # (U U U U U )
015654,000309: # ( X X Y X Z)
015655,000310: # ( )
015656,000311: # --T ( 2 )
015657,000312: # U U = (U U U U U )
015658,000313: # ( Y X Y Y Z )
015659,000314: # ( )
015660,000315: # ( 2 )
015661,000316: # (U U U U U )
015662,000317: # ( Z X Z Y Z )
015663,000318:
015664,000319:
015665,000320: # (0 -U U )
015666,000321: # ( Z Y )
015667,000322: # * ( )
015668,000323: # V = (U 0 -U )
015669,000324: # X ( Z X)
015670,000325: # ( )
015671,000326: # (-U U 0 )
015672,000327: # ( Y X )
015673,000328:
|
|
Page 360 |
015675,000330: # -
015676,000331: # U = UNIT ROTATION VECTOR RESOLVED INTO S/C AXES
015677,000332: # A = ROTATION ANGLE
015678,000333:
015679,000334: # *
015680,000335: # THE INTERPRETATION OF DEL IS AS FOLLOWS
015681,000336:
015682,000337: # IF A , A , A REPRESENT THE COMPONENT OF A VECTOR INTHE ROTATED FRAME, THEN THE COMPONENTS OF THE SAME
015683,000338: # X Y Z
015684,000339: # VECTOR IN THE ORIGINAL S/C AXES (B , B , B ) ARE
015685,000340: # X Y Z
015686,000341:
015687,000342: # (B ) (A )
015688,000343: # ( X) ( X)
015689,000344: # ( ) * ( )
015690,000345: # (B ) = DEL (A )
015691,000346: # ( Y) ( Y)
015692,000347: # ( ) ( )
015693,000348: # (B ) (A )
015694,000349: # ( Z) ( Z)
015695,000350:
015696,000351: # THE ROUTINE WILL STORE THIS MATRIX (SCALED UNITY) IN SEQUENTIAL LOCATIONS OF ERASABLE MEMORY BEGINNING WITH
015697,000352: # -
015698,000353: # THE LOCATION CALLED DEL. IN ORDER TO USE THE ROUTINE, THE CALLING PROGRAM MUST FIRST STORE U (A HALF UNIT
015699,000354: # DOUBLE PRECISION VECTOR) IN THE SET OF ERASABLE LOCATIONS BEGINNING WITH THE ADDRESS CALLED COF. THE ANGLE, A,
015700,000355: # MUST THEN BE LOADED INTO D(MPAC).
015701,000356:
015702,000357: # INTERNALLY, THE PROGRAM ALSO USES THE FIRST 10 LOCATIONS OF THE PUSH DOWN LIST.
015703,000358:
015704,000359:
015705,000360: # READCDUK
015706,000361: # --------
015707,000362:
015708,000363: # THIS BASIC LANGUAGE SUBROUTINE LOADS T(MPAC) WITH THE THREE CDU ANGLES.
015709,000364:
015710,000365:
015711,000366: # SIGNMPAC
015712,000367: # --------
015713,000368:
015714,000369: # THIS IS A BASIC LANGUAGE SUBROUTINE WHICH LIMITS THE MAGNITUDE OF D(MPAC) TO + OR - DPOSMAX ON OVERFLOW.
015715,000370:
015716,000371:
015717,000372: # PROGRAM STORAGE ALLOCATION
015718,000373:
015719,000374: # 1) FIXED MEMORY 1059 WORDS
015720,000375: # 2) ERASABLE MEMORY 98
015721,000376: # 3) STATE SWITCHES 3
015722,000377:
|
|
Page 361 |
015724,000379: # 4) FLAGS 1
015725,000380:
015726,000381:
015727,000382: # JOB PRIORITIES
015728,000383:
015729,000384: # 1) KALCMANU TBD
015730,000385: # 2) ONE SECOND UPDATE TBD
015731,000386:
015732,000387:
015733,000388: # SUMMARY OF STATE SWITCHES AND FLAGWORDS USED BY KALCMANU.
015734,000389:
015735,000390:
015736,000391: # STATE FLAGWRD 2 SETTING MEANING
015737,000392: # SWITCH NO. BIT NO.
015738,000393:
015739,000394: # *
015740,000395: # 31 14 0 MANEUVER WENT THROUGH GIMBAL LOCK
015741,000396: # 1 MANEUVER DID NOT GO THROUGH GIMBAL LOCK
015742,000397:
015743,000398: # *
015744,000399: # 32 13 0 CONTINUE UPDATE PROCESS
015745,000400: # 1 START UPDATE PROCESS
015746,000401:
015747,000402: # 33 12 0 PERFORM FINAL P-AXIS YAW IF REQUIRED
015748,000403: # 1 IGNORE ANY FINAL P-AXIS YAW
015749,000404:
015750,000405: # 34 11 0 SIGNAL END OF KALCMANU
015751,000406: # 1 KALCMANU IN PROCESS USER MUST SET SWITCH BEFORE INITIATING
015752,000407:
015753,000408:
015754,000409: # * INTERNAL TO KALCMANU
015755,000410:
015756,000411:
015757,000412: # SUGGESTIONS FOR PROGRAM INTEGRATION
015758,000413:
015759,000414: # THE FOLLOWING VARIABLES SHOULD BE ASSIGNED TO UNSWITCH ERASABLE
015760,000415:
015761,000416: # CPHI
015762,000417: # CTHETA
015763,000418: # CPSI
015764,000419: # POINTVSM +5
015765,000420: # SCAXIS +5
015766,000421: # DELDCDU
015767,000422: # DELDCDU1
015768,000423: # DELDCDU2
015769,000424: # RATEINDX
015770,000425:
015771,000426: # THE FOLLOWING SUBROUTINES MAY BE PUT IN A DIFFERENT BANK
015772,000427:
015773,000428: # MXM3
015774,000429:
|
|
Page 362 |
015776,000431: # TRANSPOS
015777,000432: # SIGNMPAC
015778,000433: # READCDUK
015779,000434: # CDUTODCM
015780,000435:
|
|
Page 363 |
015782,000437: 15,2162 BANK 15
015783,000438: 22,2000 SETLOC KALCMON1
015784,000439: 22,2000 BANK
015785,000440:
015786,000441: 22,2004 E6,1675 EBANK= BCDU
015787,000442:
015788,000443: # THE THREE DESIRED CDU ANGLES MUST BE STORED AS SINGLE PRECISION TWOS COMPLEMENT ANGLES IN THE THREE SUCCESSIVE
015789,000444: # LOCATIONS, CPHI, CTHETA, CPSI.
015790,000445:
015791,000446: 22,2004 COUNT* $$/KALC
015792,000447: 22,2004 06060 KALCMAN3 TC INTPRET # PICK UP THE CURRENT CDU ANGLES AND
015793,000448: 22,2005 77634 RTB # COMPUTE THE MATRIX FROM INITIAL S/C
015794,000449: 22,2006 44403 READCDUK # AXES TO FINAL S/C AXES
015795,000450: 22,2007 03276 STORE BCDU # STORE INITIAL S/C ANGLES
015796,000451: 22,2010 51535 SLOAD ABS # CHECK THE MAGNITUDE OF THE DESIRED
015797,000452: 22,2011 00324 CPSI # MIDDLE GIMBAL ANGLE
015798,000453: 22,2012 51025 DSU BPL
015799,000454: 22,2013 04403 LOCKANGL # IF GREATER THAN 70 DEG ABORT MANEUVER
015800,000455: 22,2014 44724 TOOBADF
015801,000456: 22,2015 72364 AXC,2 TLOAD
015802,000457: 22,2016 03245 MIS
015803,000458: 22,2017 03276 BCDU
015804,000459: 22,2020 77624 CALL # COMPUTE THE TRANSFORMATION FROM INITIAL
015805,000460: 22,2021 44410 CDUTODCM # S/C AXES TO STABLE MEMBER AXES
015806,000461: 22,2022 72364 AXC,2 TLOAD
015807,000462: 22,2023 02221 MFS # PREPARE TO CALCULATE ARRAY MFS
015808,000463: 22,2024 00322 CPHI
015809,000464: 22,2025 77624 CALL
015810,000465: 22,2026 44410 CDUTODCM
015811,000466: 22,2027 45160 SECAD AXC,1 CALL # MIS AND MFS ARRAYS CALCULATED $2
015812,000467: 22,2030 03245 MIS
015813,000468: 22,2031 44326 TRANSPOS
015814,000469: 22,2032 45575 VLOAD STADR
015815,000470: 22,2033 50460 STOVL TMIS +12D
015816,000471: 22,2034 77626 STADR
015817,000472: 22,2035 50466 STOVL TMIS +6
015818,000473: 22,2036 77626 STADR
015819,000474: 22,2037 74474 STORE TMIS # TMIS = TRANSPOSE(MIS) SCALED BY 2
015820,000475: 22,2040 75160 AXC,1 AXC,2
015821,000476: 22,2041 03302 TMIS
015822,000477: 22,2042 02221 MFS
015823,000478: 22,2043 77624 CALL
015824,000479: 22,2044 44312 MXM3
015825,000480: 22,2045 45575 VLOAD STADR
015826,000481: 22,2046 51541 STOVL MFI +12D
015827,000482: 22,2047 77626 STADR
015828,000483: 22,2050 51547 STOVL MFI +6
015829,000484: 22,2051 77626 STADR
015830,000485: 22,2052 75555 STORE MFI # MFI = TMIS MFS (SCALED BY 4)
015831,000486: 22,2053 45001 SETPD CALL # TRANSPOSE MFI IN PD LIST
015832,000487:
|
|
Page 364 |
015834,000489: 22,2054 00023 18D
015835,000490: 22,2055 44335 TRNSPSPD
015836,000491: 22,2056 45575 VLOAD STADR
015837,000492: 22,2057 50460 STOVL TMFI +12D
015838,000493: 22,2060 77626 STADR
015839,000494: 22,2061 50466 STOVL TMFI +6
015840,000495: 22,2062 77626 STADR
015841,000496: 22,2063 74474 STORE TMFI # TMFI = TRANSPOSE (MFI) SCALED BY 4
015842,000497:
015843,000498: # CALCULATE COFSKEW AND MFISYM
015844,000499:
015845,000500: 22,2064 45345 DLOAD DSU
015846,000501: 22,2065 03305 TMFI +2
015847,000502: 22,2066 02224 MFI +2
015848,000503: 22,2067 45325 PDDL DSU # CALCULATE COF SCALED BY 2/SIN(AM)
015849,000504: 22,2070 02226 MFI +4
015850,000505: 22,2071 03307 TMFI +4
015851,000506: 22,2072 45325 PDDL DSU
015852,000507: 22,2073 03315 TMFI +10D
015853,000508: 22,2074 02234 MFI +10D
015854,000509: 22,2075 77666 VDEF
015855,000510: 22,2076 03325 STORE COFSKEW # EQUALS MFISKEW
015856,000511:
015857,000512: # CALCULATE AM AND PROCEED ACCORDING TO ITS MAGNITUDE
015858,000513:
015859,000514: 22,2077 43345 DLOAD DAD
015860,000515: 22,2100 02222 MFI
015861,000516: 22,2101 02242 MFI +16D
015862,000517: 22,2102 43225 DSU DAD
015863,000518: 22,2103 06475 DP1/4TH
015864,000519: 22,2104 02232 MFI +8D
015865,000520: 22,2105 03333 STORE CAM # CAM = (MFI0+MFI4+MFI8-1)/2 HALF SCALE
015866,000521: 22,2106 77726 ARCCOS
015867,000522: 22,2107 03335 STORE AM # AM=ARCCOS(CAM) (AM SCALED BY 2)
015868,000523: 22,2110 51025 DSU BPL
015869,000524: 22,2111 04363 MINANG
015870,000525: 22,2112 44117 CHECKMAX
015871,000526: 22,2113 77751 TLOAD # MANEUVER LESS THAN .25 DEGREES
015872,000527: 22,2114 00322 CPHI # GO DIRECTLY INTO ATTITUDE HOLD
015873,000528: 22,2115 37235 STCALL CDUXD # ABOUT COMMANDED ANGLES
015874,000529: 22,2116 44742 TOOBADI # STOP RATE AND EXIT
015875,000530:
015876,000531: 22,2117 45345 CHECKMAX DLOAD DSU
015877,000532: 22,2120 03335 AM
015878,000533: 22,2121 04365 MAXANG
015879,000534: 22,2122 77244 BPL VLOAD
015880,000535: 22,2123 44131 ALTCALC # UNIT
015881,000536: 22,2124 03325 COFSKEW # COFSKEW
015882,000537: 22,2125 77656 UNIT
015883,000538: 22,2126 03270 STORE COF # COF IS THE MANEUVER AXIS
015884,000539:
|
|
Page 365 |
015886,000541: 22,2127 77650 GOTO # SEE IF MANEUVER GOES THRU GIMBAL LOCK
015887,000542: 22,2130 44744 LOCSKIRT
015888,000543: 22,2131 53375 ALTCALC VLOAD VAD # IF AM GREATER THAN 170 DEGREES
015889,000544: 22,2132 02222 MFI
015890,000545: 22,2133 03303 TMFI
015891,000546: 22,2134 77762 VSR1
015892,000547: 22,2135 27303 STOVL MFISYM
015893,000548: 22,2136 02230 MFI +6
015894,000549: 22,2137 74455 VAD VSR1
015895,000550: 22,2140 03311 TMFI +6
015896,000551: 22,2141 27311 STOVL MFISYM +6
015897,000552: 22,2142 02236 MFI +12D
015898,000553: 22,2143 74455 VAD VSR1
015899,000554: 22,2144 03317 TMFI +12D
015900,000555: 22,2145 03317 STORE MFISYM +12D # MFISYM=(MFI+TMFI)/2 SCALED BY 4
015901,000556:
015902,000557:
015903,000558: # CALCULATE COF
015904,000559:
015905,000560:
015906,000561: 22,2146 70545 DLOAD SR1
015907,000562: 22,2147 03333 CAM
015908,000563: 22,2150 45325 PDDL DSU # PDO CAM $4
015909,000564: 22,2151 06503 DPHALF
015910,000565: 22,2152 03333 CAM
015911,000566: 22,2153 65204 BOVB PDDL # PD2 1 - CAM $2
015912,000567: 22,2154 31765 SIGNMPAC
015913,000568: 22,2155 03323 MFISYM +16D
015914,000569: 22,2156 56225 DSU DDV
015915,000570: 22,2157 00001 0
015916,000571: 22,2160 00003 2
015917,000572: 22,2161 65366 SQRT PDDL # COFZ = SQRT(MFISYM8-CAM)/1-CAM)
015918,000573: 22,2162 03313 MFISYM +8D # $ ROOT 2
015919,000574: 22,2163 56225 DSU DDV
015920,000575: 22,2164 00001 0
015921,000576: 22,2165 00003 2
015922,000577: 22,2166 65366 SQRT PDDL # COFY = SQRT(MFISYM4-CAM)/(1-CAM) $ROOT2
015923,000578: 22,2167 03303 MFISYM
015924,000579: 22,2170 56225 DSU DDV
015925,000580: 22,2171 00001 0
015926,000581: 22,2172 00003 2
015927,000582: 22,2173 55566 SQRT VDEF # COFX = SQRT(MFISYM-CAM)/(1-CAM) $ROOT 2
015928,000583: 22,2174 77656 UNIT
015929,000584: 22,2175 03270 STORE COF
015930,000585:
015931,000586: # DETERMINE LARGEST COF AND ADJUST ACCORDINGLY
015932,000587:
015933,000588: 22,2176 45345 COFMAXGO DLOAD DSU
015934,000589: 22,2177 03270 COF
015935,000590: 22,2200 03272 COF +2
015936,000591: 22,2201 71240 BMN DLOAD # COFY G COFX
015937,000592:
|
|
Page 366 |
015939,000594: 22,2202 44211 COMP12
015940,000595: 22,2203 03270 COF
015941,000596: 22,2204 50025 DSU BMN
015942,000597: 22,2205 03274 COF +4
015943,000598: 22,2206 44266 METHOD3 # COFZ G COFX OR COFY
015944,000599: 22,2207 77650 GOTO
015945,000600: 22,2210 44242 METHOD1 # COFX G COFY OR COFZ
015946,000601: 22,2211 45345 COMP12 DLOAD DSU
015947,000602: 22,2212 03272 COF +2
015948,000603: 22,2213 03274 COF +4
015949,000604: 22,2214 77640 BMN
015950,000605: 22,2215 44266 METHOD3 # COFZ G COFY OR COFX
015951,000606:
015952,000607: 22,2216 51145 METHOD2 DLOAD BPL # COFY MAX
015953,000608: 22,2217 03327 COFSKEW +2 # UY
015954,000609: 22,2220 44224 U2POS
015955,000610: 22,2221 57575 VLOAD VCOMP
015956,000611: 22,2222 03270 COF
015957,000612: 22,2223 03270 STORE COF
015958,000613: 22,2224 51145 U2POS DLOAD BPL
015959,000614: 22,2225 03305 MFISYM +2 # UX UY
015960,000615: 22,2226 44232 OKU21
015961,000616: 22,2227 57545 DLOAD DCOMP # SIGN OF UX OPPOSITE TO UY
015962,000617: 22,2230 03270 COF
015963,000618: 22,2231 03270 STORE COF
015964,000619: 22,2232 51145 OKU21 DLOAD BPL
015965,000620: 22,2233 03315 MFISYM +10D # UY UZ
015966,000621: 22,2234 44744 LOCSKIRT
015967,000622: 22,2235 57545 DLOAD DCOMP # SIGN OF UZ OPPOSITE TO UY
015968,000623: 22,2236 03274 COF +4
015969,000624: 22,2237 03274 STORE COF +4
015970,000625: 22,2240 77650 GOTO
015971,000626: 22,2241 44744 LOCSKIRT
015972,000627: 22,2242 51145 METHOD1 DLOAD BPL # COFX MAX
015973,000628: 22,2243 03325 COFSKEW # UX
015974,000629: 22,2244 44250 U1POS
015975,000630: 22,2245 57575 VLOAD VCOMP
015976,000631: 22,2246 03270 COF
015977,000632: 22,2247 03270 STORE COF
015978,000633: 22,2250 51145 U1POS DLOAD BPL
015979,000634: 22,2251 03305 MFISYM +2 # UX UY
015980,000635: 22,2252 44256 OKU12
015981,000636: 22,2253 57545 DLOAD DCOMP
015982,000637: 22,2254 03272 COF +2 # SIGN OF UY OPPOSITE TO UX
015983,000638: 22,2255 03272 STORE COF +2
015984,000639: 22,2256 51145 OKU12 DLOAD BPL
015985,000640: 22,2257 03307 MFISYM +4 # UX UZ
015986,000641: 22,2260 44744 LOCSKIRT
015987,000642: 22,2261 57545 DLOAD DCOMP # SIGN OF UZ OPPOSITE TO UY
015988,000643: 22,2262 03274 COF +4
015989,000644:
|
|
Page 367 |
015991,000646: 22,2263 03274 STORE COF +4
015992,000647: 22,2264 77650 GOTO
015993,000648: 22,2265 44744 LOCSKIRT
015994,000649: 22,2266 51145 METHOD3 DLOAD BPL # COFZ MAX
015995,000650: 22,2267 03331 COFSKEW +4 # UZ
015996,000651: 22,2270 44274 U3POS
015997,000652: 22,2271 57575 VLOAD VCOMP
015998,000653: 22,2272 03270 COF
015999,000654: 22,2273 03270 STORE COF
016000,000655: 22,2274 51145 U3POS DLOAD BPL
016001,000656: 22,2275 03307 MFISYM +4 # UX UZ
016002,000657: 22,2276 44302 OKU31
016003,000658: 22,2277 57545 DLOAD DCOMP
016004,000659: 22,2300 03270 COF # SIGN OF UX OPPOSITE TO UZ
016005,000660: 22,2301 03270 STORE COF
016006,000661: 22,2302 51145 OKU31 DLOAD BPL
016007,000662: 22,2303 03315 MFISYM +10D # UY UZ
016008,000663: 22,2304 44744 LOCSKIRT
016009,000664: 22,2305 57545 DLOAD DCOMP
016010,000665: 22,2306 03272 COF +2 # SIGN OF UY OPPOSITE TO UZ
016011,000666: 22,2307 03272 STORE COF +2
016012,000667: 22,2310 77650 GOTO
016013,000668: 22,2311 44744 LOCSKIRT
|
|
Page 368 |
016015,000670: # MATRIX OPERATIONS
016016,000671:
016017,000672: 13,2207 BANK 13
016018,000673: 22,2000 SETLOC KALCMON2
016019,000674: 22,2000 BANK
016020,000675:
016021,000676: 22,2312 E6,1675 EBANK= BCDU
016022,000677:
016023,000678: 22,2312 76601 MXM3 SETPD VLOAD* # MXM3 MULTIPLIES 2 3X3 MATRICES
016024,000679: 22,2313 00001 0 # AND LEAVES RESULT IN PD LIST
016025,000680: 22,2314 00001 0,1 # AND MPAC
016026,000681: 22,2315 62703 VXM* PDVL*
016027,000682: 22,2316 77776 0,2
016028,000683: 22,2317 00007 6,1
016029,000684: 22,2320 62703 VXM* PDVL*
016030,000685: 22,2321 77776 0,2
016031,000686: 22,2322 00015 12D,1
016032,000687: 22,2323 41503 VXM* PUSH
016033,000688: 22,2324 77776 0,2
016034,000689: 22,2325 77616 RVQ
016035,000690:
016036,000691:
016037,000692: # RETURN WITH M1XM2 IN PD LIST
016038,000693:
016039,000694: 22,2326 76601 TRANSPOS SETPD VLOAD* # TRANSPOS TRANSPOSES A 3X3 MATRIX
016040,000695: 22,2327 00001 0 # AND LEAVES RESULT IN PD LIST
016041,000696: 22,2330 00001 0,1 # MATRIX ADDRESS IN XR1
016042,000697: 22,2331 62713 PDVL* PDVL*
016043,000698: 22,2332 00007 6,1
016044,000699: 22,2333 00015 12D,1
016045,000700: 22,2334 77606 PUSH # MATRIX IN PD
016046,000701: 22,2335 77776 TRNSPSPD EXIT # ENTER WITH MATRIX AT 0 IN PD LIST
016047,000702: 22,2336 50120 INDEX FIXLOC
016048,000703: 22,2337 52013 DXCH 12
016049,000704: 22,2340 50120 INDEX FIXLOC
016050,000705: 22,2341 52017 DXCH 16
016051,000706: 22,2342 50120 INDEX FIXLOC
016052,000707: 22,2343 52013 DXCH 12
016053,000708: 22,2344 50120 INDEX FIXLOC
016054,000709: 22,2345 52015 DXCH 14
016055,000710: 22,2346 50120 INDEX FIXLOC
016056,000711: 22,2347 52005 DXCH 4
016057,000712: 22,2350 50120 INDEX FIXLOC
016058,000713: 22,2351 52015 DXCH 14
016059,000714: 22,2352 50120 INDEX FIXLOC
016060,000715: 22,2353 52003 DXCH 2
016061,000716: 22,2354 50120 INDEX FIXLOC
016062,000717: 22,2355 52007 DXCH 6
016063,000718: 22,2356 50120 INDEX FIXLOC
016064,000719: 22,2357 52003 DXCH 2
016065,000720:
|
|
Page 369 |
016067,000722: 22,2360 06060 TC INTPRET
016068,000723: 22,2361 77616 RVQ
016069,000724:
016070,000725: 15,2162 BANK 15
016071,000726: 22,2000 SETLOC KALCMON1
016072,000727: 22,2000 BANK
016073,000728:
016074,000729: 22,2362 E6,1675 EBANK= BCDU
016075,000730:
016076,000731: 22,2362 00013 13563 MINANG 2DEC 0.00069375
016077,000732: 22,2364 17070 34343 MAXANG 2DEC 0.472222222
016078,000733: # GIMBAL LOCK CONSTANTS
016079,000734:
016080,000735: # D = MGA CORRESPONDING TO GIMBAL LOCK = 60 DEGREES
016081,000736: # NGL = BUFFER ANGLE (TO AVOID DIVISIONS BY ZERO) = 2 DEGREES
016082,000737:
016083,000738: 22,2366 15666 20443 SD 2DEC .433015 # = SIN(D) $2
016084,000739: 22,2370 33555 01106 K3S1 2DEC .86603 # = SIN(D) $1
016085,000740: 22,2372 67777 77777 K4 2DEC -.25 # = -COS(D) $2
016086,000741: 22,2374 04000 00000 K4SQ 2DEC .125 # = COS(D)COS(D) $2
016087,000742: 22,2376 00216 36323 SNGLCD 2DEC .008725 # = SIN(NGL)COS(D) $2
016088,000743: 22,2400 17773 00057 CNGL 2DEC .499695 # COS(NGL) $2
016089,000744: 22,2402 14344 LOCKANGL DEC .388889 # = 70 DEGREES
016090,000745: # INTERPRETIVE SUBROUTINE TO READ THE CDU ANGLES
016091,000746:
016092,000747: 22,2403 30034 READCDUK CA CDUZ # LOAD T(MPAC) WITH CDU ANGLES
016093,000748: 22,2404 54156 TS MPAC +2
016094,000749: 22,2405 00006 EXTEND
016095,000750: 22,2406 30033 DCA CDUX # AND CHANGE MODE TO TRIPLE PRECISION
016096,000751: 22,2407 16517 TCF TLOAD +6
016097,000752:
016098,000753:
016099,000754: 22,2410 66370 CDUTODCM AXT,1 SSP
016100,000755: 22,2411 00003 OCT 3
016101,000756: 22,2412 00051 S1
016102,000757: 22,2413 00001 OCT 1 # SET XR1, S1, AND PD FOR LOOP
016103,000758: 22,2414 00010 STORE 7
016104,000759: 22,2415 77601 SETPD
016105,000760: 22,2416 00001 0
016106,000761: 22,2417 47133 LOOPSIN SLOAD* RTB
016107,000762: 22,2420 00013 10D,1
016108,000763: 22,2421 21606 CDULOGIC
016109,000764:
|
|
Page 370 |
016111,000766: 22,2422 00013 STORE 10D # LOAD PD WITH 0 SIN(PHI)
016112,000767: 22,2423 65356 SIN PDDL # 2 COS(PHI)
016113,000768: 22,2424 00013 10D # 4 SIN(THETA)
016114,000769: 22,2425 41546 COS PUSH # 6 COS(THETA)
016115,000770: 22,2426 71300 TIX,1 DLOAD # 8 SIN(PSI)
016116,000771: 22,2427 44417 LOOPSIN # 10 COS(PSI)
016117,000772: 22,2430 00007 6
016118,000773: 22,2431 72405 DMP SL1
016119,000774: 22,2432 00013 10D
016120,000775: 22,2433 10001 STORE 0,2 # C0=COS(THETA)COS(PSI)
016121,000776: 22,2434 41345 DLOAD DMP
016122,000777: 22,2435 00005 4
016123,000778: 22,2436 00001 0
016124,000779: 22,2437 41325 PDDL DMP # (PD6 SIN(THETA)SIN(PHI))
016125,000780: 22,2440 00007 6
016126,000781: 22,2441 00011 8D
016127,000782: 22,2442 72405 DMP SL1
016128,000783: 22,2443 00003 2
016129,000784: 22,2444 72421 BDSU SL1
016130,000785: 22,2445 00015 12D
016131,000786: 22,2446 10003 STORE 2,2 # C1=-COS(THETA)SIN(PSI)COS(PHI)
016132,000787: 22,2447 41345 DLOAD DMP
016133,000788: 22,2450 00003 2
016134,000789: 22,2451 00005 4
016135,000790: 22,2452 41325 PDDL DMP # (PD7 COS(PHI)SIN(THETA)) SCALED 4
016136,000791: 22,2453 00007 6
016137,000792: 22,2454 00011 8D
016138,000793: 22,2455 72405 DMP SL1
016139,000794: 22,2456 00001 0
016140,000795: 22,2457 72415 DAD SL1
016141,000796: 22,2460 00017 14D
016142,000797: 22,2461 10005 STORE 4,2 # C2=COS(THETA)SIN(PSI)SIN(PHI)
016143,000798: 22,2462 77745 DLOAD
016144,000799: 22,2463 00011 8D
016145,000800: 22,2464 10007 STORE 6,2 # C3=SIN(PSI)
016146,000801: 22,2465 77745 DLOAD
016147,000802: 22,2466 00013 10D
016148,000803: 22,2467 72405 DMP SL1
016149,000804: 22,2470 00003 2
016150,000805: 22,2471 10011 STORE 8D,2 # C4=COS(PSI)COS(PHI)
016151,000806: 22,2472 41345 DLOAD DMP
016152,000807: 22,2473 00013 10D
016153,000808: 22,2474 00001 0
016154,000809: 22,2475 72476 DCOMP SL1
016155,000810: 22,2476 10013 STORE 10D,2 # C5=-COS(PSI)SIN(PHI)
016156,000811: 22,2477 41345 DLOAD DMP
016157,000812: 22,2500 00005 4
016158,000813: 22,2501 00013 10D
016159,000814: 22,2502 72476 DCOMP SL1
016160,000815: 22,2503 10015 STORE 12D,2 # C6=-SIN(THETA)COS(PSI)
016161,000816:
|
|
Page 371 |
016163,000818: 22,2504 77745 DLOAD
016164,000819: 22,2505 72405 DMP SL1 # (PUSH UP 7)
016165,000820: 22,2506 00011 8D
016166,000821: 22,2507 41325 PDDL DMP # (PD7 COS(PHI)SIN(THETA)SIN(PSI)) SCALE 4
016167,000822: 22,2510 00007 6
016168,000823: 22,2511 00001 0
016169,000824: 22,2512 72415 DAD SL1 # (PUSH UP 7)
016170,000825: 22,2513 77626 STADR # C7=COS(PHI)SIN(THETA)SIN(PSI)
016171,000826: 22,2514 67760 STORE 14D,2 # +COS(THETA)SIN(PHI)
016172,000827: 22,2515 77745 DLOAD
016173,000828: 22,2516 72405 DMP SL1 # (PUSH UP 6)
016174,000829: 22,2517 00011 8D
016175,000830: 22,2520 41325 PDDL DMP # (PD6 SIN(THETA)SIN(PHI)SIN(PSI)) SCALE 4
016176,000831: 22,2521 00007 6
016177,000832: 22,2522 00003 2
016178,000833: 22,2523 72425 DSU SL1 # (PUSH UP 6)
016179,000834: 22,2524 77626 STADR
016180,000835: 22,2525 67756 STORE 16D,2 # C8=-SIN(THETA)SIN(PHI)SIN(PSI)
016181,000836: 22,2526 77616 RVQ # +COS(THETA)COS(PHI)
016182,000837:
016183,000838: # CALCULATION OF THE MATRIX DEL......
016184,000839:
016185,000840: # * * --T *
016186,000841: # DEL = (IDMATRIX)COS(A)+UU (1-COS(A))+UX SIN(A) SCALED 1
016187,000842:
016188,000843: # -
016189,000844: # WHERE U IS A UNIT VECTOR (DP SCALED 2) ALONG THE AXIS OF ROTATION.
016190,000845: # A IS THE ANGLE OF ROTATION (DP SCALED 2)
016191,000846: # -
016192,000847: # UPON ENTRY THE STARTING ADDRESS OF U IS COF, AND A IS IN MPAC
016193,000848:
016194,000849: 22,2527 41401 DELCOMP SETPD PUSH # MPAC CONTAINS THE ANGLE A
016195,000850: 22,2530 00001 0
016196,000851: 22,2531 65356 SIN PDDL # PD0 = SIN(A)
016197,000852: 22,2532 41546 COS PUSH # PD2 = COS(A)
016198,000853: 22,2533 65302 SR2 PDDL # PD2 = COS(A) $8
016199,000854: 22,2534 41021 BDSU BOVB
016200,000855: 22,2535 06503 DPHALF
016201,000856: 22,2536 31765 SIGNMPAC
016202,000857: 22,2537 77725 PDDL # PD4 = 1-COS(A)
016203,000858:
016204,000859: # COMPUTE THE DIAGONAL COMPONENTS OF DEL
016205,000860:
016206,000861: 22,2540 03270 COF
016207,000862: 22,2541 41316 DSQ DMP
016208,000863: 22,2542 00005 4
016209,000864: 22,2543 52415 DAD SL3
016210,000865: 22,2544 00003 2
016211,000866: 22,2545 77604 BOVB
016212,000867: 22,2546 31765 SIGNMPAC
016213,000868:
|
|
Page 372 |
016215,000870: 22,2547 16222 STODL KEL # UX UX(1-COS(A)) +COS(A) $1
016216,000871: 22,2550 03272 COF +2
016217,000872: 22,2551 41316 DSQ DMP
016218,000873: 22,2552 00005 4
016219,000874: 22,2553 52415 DAD SL3
016220,000875: 22,2554 00003 2
016221,000876: 22,2555 77604 BOVB
016222,000877: 22,2556 31765 SIGNMPAC
016223,000878: 22,2557 16232 STODL KEL +8D # UY UY(1-COS(A)) +COS(A) $1
016224,000879: 22,2560 03274 COF +4
016225,000880: 22,2561 41316 DSQ DMP
016226,000881: 22,2562 00005 4
016227,000882: 22,2563 52415 DAD SL3
016228,000883: 22,2564 00003 2
016229,000884: 22,2565 77604 BOVB
016230,000885: 22,2566 31765 SIGNMPAC
016231,000886: 22,2567 02242 STORE KEL +16D # UZ UZ(1-COS(A)) +COS(A) $1
016232,000887:
016233,000888: # COMPUTE THE OFF DIAGONAL TERMS OF DEL
016234,000889:
016235,000890: 22,2570 41345 DLOAD DMP
016236,000891: 22,2571 03270 COF
016237,000892: 22,2572 03272 COF +2
016238,000893: 22,2573 72405 DMP SL1
016239,000894: 22,2574 00005 4
016240,000895: 22,2575 41325 PDDL DMP # D6 UX UY (1-COS A) $4
016241,000896: 22,2576 03274 COF +4
016242,000897: 22,2577 00001 0
016243,000898: 22,2600 43206 PUSH DAD # D8 UZ SIN A $4
016244,000899: 22,2601 00007 6
016245,000900: 22,2602 41112 SL2 BOVB
016246,000901: 22,2603 31765 SIGNMPAC
016247,000902: 22,2604 16230 STODL KEL +6
016248,000903: 22,2605 62421 BDSU SL2
016249,000904: 22,2606 77604 BOVB
016250,000905: 22,2607 31765 SIGNMPAC
016251,000906: 22,2610 16224 STODL KEL +2
016252,000907: 22,2611 03270 COF
016253,000908: 22,2612 41205 DMP DMP
016254,000909: 22,2613 03274 COF +4
016255,000910: 22,2614 00005 4
016256,000911: 22,2615 65352 SL1 PDDL # D6 UX UZ (1-COS A) $4
016257,000912: 22,2616 03272 COF +2
016258,000913: 22,2617 41405 DMP PUSH # D8 UY SIN(A)
016259,000914: 22,2620 00001 0
016260,000915: 22,2621 62415 DAD SL2
016261,000916: 22,2622 00007 6
016262,000917: 22,2623 77604 BOVB
016263,000918: 22,2624 31765 SIGNMPAC
016264,000919: 22,2625 16226 STODL KEL +4 # UX UZ (1-COS(A))+UY SIN(A)
016265,000920:
|
|
Page 373 |
016267,000922: 22,2626 62421 BDSU SL2
016268,000923: 22,2627 77604 BOVB
016269,000924: 22,2630 31765 SIGNMPAC
016270,000925: 22,2631 16236 STODL KEL +12D # UX UZ (1-COS(A))-UY SIN(A)
016271,000926: 22,2632 03272 COF +2
016272,000927: 22,2633 41205 DMP DMP
016273,000928: 22,2634 03274 COF +4
016274,000929: 22,2635 00005 4
016275,000930: 22,2636 65352 SL1 PDDL # D6 UY UZ (1-COS(A)) $4
016276,000931: 22,2637 03270 COF
016277,000932: 22,2640 41405 DMP PUSH # D8 UX SIN(A)
016278,000933: 22,2641 00001 0
016279,000934: 22,2642 62415 DAD SL2
016280,000935: 22,2643 00007 6
016281,000936: 22,2644 77604 BOVB
016282,000937: 22,2645 31765 SIGNMPAC
016283,000938: 22,2646 16240 STODL KEL +14D # UY UZ(1-COS(A)) +UX SIN(A)
016284,000939: 22,2647 62421 BDSU SL2
016285,000940: 22,2650 77604 BOVB
016286,000941: 22,2651 31765 SIGNMPAC
016287,000942: 22,2652 02234 STORE KEL +10D # UY UZ (1-COS(A)) -UX SIN(A)
016288,000943: 22,2653 77616 RVQ
016289,000944:
016290,000945:
016291,000946: # DIRECTION COSINE MATRIX TO CDU ANGLE ROUTINE
016292,000947: # X1 CONTAINS THE COMPLEMENT OF THE STARTING ADDRESS FOR MATRIX (SCALED 2)
016293,000948: # LEAVES CDU ANGLES SCALED 2PI IN V(MPAC)
016294,000949: # COS(MGA) WILL BE LEFT IN S1 (SCALED 1)
016295,000950:
016296,000951: # THE DIRECTION COSINE MATRIX RELATING S/C AXES TO STABLE MEMBER AXES CAN BE WRITTEN AS***
016297,000952:
016298,000953: # C =COS(THETA)COS(PSI)
016299,000954: # 0
016300,000955: # C =-COS(THETA)SIN(PSI)COS(PHI)+SI (THETA)SIN(PHI)
016301,000956: # 1
016302,000957: # C =COS(THETA)SIN(PSI)SIN(PHI) + S N(THETA)COS(PHI)
016303,000958: # 2
016304,000959: # C = SIN(PSI)
016305,000960: # 3
016306,000961: # C = COS(PSI)COS(PHI)
016307,000962: # 4
016308,000963: # C =-COS(PSI)SIN(PHI)
016309,000964: # 5
016310,000965: # C =-SIN(THETA)COS(PSI)
016311,000966: # 6
016312,000967: # C =SIN(THETA)SIN(PSI)COS(PHI)+COS THETA) SIN(PHI)
016313,000968: # 7
016314,000969: # C =-SIN(THETA)SIN(PSI)SIN(PHI)+CO (THETA)COS(PHI)
016315,000970: # 8
016316,000971:
|
|
Page 374 |
016318,000973:
016319,000974: # WHERE PHI = OGA
016320,000975: # THETA = IGA
016321,000976: # PSI = MGA
016322,000977:
016323,000978: 22,2654 67543 DCMTOCDU DLOAD* ARCSIN
016324,000979: 22,2655 00007 6,1
016325,000980: 22,2656 71406 PUSH COS # PD +0 PSI
016326,000981: 22,2657 41152 SL1 BOVB
016327,000982: 22,2660 31765 SIGNMPAC
016328,000983: 22,2661 00051 STORE S1
016329,000984: 22,2662 57543 DLOAD* DCOMP
016330,000985: 22,2663 00015 12D,1
016331,000986: 22,2664 67471 DDV ARCSIN
016332,000987: 22,2665 00051 S1
016333,000988: 22,2666 51123 PDDL* BPL # PD +2 THETA
016334,000989: 22,2667 00001 0,1 # MUST CHECK THE SIGN OF COS(THETA)
016335,000990: 22,2670 44702 OKTHETA # TO DETERMINE THE PROPER QUADRANT
016336,000991: 22,2671 57545 DLOAD DCOMP
016337,000992: 22,2672 43244 BPL DAD
016338,000993: 22,2673 44677 SUHALFA
016339,000994: 22,2674 06503 DPHALF
016340,000995: 22,2675 77650 GOTO
016341,000996: 22,2676 44701 CALCPHI
016342,000997: 22,2677 77625 SUHALFA DSU
016343,000998: 22,2700 06503 DPHALF
016344,000999: 22,2701 77606 CALCPHI PUSH
016345,001000: 22,2702 57543 OKTHETA DLOAD* DCOMP
016346,001001: 22,2703 00013 10D,1
016347,001002: 22,2704 67471 DDV ARCSIN
016348,001003: 22,2705 00051 S1
016349,001004: 22,2706 51123 PDDL* BPL # PUSH DOWN PHI
016350,001005: 22,2707 00011 8D,1
016351,001006: 22,2710 44722 OKPHI
016352,001007: 22,2711 57545 DLOAD DCOMP # PUSH UP PHI
016353,001008: 22,2712 43244 BPL DAD
016354,001009: 22,2713 44717 SUHALFAP
016355,001010: 22,2714 06503 DPHALF
016356,001011: 22,2715 77650 GOTO
016357,001012: 22,2716 44723 VECOFANG
016358,001013: 22,2717 52025 SUHALFAP DSU GOTO
016359,001014: 22,2720 06503 DPHALF
016360,001015: 22,2721 44723 VECOFANG
016361,001016: 22,2722 77745 OKPHI DLOAD # PUSH UP PHI
016362,001017: 22,2723 43466 VECOFANG VDEF RVQ
016363,001018:
|
|
Page 375 |
016365,001020: # ROUTINES FOR TERMINATING THE AUTOMATIC MANEUVER AND RETURNING TO USER
016366,001021:
016367,001022: 22,2724 77776 TOOBADF EXIT
016368,001023: 22,2725 05571 TC ALARM
016369,001024: 22,2726 00401 OCT 00401
016370,001025:
016371,001026: 22,2727 12732 TCF NOGO # DO NOT ZERO ATTITUDE ERRORS
016372,001027:
016373,001028: 22,2730 04607 TC BANKCALL
016374,001029: 22,2731 40154 CADR ZATTEROR # ZERO ATTITUDE ERRORS
016375,001030:
016376,001031: 22,2732 04607 NOGO TC BANKCALL
016377,001032: 22,2733 40166 CADR STOPRATE # STOP RATES
016378,001033:
016379,001034: 22,2734 34743 CAF TWO
016380,001035: 22,2735 00004 INHINT # ALL RETURNS ARE NOW MADE VIA GOODEND
016381,001036: 22,2736 05205 TC WAITLIST
016382,001037: 22,2737 E6,1675 EBANK= BCDU
016383,001038: 22,2737 03234 44066 2CADR GOODMANU
016384,001039:
016385,001040: 22,2741 15146 TCF ENDOFJOB
016386,001041:
016387,001042: 22,2742 77776 TOOBADI EXIT
016388,001043: 22,2743 12732 TCF NOGO
End of include-file ATTITUDE_MANEUVER_ROUTINE.agc. Parent file is MAIN.agc