# Copyright: Public domain.
# Filename: TVCDAPS.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: 961-978
# Mod history: 2009-05-13 RSB Adapted from the Colossus249/ file of the
# same name, using Comanche055 page images.
# 2009-05-20 RSB Corrections:  Eliminated an extraneous EXTEND,
# added a missing instruction to PFORWARD.
# 2000-05-21 RSB Wrong opcode was used with DELBRTMP and
# DELBRTMP +1 operands in 4 places.  Corrected
# an MP operation in 2CASFLTR.
#
# 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 961
# PROGRAM NAME....TVCDAP, CONSISTING OF PITCHDAP, YAWDAP, ETC.
# LOG SECTION...TVCDAPS SUBROUTINE...DAPCSM
# MODIFIED BY SCHLUNDT 21 OCTOBER 1968
# FUNCTIONAL DESCRIPTION....

# SELF-PERPETUATING T5 TASKS WHICH GENERATE THE COMMAND SIGNALS
# FOR THE PITCH AND YAW SPS GIMBAL ACTUATORS DURING TVC (SPS) BURNS,
# IN RESPONSE TO BODY-AXIS RATE COMMANDS FROM CROSS-PRODUCT STEERING
# (S40.8).  IF NO STEERING (IMPULSIVE BURNS) MAINTAINS ATTITUDE-HOLD
# ABOUT THE REFERENCE (INITIAL) DIRECTIONS (ZERO RATE COMMANDS).

# THE PITCH AND YAW LOOPS ARE SEPARATE, BUT STRUCTURED IDENTICALLY.
# EACH ATTITUDE-RATE LOOP INCLUDES GIMBAL ANGLE RATE DERIVATION,
# GIMBAL/BODY AXIS TRANSFORMATION, BODY-AXIS ATTITUDE ERROR
# INTEGRATION WITH ERROR LIMITING, THE GENERALIZED 6TH-ORDER FILTER
# FOR CSM OR CSM/LM OPERATION. A FILTER OUTPUT LIMITER.
# CG-OFFSET TRACKER FILTER, AND THE CG-TRACKER MINOR LOOP.

# THE DAPS ARE CYCLIC, CALLING EACH OTHER AT 1/2 THE DAP SAMPLE
# TIME, AS DETERMINED BY T5TVCDT.  THE ACTUATOR COMMANDS ARE
# REGENERATED AS ANALOG VOLTAGES BY THE OPTICS ERROR COUNTERS, WHICH
# TRANSMIT THE SIGNAL TO THE ACTUATOR SERVOS WHEN THERE IS PROPER CDU
# MODING.

# CALLING SEQUENCE.... (TYPICALLY)

# T5 CALL OF TVCDAPON (TVCINITIALIZE) BY DOTVCON (P40)
# T5 CALL OF DAPINIT (TVCDAPS) BY TVCINIT4 (TVCINITIALIZE)
# T5 CALL OF PITCHDAP BY DAPINIT
# T5 CALL OF YAWDAP BY PITCHDAP
# T5 CALL OF PITCHDAP BY YAWDAP
#    ETC.
# (AUTOMATIC SEQUENCING FROM TVCDAPON)

# NORMAL EXIT MODE....RESUME

# ALARM OR ABORT EXIT MODES....NONE

# SUBROUTINES CALLED....

# HACK FOR STROKE TEST (V68) WAVEFORM GENERATION
# PCOPY, YCOPY FOR COPY-CYCLES (USED ALSO BY TVC RESTART PACKAGE)
# DAPINIT FOR INITIAL CDUS FOR RATE MEASUREMENTS
# ERRORLIM, ACTLIM FOR INPUT (ATTITUDE-ERROR INTEGRATION) AND
# OUTPUT (ACTUATOR COMMAND) LIMITING, COMMON TO PITCH AND
# YAW DAPS
# FWDFLTR (INCLUDING OPTVARK) AND PRECOMP, TO COMPUTE FILTER
# OUTPUTS AND STORAGE VALUES
# RESUME

