POWERED_FLIGHT_SUBROUTINES.s

# Copyright:	Public domain.
# Filename:	POWERED_FLIGHT_SUBROUTINES.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:	1365-1372 
# Mod history:  2009-05-10 SN   (Sergio Navarro).  Started adapting
#                               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 1365
		BANK	14		# SAME FBANK AS THE FINDCDUD SUB-PROGRAM
		SETLOC	POWFLITE
		BANK

		EBANK=	DEXDEX
		COUNT*	$$/POWFL

# CDUTRIG, CDUTRIG1, CDUTRIG2, AND CD*GR*GS ALL COMPUTE THE SINES AND
# COSINES OF THREE 2'S COMPLEMENT ANGLES AND PLACE THE RESULT, DOUBLE
# PRECISION, IN THE SAME ORDER AS THE INPUTS, AT SINCDU AND COSCDU.  AN
# ADDITIONAL OUTPUT IS THE 1'S COMPLENT ANGLES AT CDUSPOT.  THESE
# ROUTINES GO OUT OF THEIR WAY TO LEAVE THE MPAC AREA AS THEY FIND IT.
# EXCEPT FOR THE GENERALLY UNIMPORTANT MPAC +2.  THEY DIFFER ONLY IN
# WHERE THEY GET THE ANGLES, AND IN METHOD OF CALLING.
#
# CDUTRIG (AND CDUTRIG1, WHICH CAN BE CALLED IN BASIC) COMPUTE THE
# SINES AND COSINES FROM THE CURRENT CONTENTS OF THE CDU REGISTERS.
# THE CONTENTS OF CDUTEMP, ETC., ARE NOT TOUCHED SO THAT THEY MAY
# CONTINUE TO FORM A CONSISTENT SET WITH THE LATEST PIPA READINGS.
#
# CDUTRIG1 IS LIKE CDUTRIG EXCEPT THAT IT CAN BE CALLED IN BASIC.
#
# CD*TR*GS FINDS CDU VALUES IN CDUSPOT RATHER THAN IN CDUTEMP.  THIS
# ALLOWS USERS TO MAKE TRANSFORMATIONS USING ARBITRARY ANGLES, OR REAL
# ANGLES IN AN ORDER OTHER THAN X Y Z.  A CALL TO THIS ROUTINE IS
# NECESSARY IN PREPARATION FOR A CALL TO AX*SR*T IN EITHER OF ITS TWO
# MODES (SMNB OR NBSM).  SINCE AX*SR*T EXPECTS TO FIND THE SINES AND
# COSINES IN THE ORDER Y Z X THE ANGLES MUST HAVE BEEN PLACED IN CDUSPOT
# IN THIS ORDER.  CD*TR*GS NEED NOT BE REPEATED WHEN AX*SR*T IS CALLED
# MORE THAN ONCE, PROVIDED THE ANGLES HAVE NOT CHANGED.  NOTE THAT SINCE
# IT CLOBBERS BUF2 (IN THE SINE AND COSINE ROUTINES) CD*TR*GS CANNOT BE
# CALLED USING BANKCALL.  SORRY.
#
# CD*TR*G IS LIKE CD*TR*GS EXCEPT THAT IT CAN BE CALLED IN
# INTERPRETIVE.

CDUTRIG		EXIT
		TC	CDUTRIGS
		TC	INTPRET
		RVQ

CD*TR*G		EXIT
		TC	CD*TR*GS
		TC	INTPRET
		RVQ

CDUTRIGS	CA	CDUX
		TS	CDUSPOT +4
		CA	CDUY
		TS	CDUSPOT
# Page 1366
		CA	CDUZ
		TS	CDUSPOT +2

CD*TR*GS	EXTEND
		QXCH	TEM2
		CAF	FOUR
