### FILE="Main.annotation"
## Copyright:	Public domain.
## Filename:	ANGLFIND.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:	399-411
## Mod history:	2009-05-09 RSB	Adapted from the Colossus249/ file 
##				of the same name, using Comanche055 page 
##				images.
##		2009-05-22 RSB	In NOGOM2, TC ZEROEROR corrected to 
##				CADR ZEROEROR.
##		2017-01-07 RSB	Fixed comment errors detected in cross-diff vs
##				Colossus 237 and Colossus 249.
##		2021-05-30 ABS  ENDOCM -> ENDDCM
##				ZEROERROR -> ZEROEROR
##
## 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 399
		BANK	15
		SETLOC	KALCMON1
		BANK
		
		EBANK=	BCDU
		
		COUNT	22/KALC
		
KALCMAN3	TC	INTPRET
		RTB
			READCDUK	# PICK UP CURRENT CDU ANGLES
		STORE	BCDU		# STORE THE INITIAL S/C ANGLES
		AXC,2	TLOAD		# COMPUTE THE TRANSFORMATION FROM
			MIS		# INITIAL S/C AXES TO STABLE MEMBER AXES
			BCDU		# (MIS)
		CALL
			CDUTODCM	
		AXC,2	TLOAD		# COMPUTE THE TRANSFORMATION FROM
			MFS		# FINAL S/C AXES TO STABLE MEMBER AXES
			CPHI		# (MFS)
		CALL
			CDUTODCM
SECAD		AXC,1	CALL		# MIS AND MFS ARRAYS CALCULATED		$2
			MIS
			TRANSPOS
		VLOAD
		STADR
		STOVL	TMIS	+12D
		STADR
		STOVL	TMIS	+6
		STADR
		STORE	TMIS		# TMIS = TRANSPOSE(MIS) SCALED BY 2
		AXC,1	AXC,2
			TMIS
			MFS
		CALL
			MXM3
		VLOAD	STADR
		STOVL	MFI	+12D
		STADR
		STOVL	MFI	+6
		STADR
		STORE	MFI		# MFI = TMIS MFS (SCALED BY 4)
		SETPD	CALL		# TRANSPOSE MFI IN PD LIST
			18D
			TRNSPSPD
		VLOAD	STADR
		STOVL	TMFI	+12D
		STADR
		STOVL	TMFI	+6
## Page 400
		STADR
		STORE	TMFI		# TMFI = TRANSPOSE (MFI) SCALED BY 4
		
# CALCULATE COFSKEW AND MFISYM

		DLOAD	DSU
			TMFI	+2
			MFI	+2
		PDDL	DSU		# CALCULATE COF SCALED BY 2/SIN(AM)
			MFI	+4
			TMFI	+4
		PDDL	DSU
			TMFI	+10D
			MFI	+10D
		VDEF
		STORE	COFSKEW		# EQUALS MFISKEW
		
# CALCULATE AM AND PROCEED ACCORDING TO ITS MAGNITUDE

		DLOAD	DAD
			MFI
			MFI	+16D
		DSU	DAD
			DP1/4TH
			MFI	+8D
		STORE	CAM		# CAM = (MFI0+MFI4+MFI8-1)/2 HALF SCALE
		ARCCOS
		STORE	AM		# AM=ARCCOS(CAM)  (AM SCALED BY 2)
		DSU	BPL
			MINANG
			CHECKMAX
		EXIT			# MANEUVER LESS THAN 0.25 DEG
		INHINT			# GO DIRECTLY INTO ATTITUDE HOLD
		CS	ONE		# ABOUT COMMANDED ANGLES
		TS	HOLDFLAG	# NOGO WILL STOP ANY RATE AND SET UP FOR A
		TC	LOADCDUD	# GOOD RETURN
		TCF	NOGO
		
CHECKMAX	DLOAD	DSU
			AM
			MAXANG
		BPL	VLOAD
			ALTCALC		# UNIT
			COFSKEW		# COFSKEW
		UNIT
		STORE	COF		# COF IS THE MANEUVER AXIS
		GOTO			# SEE IF MANEUVER GOES THRU GIMBAL LOCK
			LOCSKIRT
