2016-07-07 08:47:26 +00:00
|
|
|
# Copyright: Public domain.
|
|
|
|
|
# Filename: RCS-CSM_DIGITAL_AUTOPILOT.agc
|
|
|
|
|
# Purpose: Part of the source code for Colossus 2A, AKA Comanche 055.
|
|
|
|
|
# It is part of the source code for the Command Module's (CM)
|
|
|
|
|
# Apollo Guidance Computer (AGC), for Apollo 11.
|
|
|
|
|
# Assembler: yaYUL
|
|
|
|
|
# Contact: Ron Burkey <info@sandroid.org>.
|
|
|
|
|
# Website: www.ibiblio.org/apollo.
|
|
|
|
|
# Pages: 1002-1024
|
|
|
|
|
# Mod history: 2009-05-13 RSB Adapted from the Colossus249/ file of the
|
|
|
|
|
# same name, using Comanche055 page images.
|
|
|
|
|
#
|
|
|
|
|
# This source code has been transcribed or otherwise adapted from digitized
|
|
|
|
|
# images of a hardcopy from the MIT Museum. The digitization was performed
|
|
|
|
|
# by Paul Fjeld, and arranged for by Deborah Douglas of the Museum. Many
|
|
|
|
|
# thanks to both. The images (with suitable reduction in storage size and
|
|
|
|
|
# consequent reduction in image quality as well) are available online at
|
|
|
|
|
# www.ibiblio.org/apollo. If for some reason you find that the images are
|
|
|
|
|
# illegible, contact me at info@sandroid.org about getting access to the
|
|
|
|
|
# (much) higher-quality images which Paul actually created.
|
|
|
|
|
#
|
|
|
|
|
# Notations on the hardcopy document read, in part:
|
|
|
|
|
#
|
|
|
|
|
# Assemble revision 055 of AGC program Comanche by NASA
|
|
|
|
|
# 2021113-051. 10:28 APR. 1, 1969
|
|
|
|
|
#
|
|
|
|
|
# This AGC program shall also be referred to as
|
|
|
|
|
# Colossus 2A
|
|
|
|
|
|
|
|
|
|
# Page 1002
|
|
|
|
|
# T5 INTERRUPT PROGRAM FOR THE RCS-CSM AUTOPILOT
|
|
|
|
|
|
|
|
|
|
# START OF T5 INTERRUPT PROGRAM
|
|
|
|
|
|
|
|
|
|
BANK 20
|
|
|
|
|
SETLOC DAPS3
|
|
|
|
|
BANK
|
|
|
|
|
|
|
|
|
|
COUNT 21/DAPRC
|
|
|
|
|
|
|
|
|
|
EBANK= KMPAC
|
|
|
|
|
REDORCS LXCH BANKRUPT # RESTART OF AUTOPILOT COMES HERE
|
|
|
|
|
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
|
|
|
|
|
CS ONE
|
|
|
|
|
TS T5PHASE
|
|
|
|
|
EXTEND
|
|
|
|
|
DCA RCSLOC
|
|
|
|
|
DXCH T5LOC # HOOK UP T5RUPT TO AUTOPILOT
|
|
|
|
|
TCF RCSATT +1
|
|
|
|
|
EBANK= KMPAC
|
|
|
|
|
RCSLOC 2CADR RCSATT
|
|
|
|
|
|
|
|
|
|
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
|
|
|
|
|
RAND CHAN31 # PILOT IS FULLY ENABLED)
|
|
|
|
|
EXTEND
|
|
|
|
|
BZF SETT5 # IF G/C AUTOPILOT IS FULLY ENABLED,
|
|
|
|
|
# GO TO SETT5
|
|
|
|
|
|
|
|
|
|
CS RCSFLAGS # IF G/C AUTOPILOT IS NOT FULLY ENABLED,
|
|
|
|
|
MASK BIT14
|
|
|
|
|
ADS RCSFLAGS # SET NORATE FLAG,
|
|
|
|
|
CAF POSMAX
|
|
|
|
|
TS HOLDFLAG # SET HOLDFLAG +,
|
|
|
|
|
CAF ZERO # ZERO ERRORX, ERRORY, AND ERRORZ,
|
|
|
|
|
TS ERRORX
|
|
|
|
|
TS ERRORY
|
|
|
|
|
TS ERRORZ
|
|
|
|
|
CAF BIT14
|
|
|
|
|
EXTEND
|
|
|
|
|
RAND CHAN31 # AND CHECK FREE FUNCTION (BIT14 CHAN31).
|
|
|
|
|
EXTEND
|
|
|
|
|
# Page 1003
|
|
|
|
|
BZF SETT5 # IF IN FREE MODE, GO TO SETT5.
|
|
|
|
|
|
|
|
|
|
TS T5PHASE # IF NOT IN FREE MODE,
|
|
|
|
|
CAF OCT37766 # SCHEDULE REINITIALIZATION (FRESHDAP)
|
|
|
|
|
TS TIME5 # IN 100 MS VIA T5RUPT
|
|
|
|
|
|
|
|
|
|
TCR ZEROJET # ZERO JET CHANNELS IN 14 MS VIA ZEROJET
|
|
|
|
|
|
|
|
|
|
TCF KMATRIX
|
|
|
|
|
DELTATT OCT 37770 # 80MS (TIME5)
|
|
|
|
|
DELTATT2 OCT 37776 # 20MS (TIME5)
|
|
|
|
|
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
|
|
|
|
|
# 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
|
|
|
|
|
# -0 = PHASE1 OF THE T5 PROGRAM
|
|
|
|
|
|
|
|
|
|
SETT5 CCS T5PHASE
|
|
|
|
|
TCF FRESHDAP # TURN ON AUTOPILOT
|
|
|
|
|
TCF T5PHASE2 # BRANCH TO PHASE2 OF PROGRAM
|
|
|
|
|
TCF REDAP # RESTART AUTOPILOT
|
|
|
|
|
|
|
|
|
|
TS T5PHASE # PHASE 1 RESET FOR PHASE 2
|
|
|
|
|
CA TIME5
|
|
|
|
|
TS T5TIME # USED IN COMPENSATING FOR DELAYS IN T5
|
|
|
|
|
CAF DELTATT2 # RESET FOR T5RUPT IN 20MS FOR PHASE2
|
|
|
|
|
TS TIME5 # OF PROGRAM
|
|
|
|
|
|
|
|
|
|
# Page 1004
|
|
|
|
|
# IMU STATUS CHECK
|
|
|
|
|
|
|
|
|
|
CS IMODES33 # CHECK IMU STATUS
|
|
|
|
|
MASK BIT6 # BIT6 = 0 IMU OK
|
|
|
|
|
CCS A # BIT6 = 1 NO IMU
|
|
|
|
|
TCF RATEFILT
|
|
|
|
|
FREECHK CS RCSFLAGS # BIT14 INDICATES THAT RATES HAVE NOT BEEN
|
|
|
|
|
MASK BIT14 # INITIALIZED
|
|
|
|
|
ADS RCSFLAGS
|
|
|
|
|
CAF BIT14 # NO ATTITUDE REFERENCE
|
|
|
|
|
TS HOLDFLAG # STOP ANY AUTOMATIC STEERING AND PREPARE
|
|
|
|
|
# TO PICK UP CDU ANGLES UPON RESUMPTION OF
|
|
|
|
|
# ATTITUDE HOLD
|
|
|
|
|
EXTEND
|
|
|
|
|
RAND CHAN31 # CHECK FOR FREE MODE
|
|
|
|
|
EXTEND
|
|
|
|
|
BZF KRESUME1 # IN FREE MODE PROVIDE FREE CONTROL ONLY
|
|
|
|
|
TCF REINIT # .....TILT...............................
|
|
|
|
|
BITS4,5 OCT 30
|
|
|
|
|
|
|
|
|
|
RATEFILT CA RCSFLAGS # SEE IF RATEFILTER HAS BEEN INITIALIZED
|
|
|
|
|
MASK BIT14
|
|
|
|
|
EXTEND # IF SO, PROCEED WITH RATE DERIVATION
|
|
|
|
|
BZF +2
|
|
|
|
|
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
|
|
|
|
|
|
|
|
|
|
CAF TWO
|
|
|
|
|
DRHOLOOP TS SPNDX
|
|
|
|
|
DOUBLE
|
|
|
|
|
TS DPNDX
|
|
|
|
|
INDEX DPNDX
|
|
|
|
|
CS DRHO # DRHO SCALED 180 DEGS
|
|
|
|
|
EXTEND
|
|
|
|
|
INDEX ATTKALMN # PICK UP DESIRED FILTER GAIN
|
|
|
|
|
MP GAIN1
|
|
|
|
|
INDEX DPNDX
|
|
|
|
|
DAS DRHO # (1 -.064) DRHO
|
|
|
|
|
EXTEND
|
|
|
|
|
# Page 1005
|
|
|
|
|
INDEX DPNDX
|
|
|
|
|
DCS ADOT
|
|
|
|
|
DXCH KMPAC # -(.1)ADOT
|
|
|
|
|
CA QUARTER
|
|
|
|
|
TC SMALLMP
|
|
|
|
|
DXCH KMPAC
|
|
|
|
|
INDEX DPNDX
|
|
|
|
|
DAS DRHO
|
|
|
|
|
CCS SPNDX
|
|
|
|
|
TCF DRHOLOOP
|
|
|
|
|
|
|
|
|
|
CA CDUX # MEASURED BODY RATES--
|
|
|
|
|
XCH RHO
|
|
|
|
|
EXTEND
|
|
|
|
|
MSU RHO # _ * _ _
|
|
|
|
|
COM # DELRHO = AMGB (CDU - CDU )
|
|
|
|
|
# -1
|
|
|
|
|
ZL
|
|
|
|
|
DXCH DELTEMPX
|
|
|
|
|
CA CDUY
|
|
|
|
|
XCH RHO1
|
|
|
|
|
EXTEND
|
|
|
|
|
MSU RHO1
|
|
|
|
|
COM
|
|
|
|
|
TS T5TEMP # (CDUY - RHO1) SCALED 90 DEGS
|
|
|
|
|
EXTEND
|
|
|
|
|
MP AMGB1
|
|
|
|
|
DAS DELTEMPX # DELTEMPX = (CDUX-RHO) + AMGB1(CDUY-RHO1)
|
|
|
|
|
# MUST BE DOUBLE PRECISION OR WILL LOSE
|
|
|
|
|
# PULSES
|
|
|
|
|
CA AMGB4
|
|
|
|
|
EXTEND
|
|
|
|
|
MP T5TEMP
|
|
|
|
|
DXCH DELTEMPY
|
|
|
|
|
CA AMGB7
|
|
|
|
|
EXTEND
|
|
|
|
|
MP T5TEMP
|
|
|
|
|
DXCH DELTEMPZ
|
|
|
|
|
CA CDUZ
|
|
|
|
|
XCH RHO2
|
|
|
|
|
EXTEND
|
|
|
|
|
MSU RHO2
|
|
|
|
|
COM
|
|
|
|
|
TS T5TEMP # (CDUZ - RHO2) SCALED 90 DEGS
|
|
|
|
|
EXTEND
|
|
|
|
|
MP AMGB5
|
|
|
|
|
DAS DELTEMPY # DELTEMPY = AMGB4(CDUY-RHO1)
|
|
|
|
|
# + AMGB5(CDUZ-RHO2)
|
|
|
|
|
CA AMGB8
|
|
|
|
|
EXTEND
|
|
|
|
|
|
|
|
|
|
# Page 1006
|
|
|
|
|
MP T5TEMP
|
|
|
|
|
DAS DELTEMPZ # DELTEMPZ = AMBG7(CDUY-RHO1)
|
|
|
|
|
# + AMGB8(CDUZ-RHO2)
|
|
|
|
|
CAF TWO
|
|
|
|
|
ADOTLOOP TS SPNDX
|
|
|
|
|
DOUBLE
|
|
|
|
|
TS DPNDX
|
|
|
|
|
EXTEND
|
|
|
|
|
INDEX DPNDX
|
|
|
|
|
DCA DELTEMPX
|
|
|
|
|
INDEX DPNDX
|
|
|
|
|
DAS DRHO
|
|
|
|
|
EXTEND
|
|
|
|
|
INDEX DPNDX
|
|
|
|
|
DCA DELTEMPX
|
|
|
|
|
INDEX DPNDX
|
|
|
|
|
DAS MERRORX
|
|
|
|
|
INDEX DPNDX
|
|
|
|
|
CA DRHO
|
|
|
|
|
DOUBLE # N.B.
|
|
|
|
|
DOUBLE # N.B.
|
|
|
|
|
EXTEND
|
|
|
|
|
INDEX ATTKALMN # PICK UP DESIRED FILTER GAINS
|
|
|
|
|
MP GAIN2
|
|
|
|
|
INDEX DPNDX # ADOT + (.16)(.1)DRHO
|
|
|
|
|
DAS ADOT # -1
|
|
|
|
|
INDEX SPNDX # S/C TORQUE TO INERTIA RATIO
|
|
|
|
|
CA KMJ # SCALED (450)(1600)/(57.3)(16384)=1/1.3
|
|
|
|
|
EXTEND
|
|
|
|
|
INDEX SPNDX
|
|
|
|
|
MP DFT
|
|
|
|
|
INDEX DPNDX
|
|
|
|
|
DAS ADOT # KMJ(DFT)
|
|
|
|
|
CCS SPNDX
|
|
|
|
|
TCF ADOTLOOP # END CALCULATION OF VEHICLE RATES
|
|
|
|
|
KMATRIX CA ATTSEC
|
|
|
|
|
MASK LOW4
|
|
|
|
|
CCS A
|
|
|
|
|
TCF TENTHSEK
|
|
|
|
|
CAF PRIO34 # CALL FOR 1 SEC UPDATE OF TRANSFORMATION
|
|
|
|
|
TC NOVAC # MATRIX FROM GIMBAL AXES TO BODY AXES
|
|
|
|
|
EBANK= KMPAC
|
|
|
|
|
2CADR AMBGUPDT
|
|
|
|
|
|
|
|
|
|
CAF NINE
|
|
|
|
|
|
|
|
|
|
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:
|
|
|
|
|
# 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:
|
|
|
|
|
#
|
|
|
|
|
# + = GRAB PRESENT CDU ANGLES AND STORE IN THETADX, THETADY, THETADZ
|
|
|
|
|
# AND PERFORM ATTITUDE HOLD ABOUT THESE ANGLES
|
|
|
|
|
# ALSO IGNORE AUTOMATIC STEERING
|
|
|
|
|
# SET = + BY
|
|
|
|
|
# 1) INITIALIZATION PHASE OF AUTOPILOT
|
2016-07-12 23:52:11 +00:00
|
|
|
# 2) OCCURANCE OF RHC COMMANDS
|
2016-07-07 08:47:26 +00:00
|
|
|
# 3) FREE MODE
|
|
|
|
|
# 4) SWITCH OVER TO ATTITUDE HOLD FROM AUTO
|
|
|
|
|
# WHILE DOING AUTOMATIC STEERING (IN THIS CASE
|
|
|
|
|
# HOLDFLAG IS NOT ACTUALLY SET TO +, BUT THE LOGIC
|
|
|
|
|
# FUNCTIONS AS IF IT WERE.)
|
|
|
|
|
# 5) S/C CONTROL SWITCH IS SCS
|
|
|
|
|
# 6) IMU POWER OFF
|
|
|
|
|
# +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.
|
|
|
|
|
|
|
|
|
|
CS HOLDFLAG
|
|
|
|
|
EXTEND
|
|
|
|
|
BZMF DACNDLS # IF HOLDFLAG +0,-0,+, BYPASS AUTOMATIC
|
|
|
|
|
# COMMANDS.
|
|
|
|
|
DCDUINCR CAF TWO
|
|
|
|
|
DELOOP TS SPNDX
|
|
|
|
|
DOUBLE
|
|
|
|
|
TS DPNDX
|
|
|
|
|
EXTEND
|
|
|
|
|
INDEX A
|
|
|
|
|
DCA CDUXD
|
|
|
|
|
# Page 1008
|
|
|
|
|
DXCH KMPAC
|
|
|
|
|
EXTEND
|
|
|
|
|
INDEX DPNDX
|
|
|
|
|
DCA DELCDUX
|
|
|
|
|
TC DPADD
|
|
|
|
|
EXTEND
|
|
|
|
|
DCA KMPAC
|
|
|
|
|
INDEX SPNDX
|
|
|
|
|
TS THETADX
|
|
|
|
|
INDEX DPNDX
|
|
|
|
|
DXCH CDUXD
|
|
|
|
|
CCS SPNDX
|
|
|
|
|
TCF DELOOP
|
|
|
|
|
|
|
|
|
|
# Page 1009
|
|
|
|
|
# RCS-CSM AUTOPILOT ATTITUDE ERROR DISPLAY
|
|
|
|
|
#
|
|
|
|
|
# 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 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.
|
|
|
|
|
|
|
|
|
|
DACNDLS CS RCSFLAGS # ALTERNATE BETWEEN FDAIDSP1 AND FDAIDSP2
|
|
|
|
|
MASK BIT4
|
|
|
|
|
EXTEND
|
|
|
|
|
BZF FDAIDSP2
|
|
|
|
|
|
|
|
|
|
FDAIDSP1 ADS RCSFLAGS
|
|
|
|
|
TC NEEDLER
|
|
|
|
|
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
|
|
|
|
|
#
|
2016-07-12 23:52:11 +00:00
|
|
|
# THEREAFTER, THE ATTITUDE ERRORS GENERATED BY THE USER SHOULD BE TRANSFERED TO THE FOLLOWING LOCATIONS IN EBANK6:
|
2016-07-07 08:47:26 +00:00
|
|
|
#
|
|
|
|
|
# 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
|
|
|
|
|
# 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
|
|
|
|
|
# AK2 CDUZCMD
|
|
|
|
|
# EDRIVEX A,L,Q
|
|
|
|
|
# EDRIVEY T5TEMP
|
|
|
|
|
# EDRIVEZ SPNDX
|
|
|
|
|
#
|
|
|
|
|
# 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
|
|
|
|
|
# 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
|
|
|
|
|
EXTEND
|
|
|
|
|
BZF NEEDLER1
|
|
|
|
|
CS RCSFLAGS # SET BIT3 FOR INITIALIZATION PASS
|
|
|
|
|
MASK BIT3
|
|
|
|
|
ADS RCSFLAGS
|
|
|
|
|
TC Q
|
|
|
|
|
|
|
|
|
|
NEEDLER1 CA RCSFLAGS
|
|
|
|
|
MASK SIX
|
|
|
|
|
EXTEND
|
|
|
|
|
BZF NEEDLES3
|
|
|
|
|
MASK BIT3
|
|
|
|
|
EXTEND
|
|
|
|
|
BZF NEEDLER2 # BIT3 = 0, BIT2 = 1
|
|
|
|
|
|
|
|
|
|
CS BIT6 # FIRST PASS BIT3 = 1
|
|
|
|
|
EXTEND # DISABLE IMU ERROR COUNTER TO ZERO DACS
|
|
|
|
|
WAND CHAN12 # MUST WAIT AT LEAST 60 MS. BEFORE
|
|
|
|
|
NEEDLE11 CS ZERO # ENABLING COUNTERS.
|
|
|
|
|
TS AK # ZERO THE INPUTS ON FIRST PASS
|
|
|
|
|
TS AK1
|
|
|
|
|
TS AK2
|
|
|
|
|
TS EDRIVEX # ZERO THE DISPLAY REGISTERS
|
|
|
|
|
TS EDRIVEY
|
|
|
|
|
TS EDRIVEZ
|
|
|
|
|
TS CDUXCMD # ZERO THE OUT COUNTERS
|
|
|
|
|
TS CDUYCMD
|
|
|
|
|
TS CDUZCMD
|
|
|
|
|
CS SIX # RESET RCSFLAGS FOR PASS2
|
|
|
|
|
MASK RCSFLAGS
|
|
|
|
|
AD BIT2
|
|
|
|
|
TS RCSFLAGS
|
|
|
|
|
TC Q # END PASS1
|
|
|
|
|
|
|
|
|
|
NEEDLER2 CAF BIT6 # ENABLE IMU ERROR COUNTERS
|
|
|
|
|
EXTEND
|
|
|
|
|
WOR CHAN12
|
|
|
|
|
CS SIX # RESET RCSFLAGS TO DISPLAY ATTITUDE
|
|
|
|
|
# Page 1012
|
|
|
|
|
MASK RCSFLAGS # ERRORS WAIT AT LEAST 4 MS FOR
|
|
|
|
|
TS RCSFLAGS # RELAY CLOSURE
|
|
|
|
|
TC Q
|
|
|
|
|
|
|
|
|
|
NEEDLES3 CAF BIT6 # CHECK TO SEE IF IMU ERROR COUNTER
|
|
|
|
|
EXTEND # IS ENABLED
|
|
|
|
|
RAND CHAN12
|
|
|
|
|
EXTEND # IF NOT RECYCLE NEEDLES
|
|
|
|
|
BZF NEEDLER +5
|
|
|
|
|
|
|
|
|
|
NEEDLES CAF TWO
|
|
|
|
|
DACLOOP TS SPNDX
|
|
|
|
|
CS QUARTER
|
|
|
|
|
EXTEND
|
|
|
|
|
INDEX SPNDX
|
|
|
|
|
MP AK
|
|
|
|
|
TS L
|
|
|
|
|
CCS A
|
|
|
|
|
CA DACLIMIT
|
|
|
|
|
TCF +2
|
|
|
|
|
CS DACLIMIT
|
|
|
|
|
AD L
|
|
|
|
|
TS T5TEMP # OVFLO CHK
|
|
|
|
|
TCF +4
|
|
|
|
|
INDEX A # ON OVERFLOW LIMIT OUTPUT TO +-384
|
|
|
|
|
CAF DACLIMIT
|
|
|
|
|
TS L
|
|
|
|
|
INDEX SPNDX
|
|
|
|
|
CS EDRIVEX # CURRENT VALUE OF DAC
|
|
|
|
|
AD L
|
|
|
|
|
INDEX SPNDX
|
|
|
|
|
ADS CDUXCMD
|
|
|
|
|
INDEX SPNDX
|
|
|
|
|
LXCH EDRIVEX
|
|
|
|
|
CCS SPNDX
|
|
|
|
|
TCF DACLOOP
|
|
|
|
|
CAF 13,14,15
|
|
|
|
|
EXTEND
|
|
|
|
|
WOR CHAN14 # SET DAC ACTIVITY BITS
|
|
|
|
|
TC Q
|
|
|
|
|
|
|
|
|
|
REINIT CAF DELAY200 # ........TILT LOGIC
|
|
|
|
|
TS TIME5 # REINITIALIZE DAP IN 200MS
|
|
|
|
|
TS T5PHASE
|
|
|
|
|
TCF RESUME
|
|
|
|
|
DELAY200 DEC 16364 # 200MS
|
|
|
|
|
|
|
|
|
|
DEC -384
|
|
|
|
|
|
|
|
|
|
# Page 1013
|
|
|
|
|
DACLIMIT DEC 16000
|
|
|
|
|
DEC 384
|
|
|
|
|
|
|
|
|
|
# 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
|
|
|
|
|
# ADOT (+1) DRHO (+1) BIAS2
|
|
|
|
|
# ADOT1 (+1) DRHO1 (+1) ERRORX
|
|
|
|
|
# ADOT2 (+1) DRHO2 (+1) ERRORY
|
|
|
|
|
# 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
|
|
|
|
|
|
|
|
|
|
REDAP TC IBNKCALL # DECODE DAPDATR1, DAPDATR2 FOR DEADBANDS
|
|
|
|
|
CADR S41.2 # RATES, QUADFAILS, QUAD MANAGEMENT
|
|
|
|
|
|
|
|
|
|
TC IBNKCALL # DECODE IXX, IAVG, AND CONVERT
|
|
|
|
|
CADR S40.14 # TO AUTOPILOT GAINS
|
|
|
|
|
|
|
|
|
|
CAF NO.T5VAR # NO. LOCATIONS TO BE ZEROED MINUS ONE
|
|
|
|
|
ZEROT5 TS SPNDX # ZERO ALL NECESSARY ERASABLE REGISTERS
|
|
|
|
|
CAF ZERO
|
|
|
|
|
INDEX SPNDX
|
|
|
|
|
TS WBODY
|
|
|
|
|
CCS SPNDX
|
|
|
|
|
TCF ZEROT5
|
|
|
|
|
TCR ZEROJET
|
|
|
|
|
|
|
|
|
|
CS ZERO
|
|
|
|
|
TS CHANTEMP # INITIALIZE MINIMUM IMPULSE CONTROL
|
|
|
|
|
|
|
|
|
|
TS CH31TEMP # INITIALIZE RHC POSITION MEMORY FOR
|
|
|
|
|
# MANUAL RATE MODES
|
|
|
|
|
|
|
|
|
|
CAF =.24
|
|
|
|
|
TS SLOPE # INITIALIZE SWITCHING LOGIC SLOPE
|
|
|
|
|
|
|
|
|
|
CAF FOUR
|
|
|
|
|
TS T5TIME # PHASE 0 RESETS FOR PHASE 2 INTERRUPT IN
|
|
|
|
|
# 60 MS. PHASE 2 RESETS FOR PHASE 1 RUPT
|
|
|
|
|
# IN (80MS - T5TIME(40MS)). THEREFORE
|
|
|
|
|
# PHASE 1 (RATEFILTER) BEGINS CYCLING 100
|
|
|
|
|
# MS FROM NOW AND EVERY 100MS THEREAFTER.
|
|
|
|
|
|
|
|
|
|
CAF ELEVEN
|
|
|
|
|
TS ATTKALMN # RESET TO PICK UP KALMAN FILTER TAINS
|
|
|
|
|
# TO INITIALIZE THE S/C ANGULAR RATES
|
|
|
|
|
# Page 1015
|
|
|
|
|
CA CDUX
|
|
|
|
|
TS RHO
|
|
|
|
|
CA CDUY
|
|
|
|
|
TS RHO1
|
|
|
|
|
CA CDUZ
|
|
|
|
|
TS RHO2
|
|
|
|
|
CAF ZERO # RESET AUTOPILOT TO BEGIN EXECUTING
|
|
|
|
|
TS T5PHASE # PHASE2 OF PROGRAM
|
|
|
|
|
|
|
|
|
|
CS IMODES33 # CHECK IMU STATUS
|
|
|
|
|
MASK BIT6 # IF BIT6 = 0 IMU IN FINE ALIGN
|
|
|
|
|
CCS A # IF BIT6 = 1 IMU NOT READY
|
|
|
|
|
TCF IMUAOK
|
|
|
|
|
TS ATTKALMN # CANNOT USE IMU
|
|
|
|
|
CAF RCSINITB # PROVIDE FREE CONTROL ONLY
|
|
|
|
|
TCF RCSSWIT # DON'T START UP RATE FILTER
|
|
|
|
|
# SIGNAL NO RATE FILTER
|
|
|
|
|
|
|
|
|
|
IMUAOK CAF PRIO34 # START MATRIX INITIALIZATION
|
|
|
|
|
TC NOVAC # BYPASS IF IMU NOT IN FINE ALIGN
|
|
|
|
|
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 T5WAIT60 # NEXT T5RUPT 60 MS FROM NOW TO ALLOW IMU
|
|
|
|
|
# ERROR COUNTER TO ZERO.
|
|
|
|
|
# (MINIMUM DELAY = 15 MS)
|
|
|
|
|
TS TIME5 # SINCE ATTKALMN IS +11, PROGRAM WILL THEN
|
|
|
|
|
TC RESUME # PICK UP THE KALMAN FILTER GAINS. RATE
|
|
|
|
|
# FILTER WILL BEGIN OPERATING ZOOMS FROM
|
|
|
|
|
# NOW
|
|
|
|
|
|
|
|
|
|
# CONSTANTS USED IN INITIALIZATION PROGRAM
|
|
|
|
|
|
|
|
|
|
NO.T5VAR DEC 36
|
|
|
|
|
=.24 DEC .24 # = SLOPE OF 0.6/SEC
|
|
|
|
|
RCSINIT OCT 00004
|
|
|
|
|
RCSINITB OCT 20004
|
|
|
|
|
T5WAIT60 DEC 16378 # = 6 CS
|
|
|
|
|
EBANK= KMPAC
|
|
|
|
|
T6ADDR 2CADR T6START
|
|
|
|
|
|
|
|
|
|
ZEROJET CAF ELEVEN # ZERO BLAST2, BLAST1, BLAST, YWORD2,
|
|
|
|
|
TS SPNDX # YWORD1, PWORD2, PWORD1, RWORD2,
|
|
|
|
|
CAF ZERO # AND RWORD1
|
|
|
|
|
|
|
|
|
|
# Page 1016
|
|
|
|
|
INDEX SPNDX
|
|
|
|
|
TS RWORD1
|
|
|
|
|
CCS SPNDX
|
|
|
|
|
TCF ZEROJET +1
|
|
|
|
|
|
|
|
|
|
CAF FOUR
|
|
|
|
|
TS BLAST1 +1
|
|
|
|
|
CAF ELEVEN
|
|
|
|
|
TS BLAST2 +1
|
|
|
|
|
|
|
|
|
|
CS BIT1
|
|
|
|
|
MASK RCSFLAGS
|
|
|
|
|
TS RCSFLAGS # RESET BIT1 OF RCSFLAGS TO 0
|
|
|
|
|
|
|
|
|
|
EXTEND
|
|
|
|
|
DCA T6ADDR
|
|
|
|
|
DXCH T6LOC
|
|
|
|
|
CAF =+14MS # ENABLE T6RUPT TO SHUT OFF JETS IN 14 MS.
|
|
|
|
|
TS TIME6
|
|
|
|
|
CAF BIT15
|
|
|
|
|
EXTEND
|
|
|
|
|
WOR CHAN13
|
|
|
|
|
|
|
|
|
|
TC Q
|
|
|
|
|
|
|
|
|
|
T5PHASE2 CCS ATTKALMN # IF (+) INITIALIZE RATE ESTIMATE
|
|
|
|
|
TCF KALUPDT
|
|
|
|
|
|
|
|
|
|
TCF +2 # ONLY IF ATTKALMN POSITIVE
|
|
|
|
|
TCF +1
|
|
|
|
|
CA DELTATT2 # RESET FOR PHASE3 IN 20 MS
|
|
|
|
|
XCH TIME5 # (JET SELECTION LOGIC)
|
|
|
|
|
ADS T5TIME # TO COMPENSATE FOR DELAYS IN T5RUPT
|
|
|
|
|
|
|
|
|
|
CA RCSFLAGS # IF A HIGH RATE AUTO MANEUVER IS IN
|
|
|
|
|
MASK BIT15 # PROGRESS (BIT 15 OF RCSFLAGS SET), SET
|
|
|
|
|
EXTEND # ATTKALMN TO -1
|
|
|
|
|
BZF NOHIAUTO # OTHERWISE SET ATTKALMN TO 0.
|
|
|
|
|
CS ONE
|
|
|
|
|
NOHIAUTO TS ATTKALMN
|
|
|
|
|
|
|
|
|
|
# Page 1017
|
|
|
|
|
# MANUAL ROTATION COMMANDS
|
|
|
|
|
|
|
|
|
|
CS OCT01760 # RESET FORCED FIRING BITS (BITS 10 TO 5
|
|
|
|
|
MASK RCSFLAGS # OF RCSFLAGS) TO ZERO
|
|
|
|
|
TS RCSFLAGS
|
|
|
|
|
|
|
|
|
|
EXTEND
|
|
|
|
|
READ CHAN31
|
|
|
|
|
TS L
|
|
|
|
|
CA CH31TEMP
|
|
|
|
|
EXTEND
|
|
|
|
|
RXOR LCHAN
|
|
|
|
|
MASK MANROT # = OCT00077
|
|
|
|
|
EXTEND
|
|
|
|
|
BZMF NOCHANGE
|
|
|
|
|
|
|
|
|
|
LXCH A
|
|
|
|
|
TS CH31TEMP # SAVE CONTENTS OF CHANNEL 31 IN CH31TEMP
|
|
|
|
|
|
|
|
|
|
CA L
|
|
|
|
|
EXTEND
|
|
|
|
|
MP BIT5 # PUT BITS 6-1 OF A IN BITS 10-5 OF L
|
|
|
|
|
CA L
|
|
|
|
|
ADS RCSFLAGS # SET FORCED FIRING BITS FOR AXES WITH
|
|
|
|
|
# CHANGES IN COMMAND. BITS 10,9 FOR
|
|
|
|
|
# ROLL, BITS 8,7 FOR YAW, BITS 6,5 FOR
|
|
|
|
|
# PITCH
|
|
|
|
|
|
|
|
|
|
CS RCSFLAGS # SET RATE DAMPING FLAGS (BITS 13,12, AND
|
|
|
|
|
MASK OCT16000 # 11 OF RCSFLAGS)
|
|
|
|
|
ADS RCSFLAGS
|
|
|
|
|
|
|
|
|
|
NOCHANGE CS CH31TEMP
|
|
|
|
|
MASK MANROT
|
|
|
|
|
EXTEND
|
|
|
|
|
BZMF AHFNOROT # IF NO MANUAL COMMANDS, GO TO AHFNOROT
|
|
|
|
|
|
|
|
|
|
TS HOLDFLAG # SET HOLDFLAG +
|
|
|
|
|
|
|
|
|
|
TC STICKCHK # WHEN THE RHC IS OUT OF DETENT, PMANNDX,
|
|
|
|
|
# YMANNDX, AND RMANNDX ARE ALL SET, BY
|
|
|
|
|
# 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
|
|
|
|
|
# STICKCHK SO PMANNDX, YMANNDX, AND
|
|
|
|
|
# RMANNDX ARE NOT RESET TO ZERO BUT RATHER
|
|
|
|
|
# LEFT SET TO THEIR LAST OUT OF DETENT
|
|
|
|
|
# Page 1018
|
|
|
|
|
# VALUES.
|
|
|
|
|
|
|
|
|
|
CS FLAGWRD1 # SET STIKFLAG TO INFORM STEERING
|
|
|
|
|
MASK BIT14 # PROGRAMS (P20) THAT ASTRONAUT HAS
|
|
|
|
|
ADS FLAGWRD1 # ASSUMED ROTATIONAL CONTROL OF SPACECRAFT
|
|
|
|
|
|
|
|
|
|
CAF BIT14
|
|
|
|
|
EXTEND
|
|
|
|
|
RAND CHAN31
|
|
|
|
|
EXTEND
|
|
|
|
|
BZMF FREEFUNC
|
|
|
|
|
|
|
|
|
|
CA RCSFLAGS # EXAMINE RCSFLAGS TO SEE IF RATE FILTER
|
|
|
|
|
MASK BIT14 # HAS BEEN INITIALIZED
|
|
|
|
|
CCS A # IF SO, PROCEED WITH MANUAL RATE COMMANDS
|
|
|
|
|
TCF REINIT # .....TILT, RECYCLE TO INITIALIZE FILTER
|
|
|
|
|
|
|
|
|
|
CS FIVE # IF MANUAL MANEUVER IS AT HIGH RTE, SET
|
|
|
|
|
AD RATEINDX # ATTKALMN TO -1.
|
|
|
|
|
EXTEND # OTHERWISE, LEAVE ATTKALMN ALONE.
|
|
|
|
|
BZMF +3
|
|
|
|
|
CS ONE
|
|
|
|
|
TS ATTKALMN
|
|
|
|
|
|
|
|
|
|
CAF TWO # AUTO-HOLD MANUAL ROTATION
|
|
|
|
|
SETWBODY TS SPNDX
|
|
|
|
|
DOUBLE
|
|
|
|
|
TS DPNDX
|
|
|
|
|
INDEX SPNDX # RMANNDX = 0 NO ROTATION
|
|
|
|
|
CA RMANNDX # = 1 + ROTATION
|
|
|
|
|
EXTEND # = 2 - ROTATION
|
|
|
|
|
BZF NORATE # IF NO ROTATION COMMAND ON THIS AXIS,
|
|
|
|
|
# GO TO NORATE.
|
|
|
|
|
|
|
|
|
|
AD RATEINDX # RATEINDX = 0 0.05 DEG/SEC
|
|
|
|
|
TS Q # = 2 0.2 DEG/SEC
|
|
|
|
|
INDEX Q # = 4 0.5 DEG/SEC
|
|
|
|
|
CA MANTABLE -1 # = 6 2.0 DEG/SEC
|
|
|
|
|
EXTEND
|
|
|
|
|
MP BIT9 # MULTIPLY MANTABLE BY 2 TO THE -6
|
|
|
|
|
INDEX DPNDX # TO GET COMMANDED RATE.
|
|
|
|
|
DXCH WBODY # SET WBODY TO COMMANDED RATE.
|
|
|
|
|
|
|
|
|
|
CA RCSFLAGS
|
|
|
|
|
MASK OCT16000 # IS RATE DAMPING COMPLETED (BITS 13,12 AND
|
|
|
|
|
EXTEND # 11 OF RCSFLAGS ALL ZERO.) IF SO, GO TO
|
|
|
|
|
BZF MERUPDAT # MERUPDAT TO UPDATE CUMULATIVE ATTITUDE
|
|
|
|
|
# ERROR.
|
|
|
|
|
|
|
|
|
|
# Page 1019
|
|
|
|
|
ZEROER CA ZERO # ZEROER ZEROS MERRORS
|
|
|
|
|
ZL
|
|
|
|
|
INDEX DPNDX
|
|
|
|
|
DXCH MERRORX
|
|
|
|
|
TCF SPNDXCHK
|
|
|
|
|
|
|
|
|
|
NORATE ZL
|
|
|
|
|
INDEX DPNDX
|
|
|
|
|
DXCH WBODY # ZERO WBODY FOR THIS AXIS
|
|
|
|
|
CA RCSFLAGS
|
|
|
|
|
MASK OCT16000
|
|
|
|
|
EXTEND # IS RATE DAMPING COMPLETED
|
|
|
|
|
BZF SPNDXCHK # YES, KEEP CURRENT MERRORX GO TO SPNDXCHK
|
|
|
|
|
TCF ZEROER # NO, GO TO ZEROER
|
|
|
|
|
|
|
|
|
|
MERUPDAT INDEX Q # MERRORX=MERRORX+MEASURED CHANGE IN ANGLE
|
|
|
|
|
CS MANTABLE -1 # -COMMANDED CHANGE IN ANGLE
|
|
|
|
|
EXTEND # THE ADDITION OF MEASURED CHANGE IN ANGLE
|
|
|
|
|
MP BIT7 # HAS ALREADY BEEN DONE IN THE RATE FILTER
|
|
|
|
|
INDEX DPNDX # COMMANDED CHANGE IN ANGLE = WBODY TIMES
|
|
|
|
|
DAS MERRORX # .1SEC = MANTABLE ENTRY TIMES 2 TO THE -8
|
|
|
|
|
|
|
|
|
|
SPNDXCHK INDEX DPNDX
|
|
|
|
|
CA MERRORX
|
|
|
|
|
INDEX SPNDX
|
|
|
|
|
TS ERRORX # ERRORX = HIGH ORDER WORD OF MERRORX
|
|
|
|
|
CCS SPNDX
|
|
|
|
|
TCF SETWBODY
|
|
|
|
|
TCF JETS
|
|
|
|
|
|
|
|
|
|
OCT01760 OCT 01760 # FORCED FIRING BITS MASK
|
|
|
|
|
|
|
|
|
|
OCT01400 OCT 01400 # ROLL FORCED FIRING MASK ORDER OF
|
|
|
|
|
OCT00060 OCT 00060 # PITCH FORCED FIRING MASK DEFINITION
|
|
|
|
|
OCT00300 OCT 00300 # YAW FORCED FIRING MASK MUST BE
|
|
|
|
|
# PRESERVED
|
|
|
|
|
# FOR INDEXING
|
|
|
|
|
MANROT OCT 77
|
|
|
|
|
OCT16000 OCT 16000 # RATE DAMPING FLAGS MASK
|
|
|
|
|
MANTABLE DEC .0071111
|
|
|
|
|
DEC -.0071111
|
|
|
|
|
DEC .028444
|
|
|
|
|
DEC -0.028444
|
|
|
|
|
DEC .071111
|
|
|
|
|
DEC -.071111
|
|
|
|
|
DEC .284444
|
|
|
|
|
DEC -.284444
|
|
|
|
|
=+14MS DEC 23
|
|
|
|
|
FREEFUNC INDEX RMANNDX # ACCELERATION
|
|
|
|
|
# Page 1020
|
|
|
|
|
CA FREETAU # COMMANDS
|
|
|
|
|
TS TAU
|
|
|
|
|
INDEX PMANNDX
|
|
|
|
|
CA FREETAU # FREETAU 0 SEC
|
|
|
|
|
TS TAU1 # +1 +0.10 SEC
|
|
|
|
|
INDEX YMANNDX # +2 -0.10 SEC
|
|
|
|
|
CA FREETAU # (+3) 0 SEC
|
|
|
|
|
TS TAU2
|
|
|
|
|
TCF T6PROGM
|
|
|
|
|
|
|
|
|
|
FREETAU DEC 0
|
|
|
|
|
DEC 480
|
|
|
|
|
DEC -480
|
|
|
|
|
DEC 0
|
|
|
|
|
|
|
|
|
|
T6PROGM CAF ZERO # FOR MANUAL ROTATIONS
|
|
|
|
|
TS ERRORX
|
|
|
|
|
TS ERRORY
|
|
|
|
|
TS ERRORZ
|
|
|
|
|
TCF T6PROG
|
|
|
|
|
|
|
|
|
|
# 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
|
|
|
|
|
GAIN1 DEC .0640 # KALMAN FILTER GAINS FOR INITIALIZATION
|
|
|
|
|
DEC .3180 # OF ATTITUDE RATES
|
|
|
|
|
DEC .3452
|
|
|
|
|
DEC .3774
|
|
|
|
|
DEC .4161
|
|
|
|
|
DEC .4634
|
|
|
|
|
DEC .5223
|
|
|
|
|
DEC .5970
|
|
|
|
|
DEC .6933
|
|
|
|
|
DEC .8151
|
|
|
|
|
DEC .9342
|
|
|
|
|
|
|
|
|
|
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
|
|
|
|
|
GAIN2 DEC .0016 # SCALED 10
|
|
|
|
|
DEC .0454
|
|
|
|
|
DEC .0545
|
|
|
|
|
DEC .0666
|
|
|
|
|
DEC .0832
|
|
|
|
|
DEC .1069
|
|
|
|
|
DEC .1422
|
|
|
|
|
DEC .1985
|
|
|
|
|
DEC .2955
|
|
|
|
|
DEC .4817
|
|
|
|
|
DEC .8683
|
|
|
|
|
STICKCHK TS T5TEMP
|
|
|
|
|
MASK THREE # INDICES FOR MANUAL ROTATION
|
|
|
|
|
TS PMANNDX
|
|
|
|
|
CA T5TEMP
|
|
|
|
|
EXTEND # MAN RATE 0 0 RATE (DP)
|
|
|
|
|
MP QUARTER # +1 + RATE (DP)
|
|
|
|
|
TS T5TEMP # +2 - RATE (DP)
|
|
|
|
|
MASK THREE # (+3) 0 RATE (DP)
|
|
|
|
|
TS YMANNDX
|
|
|
|
|
CA T5TEMP
|
|
|
|
|
EXTEND
|
|
|
|
|
MP QUARTER
|
|
|
|
|
TS RMANNDX
|
|
|
|
|
TC Q
|
|
|
|
|
KALUPDT TS ATTKALMN # INITIALIZATION OF ATTITUDE RATES USING
|
|
|
|
|
# KALMAN FILTER TAKES 1.1 SEC
|
|
|
|
|
|
|
|
|
|
CA DELTATT # =1SEC - 80MS
|
|
|
|
|
AD T5TIME # + DELAYS
|
|
|
|
|
# Page 1022
|
|
|
|
|
TS TIME5
|
|
|
|
|
TCF +3
|
|
|
|
|
CAF DELTATT2 # SAFETY PLAY TO ASSURE
|
|
|
|
|
TS TIME5 # A T5RUPT
|
|
|
|
|
|
|
|
|
|
KRESUME2 CS ZERO # RESET FOR PHASE1
|
|
|
|
|
TS T5PHASE # RESUME INTERRUPTED PROGRAM
|
|
|
|
|
TCF RESUME
|
|
|
|
|
|
|
|
|
|
FDAIDSP2 CS BIT4 # RESET FOR FDAIDSP1
|
|
|
|
|
MASK RCSFLAGS
|
|
|
|
|
TS RCSFLAGS
|
|
|
|
|
|
|
|
|
|
CS FLAGWRD0 # ON - DISPLAY ONE OF THE TOTAL ATTITUDE
|
|
|
|
|
MASK BIT9 # ERRORS
|
|
|
|
|
EXTEND
|
|
|
|
|
BZF FDAITOTL
|
|
|
|
|
EXTEND
|
|
|
|
|
DCS ERRORX # OFF - DISPLAY AUTOPILOT FOLLOWING ERROR
|
|
|
|
|
DXCH AK
|
|
|
|
|
CS ERRORZ
|
|
|
|
|
TS AK2
|
|
|
|
|
TCF RESUME # END PHASE 1
|
|
|
|
|
|
|
|
|
|
FDAITOTL CA FLAGWRD9
|
|
|
|
|
MASK BIT6
|
|
|
|
|
EXTEND
|
|
|
|
|
BZF WRTN17 # IS N22ORN17 (BIT6 OF FLAGWRD9) = 0
|
|
|
|
|
# IF SO, GO TO WRTN17
|
|
|
|
|
WRTN22 EXTEND # OTHERWISE, CONTINUE ON TO WRTN22 AND
|
|
|
|
|
DCA CTHETA # GET SET TO COMPUTE TOTAL ATTITUDE
|
|
|
|
|
DXCH WTEMP # ERROR WRT N22 BY PICKING UP THE THREE
|
|
|
|
|
CA CPHI # COMPONENTS OF N22
|
|
|
|
|
|
|
|
|
|
GETAKS EXTEND # COMPUTE TOTAL ATTITUDE ERROR FOR
|
|
|
|
|
MSU CDUX # DISPLAY ON FDAI ERROR NEEDLES
|
|
|
|
|
TS AK
|
|
|
|
|
CA WTEMP
|
|
|
|
|
EXTEND
|
|
|
|
|
MSU CDUY
|
|
|
|
|
TS T5TEMP
|
|
|
|
|
EXTEND
|
|
|
|
|
MP AMGB1
|
|
|
|
|
ADS AK
|
|
|
|
|
CA T5TEMP
|
|
|
|
|
EXTEND
|
|
|
|
|
MP AMGB4
|
|
|
|
|
# Page 1023
|
|
|
|
|
TS AK1
|
|
|
|
|
CA T5TEMP
|
|
|
|
|
EXTEND
|
|
|
|
|
MP AMGB7
|
|
|
|
|
TS AK2
|
|
|
|
|
CA WTEMP +1
|
|
|
|
|
EXTEND
|
|
|
|
|
MSU CDUZ
|
|
|
|
|
TS T5TEMP
|
|
|
|
|
EXTEND
|
|
|
|
|
MP AMGB5
|
|
|
|
|
ADS AK1
|
|
|
|
|
CA T5TEMP
|
|
|
|
|
EXTEND
|
|
|
|
|
MP AMGB8
|
|
|
|
|
ADS AK2
|
|
|
|
|
TCF RESUME # END PHASE1 OF RCS DAP
|
|
|
|
|
|
|
|
|
|
WRTN17 EXTEND # GET SET TO COMPUTE TOTAL ASTRONAUT
|
|
|
|
|
DCA CPHIX +1 # ATTITUDE ERROR WRT N17 BY PICKING UP
|
|
|
|
|
DXCH WTEMP # THE THREE COMPONENTS OF N17
|
|
|
|
|
CA CPHIX
|
|
|
|
|
TCF GETAKS
|
|
|
|
|
|
|
|
|
|
# Page 1024 (empty page)
|
|
|
|
|
|
