TVCDAPS.s

# 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 FILTERS
#	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 QUICTRIG1 (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 INCREMENTAL
#		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....(INCOPORATES 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
		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		# YAW ACTUATOR-COMMAND-LIMITER

YOUT		CS	YCMD		# INCRMENTAL 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 INHERENT 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			# SECOND-ORDER NUMERATOR COEFF.
		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)