ALTCALC		VLOAD	VAD		# IF AM GREATER THAN 170 DEGREES
			MFI
## Page 401
			TMFI
		VSR1
		STOVL	MFISYM
			MFI	+6
		VAD	VSR1
			TMFI	+6
		STOVL	MFISYM	+6
			MFI	+12D
		VAD	VSR1
			TMFI	+12D
		STORE	MFISYM	+12D	# MFISYM=(MFI+TMFI)/2	SCALED BY 4
		
# CALCULATE COF

		DLOAD	SR1
			CAM
		PDDL	DSU		# PD0 CAM		 	       $4
			DPHALF
			CAM
		BOVB	PDDL		# PD2 1 - CAM			       $2
			SIGNMPAC
			MFISYM	+16D
		DSU	DDV
			0
			2
		SQRT	PDDL		# COFZ = SQRT(MFISYM8-CAM)/1-CAM)
			MFISYM	+8D	#			  	 $ ROOT 2
		DSU	DDV
			0
			2
		SQRT	PDDL		# COFY = SQRT(MFISYM4-CAM)/(1-CAM) $ROOT2
			MFISYM
		DSU	DDV
			0
			2
		SQRT	VDEF		# COFX = SQRT(MFISYM-CAM)/(1-CAM) $ROOT 2
		UNIT
		STORE	COF
		
# DETERMINE LARGEST COF AND ADJUST ACCORDINGLY

COFMAXGO	DLOAD	DSU
			COF
			COF	+2
		BMN	DLOAD		# COFY G COFX
			COMP12
			COF
		DSU	BMN
			COF	+4
## Page 402
			METHOD3		# COFZ G COFX OR COFY
		GOTO
			METHOD1		# COFX G COFY OR COFZ
COMP12		DLOAD	DSU
			COF	+2
			COF	+4
		BMN
			METHOD3		# COFZ G COFY OR COFX
			
METHOD2		DLOAD	BPL		# COFY MAX
			COFSKEW	+2	# UY
			U2POS
		VLOAD	VCOMP
			COF
		STORE	COF
U2POS		DLOAD	BPL
			MFISYM	+2	# UX UY
			OKU21
		DLOAD	DCOMP		# SIGN OF UX OPPOSITE TO UY
			COF
		STORE	COF
OKU21		DLOAD	BPL
			MFISYM	+10D	# UY UZ
			LOCSKIRT
		DLOAD	DCOMP		# SIGN OF UZ OPPOSITE TO UY
			COF	+4
		STORE	COF	+4
		GOTO
			LOCSKIRT
METHOD1		DLOAD	BPL		# COFX MAX
			COFSKEW		# UX
			U1POS
		VLOAD	VCOMP
			COF
		STORE	COF
U1POS		DLOAD	BPL
			MFISYM	+2	# UX UY
			OKU12
		DLOAD	DCOMP
			COF	+2	# SIGN OF UY OPPOSITE TO UX
		STORE	COF	+2
OKU12		DLOAD	BPL
			MFISYM	+4	# UX UZ
			LOCSKIRT
		DLOAD	DCOMP		# SIGN OF UZ OPPOSITE TO UY
			COF	+4
		STORE	COF	+4
		GOTO
			LOCSKIRT
METHOD3		DLOAD	BPL		# COFZ MAX
## Page 403
			COFSKEW	+4	# UZ
			U3POS
		VLOAD	VCOMP
			COF
		STORE	COF
U3POS		DLOAD	BPL
			MFISYM	+4	# UX UZ
			OKU31
		DLOAD	DCOMP
			COF		# SIGN OF UX OPPOSITE TO UZ
		STORE	COF
OKU31		DLOAD	BPL
			MFISYM	+10D	# UY UZ
			LOCSKIRT
		DLOAD	DCOMP
			COF	+2	# SIGN OF UY OPPOSITE TO UZ
		STORE	COF	+2
		GOTO
			LOCSKIRT

## Page 404
# MATRIX OPERATIONS

