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