TR*GL**P	MASK	SIX		# MAKE IT EVEN AND SMALLER
		TS	TEM3
		INDEX	TEM3
		CA	CDUSPOT
		DXCH	MPAC		# STORING 2'S COMP ANGLE, LOADING MPAC
		DXCH	VBUF +4		# STORING MPAC FOR LATER RESTORATION
		TC	USPRCADR
		CADR	CDULOGIC
		EXTEND
		DCA	MPAC
		INDEX	TEM3
		DXCH	CDUSPOT		# STORING 1'S COMPLEMENT ANGLE
		TC	USPRCADR
		CADR	COSINE
		DXCH	MPAC
		INDEX	TEM3
		DXCH	COSCDU		# STORING COSINE
		EXTEND
		INDEX	TEM3
		DCA	CDUSPOT		# LOADING 1'S COMPLEMENT ANGLE
		TC	USPRCADR
		CADR	SINE +1		# SINE +1 EXPECTS ARGUMENT IN A AND L
		DXCH	VBUF +4		# BRINGING UP PRIOR MPAC TO BE RESTORED
		DXCH	MPAC
		INDEX	TEM3
		DXCH	SINCDU
		CCS	TEM3
		TCF	TR*GL**P
		TC	TEM2
# Page 1367
# *******************************************************************************************************
# QUICTRIG, INTENDED FOR QUIDANCE CYCLE USE WHERE TIME IS CRITICAL, IS A MUCH FASTER VERSION OF CD*TR*GS.
# QUICTRIG COMPUTES AND STORES THE SINES AND COSINES OF THE 2'S COMPLEMENT ANGLES AT CDUSPOT, CDUSPOT +2,
# AND CDUSPOT +4.  UNLIKE CD*TR*GS, QUICTRIG DOES NOT LEAVE THE 1'S COMPLEMENT VERSIONS OF THE ANGLES IN
# CDUSPOT.  QUICTRIG'S EXECUTION TIME IS 4.1 MS;    THIS IS 10 TIMES AS FAST AS CD*TR*GS.    QUICTRIG MAY BE
# CALLED FROM INTERPRETIVE AS AN RTB OP-CODE, OR FROM BASIC VIA BANKCALL OR IBNKCALL.

QUICTRIG	INHINT			# INHINT SINCE DAP USES THE SAME TEMPS
		EXTEND
		QXCH	ITEMP1
		CAF	FOUR
	+4	MASK	SIX
		TS	ITEMP2
		INDEX	ITEMP2
		CA	CDUSPOT
		TC	SPSIN
		EXTEND
		MP	BIT14		# SCALE DOWN TO MATCH INTERPRETER OUTPUTS
		INDEX	ITEMP2
		DXCH	SINCDU
		INDEX	ITEMP2
		CA	CDUSPOT
		TC	SPCOS
		EXTEND
		MP	BIT14
		INDEX	ITEMP2
		DXCH	COSCDU
		CCS	ITEMP2
		TCF	QUICTRIG +4
		CA	ITEMP1
		RELINT
		TC	A
# Page 1368
#****************************************************************************
# THESE INTERFACE ROUTINES MAKE IT POSSIBLE TO CALL AX*SR*T, ETC., IN
# INTERPRETIVE.  LATER, WHERE POSSIBLE, THEY WILL BE ELIMINATED.
#
# NBSM WILL BE THE FIRST TO GO.  IT SHOULD NOT BE USED.

NBSM		STQ
			X2
		LXC,1	VLOAD*	
			S1		# BASE ADDRESS OF THE CDU ANGLES IS IN S1
			0,1
		STOVL	CDUSPOT
			32D		# VECTOR TO BE TRANSFORMED IS IN 32D
		CALL
			TRG*NBSM
		STCALL	32D		# SINCE THERE'S NO STGOTO
			X2

# THESE INTERFACE ROUTINES ARE PERMANENT.  ALL RESTORE USER'S EBANK
# SETTING. ALL ARE STRICT INTERPRETIVE SUBROUTINES, CALLED USING "CALL",
# RETURNING VIA QPRET.  ALL EXPECT AND RETURN THE VECTOR TO BE TRANSFOR-
# MED INTERPRETER-STYLE IN MPAC: COMPONENTS AT MPAC, MPAC +3, AND MPAC +5.
#
# TRG*SMNB AND TRG*NBSM BOTH EXPECT TO SEE THE 2'S COMPLEMENT ANGLES
# AT CDUSPOT (ORDER Y Z X, AT CDUSPOT, CDUSPOT +2, AND CDUSPOT +4. ODD
# LOCATIONS NEED NOT BE ZEROED).  TRG*NBSM DOES THE NB TO SM TRANSFOR-
# MATION:  TRG*SMNB, VICE VERSA.
#
# CDU*NBSM DOES ITS TRANSFORMATION USING THE PRESENT CONTENTS OF
# THE CDL COUNTERS.  OTHERWISE IT IS LIKE TRG*NBSM.
#
# CDU*SMNB IS THE COMPLEMENT OF CDU*NBSM.