# Page 962
# OTHER INTERFACES....

# S40.8 CROSS-PRODUCT STEERING FOR BODY AXIS RATE COMMANDS OMEGAY,ZC
# S40.15 FOR THE INITIAL DAP GAINS VARK AND 1/CONACC
# TVCEXECUTIVE FOR DAP GAIN UPDATES AND TMC LOOP OPERATIONS
# TVCRESTART PACKAGE FOR TVC RESTART PROTECTION.

# ERASABLE INITIALIZATION REQUIRED....

#  PAD-LOAD ERASABLES ( SEE ERASABLE ASSIGNMENTS )
# CONFIGURATION BITS (14, 13) OF DAPDATR1 AS IN R03
# ENGINE-ON BIT (11.13) FOR RESTARTS
# TVCPHASE FOR RESTARTS ( SEE DOTVCON, AND TVCINIT4 )
# T5 BITS (15,14 OF FLAGWRD6) FOR RESTARTS
# MISCELLANEOUS VARIABLES SET UP OR COMPUTED BY TVCDAPON....TVCINIT4,
# INCLUDING THE ZEROING OF TEMPORARIES BY MRCLEAN
# CDUX,Y,Z AND SINCDUX....COSCDUX AS PREPARED BY QUICTRIG (WITH
# UPDATES EVERY 1/2 SECOND)
# ALSO G+N PRIMARY, TVC ENABLE, AND OPTICS ERROR COUNTER ENABLE
# UNLESS BENCH-TESTING.
#
# OUTPUT....
#
# TVCPITCH AND TVCYAW WITH COUNTER RELEASE (11.14 AND 11.13 INCREMEN-
# TAL COMMANDS TO OPTICS ERROR COUNTERS), FILTER NODES, BODY-
# AXIS ATTITUDE ERROR INTEGRATOR, TOTAL ACTUATOR COMMANDS,
# OFFSET-TRACKER-FILTER OUTPUTS, ETC.
# DEBRIS....

# MUCH, SHAREABLE WITH RCS/ENTRY, IN EBANK6 ONLY

  BANK 17
  SETLOC DAPS2
  BANK

  EBANK= BZERO

  COUNT* $$/DAPS

# Page 963
# PITCH TVCDAP STARTS HERE....(INCORPORATES CSM/LEM DAP FILTER, MODOR DESIGN)

PITCHDAP LXCH BANKRUPT # T5 ENTRY, NORMAL OR VIA DAPINIT
  EXTEND
  QXCH QRUPT

  CAF YAWT5  # SET UP T5 CALL FOR YAW AUTOPILOT (LOW-
  TS T5LOC  # ORDER PART OF 2CADR ALREADY THERE)
  CAE T5TVCDT
  TS TIME5

PSTROKER CCS STROKER  # (STRKFLG) CHECK FOR STROKE TEST
  TC HACK  # TEST-START OR TEST-IN-PROGRESS
  TCF +2  # NO-TEST
  TC HACK  # TEST-IN-PROGRESS

PCDUDOTS CAE CDUY  # COMPUTE CDUYDOT (USED BY PITCH AND YAW)
  XCH PCDUYPST
  EXTEND
  MSU PCDUYPST
  TCR RLIMTEST # RATE TEST
  TS MCDUYDOT # (MINUS, SC.AT 1/2TVCDT REVS/SEC)

  CAE CDUZ  # COMPUTE CDUZDOT (USED BY PITCH AND YAW)
  XCH PCDUZPST
  EXTEND
  MSU PCDUZPST
  TCR RLIMTEST # RATE TEST
  TS MCDUZDOT # (MINUS, SC.AT 1/2TVCDT REVS/SEC)
  TCF PINTEGRL

RLIMTEST TS TTMP1  # TEST FOR EXCESSIVE CDU RATES (GREATER
  EXTEND   # THAN 2.33 DEG IN ONE SAMPLE PERIOD)
  MP 1/RTLIM
  EXTEND
  BZF +3
  CAF ZERO
  TS TTMP1
  CAE TTMP1
  TC Q