MXM3		SETPD			# MXM3 MULTIPLIES 2 3X3 MATRICES
			0		# AND LEAVES RESULT IN PD LIST
		DLOAD*	PDDL*		# ADDRESS OF 1ST MATRIX IN XR1
			12D,2		# ADDRESS OF 2ND MATRIX IN XR2
			6,2
		PDDL*	VDEF		# DEFINE VECTOR M2(COL 1)
			0,2
		MXV*	PDDL*		# M1XM2(COL 1) IN PD
			0,1
			14D,2
		PDDL*	PDDL*
			8D,2
			2,2
		VDEF	MXV*		# DEFINE VECTOR M2(COL 2)
			0,1
		PDDL*	PDDL*		# M1XM2(COL2) IN PD
			16D,2
			10D,2
		PDDL*	VDEF		# DEFINE VECTOR M2(COL 3)
			4,2
		MXV*	PUSH		# M1XM2(COL 3) IN PD
			0,1
		GOTO
			TRNSPSPD	# REVERSE ROWS AND COLS IN PD AND

# RETURN WITH M1XM2 IN PD LIST
TRANSPOS	SETPD	VLOAD*		# TRANSPOS TRANSPOSES A 3X3 MATRIX
			0		#	AND LEAVES RESULT IN PD LIST
			0,1		# MATRIX ADDRESS IN XR1
		PDVL*	PDVL*
			6,1
			12D,1
		PUSH			# MATRIX IN PD
TRNSPSPD	DLOAD	PDDL		# ENTER WITH MATRIX IN PD LIST
			2
			6
		STODL	2
		STADR
		STODL	6
			4
		PDDL
			12D
		STODL	4
		STADR
		STODL	12D
			10D
		PDDL
## Page 405
			14D
		STODL	10D
		STADR
		STORE	14D
		RVQ			# RETURN WITH TRANSPOSED MATRIX IN PD LIST
MINANG		DEC	.00069375
MAXANG		DEC	.472222

# GIMBAL LOCK CONSTANTS

# D = MGA CORRESPONDING TO GIMBAL LOCK = 60 DEGREES
# NGL = BUFFER ANGLE (TO AVOID DIVISIONS BY ZERO) = 2 DEGREES

SD		DEC	.433015		# = SIN(D)				$2
K3S1		DEC	.86603		# = SIN(D)				$1
K4		DEC	-.25		# = -COS(D)				$2
K4SQ		DEC	.125		# = COS(D)COS(D)			$2
SNGLCD		DEC	.008725		# = SIN(NGL)COS(D)			$2
CNGL		DEC	.499695		# = COS(NGL)				$2
READCDUK	INHINT			# LOAD T(MPAC) WITH THE CURRENT CDU ANGLES
		CA	CDUZ
		TS	MPAC	+2
		EXTEND
		DCA	CDUX
		RELINT
		TCF	TLOAD	+6
		BANK	16
		SETLOC	KALCMON2
		BANK
		
		COUNT*	$$/KALC
		
CDUTODCM	AXT,1	SSP		# SUBROUTINE TO COMPUTE DIRECTION COSINE
		OCT	3		# MATRIX RELATING S/C AXES TO STABLE
			S1		# MEMBER AXES FROM 3 CDU ANGLES IN T(MPAC)
		OCT	1		# SET XR1, S1 AND PD FOR LOOP
		STORE	7
		SETPD
			0
LOOPSIN		SLOAD*	RTB
			10D,1
			CDULOGIC
		STORE	10D		# LOAD PD WITH 0	SIN(PHI)
		SIN	PDDL		#	       2	COS(PHI)
			10D		#	       4	SIN(THETA)
		COS	PUSH		#	       6	COS(THETA)
		TIX,1	DLOAD		#	       8	SIN(PSI)
			LOOPSIN		#	      10	COS(PSI)
			6
		DMP	SL1
			10D