CDU*SMNB	EXIT
		TC	CDUTRIGS
		TCF	C*MM*N1

TRG*SMNB	EXIT
		TC	CD*TR*GS
C*MM*N1		TC	MPACVBUF	# AX*SR*T EXPECTS VECTOR IN VBUF
		CS	THREE		# SIGNAL FOR SM TO NB TRANSFORMATION.
C*MM*N2		TC	AX*SR*T
		TC	INTPRET
		VLOAD	RVQ
			VBUF

CDU*NBSM	EXIT
		TC	CDUTRIGS

# Page 1369

		TCF	C*MM*N3

TRG*NBSM	EXIT
		TC	CD*TR*GS
C*MM*N3		TC	MPACVBUF	# FOR AX*SR*T
		CA	THREE		# SIGNAL FOR NB TO SM TRANSFORMATION
		TCF	C*MM*N2

# *NBSM* AND *SMNB* EXPECT TO SEE THE SINES AND COSINES (AT SINCDU
# AND COSCDU) RATHER THAN THE ANGLES THEMSELVES.  OTHERWISE THEY ARE
# LIKE TRG*NBSM AND TRG*SMNB.
#
# NOTE THAT JUST AS CD*TR*GS NEED BE CALLED ONLY ONCE FOR EACH SERIES
# OF TRANSFORMATIONS USING THE SAME ANGLES, SO TOO ONLY ONE OF TRG*NBSM
# AND TRG*SMNB NEED BE CALLED FOR EACH SERIES.  FOR SUBSEQUENT TRANFOR-
# MATIONS USE *NBSM* AND *SMNB*.

*SMNB*		EXIT
		TCF	C*MM*N1

*NBSM*		EXIT
		TCF	C*MM*N3

# AX*SR*T COMBINES THE OLD SMNB AND NBSM.  FOR THE NB TO SM
# TRANSFORMATION, ENTER WITH +3 IN A.  FOR SM TO NB, ENTER WITH -3.
# THE VECTOR TO BE TRANSFORMED ARRIVES, AND IS RETURNED, IN VBUF.
# AX*SR*T EXPECTS TO FIND THE SINES AND COSINES OF THE ANGLES OF ROTATION
# AT SINCDU AND COSCDU, IN THE ORDER Y Z X.  A CALL TO CD*TR*GS, WITH
# THE 2'S COMPLEMENT ANGLES (ORDER Y Z X) AT CDUSPOT, WILL TAKE CARE OF
# THIS.  HERE IS A SAMPLE CALLING SEQUENCE:--
#		TC	CDUTRIGS
#		CS	THREE		# ("CA THREE" FOR NBSM)
#		TC	AX*SR*T
# THE CALL TO CD*TR*GS NEED NOT BE REPEATED, WHEN AX*SR*T IS CALLED MORE
# THAN ONCE, UNLESS THE ANGLES HAVE CHANGED.
#
# AX*SR*T IS GUARANTEED SAFE ONLY FOR VECTORS OF MAGNITUDE LESS THAN
# UNITY.  A LOOK AT THE CASE IN WHICH A VECTOR OF GREATER MAGNITUDE
# HAPPENS TO LIE ALONG AN AXIS OF THE SYSTEM TO WHICH IT IS TO BE TRANS-
# FORMED CONVINCES ONE THAT THIS IS A RESTRICTION WHICH MUST BE ACCEPTED.

AX*SR*T		TS	DEXDEX		# WHERE IT BECOMES THE INDEX OF INDEXES.
		EXTEND
		QXCH	RTNSAVER

