proof IMU_COMPENSATION_PACKAGE (#343, closes #212)

This commit is contained in:
cai 2018-07-19 06:10:17 +08:00 committed by James Harris
parent b47cb532c2
commit 8bb44091be

View File

@ -45,9 +45,8 @@
TCF +2 TCF +2
TCF IRIG1 # RETURN TCF IRIG1 # RETURN
INHINT # ASSURE COMPLETE COMPENSATION OF DELV'S INHINT # ASSURE COMPLETE COMPENSATION OF DELV'S
# FOR DOWNLINK. # FOR DOWNLINK.
1/PIPA1 CAF FOUR # PIPAZ, PIPAY, PIPAX 1/PIPA1 CAF FOUR # PIPAZ, PIPAY, PIPAX
TS BUF +2 TS BUF +2
@ -68,7 +67,7 @@
EXTEND EXTEND
MP BIT6 # SCALE 2(+9) SHIFT RIGHT 9 MP BIT6 # SCALE 2(+9) SHIFT RIGHT 9
INDEX BUF +2 INDEX BUF +2
DAS DELVX # (PIPAI) + (PIPAI)(SF) DAS DELVX # (PIPAI) + (PIPAI)(SFE)
INDEX BUF +2 INDEX BUF +2
CS PIPABIAS # (PIPA PULSES)/(CS) X 2(-8) * CS PIPABIAS # (PIPA PULSES)/(CS) X 2(-8) *
@ -83,12 +82,12 @@
# Page 298 # Page 298
AD NEG1 AD NEG1
TCF 1/PIPA1 +1 TCF 1/PIPA1 +1
NOOP # LESS THAN ZERO IMPOSSIBLE NOOP # LESS THAN ZERO IMPOSSIBLE.
RELINT RELINT
# Page 299 # Page 299
IRIGCOMP TS GCOMPSW # INDICATE COMMANDS 2 PULSES OR LESS. IRIGCOMP TS GCOMPSW # INDICATE COMMANDS 2 PULSES OR LESS.
TS BUF # INDEX COUNTER. IRIGX, IRIGY, IRIGZ. TS BUF # INDEX COUNTER - IRIGX, IRIGY, IRIGZ.
IRIGX EXTEND IRIGX EXTEND
DCS DELVX # (PIPA PULSES) X 2(+14) DCS DELVX # (PIPA PULSES) X 2(+14)
@ -97,10 +96,10 @@ IRIGX EXTEND
TC GCOMPSUB # -(ADIAX)(PIPAX) (GYRO PULSES) X 2(+14) TC GCOMPSUB # -(ADIAX)(PIPAX) (GYRO PULSES) X 2(+14)
EXTEND # EXTEND #
DCS DELVY # (PIPA PULSES) X 2(+14) DCS DELVY # (PIPA PULSES) X 2(+14)
DXCH MPAC # DXCH MPAC #
CS ADSRAX # (GYRO PULSES)/(PIPA PULSE) X 2(-3) * CS ADSRAX # (GYRO PULSES)/(PIPA PULSE) X 2(-3) *
TC GCOMPSUB # -(ADSRAX)(PIPAY) (GYRO PULSES) X 2(+14) TC GCOMPSUB # +(ADSRAX)(PIPAY) (GYRO PULSES) X 2(+14)
# EXTEND # *** # EXTEND # ***
# DCS DELVY # *** (PIPA PULSES) X 2(+14) # DCS DELVY # *** (PIPA PULSES) X 2(+14)
@ -108,8 +107,8 @@ IRIGX EXTEND
# CA ADOAX # *** (GYRO PULSES)/(PIPA PULSE) X 2(-3) * # CA ADOAX # *** (GYRO PULSES)/(PIPA PULSE) X 2(-3) *
# TC GCOMPSUB # *** -(ADOAX)(PIPAZ) (GYRO PULSES) X 2(+14) # TC GCOMPSUB # *** -(ADOAX)(PIPAZ) (GYRO PULSES) X 2(+14)
CS NBDX # (GYRO PULSES)/(CS) X 2(-3) CS NBDX # (GYRO PULSES)/(CS) X 2(-5)
TC DRIFTSUB # -(NBDX)(DELTAT) (GYRO PULSES) X 2(+14) TC DRIFTSUB # -(NBDX)(DELTAT) (GYRO PULSES) X 2(+14)
IRIGY EXTEND IRIGY EXTEND
DCS DELVY # (PIPA PULSES) X 2(+14) DCS DELVY # (PIPA PULSES) X 2(+14)
@ -126,14 +125,14 @@ IRIGY EXTEND
# EXTEND # *** # EXTEND # ***
# DCS DELVX # *** (PIPA PULSES) X 2(+14) # DCS DELVX # *** (PIPA PULSES) X 2(+14)
# DXCH MPAC # *** # DXCH MPAC # ***
# CA ADOAY # *** (GYRO PULSES)/(PIPA PULS) X 2(-3) * # CA ADOAY # *** (GYRO PULSES)/(PIPA PULSE) X 2(-3) *
# TC GCOMPSUB # *** -(ADOAY)(/PIPAX) (GYRO PULSES) X 2(+14) # TC GCOMPSUB # *** -(ADOAY)(PIPAZ) (GYRO PULSES) X 2(+14)
CS NBDY # (GYRO PULSES)/(CS) X 2(-5) 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)
IRIGZ EXTEND IRIGZ EXTEND
DCS DELVY # (PIPA PULSES) X 2(-14) DCS DELVY # (PIPA PULSES) X 2(+14)
DXCH MPAC DXCH MPAC
CA ADSRAZ # (GYRO PULSES)/(PIPA PULSE) X 2(-3) * CA ADSRAZ # (GYRO PULSES)/(PIPA PULSE) X 2(-3) *
# Page 300 # Page 300
@ -151,15 +150,15 @@ IRIGZ EXTEND
# CS ADOAZ # *** (GYRO PULSES)/(PIPA PULSE) X 2(-3) * # CS ADOAZ # *** (GYRO PULSES)/(PIPA PULSE) X 2(-3) *
# TC GCOMPSUB # *** +(ADOAZ)(PIPAX) (GYRO PULSES) X 2(+14) # TC GCOMPSUB # *** +(ADOAZ)(PIPAX) (GYRO PULSES) X 2(+14)
CA NBDZ # (GYRO PULSES)/(CS) X 2(-5) 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)
# Page 301 # Page 301
CCS GCOMPSW # ARE GYRO COMMANDS GREATER THAN 2 PULSES CCS GCOMPSW # ARE GYRO COMMANDS GREATER THAN 2 PULSES
TCF +2 # YES TCF +2 # YES
TCF IRIG1 # NO TCF IRIG1 # NO
CA PRIO21 # HIGHER THAN SERVICER -- LESS THAN PRELAUNCH CAF PRIO21 # HIGHER THAN SERVICER-LESS THAN PRELAUNCH
TC NOVAC TC NOVAC
EBANK= NBDX EBANK= NBDX
2CADR 1/GYRO 2CADR 1/GYRO
@ -181,19 +180,19 @@ GCOMPSUB XCH MPAC # ADIA OR ADSRA COEFFICIENT ARRIVES IN A
CAF ZERO CAF ZERO
DAS VBUF # NOW = (GYRO PULSES) X 2(+11) * DAS VBUF # NOW = (GYRO PULSES) X 2(+11) *
CA VBUF # PARTIAL RESULT -- MAJOR CA VBUF # PARTIAL RESULT - MAJOR
EXTEND EXTEND
MP BIT12 # SCALE 2(+3) SHIFT RIGHT 3 * MP BIT12 # SCALE 2(+3) SHIFT RIGHT 3 *
INDEX BUF # RESULT = (GYRO PULSES) X 2(+14) 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 EXTEND
MP BIT12 # SCALE 2(+3) SHIFT RIGHT 3 * MP BIT12 # SCALE 2(+3) SHIFT RIGHT 3 *
TS L TS L
CAF ZERO CAF ZERO
INDEX BUF # RESULT = (GYRO PULSES) X 2(+14) INDEX BUF # RESULT = (GYRO PULSES) X 2(+14)
DAS GCOMP # (ADIA)(PIPAI) OR (ADSRA)(PIPAI) DAS GCOMP # (ADIA)(PIPAI) OR (ADSRA)(PIPAI)
TC Q TC Q
@ -227,7 +226,7 @@ DRFTSUB2 CAF TWO # PIPAX, PIPAY, PIPAZ
MASK NEGONE MASK NEGONE
CCS A # ARE GYRO COMMANDS GREATER THAN 2 PULSES CCS A # ARE GYRO COMMANDS GREATER THAN 2 PULSES
TS GCOMPSW # YES -- SET GCOMPSW POSITIVE TS GCOMPSW # YES - SET GCOMPSW POSITIVE
TC BUF +1 # NO TC BUF +1 # NO
# Page 303 # Page 303
@ -267,14 +266,13 @@ GCOMP1 CAF FOUR # PIPAZ, PIPAY, PIPAX
INDEX BUF # RESCALE INDEX BUF # RESCALE
CA GCOMP +1 CA GCOMP +1
EXTEND EXTEND
MP BIT8 # SHIFT MINOR PART LEFT 7 -- MAJOR PART = 0 MP BIT8 # SHIFT MINOR PART LEFT 7 - MAJOR PART = 0
INDEX BUF INDEX BUF
LXCH GCOMP +1 # BITS 8-14 OF MINOR PART WERE = 0 LXCH GCOMP +1 # BITS 8-14 OF MINOR PART WERE = 0
CCS BUF # PIPAZ, PIPAY, PIPAX CCS BUF # PIPAZ, PIPAY, PIPAX
AD NEG1 AD NEG1
TCF GCOMP1 +1 TCF GCOMP1 +1
V06N30S VN 0630 V06N30S VN 0630
TCF ENDOFJOB TCF ENDOFJOB
@ -295,7 +293,7 @@ NBDONLY CCS GCOMPSW # BYPASS IF GCOMPSW NEGATIVE
RELINT RELINT
COM COM
AD 1/PIPADT AD 1/PIPADT
NBD2 CCS A # CALCULATE ELAPSED TIME. NBD2 CCS A # CALCULATE ELAPSED TIME
AD ONE # NO TIME1 OVERFLOW AD ONE # NO TIME1 OVERFLOW
TCF NBD3 # RESTORE TIME DIFFERENCE AND JUMP TCF NBD3 # RESTORE TIME DIFFERENCE AND JUMP
TCF +2 # TIME1 OVERFLOW TCF +2 # TIME1 OVERFLOW
@ -312,8 +310,8 @@ NBD3 EXTEND # C(A) = DELTAT (CS) X 2(+14)
DXCH MPAC # DELTAT NOW SCALED (CS) X 2(+19) DXCH MPAC # DELTAT NOW SCALED (CS) X 2(+19)
CAF ZERO CAF ZERO
TS GCOMPSW # INDICATE COMMANDS 2 PULSES OR LESS. TS GCOMPSW # INDICATE COMMANDS 2 PULSES OR LESS
TS BUF # INDEX X, Y, Z. TS BUF # PIPAX, PIPAY, PIPAZ
CS NBDX # (GYRO PULSES)/(CS) X 2(-5) CS NBDX # (GYRO PULSES)/(CS) X 2(-5)
TC FBIASSUB # -(NBOX)(DELTAT) (GYRO PULSES) X 2(+14) TC FBIASSUB # -(NBOX)(DELTAT) (GYRO PULSES) X 2(+14)