PINTEGRL EXTEND   # COMPUTE INTEGRAL OF BODY-AXIS PITCH-RATE
  DCA PERRB  # ERROR, SC.AT B-1 REVS
  DXCH ERRBTMP

  EXTEND
  DCA OMEGAYC
  DAS ERRBTMP

# Page 964
  CS COSCDUZ  # PREPARE BODY-AXIS PITCH RATE, OMEGAYB
  EXTEND
  MP COSCDUX
  DDOUBL
  EXTEND
  MP MCDUYDOT
  DDOUBL
  DXCH OMEGAYB

  CS MCDUZDOT
  EXTEND
  MP SINCDUX
  DDOUBL
  DAS OMEGAYB  # (COMPLETED OMEGAYB, SC.AT 1/2TVCDT REVS)

  EXTEND   # PICK UP -OMEGAYB (SIGN CHNG, INTEGRATE)
  DCS OMEGAYB
  DAS ERRBTMP

PERORLIM TCR ERRORLIM # PITCH BODY-AXIS-ERROR INPUT LIMITER

PFORWARD EXTEND   #  PREPARE THE FILTER STORAGE LOCATIONS
  DCA PTMP1  # FOR THE PITCH CHANNEL
  DXCH TMP1
  EXTEND
  DCA PTMP3
  DXCH TMP3
  EXTEND
  DCA PTMP5
  DXCH TMP5

  TCR FWDFLTR  # GO COMPUTE PRESENT OUTPUT
     # (INCLUDES VARIABLE GAIN PACKAGE)

POFFSET  EXTEND
  DCA PDELOFF
  DAS CMDTMP  # NO SCALED AT B+0 ASCREV

PACLIM  TCR ACTLIM  # ROUND OFF & LIMIT PITCH ACTUATOR COMMAND

POUT  CS PCMD  # INCREMENTAL PITCH COMMAND
  AD CMDTMP
  ADS TVCPITCH # UPDATE THE ERROR COUNTER (NO RESTART-
     # PROTECT. SINCE ERROR CNTR ZEROED)

  CAF BIT11  # BIT FOR TVCPITCH COUNT RELEASE
  EXTEND
  WOR CHAN14

PPRECOMP EXTEND   # PREPARE THE FILTER STORAGE FOR PITCH
# Page 965
  DCA PTMP2
  DXCH TMP2
  EXTEND
  DCA PTMP4
  DXCH TMP4
  EXTEND
  DCA PTMP6
  DXCH TMP6

  TCR PRECOMP  # TO THE FILTER FOR PRECOMPUTATION

DELBARP  CAE DELPBAR +1 # UPDATE PITCH OFFSET-TRACKER-FILTER
  EXTEND
  MP E(-AT)
  TS DELBRTMP +1
  CAE DELPBAR
  EXTEND
  MP E(-AT)
  DAS DELBRTMP
  CAE CMDTMP
  EXTEND
  MP 1-E(-AT)
  DAS DELBRTMP

PCOPYCYC TCR PCOPY  # PITCH COPYCYCLE

PDAPEND  TCF RESUME  # PITCH DAP COMPLETED
# Page 966
# PITCH TVCDAP COPYCYCLE SUBROUTINE (CALLED VIA PITCH TVCDAP OR TVC RESTART PACKAGE)

PCOPY  INCR TVCPHASE # RESTART-PROTECT THE COPYCYCLE. (1)
     # NOTE POSSIBLE RE-ENTRY FROM RESTART
     # PACKAGE, SHOULD A RESTART OCCUR
     # DURING PITCH COPYCYCLE.

  EXTEND
  DCA TMP1
  DXCH PTMP1
  EXTEND
  DCA TMP2
  DXCH PTMP2
  EXTEND
  DCA TMP3
  DXCH PTMP3
  EXTEND
  DCA TMP4
  DXCH PTMP4
  EXTEND
  DCA TMP5
  DXCH PTMP5
  EXTEND
  DCA TMP6
  DXCH PTMP6