## Page 406
		STORE	0,2
		DLOAD
			4
		DMP	PDDL
			0		# (PD6 SIN(THETA)SIN(PHI))
			6
		DMP	DMP
			8D
			2
		SL1	BDSU
			12D
		SL1
		STORE	2,2
		DLOAD
			2
		DMP	PDDL		# (PD7 COS(PHI)SIN(THETA)) SCALED 4
			4
			6
		DMP	DMP
			8D
			0
		SL1
		DAD	SL1
			14D
		STORE	4,2
		DLOAD
			8D
		STORE	6,2
		DLOAD
			10D
		DMP	SL1
			2
		STORE	8D,2
		DLOAD
			10D
		DMP	DCOMP
			0
		SL1
		STORE	10D,2
		DLOAD
			4
		DMP	DCOMP
			10D
		SL1
		STORE	12D,2
		DLOAD
		DMP	SL1		# (PUSH UP 7)
			8D
		PDDL	DMP		# (PD7 COS(PHI)SIN(THETA)SIN(PSI)) SCALE 4
			6
## Page 407
			0
		DAD	SL1		# (PUSH UP 7)
		STADR			# C7=COS(PHI)SIN(THETA)SIN(PSI)
		STORE	14D,2
		DLOAD
		DMP	SL1		# (PUSH UP 6)
			8D
		PDDL	DMP		# (PD6 SIN(THETA)SIN(PHI)SIN(PSI)) SCALE 4
			6
			2
		DSU	SL1		# (PUSH UP 6)
		STADR
		STORE	16D,2		# C8=-SIN(THETA)SIN(PHI)SIN(PSI)
		RVQ			#    +COS(THETA)COS(PHI)
ENDDCM		EQUALS

		BANK	15
		SETLOC	KALCMON1
		BANK
		
# CALCULATION OF THE MATRIX DEL......
#
#	*      *               --T           *
#	DEL = (IDMATRIX)COS(A)+UU (1-COS(A))+UX SIN(A)		SCALED 1
#
#             -
#	WHERE U IS A UNIT VECTOR (DP SCALED 2) ALONG THE AXIS OF ROTATION.
#	A IS THE ANGLE OF ROTATION (DP SCALED 2)
#					   -
#	UPON ENTRY THE STARTING ADDRESS OF U IS COF, AND A IS IN MPAC

		COUNT	22/KALC
		
DELCOMP		SETPD	PUSH		# MPAC CONTAINS THE ANGLE A
			0
		SIN	PDDL		# PD0 = SIN(A)
		COS	PUSH		# PD2 = COS(A)
		SR2	PDDL		# PD2 = COS(A)				$8
		BDSU	BOVB		# PD4 = 1-COS(A)			$2
			DPHALF
			SIGNMPAC
			
# COMPUTE THE DIAGONAL COMPONENTS OF DEL

		PDDL
			COF
		DSQ	DMP
			4
		DAD	SL3
## Page 408
			2
		BOVB
			SIGNMPAC
		STODL	DEL		# UX UX(U-COS(A)) +COS(A)		$1
			COF	+2
		DSQ	DMP
			4
		DAD	SL3
			2
		BOVB	
			SIGNMPAC
		STODL	DEL	+8D	# UY UY(1-COS(A)) +COS(A)		$1
			COF	+4
		DSQ	DMP
			4
		DAD	SL3
			2
		BOVB
			SIGNMPAC
		STORE	DEL	+16D	# UZ UZ(1-COS(A)) +COS(A)		$1
		
# COMPUTE THE OFF DIAGONAL TERMS OF DEL

		DLOAD	DMP
			COF
			COF	+2
		DMP	SL1
			4
		PDDL	DMP		# D6	UX UY (1-COS A)			$ 4
			COF	+4
			0
		PUSH	DAD		# D8	UZ SIN A			$ 4
			6
		SL2	BOVB
			SIGNMPAC
		STODL	DEL	+6
		BDSU	SL2
		BOVB
			SIGNMPAC
		STODL	DEL	+2
			COF
		DMP	DMP
			COF	+4
			4
		SL1	PDDL		# D6	UX UZ (1-COS A )		$ 4
			COF	+2
		DMP	PUSH		# D8	UY SIN(A)
			0
		DAD	SL2
			6
