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