Source Code
061327,000001: # Copyright: Public domain.
061328,000002: # Filename: TRIM_GIMBAL_CNTROL_SYSTEM.agc
061329,000003: # Purpose: A section of Luminary 1C, revision 131.
061330,000004: # It is part of the source code for the Lunar Module's (LM)
061331,000005: # Apollo Guidance Computer (AGC) for Apollo 13 and Apollo 14.
061332,000006: # This file is intended to be a faithful transcription, except
061333,000007: # that the code format has been changed to conform to the
061334,000008: # requirements of the yaYUL assembler rather than the
061335,000009: # original YUL assembler.
061336,000010: # Reference: pp. 1469-1481 of 1729.pdf.
061337,000011: # Contact: Ron Burkey <info@sandroid.org>.
061338,000012: # Website: www.ibiblio.org/apollo/index.html
061339,000013: # Mod history: 06/08/03 RSB. Began transcribing.
061340,000014: # 05/14/05 RSB Corrected website reference above.
061341,000015:
061342,000016: # Page 1469
061343,000017: 21,3076 BANK 21
061344,000018: 21,3076 E6,1450 EBANK= QDIFF
061345,000019: 21,2000 SETLOC DAPS4
061346,000020: 21,2000 BANK
061347,000021:
061348,000022: 21,3076 COUNT* $$/DAPGT
061349,000023:
061350,000024: # CONTROL REACHES THIS POINT UNDER EITHER OF THE FOLLOWING TWO CONDITIONS ONCE THE DESCENT ENGINE AND THE DIGITAL
061351,000025: # AUTOPILOT ARE BOTH ON:
061352,000026: # A) THE TRIM GIMBAL CONTROL LAW WAS ON DURING THE PREVIOUS Q,R-AXIS TIME5 INTERRUP1 (OR THE DAPIDLER
061353,000027: # INITIALIZATION WAS SET FOR TRIM GIMBAL CONTROL AND THIS IS THE FIRST PASS), OR
061354,000028: # B) THE Q,R-AXES RCS AUTOPILOT DETERMINED THAT THE VEHICLE WAS ENTERING (OR HAD JUST ENTERED) A COAST
061355,000029: # ZONE WITH A SMALL OFFSET ANGULAR ACCELERATION.
061356,000030: # GTS IS THE ENTRY TO THE GIMBAL TRIM SYSTEM FOR CONTROLLING ATTITUDE ERRORS AND RATES AS WELL AS ACCELERATIONS.
061357,000031:
061358,000032: 21,3076 37752 GTS CAF NEGONE # MAKE THE NEXT PASS THROUGH THE DAP BE
061359,000033: 21,3077 55630 TS COTROLER # THROUGH RCS CONTROL,
061360,000034: 21,3100 34751 CAF FOUR # AND ENSURE THAT IT IS NOT A SKIP.
061361,000035: 21,3101 55534 TS SKIPU
061362,000036: 21,3102 55535 TS SKIPV
061363,000037:
061364,000038: 21,3103 34752 CAF TWO
061365,000039: 21,3104 55632 TS INGTS # SET INDICATOR OF GTS CONTROL POSITIVE.
061366,000040: 21,3105 55631 TS QGIMTIMR # SET TIMERS TO 200 MSEC TO AVOID BOTH
061367,000041: 21,3106 55633 TS RGIMTIMR # RUNAWAY AND INTERFERENCE BY NULLING.
061368,000042:
061369,000043: # THE DRIVE SETTING ALGORITHM
061370,000044: # DEL = SGN(OMEGA + ALPHA*ABS(ALPHA)/(2*K)) 2 1/2 2 3/2
061371,000045: # NEGUSUM = ERROR*K + ALPHA*(DEL*OMETA + ALPHA /(3*K)) + DEL*K (DEL*OMEGA + ALPHA /(2*K))
061372,000046: # DRIVE = -SGN(NEGUSUM)
061373,000047:
061374,000048: 21,3107 30021 CA SR # SAVE THE SR. SHIFT IT LEFT TO CORRECT
061375,000049: 21,3110 60000 AD A # FOR THE RIGHT SHIFT DUE TO EDITING.
061376,000050: 21,3111 55505 TS SAVESR
061377,000051:
061378,000052: 21,3112 34752 GTSGO+DN CAF TWO # SET INDEXER FOR R-AXIS CALCULATIONS.
061379,000053: 21,3113 13115 TCF GOQTRIMG +1
061380,000054:
061381,000055: 21,3114 34755 GOQTRIMG CAF ZERO # SET INDEXER FOR Q-AXIS CALCULATIONS
061382,000056: 21,3115 55747 TS QRCNTR
061383,000057:
061384,000058: 21,3116 51747 INDEX QRCNTR # AOS SCALED AT PI/2
061385,000059: 21,3117 31536 CA AOSQ
061386,000060: 21,3120 00006 EXTEND
061387,000061: 21,3121 74752 MP BIT2 # RESCALE AOS TO PI/4
061388,000062: # Page 1470
061389,000063: 21,3122 00006 EXTEND
061390,000064: 21,3123 13127 BZF GTSQAXIS -3 # USE FULL SCALE FOR LARGER AOS ESTIMATES.
061391,000065:
061392,000066: 21,3124 50000 INDEX A
061393,000067: 21,3125 44734 CS LIMITS # LIMITS +1 CONTAINS NEGMAX.
061394,000068: 21,3126 56001 XCH L # LIMITS -1 CONTAINS POSMAX.
061395,000069:
061396,000070: 21,3127 11747 CCS QRCNTR # PICK UP RATE FOR THIS AXIS. RATE CELLS
061397,000071: 21,3130 50000 INDEX A # USE ADJACENT, NOT SEPARATED. AT PI/4
061398,000072: 21,3131 31436 CA EDOTQ
061399,000073: 21,3132 53743 GTSQAXIS DXCH WCENTRAL
061400,000074:
061401,000075: 21,3133 51747 INDEX QRCNTR # COLLECT K FOR THIS AXIS
061402,000076: 21,3134 31504 CA KQ
061403,000077: 21,3135 55740 TS KCENTRAL
061404,000078:
061405,000079: 21,3136 00006 EXTEND # CONTROL AUTHORITY ZERO. AVOID DRIVING
061406,000080: 21,3137 13575 BZF POSDRIVE +1 # ENGINE BELL TO THE STOPS.
061407,000081:
061408,000082: 21,3140 51747 INDEX QRCNTR # QDIFF, RDIFF ARE STORED IN D.P.
061409,000083: 21,3141 31450 CAE QDIFF
061410,000084:
061411,000085: 21,3142 00006 ALGORTHM EXTEND # Q(R)DIFF IS THETA (ERROR) SCALED AT PI.
061412,000086: 21,3143 71740 MP KCENTRAL # FORM K*ERROR AT PI(2)/2(8), IN D.P.
061413,000087: 21,3144 23736 LXCH K2THETA
061414,000088: 21,3145 00006 EXTEND
061415,000089: 21,3146 74747 MP BIT5 # RESCALE TO 4*PI(2)
061416,000090: 21,3147 53737 DXCH K2THETA
061417,000091: 21,3150 00006 EXTEND
061418,000092: 21,3151 74747 MP BIT5 # FIRST TERM OF NEGUSUM IN K2THETA.
061419,000093: 21,3152 27737 ADS K2THETA +1 # NO CARRY NEEDED D.P. AT 4*PI(2)
061420,000094:
061421,000095: 21,3153 41743 CS ACENTRAL # FORM ALPHA(2)/(2*K) AT 16*PI, IN D.P.,
061422,000096: 21,3154 00006 EXTEND # LIMITING QUOTIENT TO AVOID OVERFLOW.
061423,000097: 21,3155 74736 MP BIT14 # -ALPHA/2 IN A, SCALED AT PI/4
061424,000098: 21,3156 00006 EXTEND
061425,000099: 21,3157 71743 MP ACENTRAL # -ALPHA(2)/2 IN A,L, SCALED AT PI(2)/16)
061426,000100: 21,3160 61740 AD KCENTRAL
061427,000101: 21,3161 00006 EXTEND
061428,000102: 21,3162 63177 BZMF HUGEQUOT # K-ALPHA(2)/2 SHOULD BE PNZ FO DIVISION
061429,000103:
061430,000104: 21,3163 00006 EXTEND
061431,000105: 21,3164 40001 DCS A # ALPHA(2)/2 - K
061432,000106: 21,3165 61740 AD KCENTRAL
061433,000107: 21,3166 00006 EXTEND
061434,000108: 21,3167 11740 DV KCENTRAL # HIGH ORDER OF QUOTIENT.
061435,000109: 21,3170 57745 XCH A2CNTRAL
061436,000110: 21,3171 30001 CA L # SHIFT UP THE REMAINDER.
061437,000111: 21,3172 22007 LXCH 7 # ZERO LOW-ORDER DIVIDEND.
061438,000112: 21,3173 00006 EXTEND
061439,000113: # Page 1471
061440,000114: 21,3174 11740 DV KCENTRAL
061441,000115: 21,3175 57746 XCH A2CNTRAL +1 # QUOTIENT STORED AT 16*PI, D.P.
061442,000116: 21,3176 13202 TCF HAVEQUOT
061443,000117:
061444,000118: 21,3177 34733 HUGEQUOT CA POSMAX
061445,000119: 21,3200 54001 TS L
061446,000120: 21,3201 53746 DXCH A2CNTRAL # LIMITED QUOTIENT STORED AT 16*PI, D.P.
061447,000121:
061448,000122: 21,3202 31742 HAVEQUOT CA WCENTRAL
061449,000123: 21,3203 00006 EXTEND
061450,000124: 21,3204 74743 MP BIT9 # RESCALE OMEGA AT 16*PI IN D.P.
061451,000125: 21,3205 53742 DXCH K2CNTRAL # LOWER WORD OVERLAYS OMEGA IN WCENTRAL
061452,000126:
061453,000127: 21,3206 00006 EXTEND
061454,000128: 21,3207 31742 DCA K2CNTRAL
061455,000129: 21,3210 53751 DXCH FUNCTION
061456,000130:
061457,000131: 21,3211 31743 CA ACENTRAL # GET ALPHA*ABS(ALPHA)/(2*K)
061458,000132: 21,3212 00006 EXTEND
061459,000133: 21,3213 63217 BZMF +4
061460,000134:
061461,000135: 21,3214 00006 EXTEND
061462,000136: 21,3215 31746 DCA A2CNTRAL
061463,000137: 21,3216 13221 TCF +3
061464,000138:
061465,000139: 21,3217 00006 EXTEND
061466,000140: 21,3220 41746 DCS A2CNTRAL
061467,000141:
061468,000142: 21,3221 21751 DAS FUNCTION # OMEGA + ALPHA*ABS(ALPHA)/(2*K) AT 16*PI
061469,000143:
061470,000144: 21,3222 11750 CCS FUNCTION # DEL = +1 FOR FUNCT1 GREATER THAN ZERO.
061471,000145: 21,3223 13227 TCF POSFNCT1 # OTHERWISE DEL = -1
061472,000146: 21,3224 13226 TCF +2
061473,000147: 21,3225 13231 TCF NEGFNCT1
061474,000148:
061475,000149: 21,3226 11751 CCS FUNCTION +1 # USE LOW ORDER WORD SINCE HIGH IS ZERO
061476,000150: 21,3227 34753 POSFNCT1 CAF BIT1
061477,000151: 21,3230 13232 TCF +2
061478,000152: 21,3231 44753 NEGFNCT1 CS BIT1
061479,000153: 21,3232 55744 TS DEL
061480,000154:
061481,000155: 21,3233 11744 CCS DEL # REPLACE OMEGA BY DEL*OMEGA
061482,000156: 21,3234 13246 TCF FUNCT2 # POSITIVE DEL VALUE. PROCEED.
061483,000157: 21,3235 13237 TCF DEFUNCT
061484,000158: 21,3236 13243 TCF NEGFNCT2
061485,000159:
061486,000160: 21,3237 55741 DEFUNCT TS K2CNTRAL
061487,000161: 21,3240 55742 TS K2CNTRAL +1
061488,000162: 21,3241 13246 TCF FUNCT2
061489,000163:
061490,000164: # Page 1472
061491,000165: 21,3242 65252 NEG1/3 DEC -.33333
061492,000166:
061493,000167: 21,3243 00006 NEGFNCT2 EXTEND
061494,000168: 21,3244 41742 DCS K2CNTRAL
061495,000169: 21,3245 53742 DXCH K2CNTRAL
061496,000170:
061497,000171: 21,3246 00006 FUNCT2 EXTEND
061498,000172: 21,3247 31746 DCA A2CNTRAL
061499,000173: 21,3250 21742 DAS K2CNTRAL # DEL*OMEGA + ALPHA(2)/(2*K) AT 16*PI, D.P.
061500,000174:
061501,000175: 21,3251 31745 FUNCT3 CA A2CNTRAL
061502,000176: 21,3252 00006 EXTEND
061503,000177: 21,3253 73242 MP NEG1/3
061504,000178: 21,3254 53746 DXCH A2CNTRAL
061505,000179: 21,3255 30001 CA L
061506,000180: 21,3256 00006 EXTEND
061507,000181: 21,3257 73242 MP NEG1/3
061508,000182: 21,3260 27746 ADS A2CNTRAL +1
061509,000183: 21,3261 54001 TS L
061510,000184: 21,3262 13264 TCF +2 # A2CNTRAL NOW CONTAINS -ALPHA(2)/(6*K),
061511,000185: 21,3263 27745 ADS A2CNTRAL # SCALED AT 16*PI, IN D.P.
061512,000186:
061513,000187: 21,3264 00006 EXTEND
061514,000188: 21,3265 31742 DCA K2CNTRAL # DEL*OMEGA + ALPHA(2)/(3*K) IN A2CNTRAL,
061515,000189: 21,3266 21746 DAS A2CNTRAL # SCALED AT 16*PI, D.P.
061516,000190:
061517,000191: 21,3267 31745 CA A2CNTRAL
061518,000192: 21,3270 00006 EXTEND
061519,000193: 21,3271 71743 MP ACENTRAL
061520,000194: 21,3272 21737 DAS K2THETA
061521,000195: 21,3273 31746 CA A2CNTRAL +1
061522,000196: 21,3274 00006 EXTEND
061523,000197: 21,3275 71743 MP ACENTRAL # ACENTRAL MAY NOW BE OVERLAID.
061524,000198: 21,3276 27737 ADS K2THETA +1
061525,000199: 21,3277 54001 TS L
061526,000200: 21,3300 13302 TCF +2 # TWO TERMS OF NEGUSUM ACCUMULATED, SO FAR
061527,000201: 21,3301 27736 ADS K2THETA # SCALED AT 4*PI(2), IN D.P.
061528,000202:
061529,000203: 21,3302 31741 GETROOT CA K2CNTRAL # K*(DEL*OMEGA + ALPHA(2)/(2*K)) IS THE
061530,000204: 21,3303 00006 EXTEND # TERM FOR WHICH A SQUARE ROOT IS NEEDED.
061531,000205: 21,3304 71740 MP KCENTRAL # K AT PI/2(8)
061532,000206: 21,3305 53751 DXCH FUNCTION
061533,000207: 21,3306 31742 CA K2CNTRAL +1
061534,000208: 21,3307 00006 EXTEND
061535,000209: 21,3310 71740 MP KCENTRAL
061536,000210: 21,3311 27751 ADS FUNCTION +1
061537,000211: 21,3312 54001 TS L
061538,000212: 21,3313 13315 TCF +2
061539,000213: 21,3314 27750 ADS FUNCTION # DESIRED TERM IN FUNCTION, AT PI(2)/16
061540,000214: # Page 1473
061541,000215: 21,3315 11744 CCS DEL
061542,000216: 21,3316 13523 TCF RSTOFGTS
061543,000217: 21,3317 13565 TCF NEGUSUM
061544,000218: 21,3320 13322 TCF NEGATE
061545,000219: 21,3321 13565 TCF NEGUSUM
061546,000220:
061547,000221: 21,3322 00006 NEGATE EXTEND
061548,000222: 21,3323 41742 DCS K2CNTRAL
061549,000223: 21,3324 53742 DXCH K2CNTRAL
061550,000224: 21,3325 13523 TCF RSTOFGTS
061551,000225:
061552,000226: 16,3741 BANK 16
061553,000227: 16,3741 E6,1501 EBANK= NEGUQ
061554,000228: 16,2000 SETLOC DAPS1
061555,000229: 16,2000 BANK
061556,000230:
061557,000231: # THE WRCHN12 SUBROUTINE SETS BITS 9,10,11,12 OF CHANNEL 12 ON THE BASIS OF THE CONTENTS OF NEGUQ,NEGUR WHICH ARE
061558,000232: # THE NEGATIVES OF THE DESIRED ACCELERTION CHANGES. ACDT+C12 SETS Q(R)ACCDOT TO REFLECT TEH NEW DRIVES.
061559,000233:
061560,000234: # WARNING: ACDT+C12 AND WHCHN12 MUST BE CALLED WITH INTERRUPT INHIBITED.
061561,000235:
061562,000236: 16,3741 07400 BGIM OCTAL 07400
061563,000237: 16,3742 0066 CHNL12 EQUALS ITEMP6
061564,000238: 16,3742 41501 ACDT+C12 CS NEGUQ
061565,000239: 16,3743 00006 EXTEND # GIMBAL DRIVE REQUESTS.
061566,000240: 16,3744 71507 MP ACCDOTQ
061567,000241: 16,3745 23510 LXCH QACCDOT
061568,000242: 16,3746 41503 CS NEGUR
061569,000243: 16,3747 00006 EXTEND
061570,000244: 16,3750 71511 MP ACCDOTR
061571,000245: 16,3751 23512 LXCH RACCDOT
061572,000246:
061573,000247: 16,3752 11501 CCS NEGUQ
061574,000248: 16,3753 34742 CAF BIT10
061575,000249: 16,3754 13756 TCF +2
061576,000250: 16,3755 34743 CAF BIT9
061577,000251: 16,3756 54066 TS CHNL12
061578,000252:
061579,000253: 16,3757 11503 CCS NEGUR
061580,000254: 16,3760 34740 CAF BIT12
061581,000255: 16,3761 13763 TCF +2
061582,000256: 16,3762 34741 CAF BIT11
061583,000257: 16,3763 26066 ADS CHNL12 # (STORED RESULT NOT USED AT PRESENT)
061584,000258:
061585,000259: 16,3764 43741 CS BGIM
061586,000260: 16,3765 00006 EXTEND
061587,000261: 16,3766 02012 RAND CHAN12
061588,000262: 16,3767 60066 AD CHNL12
061589,000263: 16,3770 00006 EXTEND
061590,000264: 16,3771 01012 WRITE CHAN12
061591,000265: # Page 1474
061592,000266: 16,3772 44747 CS CALLGMBL # TURN OFF REQUEST FOR ACDT+C12 EXECUTION.
061593,000267: 16,3773 71262 MASK RCSFLAGS
061594,000268: 16,3774 55262 TS RCSFLAGS
061595,000269:
061596,000270: 16,3775 00002 TC Q # RETURN TO CALLER.
061597,000271:
061598,000272: 21,3326 BANK 21
061599,000273: 21,3326 E6,1450 EBANK= QDIFF
061600,000274: 21,2000 SETLOC DAPS4
061601,000275: 21,2000 BANK
061602,000276:
061603,000277: # Page 1475
061604,000278: # SUBROUTINE TIMEGMBL: MOD 0, OCTOBER 1967, CRAIG WORK
061605,000279:
061606,000280: # TIMEGMBL COMPUTES THE DRIVE TIME NEEDED FOR TEH TRIM GIMBAL TO POSITION THE DESCENT ENGINE NOZZLE SO AS TO NULL
061607,000281: # THE OFFSET ANGULAR ACCELERATION ABOUT THE Q (OR R) AXIS. INSTEAD OF USING AOSQ(R), TIMEBMBL USES .4*AOSQ(R),
061608,000282: # SCALED AT PI/8. FOR EACH AXIS, THE DRIVE TIME IS COMPUTED AS ABS(ALPHA/ACCDOT). AT ZERO
061609,000283: # ALPHA OR ACCDOT OR A ZERO QUOTIENT TURNS OFF THE GIMBAL DRIVE IMMEDIATELY. OTHERWISE, THE GIMBAL IS TURNED ON
061610,000284: # DRIVING IN THE CORRECT DIRECTION. THE Q(R)GIMTIMR IS SET TO TERMINATE THE DRIVE AND Q(R)ACCDOT
061611,000285: # IS STORED TO REFLECT THE NEW ACCELERATION DERIVATIVE. NEGUQ(R) WILL CONTAIN +1,+0,-1 FOR A Q(R)ACCDOT VALUE
061612,000286: # WHICH IS NEGATIVE, ZERO, OR POSITIVE.
061613,000287:
061614,000288: # INPUTS: AOSQ,AOSR, SCALED AT P1/2, AND ACCDOTQ, ACCDOTR AT PI/2(7). PI/2(7).
061615,000289:
061616,000290: # OUTPUTS: NEW GIMBAL DRIVE BITS IN CHANNEL 12, NEGUQ, NEGUR, QACCDOT, AND RACCDOT, THE LAST SCALED AT PI/2(7).
061617,000291: # Q(R)GIMTIMR WILL BE SET TO TIME AND TERMINATE GIMBAL DRIVE(S).
061618,000292:
061619,000293: # DEBRIS: A, L, Q, TEMPS 2, 3, 6, AND RUPTREG2 AND ACDT+C12 DEBRIS.
061620,000294:
061621,000295: # EXITS: VIA TC Q.
061622,000296:
061623,000297: # ALARMS, ABORTS: NONE.
061624,000298:
061625,000299: # SUBROUTINES: ACDT+C12, IBNKCALL
061626,000300:
061627,000301: # WARNING: THIS SUBROUTINE WRITES INTO CHANNEL 12 AND USES THE ITEMPS. THEREFORE IT MAY ONLY BE CALLED WITH
061628,000302: # INTERRUPT INHIBITED.
061629,000303:
061630,000304: # ERASABLE STORAGE CONFIGURATION (NEEDED BY THE INDEXING METHODS):
061631,000305: # NEGUQ ERASE +2 # NEGATIVE OF Q-AXIS GIMBAL DRIVE
061632,000306: # (SPWORD) EQUALS NEGUQ +1 # ANY S.P. ERASABLE NUMBER, NOW THRSTCMD
061633,000307: # NEGUR EQUALS NEGUQ +2 # NEGATIVE OF R-AXIS GIMBAL DRIVE
061634,000308: # ACCDOTQ ERASE +2 # Q-JERK TERM SCALED AT PI/2(7) RAD/SEC(3)
061635,000309: # (SPWORD) EQUALS ACCDOTQ +1 # ANY S.P. ERASABLE NUMBER NOW QACCDOT
061636,000310: # ACCDOTR EQUALS ACCDOTQ +2 # R-JERK TERM SCALED AT PI/2(7) RAD/SEC(3)
061637,000311: # # ACCDOTQ, ACCDOTR ARE MAGNITUDES.
061638,000312: # AOSQ ERASE +4 # Q-AXIS ACC., D.P. AT PI/2 R/SEC(2)
061639,000313: # AOSR EQUALS AOSQ +2 # R-AXIS ACCELERATION SCALED AT PI/2 R/S2
061640,000314:
061641,000315: 21,3326 0066 QRNDXER EQUALS ITEMP6
061642,000316: 21,3326 23146 OCT23146 OCTAL 23146 # DECIMAL .6
061643,000317: 21,3327 0063 NZACCDOT EQUALS ITEMP3
061644,000318:
061645,000319: 21,3327 34753 TIMEGMBL CAF ONE # INITIALZE ALLOWGTS.
061646,000320: 21,3330 55502 TS ALLOWGTS
061647,000321:
061648,000322: 21,3331 34752 CAF TWO # SET UP LOOP FOR R AXIS.
061649,000323: 21,3332 22002 LXCH Q # SAVE RETURN ADDRESS.
061650,000324: 21,3333 22071 LXCH RUPTREG2
061651,000325: # Page 1476
061652,000326: 21,3334 13336 TCF +2
061653,000327: 21,3335 34755 TIMQGMBL CAF ZERO # NOW DO THE Q-AXIS
061654,000328: 21,3336 54066 TS QRNDXER
061655,000329: 21,3337 50066 INDEX QRNDXER
061656,000330: 21,3340 31507 CA ACCDOTQ # ACCDOT IS PRESUMED TO BE AT PI/2(7).
061657,000331: 21,3341 00006 EXTEND
061658,000332: 21,3342 63412 BZMF TGOFFNOW # IS ACCDOT LESS THAN OR EQUAL TO 0?
061659,000333: 21,3343 54063 TS NZACCDOT # NO. STORE NON-ZERO, POSITIVE ACCDOT.
061660,000334:
061661,000335: 21,3344 50066 ALPHATRY INDEX QRNDXER
061662,000336: 21,3345 41536 CS AOSQ
061663,000337: 21,3346 00006 EXTEND
061664,000338: 21,3347 13412 BZF TGOFFNOW # IS ALPHA ZERO?
061665,000339:
061666,000340: 21,3350 54002 TS Q # SAVE A COPY OF -ACS.
061667,000341: 21,3351 00006 EXTEND # NO. RESCALE FOR TIMEGMBL USE.
061668,000342: 21,3352 73326 MP OCT23146 # OCTAL 23146 IS DECIMAL .6
061669,000343: 21,3353 60002 AD Q # -1.6*AOS AT PI/2 = -.4*AOS AT PI/8
061670,000344: 21,3354 54001 TS L # WAS THERE OVERFLOW?
061671,000345: 21,3355 13362 TCF SETNEGU # NO. COMPUTE DRIVE TIME.
061672,000346:
061673,000347: 21,3356 40000 CS A # RECOVER -SGN(AOS) IN THE A REGISTER.
061674,000348: 21,3357 50066 INDEX QRNDXER # YES. START DRIVE WITHOUT WAITLIST.
061675,000349: 21,3360 57501 XCH NEGUQ
061676,000350: 21,3361 13416 TCF NOTALLOW # KNOCK DOWN THE ALLOWGTS FLAG.
061677,000351:
061678,000352: 21,3362 00006 SETNEGU EXTEND
061679,000353: 21,3363 63370 BZMF POSALPH
061680,000354:
061681,000355: 21,3364 40000 COM
061682,000356: 21,3365 54062 TS ITEMP2 # STORE -ABS(.4*AOS) SCALED AT PI/8.
061683,000357: 21,3366 44753 CS BIT1
061684,000358: 21,3367 13372 TCF POSALPH +2
061685,000359: 21,3370 54062 POSALPH TS ITEMP2 # STORE -ABS(.4*AOS) SCALED AT PI/8.
061686,000360: 21,3371 34753 CA BIT1
061687,000361: 21,3372 50066 +2 INDEX QRNDXER # SGN(AOS) INTO NEGU
061688,000362: 21,3373 55501 TS NEGUQ # STORE SGN(ALPHA) AS NEGU
061689,000363:
061690,000364: 21,3374 30063 CA NZACCDOT
061691,000365: 21,3375 00006 EXTEND
061692,000366: 21,3376 74740 MP BIT12 # 2*ACCDOT, SCALED AT PI/8.
061693,000367: 21,3377 60062 AD ITEMP2 # -ABS(ALPHS) + 2*ACCDOT, AT PI/8.
061694,000368: 21,3400 00006 EXTEND
061695,000369: 21,3401 63416 BZMF NOTALLOW # IS DRIVE TIME MORE THAN TWO SECONDS?
061696,000370: 21,3402 40062 CS ITEMP2 # NO. COMPUTE DRIVE TIME.
061697,000371: 21,3403 00006 EXTEND # ABS(ALPHA) AT PI/8.
061698,000372: 21,3404 73437 MP OCT00240 # DECIMAL 10/1024
061699,000373: 21,3405 00006 EXTEND # QUOTIENT IS DRIVE TIME AT WAITLIST.
061700,000374: 21,3406 10063 DV NZACCDOT # ABS(ALPHA)/ACCDOT AT 2(14)/100
061701,000375: # Page 1477
061702,000376: 21,3407 00006 EXTEND
061703,000377: 21,3410 13412 BZF TGOFFNOW # DRIVE TIME MUST BE GREATER THAN ZERO.
061704,000378:
061705,000379: 21,3411 13424 TCF DRIVEON
061706,000380:
061707,000381: 21,3412 34755 TGOFFNOW CAF ZERO # TURN OFF GIMBAL NOW.
061708,000382: 21,3413 50066 INDEX QRNDXER
061709,000383: 21,3414 55501 TS NEGUQ
061710,000384:
061711,000385: 21,3415 13426 TCF DONEYET
061712,000386:
061713,000387: 21,3416 36010 NOTALLOW CAF OCT31
061714,000388: 21,3417 50066 INDEX QRNDXER
061715,000389: 21,3420 55631 TS QGIMTIMR
061716,000390: 21,3421 34755 CAF ZERO # DRIVE TIME IS MORE THAN 2 SECONDS, SO
061717,000391: 21,3422 55502 TS ALLOWGTS # DO NOT PERMIT FURTHER GTS ATTITUDE-RATE
061718,000392: # CONTROL UNTIL AOSTASK APPROVES.
061719,000393: 21,3423 13426 TCF DONEYET # NO WAITLIST CALL IS MADE.
061720,000394:
061721,000395: 21,3424 50066 DRIVEON INDEX QRNDXER
061722,000396: 21,3425 55631 TS QGIMTIMR # CHOOSE Q OR R AXIS.
061723,000397:
061724,000398: 21,3426 10066 DONEYET CCS QRNDXER
061725,000399: 21,3427 13335 TCF TIMQGMBL
061726,000400:
061727,000401: 21,3430 52073 DXCH RUPTREG3 # PROTECT IBNKCALL ERASABLES. ACDT+C12
061728,000402: 21,3431 52063 DXCH ITEMP2 # LEAVES ITEMPS2,3 ALONE.
061729,000403:
061730,000404: 21,3432 04674 TC IBNKCALL # TURN OFF CHANNEL BITS, SET Q(R)ACCDOTS.
061731,000405: 21,3433 35742 CADR ACDT+C12
061732,000406:
061733,000407: 21,3434 52063 DXCH ITEMP2 # RESTORE ERASABLES FOR IBNKCALL.
061734,000408: 21,3435 52073 DXCH RUPTREG3
061735,000409:
061736,000410: 21,3436 00071 TC RUPTREG2 # RETURN TO CALLER.
061737,000411:
061738,000412: 21,3437 00240 OCT00240 OCTAL 00240 # DECIMAL 10/1024
061739,000413:
061740,000414: # Page 1478
061741,000415: # THE FOLLOWING SECTION IS A CONTINUATION OF THE TRIM GIMBAL CONTROL FROM THE LAST GTS ENTRY. THE QUANTITY NEGUSUM
061742,000416: # IS COMPUTED FOR EACH AXIS (Q,R), .707*DEL*FUNCTION(3/2) + K2THETA = NEGUSUM. NEW DRIVES ARE ENTERED TO CH 12.
061743,000417:
061744,000418: # THE SUBROUTIEN GTSQRT ACCEPTS A DOUBLE PRECISION VALUE IN FUNCTION, FUNCTION +1 AND RETURNS A SINGLE-PRECISION
061745,000419: # SQUARE ROOT OF THE FOURTEEN MOST SIGNIFICATN BITS OF THE ARGUMENT. ALSO, TEH CELL SHFTFLAG CONTAINS A BINARY
061746,000420: # EXPONENT S, SUCH THAT THE SQUARE ROOT (RETURNED IN THE A REGISTER) MUST BE SHIFTED RIGHT (MULTIPLIED BY 2 TO THE
061747,000421: # POWER (-S)) IN ORDER TO BE THE TRUE SQUARE ROOT OF TEH FOURTEEN MOST SIGNIFICANT BITS OF FUNCTION, FUNCTION +1.
061748,000422: # SQUARE ROOT ERROR IS NOT MORE THAN 2 IN THE 14TH SIGNIFICANT BIT. CELLS CLOBBERED ARE A, L, SHFTFLAG, ININDEX,
061749,000423: # HALFARG, SCRATCH, SR, FUNCTION, FUNCTION +1. GTSQRT IS CALLED BY TC GTSQRT AND RETURNS VIA TC Q OR TC FUNCTION +1.
061750,000424: # ZERO OR NEGATIVE ARGUMENTS YIELD ZERO FOR SQUARE ROOTS.
061751,000425:
061752,000426: 21,3440 11750 GTSQRT CCS FUNCTION
061753,000427: 21,3441 13457 TCF GOODARG # FUNCTION IS POSITIVE. TAKE SQUARE ROOT.
061754,000428: 21,3442 13444 TCF +2 # HIGH ORDER WORD IS ZERO. TRY THE LOWER.
061755,000429: 21,3443 13450 TCF ZEROOT
061756,000430:
061757,000431: 21,3444 31751 CA FUNCTION +1
061758,000432: 21,3445 00006 EXTEND
061759,000433: 21,3446 63450 BZMF ZEROOT
061760,000434:
061761,000435: 21,3447 13453 TCF ZEROHIGH # PROCEED.
061762,000436: 21,3450 34755 ZEROOT CA ZERO
061763,000437: 21,3451 55740 TS SHFTFLAG
061764,000438: 21,3452 00002 TC Q
061765,000439:
061766,000440: 21,3453 57750 ZEROHIGH XCH FUNCTION # 14 MOST SIGNIFICANT BITS ARE IN THE
061767,000441: 21,3454 57751 XCH FUNCTION +1 # LOWER WORD. EXCHANGE THEM.
061768,000442: 21,3455 34757 CA SEVEN
061769,000443: 21,3456 13460 TCF GOODARG +1
061770,000444:
061771,000445: 21,3457 34755 GOODARG CA ZERO
061772,000446: 21,3460 55740 TS SHFTFLAG
061773,000447: 21,3461 35741 CA TWELVE # INITIALIZE THE SCALING LOOP.
061774,000448: 21,3462 55743 TS ININDEX
061775,000449: 21,3463 13472 TCF SCALLOOP
061776,000450:
061777,000451: 21,3464 31750 SCALSTRT CA FUNCTION
061778,000452: 21,3465 13506 TCF SCALDONE
061779,000453:
061780,000454: 21,3466 37751 MULBUSH CA NEG2 # IF ARG IS NOT LESS THAN 1/4, INDEX IS
061781,000455: 21,3467 27743 ADS ININDEX # ZERO, INDICATING NO SHIFT NEEDED.
061782,000456: 21,3470 00006 EXTEND # BRANCH IF ARG IS NOT LESS THAN 1/4.
061783,000457: 21,3471 63464 BZMF SCALSTRT # OTHERWISE COMPARE ARG WITH A REFERENCE
061784,000458: # WHICH IS 4 TIMES LARGER THAN THE LAST.
061785,000459: 21,3472 41750 SCALLOOP CS FUNCTION
061786,000460: 21,3473 51743 INDEX ININDEX
061787,000461: 21,3474 64735 AD BIT15 # REFERENCE MAGNITUDE LESS OR EQUAL TO 1/4
061788,000462: 21,3475 00006 EXTEND
061789,000463: 21,3476 63466 BZMF MULBUSH # IF ARG IS NOT LESS THAN REFERENCE, GO
061790,000464: # AROUND THE MULBERRY BUSH ONCE MORE.
061791,000465: # Page 1479
061792,000466: 21,3477 51743 INDEX ININDEX
061793,000467: 21,3500 34735 CA BIT15 # THIS IS THE SCALE MAGNITUDE
061794,000468: 21,3501 57746 XCH HALFARG # 2**(-ININDEX) IS THE SHIFT DIVISOR.
061795,000469: 21,3502 00006 EXTEND # RESCALE ARGUMENT.
061796,000470: 21,3503 31751 DCA FUNCTION
061797,000471: 21,3504 00006 EXTEND
061798,000472: 21,3505 11746 DV HALFARG
061799,000473: # ININDEX AND SHFTFLAG PRESERVE INFO FOR
061800,000474: # RESCALING AFTER ROOT PROCESS.
061801,000475: 21,3506 00006 SCALDONE EXTEND
061802,000476: 21,3507 23751 QXCH FUNCTION +1 # SAVE Q FOR RETURN
061803,000477: 21,3510 00006 EXTEND
061804,000478: 21,3511 74736 MP BIT14
061805,000479: 21,3512 55746 TS HALFARG
061806,000480: 21,3513 74737 MASK BIT13
061807,000481: 21,3514 10000 CCS A
061808,000482: 21,3515 33633 CA OCT11276
061809,000483: 21,3516 63631 AD ROOTHALF # INITIAL GUESS IS ROOT 1/2 OR POSMAX
061810,000484: 21,3517 03635 TC ROOTCYCL
061811,000485: 21,3520 03635 TC ROOTCYCL
061812,000486: 21,3521 03635 TC ROOTCYCL
061813,000487: 21,3522 01751 TC FUNCTION +1
061814,000488:
061815,000489: # ***************************************************************************
061816,000490:
061817,000491: 21,3523 03440 RSTOFGTS TC GTSQRT
061818,000492: 21,3524 57741 PRODUCT XCH K2CNTRAL
061819,000493: 21,3525 00006 EXTEND
061820,000494: 21,3526 71741 MP K2CNTRAL
061821,000495: 21,3527 53742 DXCH K2CNTRAL
061822,000496: 21,3530 00006 EXTEND # THE PRODUCT OF
061823,000497: 21,3531 70001 MP L # 1/2 2 1/2
061824,000498: 21,3532 27742 ADS K2CNTRAL +1 # K *(DEL*OMEGA + ALPHA /(2*K))
061825,000499: 21,3533 54001 TS L # AND
061826,000500: 21,3534 13536 TCF +2 # 2
061827,000501: 21,3535 27741 ADS K2CNTRAL # DEL*(DEL*OMEGA + ALPHA /(2*K)) NOW IN K2CNTRAL
061828,000502:
061829,000503: 21,3536 31743 DOSHIFT CA ININDEX
061830,000504: 21,3537 00006 EXTEND # MULTIPLY IN THE FACTOR 2(-S), RETURNED
061831,000505: 21,3540 74736 MP BIT14 # BY THE GTSQRT SUBROUTINE
061832,000506: 21,3541 27740 ADS SHFTFLAG
061833,000507: 21,3542 00006 EXTEND
061834,000508: 21,3543 13562 BZF ADDITIN
061835,000509: 21,3544 51740 INDEX SHFTFLAG
061836,000510: 21,3545 34735 CA BIT15
061837,000511: # Page 1480
061838,000512: 21,3546 57741 XCH K2CNTRAL
061839,000513: 21,3547 00006 EXTEND
061840,000514: 21,3550 71741 MP K2CNTRAL
061841,000515: 21,3551 21737 DAS K2THETA
061842,000516: 21,3552 57741 XCH K2CNTRAL
061843,000517: 21,3553 00006 EXTEND
061844,000518: 21,3554 71742 MP K2CNTRAL +1
061845,000519: 21,3555 27737 ADS K2THETA +1
061846,000520: 21,3556 54001 TS L
061847,000521: 21,3557 13561 TCF +2
061848,000522: 21,3560 27736 ADS K2THETA
061849,000523:
061850,000524: 21,3561 13565 TCF NEGUSUM
061851,000525:
061852,000526: 21,3562 00006 ADDITIN EXTEND
061853,000527: 21,3563 31742 DCA K2CNTRAL
061854,000528: 21,3564 21737 DAS K2THETA # NO ADD IN THE K2THETA TERM.
061855,000529: 21,3565 11736 NEGUSUM CCS K2THETA # TEST SIGN OF HIGH ORDER PART.
061856,000530: 21,3566 13572 TCF NEGDRIVE
061857,000531: 21,3567 13571 TCF +2
061858,000532: 21,3570 13574 TCF POSDRIVE
061859,000533:
061860,000534: 21,3571 11737 CCS K2THETA +1 # SIGN TEST FOR LOW ORDER PART.
061861,000535: 21,3572 34753 NEGDRIVE CA BIT1
061862,000536: 21,3573 13575 TCF +2 # STOP GIMBAL DRIVE FOR A ZERO NEGUSUM.
061863,000537: 21,3574 44753 POSDRIVE CS BIT1
061864,000538: 21,3575 54001 TS L # SAVE FOR DRIVE REVERSAL TEST.
061865,000539: 21,3576 51747 INDEX QRCNTR
061866,000540: 21,3577 57501 XCH NEGUQ
061867,000541:
061868,000542: 21,3600 00006 EXTEND
061869,000543: 21,3601 70001 MP L # MULTIPLY OLD NEGU AND NEW NEGU.
061870,000544: 21,3602 10001 CCS L
061871,000545: 21,3603 13620 TCF LOUPE # NON-ZERO GIMBAL DRIVE BEING CONTINUED.
061872,000546:
061873,000547: 21,3604 13615 TCF ZEROLOUP # NO REVERSAL PROBLEM HERE.
061874,000548:
061875,000549: 21,3605 13607 TCF REVERSAL # NON-ZERO GIMBAL DRIVE BEING REVERSED.
061876,000550: 21,3606 13615 TCF ZEROLOUP # NO REVERSAL PROBLEM HERE.
061877,000551:
061878,000552: 21,3607 51747 REVERSAL INDEX QRCNTR # A ZERO-DRIVE PAUSE IS NEEDED HERE. ZERO
061879,000553: 21,3610 55510 TS QACCDOT # IS IN A REGISTER FROM CCS ON (-1).
061880,000554: 21,3611 51747 INDEX QRCNTR
061881,000555: 21,3612 43632 CS GMBLBITA
061882,000556: 21,3613 00006 EXTEND
061883,000557: 21,3614 03012 WAND CHAN12
061884,000558:
061885,000559: 21,3615 41262 ZEROLOUP CS RCSFLAGS # SET UP REQUEST FOR ACDT+C12 CALL.
061886,000560: 21,3616 74747 MASK CALLGMBL
061887,000561: 21,3617 27262 ADS RCSFLAGS
061888,000562: # Page 1481
061889,000563: 21,3620 11747 LOUPE CCS QRCNTR # HAVE BOTH AXES BEEN PROCESSED?
061890,000564: 21,3621 13114 TCF GOQTRIMG # NO. DO Q AXIS NEXT.
061891,000565:
061892,000566: 21,3622 31505 CA SAVESR # RESTORE THE SR
061893,000567: 21,3623 54021 TS SR
061894,000568:
061895,000569: 21,3624 00006 GOCLOSE EXTEND # TERMINATE THE JASK.
061896,000570: 21,3625 33630 DCA CLOSEADR
061897,000571: 21,3626 52006 DTCB
061898,000572:
061899,000573: 21,3627 E6,1536 EBANK= AOSQ
061900,000574: 21,3627 03270 36106 CLOSEADR 2CADR CLOSEOUT # TERMINATE THE JASK.
061901,000575:
061902,000576: 21,3631 5741 TWELVE EQUALS OCT14
061903,000577: 21,3631 26501 ROOTHALF OCTAL 26501 # SQUARE ROOT OF 1/2
061904,000578: 21,3632 01400 GMBLBITA OCTAL 01400 # INDEXED WRT GMBLBITB DO NOT MOVE *******
061905,000579: 21,3633 11276 OCT11276 OCTAL 11276 # POSMAX -- ROOTHALF
061906,000580: 21,3634 06000 GMBLBITB OCTAL 06000 # INDEXED WRT GMBLBITA DO NOT MOVE *******
061907,000581:
061908,000582: # SUBROUTINE ROOTCYCL: BY CRAIG WORK, 3 APRIL 68
061909,000583:
061910,000584: # ROOTCYCL IS A SUBROUTINE WHICH EXECUTS ONE NEWTON SQUARE ALGORITHM ITERATION. THE INITIAL GUESS AT THE
061911,000585: # SQUARE ROOT IS PRESUMED TO BE IN THE A REGISTER AND ONE-HALF THE SQUARE IS TAKEN FROM HALFARG. THE NEW APPROXIMATION
061912,000586: # TO THE SQUARE ROOT IS RETURNED IN THE A REGISTER. DEBRIS: A, L, SR, SCRATCH. ROOTCYCL IS CALLED FROM
061913,000587: # LOCATION (LOC) BY A TC ROOTCYCL, AND RETURNS (TC Q) TO LOC +1.
061914,000588:
061915,000589: # WARNING: IF THE INITIAL GUESS IS NOT GREATER THAN THE SQUARE, DIVIDE OR ADD OVERFLOW IS A REAL POSSIBILITY.
061916,000590:
061917,000591: 21,3635 55745 ROOTCYCL TS SCRATCH # STORE X
061918,000592: 21,3636 54021 TS SR # X/2 NOW IN SR
061919,000593: 21,3637 31746 CA HALFARG # ARG/2 IN THE A REG
061920,000594: 21,3640 22007 ZL # PREPARE FOR DIVISION
061921,000595: 21,3641 00006 EXTEND
061922,000596: 21,3642 11745 DV SCRATCH # (ARG/X)/2
061923,000597: 21,3643 60021 AD SR # (X + ARG/X)/2 IN THE A REG
061924,000598: 21,3644 00002 TC Q
061925,000599:
061926,000600:
End of include-file TRIM_GIMBAL_CNTROL_SYSTEM.agc. Parent file is MAIN.agc