PMISC  EXTEND   # MISC....PITCH-RATE-ERROR INTEGRATOR
  DCA ERRBTMP
  TS AK1  # FOR PITCH NEEDLES, SC.AT B-1 REVS
  DXCH PERRB

  CAE CMDTMP  # PITCH ACTUATOR COMMAND
  TS PCMD

  EXTEND   #  PITCH OFFSET-TRACKER-FILTER
  DCA DELBRTMP
  DXCH DELPBAR

  INCR TVCPHASE # PITCH COPYCYCLE COMPLETED (2)

  TC Q

# Page 967
# YAW TVCDAP STARTS HERE....(INCORPORATES CSM/LEM DAP FILTER, MODOR DESIGN)

YAWDAP  LXCH BANKRUPT # T5 ENTRY, NORMAL
  EXTEND
  QXCH QRUPT

  CAF PITCHT5  # SET UP T5 CALL FOR PITCH AUTOPILOT (LOW-
  TS T5LOC  # ORDER PART OF 2CADR ALREADY THERE)
  CAE T5TVCDT
  TS TIME5

YSTROKER CCS STROKER  # (STRKFLG) CHECK FOR STROKE TEST
  TC HACK  # TEST-START OR TEST-IN-PROGRESS
  TCF +2  # NO-TEST
  TC HACK  # TEST-IN-PROGRESS

     # USE BODY RATES FROM PITCHDAP (PCDUDOTS)

YINTEGRL EXTEND   # COMPUTE INTEGRAL OF BODY-AXIS YAW-RATE
  DCA YERRB  #  ERROR, SC.AT B-1 REVS
  DXCH ERRBTMP

  EXTEND
  DCA OMEGAZC
  DAS ERRBTMP

  CAE COSCDUZ  # PREPARE BODY-AXIS YAW-RATE, OMEGAZB
  EXTEND
  MP SINCDUX
  DDOUBL
  EXTEND
  MP MCDUYDOT
  DDOUBL
  DXCH OMEGAZB

  CS MCDUZDOT
  EXTEND
  MP COSCDUX
  DDOUBL
  DAS OMEGAZB  # (COMPLETED OMEGAZB, SC.AT 1/2TVCDT REVS)

  EXTEND   # PICK UP -OMEGAZB (SIGN CHNG, INTEGRATE)
  DCS OMEGAZB
  DAS ERRBTMP

YERORLIM TCR ERRORLIM # YAW   BODY-AXIS-ERROR INPUT LIMITER

YFORWARD EXTEND   #  PREPARE THE FILTER STORAGE LOCATIONS
  DCA YTMP1  # FOR THE YAW CHANNEL
# Page 968
  DXCH TMP1
  EXTEND
  DCA YTMP3
  DXCH TMP3
  EXTEND
  DCA YTMP5
  DXCH TMP5

  TCR FWDFLTR  # GO COMPUTE PRESENT OUTPUT
     # (INCLUDES VARIABLE GAIN PACKAGE)

YOFFSET  EXTEND
  DCA YDELOFF
  DAS CMDTMP  # NOW SCALED AT B+0 ASCREV

YACLIM  TCR ACTLIM  # ROUND OFF & LIMIT YAW ACTUATOR COMMAND

YOUT  CS YCMD  # INCREMENTAL YAW COMMAND
  AD CMDTMP
  ADS TVCYAW  # UPDATE THE ERROR COUNTER (NO RESTART-
     # PROTECT, SINCE ERROR CNTR ZEROED)

  CAF BIT12  # BIT FOR TVCYAW COUNT RELEASE
  EXTEND
  WOR CHAN14

YPRECOMP EXTEND   # PREPARE THE FILTER STORAGE FOR YAW
  DCA YTMP2
  DXCH TMP2
  EXTEND
  DCA YTMP4
  DXCH TMP4
  EXTEND
  DCA YTMP6
  DXCH TMP6

  TCR PRECOMP  # TO THE FILTER FOR PRECOMPUTATION

