# 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
# 2) OCCURANCE OF RHC COMMANDS
# 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

# 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 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
# 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.  DONT 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 GAINS
     # 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  # DONT 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 2 DEGREES/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 2 DEGREES/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)