## Page 409
		BOVB
			SIGNMPAC
		STODL	DEL	+4	# UX UZ (1-COS(A))+UY SIN(A)
		BDSU	SL2
		BOVB
			SIGNMPAC
		STODL	DEL	+12D	# UX UZ (U-COS(A))-UY SIN(A)
			COF	+2
		DMP	DMP
			COF	+4
			4
		SL1	PDDL		# D6	UY UZ (1-COS(A))		$ 4
			COF
		DMP	PUSH		# D8	UX SIN(A)
			0
		DAD	SL2
			6
		BOVB
			SIGNMPAC
		STODL	DEL	+14D	# UY UZ(1-COS(A)) +UX SIN(A)
		BDSU	SL2
		BOVB	
			SIGNMPAC
		STORE	DEL	+10D	# UY UZ(1-COS(A)) -UX SIN(A)
		RVQ
		
# DIRECTION COSINE MATRIX TO CDU ANGLE ROUTINE
# X1 CONTAINS THE COMPLEMENT OF THE STARTING ADDRESS FOR MATRIX (SCALED 2)
# LEAVES CDU ANGLES SCALED 2PI IN V(MPAC)
# COS(MGA) WILL BE LEFT IN S1 (SCALED 1)
#
# THE DIRECTION COSINE MATRIX RELATING S/C AXES TO STABLE MEMBER AXES CAN BE WRITTEN AS ***
#
#	C =COS(THETA)COS(PSI)
#	 0
#
#	C =-COS(THETA)SIN(PSI)COS(PHI)+SI (THETA)SIN(PHI)
#	 1
#
#	C =COS(THETA)SIN(PSI)SIN(PHI)+S N(THETA)COS(PHI)
#	 2
#
#	C =SIN(PSI)
#	 3
#
#	C =COS(PSI)COS(PHI)
#	 4
#
#	C =-COS(PSI)SIN(PHI)
#	 5
#
#	C =-SIN(THETA)COS(PSI)
#	 6
#
#	C =SIN(THETA)SIN(PSI)COS(PHI)+COS THETA)SIN(PHI)
#	 7
## Page 410
#	C =-SIN(THETA)SIN(PSI)SIN(PHI)+CO (THETA)COS(PHI)
#	 8
#
#	WHERE	PHI = OGA
#		THETA = IGA
#		PSI = MGA

DCMTOCDU	DLOAD*	ARCSIN
			6,1
		PUSH	COS		# PD +0		PSI
		SL1	BOVB
			SIGNMPAC
		STORE	S1
		DLOAD*	DCOMP
			12D,1
		DDV	ARCSIN
			S1
		PDDL*	BPL		# PD +2		THETA
			0,1		# MUST CHECK THE SIGN OF COS(THETA)
			OKTHETA		# TO DETERMINE THE PROPER QUADRANT
		DLOAD	DCOMP
		BPL	DAD
			SUHALFA
			DPHALF
		GOTO
			CALCPHI
SUHALFA		DSU
			DPHALF
CALCPHI		PUSH
OKTHETA		DLOAD*	DCOMP
			10D,1
		DDV	ARCSIN
			S1
		PDDL*	BPL		# PUSH DOWN PHI
			8D,1
			OKPHI
		DLOAD	DCOMP		# PUSH UP PHI
		BPL	DAD
			SUHALFAP
			DPHALF
		GOTO
			VECOFANG
SUHALFAP	DSU	GOTO
			DPHALF
			VECOFANG
OKPHI		DLOAD			# PUSH UP PHI
VECOFANG	VDEF	RVQ

## Page 411
# ROUTINE FOR TERMINATING AUTOMATIC MANEUVERS

NOGOM2		INHINT			# THIS LOCATION ACCESSED BY A BZMF NOGO -2
		TC	BANKCALL
		CADR	ZEROEROR
		
NOGO		INHINT
		TC	STOPRATE
				
					# TERMINATE MANEUVER
		CAF	TWO		# NOTE - ALL RETURNS ARE NOW MADE VIA
		TC	WAITLIST	# GOODEND
		EBANK=	BCDU
		2CADR	ENDMANU
		
		TCF	ENDOFJOB