DELBARY  CAE DELYBAR +1 # UPDATE YAW OFFSET-TRACKER-FILTER
  EXTEND
  MP E(-AT)
  TS DELBRTMP +1
  CAE DELYBAR
  EXTEND
  MP E(-AT)
  DAS DELBRTMP
  CAE CMDTMP
  EXTEND
  MP 1-E(-AT)
  DAS DELBRTMP

# Page 969
YCOPYCYC TCR YCOPY  # YAW COPYCYCLE

YDAPEND  TCF RESUME  # YAW DAP COMPLETED

# Page 970
# TVCDAP COPYCYCLE SUBROUTINE (CALLED VIA YAW   TVCDAP OR TVC RESTART PACKAGE)

YCOPY  INCR TVCPHASE # RESTART-PROTECT THE COPYCYCLE. (3)
     # NOTE POSSIBLE RE-ENTRY FROM RESTART
     # PACKAGE, SHOULD A RESTART OCCUR
     # DURING YAW   COPYCYCLE.

  EXTEND
  DCA TMP1
  DXCH YTMP1
  EXTEND
  DCA TMP2
  DXCH YTMP2
  EXTEND
  DCA TMP3
  DXCH YTMP3
  EXTEND
  DCA TMP4
  DXCH YTMP4
  EXTEND
  DCA TMP5
  DXCH YTMP5
  EXTEND
  DCA TMP6
  DXCH YTMP6

YMISC  EXTEND   # MISC....YAW-RATE-ERROR INTEGRATOR
  DCA ERRBTMP
  TS AK2  # FOR YAW   NEEDLES, SC.AT B-1 REVS
  DXCH YERRB

  CAE CMDTMP
  TS YCMD

  EXTEND
  DCA DELBRTMP
  DXCH DELYBAR

  CAF ZERO  # YAW  COPYCYCLE COMPLETED
  TS TVCPHASE # RESET TVCPHASE

  TC Q

# Page 971
# SUBROUTINES COMMON TO BOTH PITCH AND YAW DAPS....


# INITIALIZATION PACKAGE FOR CDURATES....

DAPINIT  LXCH BANKRUPT # T5 RUPT ENTRY (CALLED BY TVCINT4)

  CAF NEGONE  #  SET UP
  AD T5TVCDT  # T5 CALL FOR PITCHDAP IN TVCDT SECS
  AD NEGMAX  # (T5TVCDT = POSMAX - TVCDT/2 +1)
  AD T5TVCDT
  TS TIME5
  CAF PITCHT5  # (BBCON ALREADY THERE)
  TS T5LOC

  CAE CDUY  # READ AND STORE CDUS FOR DIFFERENTIATOR
  TS PCDUYPST # PAST-VALUES
  CAE CDUZ
  TS PCDUZPST

  TCF NOQRSM

# BODY-AXIS-ERROR   INPUT LIMITER PACKAGE....

ERRORLIM CAE ERRBTMP  # CHECK FOR INPUT-ERROR LIMIT
  EXTEND   # CHECKS UPPER WORD ONLY
  MP 1/ERRLIM
  EXTEND
  BZF +6
  CCS ERRBTMP
  CAF ERRLIM
  TCF +2
  CS ERRLIM
  TS ERRBTMP  # LIMIT WRITES OVER UPPER WORD ONLY

  TC Q


# ACTUATOR-COMMAND LIMITER PACKAGE....

ACTLIM  CAE CMDTMP +1 # ROUND UP FOR OUTPUT
  DOUBLE
  TS L
  CAF ZERO
  AD CMDTMP

  EXTEND   # CHECK FOR ACTUATOR COMMAND LIMIT
  MP 1/ACTSAT
  EXTEND
# Page 972
  BZF +6
  CCS CMDTMP  # APPLY LIMITS
  CAF ACTSAT
  TCF +2
  CS ACTSAT
  TS CMDTMP  # LIMITS WRITE OVER CMDTMP

  TC Q


# FILTER COMPUTATIONS FOR PRESENT OUTPUT................

