diff --git a/Luminary099/IMU_COMPENSATION_PACKAGE.agc b/Luminary099/IMU_COMPENSATION_PACKAGE.agc index 61952ca..fd08619 100644 --- a/Luminary099/IMU_COMPENSATION_PACKAGE.agc +++ b/Luminary099/IMU_COMPENSATION_PACKAGE.agc @@ -45,34 +45,34 @@ TCF IRIG1 # RETURN 1/PIPA1 CAF FOUR # PIPAZ, PIPAY, PIPAX - TS BUF +2 + TS BUF +2 - INDEX BUF +2 + INDEX BUF +2 CA PIPASCF # (P.P.M.) X 2(-9) EXTEND - INDEX BUF +2 + INDEX BUF +2 MP DELVX # (PP) X 2(+14) NOW (PIPA PULSES) X 2(+5) TS Q # SAVE MAJOR PART CA L # MINOR PART EXTEND - MP BIT6 # SCALE 2(+9) SHIFT RIGHT 9 - INDEX BUF +2 - TS DELVX +1 # FRACTIONAL PIPA PULSES SCALED 2(+14) + MP BIT6 # SCALE 2(+9) SHIFT RIGHT 9 + INDEX BUF +2 + TS DELVX +1 # FRACTIONAL PIPA PULSES SCALED 2(+14) CA Q # MAJOR PART EXTEND MP BIT6 # SCALE 2(+9) SHIFT RIGHT 9 - INDEX BUF +2 - DAS DELVX # (PIPAI) + (PIPAI)(SF) + INDEX BUF +2 + DAS DELVX # (PIPAI) + (PIPAI)(SFE) - INDEX BUF +2 - CS PIPABIAS # (PIPA PULSES)/(CS) X 2(-5) * * + INDEX BUF +2 + CS PIPABIAS # (PIPA PULSES)/(CS) X 2(-5) * EXTEND - MP 1/PIPADT # (CS) X 2(+8) NOW (PIPA PULSES) X 2(+5) * + MP 1/PIPADT # (CS) X 2(+8) NOW (PIPA PULSES) X 2(+3)* EXTEND - MP BIT4 # SCALE 2(+11) SHIFT RIGHT 11 * - INDEX BUF +2 + MP BIT4 # SCALE 2(+11) SHIFT RIGHT 11 * + INDEX BUF +2 DAS DELVX # (PIPAI) + (PIPAI)(SFE) - (BIAS)(DELTAT) CCS BUF +2 # PIPAZ, PIPAY, PIPAX @@ -83,79 +83,79 @@ # Page 328 IRIGCOMP TS GCOMPSW # INDICATE COMMANDS 2 PULSES OR LESS. - TS BUF # INDEX COUNTER. IRIGX, IRIGY, IRIGZ. + TS BUF # INDEX COUNTER . IRIGX, IRIGY, IRIGZ. TC IRIGX # COMPENSATE ACCELERATION TERMS CS NBDX # (GYRO PULSES)/(CS) X 2(-5) - TC DRIFTSUB # -(NBOX)(DELTAT) (GYRO PULSES) X 2(+14) + TC DRIFTSUB # -(NBOX)(DELTAT) (GYRO PULSES) X 2(+14) TC IRIGY # COMPENSATE ACCELERATION TERMS CS NBDY # (GYRO PULSES)/(CS) X 2(-5) - TC DRIFTSUB # +(NBDY)(DELTAT) (GYRO PULSES) X 2(+14) + TC DRIFTSUB # -(NBDY)(DELTAT) (GYRO PULSES) X 2(+14) TC IRIGZ # COMPENSATE ACCELERATION TERMS CA NBDZ # (GYRO PULSES)/(CS) X 2(-5) - TC DRIFTSUB # +(NBDZ)(DELTAT) (GYRO PULSES) X 2(+14) + TC DRIFTSUB # +(NBDZ)(DELTAT) (GYRO PULSES) X 2(+14) CCS GCOMPSW # ARE GYRO COMMANDS GREATER THAN 2 PULSES - TCF +2 # YES SEND OUT GYRO TORQUING COMMANDS. - TCF IRIG1 # NO RETURN + TCF +2 # YES SEND OUT GYRO TORQUING COMMANDS. + TCF IRIG1 # NO RETURN - CA PRIO21 # PRIO GREATER THAN SERVICER. + CA PRIO21 # PRIO GREATER THAN SERVICER TC NOVAC # SEND OUT GYRO TORQUING COMMANDS. EBANK= NBDX 2CADR 1/GYRO RELINT -IRIG1 CA MODE # RESTORE CALLER'S EBANK +IRIG1 CA MODE # RESTORE CALLERS EBANK TS EBANK TCF SWRETURN # Page 329 IRIGX EXTEND - QXCH MPAC +2 # SAVE Q + QXCH MPAC +2 # SAVE Q EXTEND DCS DELVX # (PIPA PULSES) X 2(+14) DXCH MPAC - CA ADIAX # (GYRO PULSES)/(PIPA PULSE) X 2(-6) * - TC GCOMPSUB # -(ADIAX)(PIPAX) (GYRO PULSES) X 2(+14) + CA ADIAX # (GYRO PULSES)/(PIPA PULSE) X 2(-6) * + TC GCOMPSUB # -(ADIAX)(PIPAX) (GYRO PULSES) X 2(+14) EXTEND # - DCS DELVY # (PIPA PULSES) X 2(+14) + DCS DELVY # (PIPA PULSES) X 2(+14) DXCH MPAC # - CS ADSRAX # (GYRO PULSES)/(PIPA PULSE) X 2(-6) * - TC GCOMPSUB # -(ADSRAX)(PIPAY) (GYRO PULSES) X 2(+14) + CS ADSRAX # (GYRO PULSES)/(PIPA PULSE) X 2(-6) * + TC GCOMPSUB # +(ADSRAX)(PIPAY) (GYRO PULSES) X 2(+14) -# EXTEND # *** -# DCS DELVY # *** (PIPA PULSES) X 2(+14) -# DXCH MPAC # *** -# CA ADOAX # *** (GYRO PULSES)/(PIPA PULSE) X 2(-6) * -# TC GCOMPSUB # *** -(ADOAX)(PIPAZ) (GYRO PULSES) X 2(+14) +# EXTEND # *** +# DCS DELVZ # *** (PIPA PULSES) X 2(+14) +# DXCH MPAC # *** +# CA ADOAX # *** (GYRO PULSES)/(PIPA PULSE) X 2(-6) * +# TC GCOMPSUB # *** -(ADOAX)(PIPAZ) (GYRO PULSES) X 2(+14) - TC MPAC +2 + TC MPAC +2 IRIGY EXTEND - QXCH MPAC +2 # SAVE Q + QXCH MPAC +2 # SAVE Q EXTEND DCS DELVY # (PIPA PULSES) X 2(+14) DXCH MPAC - CA ADIAY # (GYRO PULSES)/(PIPA PULSE) X 2(-6) * - TC GCOMPSUB # -(ADIAY)(PIPAY) (GYRO PULSES) X 2(+14) + CA ADIAY # (GYRO PULSES)/(PIPA PULSE) X 2(-6) * + TC GCOMPSUB # -(ADIAY)(PIPAY) (GYRO PULSES) X 2(+14) EXTEND DCS DELVZ # (PIPA PULSES) X 2(+14) DXCH MPAC - CS ADSRAY # (GYRO PULSES)/(PIPA PULSE) X 2(-6) * - TC GCOMPSUB # +(ADSRAY)(PIPAZ) (GYRO PULSES) X 2(+14) + CS ADSRAY # (GYRO PULSES)/(PIPA PULSE) X 2(-6) * + TC GCOMPSUB # +(ADSRAY)(PIPAZ) (GYRO PULSES) X 2(+14) -# EXTEND # *** -# DCS DELVX # *** (PIPA PULSES) X 2(+14) -# DXCH MPAC # *** -# CA ADOAY # *** (GYRO PULSES)/(PIPA PULSE) X 2(-6) * -# TC GCOMPSUB # *** -(ADOAY)(/PIPAX) (GYRO PULSES) X 2(+14) +# EXTEND # *** +# DCS DELVX # *** (PIPA PULSES) X 2(+14) +# DXCH MPAC # *** +# CA ADOAY # *** (GYRO PULSES)/(PIPA PULSE) X 2(-6) * +# TC GCOMPSUB # *** -(ADOAY)(PIPAX) (GYRO PULSES) X 2(+14) TC MPAC +2 @@ -164,46 +164,46 @@ IRIGZ EXTEND EXTEND DCS DELVY # (PIPA PULSES) X 2(+14) DXCH MPAC - CA ADSRAZ # (GYRO PULSES)/(PIPA PULSE) X 2(-6) * + CA ADSRAZ # (GYRO PULSES)/(PIPA PULSE) X 2(-6) * # Page 330 - TC GCOMPSUB # -(ADSRAZ)(PIPAY) (GYRO PULSES) X 2(+14) + TC GCOMPSUB # -(ADSRAZ)(PIPAY) (GYRO PULSES) X 2(+14) EXTEND DCS DELVZ # (PIPA PULSES) X 2(+14) DXCH MPAC - CA ADIAZ # (GYRO PULSES)/(PIPA PULSE) X 2(-6) * - TC GCOMPSUB # -(ADIAZ)(PIPAZ) (GYRO PULSES) X 2(+14) + CA ADIAZ # (GYRO PULSES)/(PIPA PULSE) X 2(-6) * + TC GCOMPSUB # -(ADIAZ)(PIPAZ) (GYRO PULSES) X 2(+14) -# EXTEND # *** -# DCS DELVX # *** (PIPA PULSE) X 2(+14) -# DXCH MPAC # *** -# CS ADOAZ # *** (GYRO PULSES)/(PIPA PULSE) X 2(-6) * -# TC GCOMPSUB # *** +(ADOAZ)(PIPAX) (GYRO PULSES) X 2(+14) +# EXTEND # *** +# DCS DELVX # *** (PIPA PULSE) X 2(+14) +# DXCH MPAC # *** +# CS ADOAZ # *** (GYRO PULSES)/(PIPA PULSE) X 2(-6) * +# TC GCOMPSUB # *** +(ADOAZ)(PIPAX) (GYRO PULSES) X 2(+14) TC MPAC +2 # Page 331 GCOMPSUB XCH MPAC # ADIA OR ADSRA COEFFICIENT ARRIVES IN A EXTEND # C(MPAC) = (PIPA PULSES) X 2(+14) - MP MPAC # (GYRO PULSES)/(PIPA PULSE) X 2(-6) * - DXCH VBUF # NOW = (GYRO PULSES) X 2(+8) * + MP MPAC # (GYRO PULSES)/(PIPA PULSE) X 2(-6) * + DXCH VBUF # NOW = (GYRO PULSES) X 2(+8) * CA MPAC +1 # MINOR PART OF PIPA PULSES EXTEND MP MPAC # ADIA OR ADSRA TS L CAF ZERO - DAS VBUF # NOW = (GYRO PULSES) X 2(+8) * + DAS VBUF # NOW = (GYRO PULSES) X 2(+8) * - CA VBUF # PARTIAL RESULT -- MAJOR + CA VBUF # PARTIAL RESULT - MAJOR EXTEND - MP BIT9 # SCALE 2(+6) SHIFT RIGHT 6 * + MP BIT9 # SCALE 2(+6) SHIFT RIGHT * INDEX BUF # RESULT = (GYRO PULSES) X 2(+14) - DAS GCOMP # HI(ADIA)(PIPAI) OR HI(ADSRA)(PIPAI) + DAS GCOMP # HI(ADIA)(PIPAI) OR HI(ADSRA)(PIPAI) - CA VBUF +1 # PARTIAL RESULT -- MINOR + CA VBUF +1 # PARTIAL RESULT - MINOR EXTEND - MP BIT9 # SCALE 2(+6) SHIFT RIGHT 6 * + MP BIT9 # SCALE 2(+6) SHIFT RIGHT 6 * TS L CAF ZERO INDEX BUF # RESULT = (GYRO PULSES) X 2(+14) @@ -213,23 +213,23 @@ GCOMPSUB XCH MPAC # ADIA OR ADSRA COEFFICIENT ARRIVES IN A # Page 332 DRIFTSUB EXTEND - QXCH BUF +1 + QXCH BUF +1 EXTEND # C(A) = NBD (GYRO PULSES)/(CS) X 2(-5) - MP 1/PIPADT # (CS) X 2(+8) NO (GYRO PULSES) X 2(+3) - LXCH MPAC +1 # SAVE FOR FRACTIONAL COMPENSATION + MP 1/PIPADT # (CS) X 2(+8) NOW (GYRO PULSES) X 2(+3) + LXCH MPAC +1 # SAVE FOR FRACTIONAL COMPENSATION EXTEND - MP BIT4 # SCALE 2(+11) SHIFT RIGHT 11 + MP BIT4 # SCALE 2(+11) SHIFT RIGHT 11 INDEX BUF - DAS GCOMP # HI(NBD)(DELTAT) (GYRO PULSES) X 2(+14) + DAS GCOMP # HI(NBD)(DELTAT) (GYRO PULSES) X 2(+14) - CA MPAC +1 # NO MINOR PART + CA MPAC +1 # NOW MINOR PART EXTEND - MP BIT4 # SCALE 2(+11) SHIFT RIGHT 11 + MP BIT4 # SCALE 2(+11) SHIFT RIGHT 11 TS L CAF ZERO INDEX BUF # ADD IN FRACTIONAL COMPENSATION - DAS GCOMP # (NBD)(DELTAT) (GYRO PULSES) X 2(+14) + DAS GCOMP # (NBD)(DELTAT) (GYRO PULSES) X 2(+14) DRFTSUB2 CAF TWO # PIPAX, PIPAY, PIPAZ AD BUF @@ -237,23 +237,23 @@ DRFTSUB2 CAF TWO # PIPAX, PIPAY, PIPAZ INDEX A CCS GCOMP # ARE GYRO COMMANDS 1 PULSE OR GREATER TCF +2 # YES - TC BUF +1 # NO + TC BUF +1 # NO MASK COMPCHK # DEC -1 CCS A # ARE GYRO COMMANDS GREATER THAN 2 PULSES - TS GCOMPSW # YES -- SET GCOMPSW POSITIVE - TC BUF +1 # NO + TS GCOMPSW # YES - SET GCOMPSW POSITIVE + TC BUF +1 # NO # Page 333 1/GYRO CAF FOUR # PIPAZ, PIPAY, PIPAX TS BUF INDEX BUF # SCALE GYRO COMMANDS FOR IMUPULSE - CA GCOMP +1 # FRACTIONAL PULSES + CA GCOMP +1 # FRACTIONAL PULSES EXTEND MP BIT8 # SHIFT RIGHT 7 INDEX BUF - TS GCOMP +1 # FRACTIONAL PULSES SCALED + TS GCOMP +1 # FRACTIONAL PULSES SCALED CAF ZERO # SET GCOMP = 0 FOR DAS INSTRUCTION INDEX BUF @@ -265,7 +265,7 @@ DRFTSUB2 CAF TWO # PIPAX, PIPAY, PIPAZ CCS BUF # PIPAZ, PIPAY, PIPAX AD NEG1 - TCF 1/GYRO +1 + TCF 1/GYRO +1 LGCOMP ECADR GCOMP # LESS THAN ZERO IMPOSSIBLE CAF LGCOMP @@ -281,7 +281,7 @@ GCOMP1 CAF FOUR # PIPAZ, PIPAY, PIPAX INDEX BUF # RESCALE CA GCOMP +1 EXTEND - MP BIT8 # SHIFT MINOR PART LEFT 7 -- MAJOR PART = 0 + MP BIT8 # SHIFT MINOR PART LEFT 7 - MAJOR PART = 0 INDEX BUF LXCH GCOMP +1 # BITS 8-14 OF MINOR PART WERE = 0 @@ -303,14 +303,14 @@ NBDONLY CCS GCOMPSW # BYPASS IF GCOMPSW NEGATIVE TCF ENDOFJOB TCF +1 - CA FLAGWRD8 # IF SURFACE FLAG IS SET, SET TEM1 + CA FLAGWRD8 # IF SURFACE FLAG IS SET. SET TEM1 MASK BIT8 # POSITIVE SO THAT THE ACCELERATION TERMS TS TEM1 # WILL BE COMPENSATED. EXTEND BZF +3 # ARE WE ON THE SURFACE TC IBNKCALL # ON THE SURFACE - CADR PIPASR +3 # READ PIPAS, BUT DO NOT SCALE THEM + CADR PIPASR +3 # READ PIPAS. BUT DO NOT SCALE THEM CA TIME1 # (CS) X 2(+14) XCH 1/PIPADT # PREVIOUS TIME @@ -319,18 +319,18 @@ NBDONLY CCS GCOMPSW # BYPASS IF GCOMPSW NEGATIVE AD 1/PIPADT # PRESENT TIME - PREVIOUS TIME NBD2 AD HALF # CORRECT FOR POSSIBLE TIME1 TICK AD HALF - XCH L # IF TIME1 DID NOT DICK, REMOVE RESULTING + XCH L # IF TIME1 DID NOT TICK. REMOVE RESULTING XCH L # OVERFLOW. -NBD3 EXTEND # C(A) = DELTAT (CS) X 2(+14) +NBD3 EXTEND # C(A) = DELTAT (CS) X 2(+14) MP BIT10 # SHIFT RIGHT 5 DXCH VBUF +2 CA ZERO TS GCOMPSW # INDICATE COMMANDS 2 PULSES OR LESS. - TS BUF # INDEX X, Y, Z. + TS BUF # INDEX X. Y, Z. - CCS TEM1 # IF SURFACE FLAG IS SET, + CCS TEM1 # IF SURFACE FLAG IS SET. TC IRIGX # COMPENSATE ACCELERATION TERMS. EXTEND @@ -338,7 +338,7 @@ NBD3 EXTEND # C(A) = DELTAT (CS) X 2(+14) DXCH MPAC # DELTAT NOW SCALED (CS) X 2(+19) CS NBDX # (GYRO PULSES)/(CS) X 2(-5) - TC FBIASSUB # -(NBOX)(DELTAT) (GYRO PULSES) X 2(+14) + TC FBIASSUB # -(NBDX)(DELTAT) (GYRO PULSES) X 2(+14) CCS TEM1 # IF SURFACE FLAG IS SET, TC IRIGY # COMPENSATE ACCELERATION TERMS. @@ -347,16 +347,16 @@ NBD3 EXTEND # C(A) = DELTAT (CS) X 2(+14) DCS VBUF +2 DXCH MPAC # DELTAT SCALED (CS) X 2(+19) CA NBDY # (GYRO PULSES)/(CS) X 2(-5) - TC FBIASSUB # -(NBDY)(DELTAT) (GYRO PULSES) X 2(+14) + TC FBIASSUB # -(NBDY)(DELTAT) (GYRO PULSES) X 2(+14) - CCS TEM1 # IF SURFACE FLAG IS SET, + CCS TEM1 # IF SURFACE FLAG IS SET. TC IRIGZ # COMPENSATE ACCELERATION TERMS EXTEND DCS VBUF +2 DXCH MPAC # DELTAT SCALED (CS) X 2(+19) CS NBDZ # (GYRO PULSES)/(CS) X 2(-5) - TC FBIASSUB # +(NBDZ)(DELTAT) (GYRO PULSES) X 2(+14) + TC FBIASSUB # +(NBDZ)(DELTAT) (GYRO PULSES) X 2(+14) CCS GCOMPSW # ARE GYRO COMMANDS GREATER THAN 2 PULSES TCF 1/GYRO # YES @@ -370,7 +370,7 @@ FBIASSUB XCH Q EXTEND MP MPAC # DELTAT SCALED (CS) X 2(+19) INDEX BUF - DAS GCOMP # HI(NBD)(DELTAT) (GYRO PULSES) X 2(+14) + DAS GCOMP # HI(NBD)(DELTAT) (GYRO PULSES) X 2(+14) CA Q # NOW FRACTIONAL PART EXTEND @@ -378,7 +378,7 @@ FBIASSUB XCH Q TS L CAF ZERO INDEX BUF - DAS GCOMP # (NBD)(DELTAT) (GYRO PULSES) X 2(+14) + DAS GCOMP # (NBD)(DELTAT) (GYRO PULSES) X 2(+14) TCF DRFTSUB2 # CHECK MAGNITUDE OF COMPENSATION @@ -415,4 +415,3 @@ GCOMPZER CAF LGCOMP # ROUTINE TO ZERO GCOMP BEFORE FIRST TS GCOMP +5 TCF IRIG1 # RESTORE EBANK AND RETURN -