Proof RCS-CSM_DIGITAL_AUTOPILOT #249 (#393)

* Proof FIXED_FIXED_CONSTANT_POOL (#207)

* wip(p1002/1024): Proof RCS-CSM_DIGITAL_AUTOPILOT #249

* done(p1024/1024): Proof RCS-CSM_DIGITAL_AUTOPILOT #249
This commit is contained in:
Glenn Dwiyatcita 2019-07-15 22:37:08 +02:00 committed by James Harris
parent aba87ac00f
commit 5fac70b6a0

View File

@ -29,9 +29,9 @@
# Page 1002
# T5 INTERRUPT PROGRAM FOR THE RCS-CSM AUTOPILOT
# START OF T5 INTERRUPT PROGRAM
BANK 20
SETLOC DAPS3
BANK
@ -40,7 +40,7 @@
EBANK= KMPAC
REDORCS LXCH BANKRUPT # RESTART OF AUTOPILOT COMES HERE
CA T5PHASE # ON A T5 RUPT
CA T5PHASE # ON A T5 RUPT.
EXTEND
BZMF +2 # IF T5PHASE +0, -0, OR -, RESET TO -
TCF +3 # IF T5PHASE +, LEAVE IT +. DO A FRESHDAP
@ -57,7 +57,7 @@ RCSATT LXCH BANKRUPT # SAVE BB
EXTEND # SAVE Q
QXCH QRUPT
CAF BIT15 # BIT15 CHAN31 = 0 IF IMU POWER IS ON AND
EXTEND # S/C CONT SW IS IN CMC (I.E., IF G/C AUTO
EXTEND # S/C CONT SW IS IN CMC (I.E. IF G/C AUTO
RAND CHAN31 # PILOT IS FULLY ENABLED)
EXTEND
BZF SETT5 # IF G/C AUTOPILOT IS FULLY ENABLED,
@ -92,17 +92,15 @@ ONESEK DEC 16284 # 1 SEC(TIME5)
CHAN5 EQUALS 5
CHAN6 EQUALS 6
PRIO34A = PRIO34
# CHECK PHASE OF T5 PROGRAM
#
# BECAUSE OF THE LENGTH OF THE T5 PROGRAM,IT HAS BEEN DIVIDED INTO
# BECAUSE OF THE LENGTH OF THE T5 PROGRAM, IT HAS BEEN DIVIDED INTO
# THREE PARTS, T5PHASE1, T5PHASE2, AND THE JET SELECTION LOGIC,
# TO ALLOW FOR THE EXECUTION OF OTHER
# INTERRUPTS. T5PHASE IS ALSO USED IN THE INITIALIZATION OF THE AUTOPILOT
# VARIABLES AT TURN ON.
#
# THE CODING OF T5PHASE IS...
#
# + = INITIALIZE T5 RCS-CSM AUTOPILOT
# T5PHASE = +0 = PHASE2 OF THE T5 PROGRAM
# - = RESTART DAP
@ -147,16 +145,13 @@ RATEFILT CA RCSFLAGS # SEE IF RATEFILTER HAS BEEN INITIALIZED
TCF KMATRIX # IF NOT, SKIP RATE DERIVATION
# RATE FILTER TIMING = 7.72 MS
#
# RATE FILTER EQUATIONS
#
#
# DRHO = DELRHO - (.1)ADOT + (1 = GAIN1)DRHO
# -1
#
# ADOT = ADOT + GAIN2 DRHO + KMJ DFT
# -1
# _ * _ _
# - * - -
# WHERE DELRHO = AMGB (CDU - CDU )
# -1
@ -170,7 +165,7 @@ DRHOLOOP TS SPNDX
INDEX ATTKALMN # PICK UP DESIRED FILTER GAIN
MP GAIN1
INDEX DPNDX
DAS DRHO # (1 -.064) DRHO
DAS DRHO # (1 -.064)DRHO
EXTEND
# Page 1005
INDEX DPNDX
@ -187,7 +182,7 @@ DRHOLOOP TS SPNDX
CA CDUX # MEASURED BODY RATES--
XCH RHO
EXTEND
MSU RHO # _ * _ _
MSU RHO # - * - -
COM # DELRHO = AMGB (CDU - CDU )
# -1
ZL
@ -219,7 +214,7 @@ DRHOLOOP TS SPNDX
TS T5TEMP # (CDUZ - RHO2) SCALED 90 DEGS
EXTEND
MP AMGB5
DAS DELTEMPY # DELTEMPY = AMGB4(CDUY-RHO1)
DAS DELTEMPY # DELTEMPY =AMGB4(CDUY-RHO1)
# + AMGB5(CDUZ-RHO2)
CA AMGB8
EXTEND
@ -276,20 +271,20 @@ TENTHSEK TS ATTSEC
# Page 1007
# WHEN AUTOMATIC MANEUVERS ARE BEING PERFORMED, THE FOLLOWING ANGLE ADDITION MUST BE MADE TO PROVIDE A SMOOTH
# SEQUENCE OF ANGULAR COMMANDS TO THE AUTOPILOT--
#
# CDUXD = CDUXD + DELCDUX (DOUBLE PRECISION)
# CDUYD = CDUYD + DELCDUY (DOUBLE PRECISION)
# CDUZD = CDUZD + DELCDUZ (DOUBLE PRECISION)
#
# THE STEERING PROGRAMS:
# THE STEERING PROGRAMS-
# 1) ATTITUDE MANEUVER ROUTINE
# 2) LEM TRACKING
#
# SHOULD GENERATE THE DESIRED ANGLES (CDUXD, CDUYD, CDUZD) AS WELL AS THE INCREMENTAL ANGLES (DELCDUX, DELCDUY,
# DELCDUZ) SO THAT THE GIMBAL ANGLE COMMANDS CAN BE INTERPOLATED BETWEEN UPDATES.
#
# HOLDFLAG CODING:
#
# HOLDFLAG CODING-
# + = GRAB PRESENT CDU ANGLES AND STORE IN THETADX, THETADY, THETADZ
# AND PERFORM ATTITUDE HOLD ABOUT THESE ANGLES
# ALSO IGNORE AUTOMATIC STEERING
@ -306,7 +301,8 @@ TENTHSEK TS ATTSEC
# +0 = IN ATTITUDE HOLD ABOUT A PREVIOUSLY ESTABLISHED REFERENCE
# - = PERFORMING AUTOMATIC MANEUVER
# -0 = NOT USED AT PRESENT
#
# NOTE THAT THIS FLAG MUST BE SET = - BY THE STEERING PROGRAM IF IT IS TO COMMAND THE AUTOPILOT.
# SINCE ASTRONAUT ACTION MAY CHANGE THE HOLDFLAG SETTING, IT SHOULD BE MONITORED BY THE STEERING PROGRAM TO
# DETERMINE IF THE AUTOMATIC SEQUENCE HAS BEEN INTERRUPTED AND IF SO, TAKE THE APPROPRIATE ACTION.
@ -314,7 +310,7 @@ TENTHSEK TS ATTSEC
CS HOLDFLAG
EXTEND
BZMF DACNDLS # IF HOLDFLAG +0,-0,+, BYPASS AUTOMATIC
# COMMANDS.
# COMMANDS
DCDUINCR CAF TWO
DELOOP TS SPNDX
DOUBLE
@ -339,32 +335,32 @@ DELOOP TS SPNDX
# Page 1009
# RCS-CSM AUTOPILOT ATTITUDE ERROR DISPLAY
#
# THREE TYPES OF ATTITUDE ERRORS MAY BE DISPLAYED ON THE FDAI:
#
# THREE TYPES OF ATTITUDE ERRORS MAY BE DISPLAYED ON THE FDAI-
# MODE 1) AUTOPILOT FOLLOWING ERRORS SELECTED BY V61E
# GENERATED INTERNALLY BY THE AUTOPILOT
#
# MODE 2) TOTAL ATTITUDE ERRORS SELECTED BY V62E
# WITH RESPECT TO THE CONTENTS OF N22
#
# MODE 3) TOTAL ASTRONAUT ATTITUDE ERRORS SELECTED BY V63E
# WITH RESPECT TO THE CONTENTS OF N17
#
# MODE 1 IS PROVIDED AS A MONITOR OF THE RCS DAP AND ITS ABILITY TO TRACK AUTOMATIC STEERING COMMANDS. IN THIS
# MODE THE ATTITUDE ERRORS WILL BE ZEROED WHEN THE CMC MODE SWITCH IS IN FREE.
#
# MODE THE ATTITUDE ERRORS WILL BE ZEROED WHEN THE CMC MODE SWITCH IS IN FREE
# MODE 2 IS PROVIDED TO ASSIST THE CREW IN MANUALLY MANEUVERING THE S/C TO THE ATTITUDE (GIMBAL ANGLES) SPECIFIED
# IN N22. THE ATTITUDE ERRORS WRT THESE ANGLES AND THE CURRENT CDU ANGLES ARE RESOLVED INTO S/C CONTROL AXES
# AS A FLY-TO INDICATOR.
#
# MODE 3 IS PROVIDED TO ASSIST THE CREW IN MANUALLY MANEUVERING THE S/C TO THE ATTITUDE (GIMBAL ANGLES) SPECIFIED
# IN N17. THE ATTITUDE ERRORS WRT THESE ANGLES AND THE CURRENT CDU ANGLES ARE RESOLVED INTO S/C CONTROL AXES
# AS A FLY-TO INDICATOR.
#
# V60 IS PROVIDED TO LOAD N17 WITH A SNAPSHOT OF THE CURRENT CDU ANGLES, THUS SYNCHRONIZING THE MODE 3 DISPLAY
# WITH THE CURRENT S/C ATTITUDE. THIS VERB MAY BE USED AT ANY TIME.
#
# THESE DISPLAYS WILL BE AVAILABLE IN ANY MODE (AUTO, HOLD, FREE, G+N, OR SCS) ONCE THE RCS DAP HAS BEEN
# INITIATED VIA V46E. MODE 1, HOWEVER, WILL BE MEANINGFUL ONLY IN G+N AUTO OR HOLD. THE CREW MAY PRESET (VIA
# V25N17) AN ATTITUDE REFERENCE (DESIRED GIMBAL ANGLES) INTO N17 AT ANY TIME.
@ -380,43 +376,44 @@ KRESUME1 TCF RESUME # END PHASE 1
# Page 1010
# FDAI ATTITUDE ERROR DISPLAY SUBROUTINE
#
# PROGRAM DESCRIPTION: D. KEENE 5/24/67
#
# THIS SUBROUTINE IS USED TO DISPLAY ATTITUDE ERRORS ON THE FDAI VIA THE DIGITAL TO ANALOG CONVERTERS (DACS)
# IN THE CDUS. CARE IS TAKEN TO METER OUT THE APPROPRIATE NUMBER OF PULSES TO THE IMU ERROR COUNTERS AND PREVENT
# OVERFLOW, TO CONTROL THE RELAY SEQUENCING, AND TO AVOID INTERFERENCE WITH THE COARSE ALIGN LOOP WHICH ALSO USES
# THE DACS.
#
# CALLING SEQUENCE:
#
# DURING THE INITIALIZATION SECTION OF THE USER'S PROGRAM, BIT3 OF RCSFLAGS SHOULD BE SET TO INITIATE THE
# TURN-ON SEQUENCE WITHIN THE NEEDLES PROGRAM:
#
# CS RCSFLAGS # IN EBANK6
# MASK BIT3
# ADS RCSFLAGS
#
# THEREAFTER, THE ATTITUDE ERRORS GENERATED BY THE USER SHOULD BE TRANSFERED TO THE FOLLOWING LOCATIONS IN EBANK6:
#
# AK SCALED 180 DEGREES NOTE: THESE LOCATIONS ARE SUBJECT
# AK1 SCALED 180 DEGREES TO CHANGE
# AK2 SCALED 180 DEGREES
#
# FULL SCALED DEFLECTION CORRESPONDS TO 16 7/8 DEGREES OF ATTITUDE ERROR
# (= 384 BITS IN IMU ERROR COUNTER)
#
# A CALL TO NEEDLER WILL THEN UPDATE THE DISPLAY:
#
# INHINT
# TC IBNKCALL # NOTE: EBANK SHOULD BE SET TO E6
# CADR NEEDLER
# RELINT
#
# THIS PROCESS SHOULD BE REPEATED EACH TIME THE ERRORS ARE UPDATED. AT LEAST 3 PASSES THRU THE PRORAM ARE
# THIS PROCESS SHOULD BE REPEATED EACH TIME THE ERRORS ARE UPDATED. AT LEAST 3 PASSES THRU THE PROGRAM ARE
# REQUIRED BEFORE ANYTHING IS ACTUALLY DISPLAYED ON THE ERROR METERS.
# NOTE: EACH CALL TO NEEDLER MUST BE SEPARATED BY AT LEAST 50MS TO ASSURE PROPER RELAY SEQUENCING.
#
# ERASABLE USED:
# AK CDUXCMD
# AK1 CDUYCMD
@ -424,21 +421,23 @@ KRESUME1 TCF RESUME # END PHASE 1
# EDRIVEX A,L,Q
# EDRIVEY T5TEMP
# EDRIVEZ SPNDX
#
# SWITCHES; RCSFLAGS BITS 3,2
#
# SWITCHES: RCSFLAGS BITS 3,2
# I/O CHANNELS: CHAN12 BIT 4 (COARSE ALIGN - READ ONLY)
# Page 1011
# CHAN12 BIT 6 (IMU ERROR COUNTER ENABLE)
# CHAN14 BIT 13,14,15 (DAC ACTIVITY)
#
# SIGN CONVENTION: AK = THETAC - THETA
# SIGN CONVENTION< AK = THETAC - THETA
# WHERE THETAC = COMMAND ANGLE
# THETA = PRESENT ANGLE
NEEDLER CAF BIT4 # CHECK FOR COARSE ALIGN ENABLE
EXTEND # IF IN COARSE ALIGN DO NOT USE IMU
RAND CHAN12 # ERROR COUNTERS. DON'T USE NEEDLES
RAND CHAN12 # ERROR COUNTERS. DONT USE NEEDLES
EXTEND
BZF NEEDLER1
CS RCSFLAGS # SET BIT3 FOR INITIALIZATION PASS
@ -456,7 +455,7 @@ NEEDLER1 CA RCSFLAGS
CS BIT6 # FIRST PASS BIT3 = 1
EXTEND # DISABLE IMU ERROR COUNTER TO ZERO DACS
WAND CHAN12 # MUST WAIT AT LEAST 60 MS. BEFORE
WAND CHAN12 # MUST WAIT AT LEAST 60 MS BEFORE
NEEDLE11 CS ZERO # ENABLING COUNTERS.
TS AK # ZERO THE INPUTS ON FIRST PASS
TS AK1
@ -533,9 +532,9 @@ DACLIMIT DEC 16000
# Page 1014
# INITIALIZATION PROGRAM FOR RCS-CSM AUTOPILOT
#
# THE FOLLOWING QUANTITIES WILL BE ZEROED AND SHOULD APPEAR IN CONSECUTIVE LOCATIONS IN MEMORY AFTER WBODY
#
# WBODY (+1) DFT TAU2
# WBODY1 (+1) DFT1 BIAS
# WBODY2 (+1) DFT2 BIAS1
@ -545,7 +544,6 @@ DACLIMIT DEC 16000
# MERRORX (+1) ATTSEC ERRORZ
# MERRORY (+1) TAU
# MERRORZ (+1) TAU1
FRESHDAP CAF ONE # RESET HOLDFLAG TO STOP AUTOMATIC
TS HOLDFLAG # STEERING AND PREPARE TO PICK UP AN
# ATTITUDE HOLD REFERENCE
@ -582,7 +580,7 @@ ZEROT5 TS SPNDX # ZERO ALL NECESSARY ERASABLE REGISTERS
# MS FROM NOW AND EVERY 100MS THEREAFTER.
CAF ELEVEN
TS ATTKALMN # RESET TO PICK UP KALMAN FILTER TAINS
TS ATTKALMN # RESET TO PICK UP KALMAN FILTER GAINS
# TO INITIALIZE THE S/C ANGULAR RATES
# Page 1015
CA CDUX
@ -600,7 +598,7 @@ ZEROT5 TS SPNDX # ZERO ALL NECESSARY ERASABLE REGISTERS
TCF IMUAOK
TS ATTKALMN # CANNOT USE IMU
CAF RCSINITB # PROVIDE FREE CONTROL ONLY
TCF RCSSWIT # DON'T START UP RATE FILTER
TCF RCSSWIT # DONT START UP RATE FILTER
# SIGNAL NO RATE FILTER
IMUAOK CAF PRIO34 # START MATRIX INITIALIZATION
@ -608,10 +606,10 @@ IMUAOK CAF PRIO34 # START MATRIX INITIALIZATION
EBANK= KMPAC
2CADR AMBGUPDT
CAF RCSINIT # CLEAR BIT14 --ASSUME WE HAVE A GOOD IMU
RCSSWIT TS RCSFLAGS # CLEAR BIT1 --INITIALIZE T6 PROGRAM
# SET BIT3 --INITIALIZE NEEDLES
# CLEAR BIT4 --RESET FOR FDAIDSP1
CAF RCSINIT # CLEAR BIT14 -ASSUME WE HAVE A GOOD IMU
RCSSWIT TS RCSFLAGS # CLEAR BIT1 -INITIALIZE T6 PROGRAM
# SET BIT3 -INITIALIZE NEEDLES
# CLEAR BIT4 -RESET FOR FDAIDSP1
CAF T5WAIT60 # NEXT T5RUPT 60 MS FROM NOW TO ALLOW IMU
# ERROR COUNTER TO ZERO.
# (MINIMUM DELAY = 15 MS)
@ -632,7 +630,7 @@ T6ADDR 2CADR T6START
ZEROJET CAF ELEVEN # ZERO BLAST2, BLAST1, BLAST, YWORD2,
TS SPNDX # YWORD1, PWORD2, PWORD1, RWORD2,
CAF ZERO # AND RWORD1
CAF ZERO # AND RWORD1.
# Page 1016
INDEX SPNDX
@ -705,7 +703,7 @@ NOHIAUTO TS ATTKALMN
# ROLL, BITS 8,7 FOR YAW, BITS 6,5 FOR
# PITCH
CS RCSFLAGS # SET RATE DAMPING FLAGS (BITS 13,12, AND
CS RCSFLAGS # SET RATE DAMPING FLAGS (BITS 13,12,AND
MASK OCT16000 # 11 OF RCSFLAGS)
ADS RCSFLAGS
@ -721,7 +719,7 @@ NOCHANGE CS CH31TEMP
# MEANS OF STICKCHK, TO 0, 1, OR 2 FOR NO,
# +, OR - ROTATION RESPECTIVELY AS
# COMMANDED BY THE RHC.
#
# HOWEVER, IT IS WELL TO NOTE THAT AFTER
# THE RHC IS RETURNED TO DETENT, THE
# PROGRAM BRANCHES TO AHFNOROT AND AVOIDS
@ -792,7 +790,7 @@ NORATE ZL
MASK OCT16000
EXTEND # IS RATE DAMPING COMPLETED
BZF SPNDXCHK # YES, KEEP CURRENT MERRORX GO TO SPNDXCHK
TCF ZEROER # NO, GO TO ZEROER
TCF ZEROER # NO,GO TO ZEROER
MERUPDAT INDEX Q # MERRORX=MERRORX+MEASURED CHANGE IN ANGLE
CS MANTABLE -1 # -COMMANDED CHANGE IN ANGLE
@ -853,7 +851,7 @@ T6PROGM CAF ZERO # FOR MANUAL ROTATIONS
# Page 1021
DEC .2112 # FILTER GAIN FOR TRANSLATION, LEM ON
DEC .8400 # FILTER GAIN FOR TRANSLATION 2(ZETA)WN DT
DEC .2112 # FILTER GAIN FOR 4 DEGREE/SEC MANEUVERS
DEC .2112 # FILTER GAIN FOR 2 DEGREES/SEC MANEUVERS
GAIN1 DEC .0640 # KALMAN FILTER GAINS FOR INITIALIZATION
DEC .3180 # OF ATTITUDE RATES
DEC .3452
@ -868,7 +866,7 @@ GAIN1 DEC .0640 # KALMAN FILTER GAINS FOR INITIALIZATION
DEC .0174 # FILTER GAIN FOR TRANSLATION, LEM ON
DEC .3600 # FILTER GAIN FOR TRANSLATION (WN)(WN)DT
DEC .0174 # FILTER GAIN FOR 4 DEGREE/SEC MANEUVERS
DEC .0174 # FILTER GAIN FOR 2 DEGREES/SEC MANEUVERS
GAIN2 DEC .0016 # SCALED 10
DEC .0454
DEC .0545
@ -885,8 +883,8 @@ STICKCHK TS T5TEMP
TS PMANNDX
CA T5TEMP
EXTEND # MAN RATE 0 0 RATE (DP)
MP QUARTER # +1 + RATE (DP)
TS T5TEMP # +2 - RATE (DP)
MP QUARTER # +1 +RATE (DP)
TS T5TEMP # +2 -RATE (DP)
MASK THREE # (+3) 0 RATE (DP)
TS YMANNDX
CA T5TEMP
@ -894,6 +892,8 @@ STICKCHK TS T5TEMP
MP QUARTER
TS RMANNDX
TC Q
KALUPDT TS ATTKALMN # INITIALIZATION OF ATTITUDE RATES USING
# KALMAN FILTER TAKES 1.1 SEC
@ -918,7 +918,7 @@ FDAIDSP2 CS BIT4 # RESET FOR FDAIDSP1
EXTEND
BZF FDAITOTL
EXTEND
DCS ERRORX # OFF - DISPLAY AUTOPILOT FOLLOWING ERROR
DCS ERRORX # OFF -DISPLAY AUTOPILOT FOLLOWING ERROR
DXCH AK
CS ERRORZ
TS AK2
@ -973,4 +973,3 @@ WRTN17 EXTEND # GET SET TO COMPUTE TOTAL ASTRONAUT
TCF GETAKS
# Page 1024 (empty page)