FWDFLTR  CAF ZERO
  TS DAP1
  TS DAP2
  TS DAP3
  TS CMDTMP

  TS DELBRTMP

1DAPCAS  CAE ERRBTMP +1 # FIRST DAP CASCADE
  EXTEND
  MP N10  # N10
  TS DAP1 +1
  CA ERRBTMP
  EXTEND
  MP N10  # N10
  DAS DAP1
  DXCH TMP1
  DAS DAP1

2DAPCAS  CAE DAP1 +1 # SECOND DAP CASCADE
  EXTEND
  MP N10 +5 # N20
  TS DAP2 +1
  CA DAP1
  EXTEND
  MP N10 +5 # N20
  DAS DAP2
  DXCH TMP3
  DAS DAP2

  CAE DAPDATR1 # TEST FOR LEM ON OR OFF
  MASK BIT14
  CCS A
  TCF 3DAPCAS  # LEM ON
  EXTEND   # LEM OFF
  DCA DAP2
  DXCH DAP3
  TCF OPTVARK

# Page 973
3DAPCAS  CAE DAP2 +1 # THIRD DAP CASCADE
  EXTEND
  MP N10 +10D # N30
  TS DAP3 +1
  CA DAP2
  EXTEND
  MP N10 +10D # N30
  DAS DAP3
  DXCH TMP5
  DAS DAP3

OPTVARK  CS DAP3 +1 # VARIABLE GAIN PACKAGE
  EXTEND   # (ALSO, SIGN CHANGE IN FORWARD LOOP)
  MP VARK  # SCALED AT 1/(8 ASCREV) OF ACTUAL VALUE
  TS CMDTMP +1
  CS DAP3
  EXTEND
  MP VARK
  DAS CMDTMP

  DXCH CMDTMP  # FIX UP SCALING - SCALED B+3 ASCREVS
  DDOUBL
  DDOUBL
  DXCH CMDTMP  #  - SCALED B+1 ASCREVS
     # NOTE - THERE IS AN INHERANT GAIN OF
     # (B+1 ASCREVS) ON THE OUTPUT DACS.

  TC Q


# FILTER PRECOMPUTATIONS FOR NEXT PASS................

PRECOMP  CAF ZERO  # ***** FIRST CASCADE FILTER **********
  TS TTMP1
  TS TTMP2

  CA ERRBTMP +1 # MULTIPLY INPUT BY
  EXTEND
  MP N10 +1 # N11/2
  TS TTMP1 +1
  CA ERRBTMP
  EXTEND
  MP N10 +1 # N11/2
  DAS TTMP1

  CS DAP1 +1 # MULTIPLY OUTPUT BY
  EXTEND
  MP N10 +3 #  D11/2
  TS TTMP2 +1
  CS DAP1

# Page 974
  EXTEND
  MP N10 +3 # D11/2
  DAS TTMP2

  DXCH TTMP2
  DAS TTMP1
  DXCH TTMP1
  DDOUBL
  DAS TMP2

  DXCH TMP2
  DXCH TMP1

  CAF ZERO
  TS TTMP1
  TS TMP2

  CA ERRBTMP +1 # MULTIPLY INPUT BY
  EXTEND   # SECOND-ORDER NUMERATOR COEFF.
  MP N10 +2 # N12
  TS TTMP1 +1
  CA ERRBTMP
  EXTEND
  MP N10 +2 #  N12
  DAS TTMP1

  CS DAP1 +1 # MULTIPLY OUTPUT BY
  EXTEND
  MP N10 +4 #  D12
  TS TMP2 +1
  CS DAP1
  EXTEND
  MP N10 +4 # D12
  DAS TMP2

  DXCH TTMP1
  DAS TMP2

2CASFLTR CAF ZERO  # *****SECOND CASCADE FILTER*****
  TS TTMP1
  TS TTMP2

  CA DAP1  +1 # MULTIPLY INPUT BY
  EXTEND
  MP N10 +6 # N21/2
  TS TTMP1 +1
  CA DAP1
  EXTEND
  MP N10 +6 # N21/2
