Source Code

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