R*TL**P		CCS	DEXDEX		#       	+3 --> 0	-3 --> 2
		CS	DEXDEX		# THUS:		+2 --> 1	-2 --> 1
		AD	THREE		#		+1 --> 2	-1 --> 0
# Page 1370
		EXTEND
		INDEX	A
		DCA	INDEXI
		DXCH	DEXI

		CA	ONE
		TS	BUF
		EXTEND
		INDEX	DEX1
		DCS	VBUF
		TCF	LOOP1		# REALLY BE A SUBTRACT, AND VICE VERSA

LOOP2		DXCH	BUF		# LOADING VECTOR COMPONENT, STORING INDEX

LOOP1		DXCH	MPAC
		CA	SINESLOC
		AD	DEX1
		TS	ADDRWD

		TC	DMPSUB		# MULTIPLY BY SIN(CDUANGLE)
		CCS	DEXDEX
		DXCH	MPAC		# NBSM CASE
		TCF	+3
		EXTEND			# SMNB CASE
		DCS	MPAC
		DXCH	TERM1TMP

		CA	SIX		# SINCDU AND COSCDU (EACH 6 WORDS) MUST
		ADS	ADDRWD		#	BE CONSECUTIVE AND IN THAT ORDER

		EXTEND
		INDEX	BUF
		INDEX	DEX1
		DCA	VBUF
		DXCH	MPAC
		TC	DMPSUB		# MULTIPLY BY COS(CDUANGLE)
		DXCH	MPAC
		DAS	TERM1TMP
		DXCH	TERM1TMP
		DDOUBL
		INDEX	BUF
		INDEX	DEX1
		DXCH	VBUF
		DXCH	BUF		# LOADING INDEX, STORING VECTOR COMPONENT
		CCS	A		# 'CAUSE THAT'S WHERE THE INDEX NOW IS
		TCF	LOOP2

		EXTEND
		DIM	DEXDEX		# DECREMENT MAGNITUDE PRESERVING SIGN

# Page 1371
TSTPOINT	CCS	DEXDEX		# ONLY THE BRANCHING FUNCTION IS USED
		TCF	R*TL**P
		TC	RTNSAVER
		TCF	R*TL**P
		TC	RTNSAVER

SINESLOC	ADRES	SINCDU		# FOR USE IN SETTING ADDRWD

INDEXI		DEC	4		# **********   DON'T   ***********
		DEC	2		# **********   TOUCH   ***********
		DEC	0		# **********   THESE   ***********
		DEC	4		# ********** CONSTANTS ***********

# ******************************************************************************
# Page 1372
# THIS SUBROUTINE COMPUTES INCREMENTAL CHANGES IN CDU(GIMBAL) ANGLES FROM INCREMENTAL CHANGES ABOUT SM AXES.  IT
# REQUIRES SM INCREMENTS AS A DP VECTOR SCALED AT ONE REVOLUTION(DTHETASM,+2,+4).  SIN,COS(CDUY,Z,X) ARE IN
# SINCDU,+2,+4 AND COSCDU,+2,+4 RESPECTIVELY, SCALED TO ONE HALF.  CDU INCREMENTS ARE PLACED IN DCDU,+2,+4 SCALED TO
# ONE REVOLUTION.
#
#	*  COS(IGA)SEC(MGA)		0		-SIN(IGA)SEC(MGA) *
#	*								  *
#	* -COS(IGA)TAN(MGA)		1		 SIN(IGA)TAN(MGA) *
#	*								  *
#	*      SIN(IGA)			0		     COS(IGA)     *

		BANK	14
		SETLOC	POWFLIT1
		BANK
		
SMCDURES	DLOAD	DMP
			DTHETASM
			COSCDUY
			
		PDDL	DMP
			DTHETASM +4
			SINCDUY
			
		BDSU
		DDV
			COSCDUZ
		STORE	DCDU
		
		DMP	SL1		# SCALE
			SINCDUZ
		BDSU
		
			DTHETASM +2
		STODL	DCDU +2
			DTHETASM
			
		DMP	PDDL
			SINCDUY
			DTHETASM +4
			
		DMP	DAD
			COSCDUY
		SL1
		STORE	DCDU +4
		RVQ