# Page 975
  DAS TTMP1

  CS DAP2 +1 # MULTIPLY OUTPUT BY
  EXTEND
  MP N10 +8D #  D21/2
  TS TTMP2 +1
  CS DAP2
  EXTEND
  MP N10 +8D # D21/2
  DAS TTMP2

  DXCH TTMP2
  DAS TTMP1
  DXCH TTMP1
  DDOUBL
  DAS TMP4

  DXCH TMP4
  DXCH TMP3

  CAF ZERO
  TS TTMP1
  TS TMP4

  CA DAP1 +1 # MULTIPLY INPUT BY
  EXTEND
  MP N10 +7 # N22
  TS TTMP1 +1
  CA DAP1
  EXTEND
  MP N10 +7 #  N22
  DAS TTMP1

  CS DAP2 +1 # MULTIPLY OUTPUT BY
  EXTEND
  MP N10 +9D #  D22
  TS TMP4 +1
  CS DAP2
  EXTEND
  MP N10 +9D # D22
  DAS TMP4

  DXCH TTMP1
  DAS TMP4


  CAE DAPDATR1 # TEST FOR LEM ON OR OFF
  MASK BIT13
  CCS A
  TC Q  # EXIT IF LEM OFF

# Page 976
3CASFLTR CAF ZERO  # *****THIRD CASCADE FILTER*****
  TS TTMP1
  TS TTMP2

  CA DAP2  +1 # MULTIPLY INPUT BY (1/2)
  EXTEND
  MP N10 +11D # N31/2
  TS TTMP1 +1
  CA DAP2
  EXTEND
  MP N10 +11D # N31/2
  DAS TTMP1

  CS DAP3 +1
  EXTEND
  MP N10 +13D #  D31/2
  TS TTMP2 +1
  CS DAP3
  EXTEND
  MP N10 +13D # D31/2
  DAS TTMP2

  DXCH TTMP2
  DAS TTMP1
  DXCH TTMP1
  DDOUBL
  DAS TMP6

  DXCH TMP6
  DXCH TMP5

  CAF ZERO
  TS TTMP1
  TS TMP6

  CA DAP2 +1 # MULTIPLY INPUT BY
  EXTEND
  MP N10 +12D # N32
  TS TTMP1 +1
  CA DAP2
  EXTEND
  MP N10 +12D #  N32
  DAS TTMP1

  CS DAP3 +1
  EXTEND
  MP N10 +14D #  D32
  TS TMP6 +1
  CS DAP3
  EXTEND
# Page 977
  MP N10 +14D # D32
  DAS TMP6

  DXCH TTMP1
  DAS TMP6

  TC Q

# Page 978
# CONSTANTS FOR AUTOPILOTS

# NOTE....1 ASCREV (ACTUATOR CMD SCALING) = 85.41 ARCSEC/BIT OR 1.07975111 REVS (85.41x16384/3600/360)

#   1 SPASCREV (SPECIAL ACTUATOR CMD SCALING) = 1.04620942 REVS

ACTSAT  DEC 253  # ACTUATOR LIMIT (6 DEG), SC.AT 1ASCREV
1/ACTSAT DEC .0039525692 # RECIPROCAL (1/253)

ERRLIM  EQUALS BIT13  # FILTER INPUT LIMIT....B-3 REVS (45DEG),
1/ERRLIM EQUALS BIT3  #  SC.AT B-1 REV, AND ITS RECIPROCAL

PITCHT5  GENADR PITCHDAP # UPPER WORDS OF T5 2CADRS, LOWER WORDS
DAPT5  GENADR DAPINIT  # (BBCON) ALREADY THERE.  ORDER IS
YAWT5  GENADR YAWDAP  # REQUIRED.

1/RTLIM  DEC 0.004715 # .004715(CDUDIF) = 0 IF CDUIF < 2.33 DEG
1-E(-AT) OCT 00243  # AT = .01SEC....EITHER(1/A=4SEC, T=40MS),
E(-AT)  OCT 37535  #      OR(1/A=8SEC, T=80MS)