proof IMU_COMPENSATION_PACKAGE (#343, closes #212)

pull/345/head
cai 2018-07-19 06:10:17 +08:00 committed by James Harris
parent b47cb532c2
commit 8bb44091be
1 changed files with 25 additions and 27 deletions

View File

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