# Copyright:    Public domain.
# Filename:     CM_BODY_ATTITUDE.agc
# Purpose:      Part of the source code for Comanche, build 055. It
#               is part of the source code for the Command Module's
#               (CM) Apollo Guidance Computer (AGC), Apollo 11.
# Assembler:    yaYUL
# Reference:    pp. 883-889
# Contact:      Ron Burkey <info@sandroid.org>
# Website:      http://www.ibiblio.org/apollo.
# Mod history:  2009-05-12 RSB	Adapted from Colossus249 file of the same
#				name and Comanche 055 page images.
#
# The contents of the "Comanche055" files, in general, are transcribed
# from scanned documents.
#
#       Assemble revision 055 of AGC program Comanche by NASA
#       2021113-051.  April 1, 1969.
#
#       This AGC program shall also be referred to as Colossus 2A
#
#       Prepared by
#                       Massachusetts Institute of Technology
#                       75 Cambridge Parkway
#                       Cambridge, Massachusetts
#
#       under NASA contract NAS 9-4065.
#
# Refer directly to the online document mentioned above for further
# information.  Please report any errors to info@sandroid.org.

# Page 883
		BANK	35

		SETLOC	BODYATT
		BANK

		COUNT	37/CMBAT

# PDL 12D - 15D SAFE.

# VALUES OF GIMBAL AND BODY ANGLES VALID AT PIP TIME ARE SAVED DURING	READACCS.

		EBANK=	RTINIT		# LET INTERPRETER SET EB

CM/POSE		TC	INTPRET		# COME HERE VIA AVEGEXIT.

		SETPD	VLOAD
			0
			VN		# KVSCALE = (12800/ .3048) /2VS
		VXSC	PDVL
			-KVSCALE	# KVSCALE = .81491944
			UNITW		# FULL UNIT VECTOR
		VXV	VXSC		# VREL = V - WE*R
			UNITR
			KWE
		VAD	STADR
		STORE	-VREL		# SAVE FOR ENTRY GUIDANCE.	REF COORDS

		UNIT	LXA,1
			36D		# ABVAL( -VREL) TO X1
		STORE	UXA/2		# -UVREL			REF COORDS

		VXV	VCOMP
			UNITR		# .5 UNIT			REF COORDS
		UNIT	SSP		# THE FOLLOWING IS TO PROVIDE A STABLE
			S1		# UN FOR THE END OF THE TERMINAL PHASE.
SPVQUIT		DEC	.019405		# 1000/ 2 VS
		TIX,1	VLOAD		# IF V-VQUIT POS, BRANCH.
			CM/POSE2	# SAVE UYA IN OLDUYA
			OLDUYA		# OTHERWISE CONTINUE TO USE OLDUYA.
CM/POSE2	STORE	UYA/2		#				REF COORDS

		STORE	OLDUYA		# RESTORE, OR SAVE AS CASE MAY BE.

		VXV	VCOMP
			UXA/2		# FINISH OBTAINING TRAJECTORY TRIAD.
		VSL1
		STORE	UZA/2		#				REF COORDS
# Page 884
		TLOAD			# PICK UP CDUX, CDUY, CDUZ CORRESPONDING
			AOG/PIP		# TO PIPUP TIME IN 2S.C AND SAVE.
CM/TRIO		STODL	24D
			25D		# AIG/PIP

		RTB	PUSH		# TO PDL0
			CDULOGIC
		COS
		STODL	UBX/2		# CI /2
					# AIG/PIP FROM PDL 0
		SIN	DCOMP
		STODL	UBX/2 +4	# -SI /2
			26D		# AMG/PIP
		RTB	PUSH		# TO PDL 0
			CDULOGIC
		SIN	PDDL		# XCH PDL 0.  SAVE SM /2
		COS	PDDL		# CM /2 TO PDL 2
			0		# SM /2
		DCOMP	VXSC
			UBX/2
		VSL1			# NOISE WONT OVFL.
		STODL	UBY/2		# =(-SMCI, NOISE, SMSI)/2
			2		# CM /2 REPLACES NOISE
		STODL	UBY/2 +2	# UBY/2=(-SMCI, CM, SMSI)/2
			24D		# AOG/PIP
		RTB	PUSH		# TO PDL 4
			CDULOGIC
		SIN	PDDL		# XCH PDL 4.  SAVE SO /2
		COS	VXSC		# CO /2
			UBY/2
		STODL	UBY/2		# UBY/2=(-COSMCI, COCM, COSMSI)/4
			4D		# SO /2
		DMP	DCOMP
			UBX/2 +4	# -SI /2
		DAD
			UBY/2		# INCREMENT BY (SOSI /4)
		STODL	UBY/2
					# SO /2 FROM PDL 4
		DMP	DAD
			UBX/2		# CI /2
			UBY/2 +4
		STOVL	UBY/2 +4	# YB/4				PLATFORM COORDS

				# YB = (-COSMCI + SOSI , COCM , COSMSI + SOCI )

			UBY/2
		VXM	VSL2
			REFSMMAT	# .5 UNIT
		STODL	UBY/2		# YB/2 DONE			REF COORDS
# Page 885
					# CM /2 FROM PDL 2
		VXSC	VSL1
			UBX/2
		STODL	UBX/2		# =( CMCI, NOISE, -CMSI)/2
		STADR			# SM /2 FROM PDL 0
		STOVL	UBX/2 +2	# SM /2 REPLACES NOISE
			UBX/2		# XB/2				PLATFORM COORDS

				# XB = ( CMCI , SM , -CMSI )

		VXM	VSL1
			REFSMMAT	# .5 UNIT
		STORE	UBX/2		# XB/2 DONE			REF COORDS

		VXV	VSL1
			UBY/2
		STOVL	UBZ/2		# ZB/2 DONE			REF COORDS

				# EQUIVALENT TO
				# ZB = ( SOSMCI + COSI , -SOCM , -SOSMSI + COCI)

			UXA/2		# -UVREL/2 = -UVA/2
		VXV	UNIT		# GET UNIT(-UVREL*UBY)/2 = UL/2
			UBY/2		# YB/2
		PUSH	DOT		# UL/2 TO PDL 0,5
			UZA/2		# UNA/2
		STOVL	COSTH		# COS(ROLL)/4
			0		# UL/2

		DOT
			UYA/2
		STCALL	SINTH		# -SIN(ROLL)/4
			ARCTRIG
		STOVL	6D		# -(ROLL/180) /2
			UBY/2
		DOT	SL1		# -UVA.UBY = -SIN(BETA)
			UXA/2		# -UVREL/2
		ARCSIN
		STOVL	7D		# -(BETA/180) /2
			UBX/2		# XB/2
		DOT			# UL.UBX = -SIN(ALFA)
			0		# UL/2
		STOVL	SINTH		# -SIN(ALFA)/4
		DOT			# UL/2 FROM PDL 0
			UBZ/2
		STCALL	COSTH		# COS(ALFA)/2
			ARCTRIG
		STOVL	8D		# -(ALFA/180) /2
			UNITR		# UR/2				REF COORDS
		DOT	SL1
# Page 886
			UZA/2		# MORE ACCURATE AT LARGE ARG.
		ARCCOS
		STORE	10D		# (-GAMA/180)/2

		TLOAD	EXIT		# ANGLES IN MPAC IN THE ORDER
					# -( (ROLL, BETA, ALFA) /180)/2
			6D		# THESE VALUES CORRECT AT PIPUP TIME.

# Page 887
# BASIC SUBROUTINE TO UPDATE ATTITUDE ANGLES

		EBANK=	AOG

CM/ATUP		CA	EBAOG
		TS	EBANK
CMTR1		INDEX	FIXLOC
		CS	10D		# (GAMA/180)/2
		XCH	GAMA
		TS	L

		INHINT
				# MUST REMAIN INHINTED UNTIL UPDATE OF BODY
				# ANGLES, SO THAT GAMDIFSW IS VALID FIRST PASS
				# INDICATOR.

		CS	CM/FLAGS
		MASK	BIT11		# GAMDIFSW=94D BIT11	INITLY=0
		EXTEND			# DONT CALC GAMA DOT UNTIL HAVE FORMD
					# ONE DIFFERENCE.
		BZF	DOGAMDOT	# IS OK, GO ON.
		ADS	CM/FLAGS	# KNOW BIT IS 0
		TC	NOGAMDOT	# SET GAMDOT = 0

DOGAMDOT	CS	L
		AD	GAMA		# DEL GAMA/360= T GAMDOT/360
		EXTEND
		MP	TCDU		# TCDU = .1 SEC, T = 2 SEC.
		TS	GAMDOT		# GAMA DOT TCDU / 180

		EXTEND			# IGNORE GAMDOT IF LEQ .5 DEG/SEC
		BZMF	+2
		COM
		AD	FIVE
		EXTEND
		BZMF	+3		# SET GAMDOT=+0 AS TAG IF TOO SMALL.

NOGAMDOT	CA	ZERO		# COME HERE INHINTED.
		TS	GAMDOT
					# FOR NOW LEAVE IN 2S.C
					# UPDATE ANGLES BY CORRECTING EUILER ANG
					# FOR ACCRUED INCREMENT SINCE PIPUP
					# R = R EUIL + R(NOW) -R(PIPUP)
		CS	MPAC		# GET (R EUL/180) /2
		DOUBLE			# POSSIBLE OVERFLOW
		TC	CORANGOV	# CORRECT FOR OVFL IF ANY
		EXTEND
		SU	ROLL/PIP	# GET INCR SINCE PIPUP
		AD	ROLL/180	# ONLY SINGLE OVFL POSSIBLE.
		TC	CORANGOV	# CORRECT FOR OVFL IF ANY
# Page 888
		TS	TEMPROLL

		CS	MPAC +2		# GET (ALFA EUL/180) /2
		DOUBLE			# SAME AS FOR ROLL.  NEEDED FOR EXT ATM DAP
		TC	CORANGOV	# CORRECT FOR OVFL IF ANY
		EXTEND
		SU	ALFA/PIP
		AD	ALFA/180
		TC	CORANGOV	# CORRECT FOR OVFL IF ANY
		TS	TEMPALFA

		CS	MPAC +1		# GET (BETA EUL/180) /2
CMTR2		DOUBLE
		EXTEND
		SU	BETA/PIP
		AD	BETA/180
		XCH	TEMPBETA	# OVFL NOT EXPECTED.

		CA	EBANK3
		TS	EBANK

		EBANK=	PHSNAME5
		EXTEND
		DCA	REPOSADR	# THIS ASSUMES THAT THE		TC  PHASCHNG
		DXCH	PHSNAME5	# IS NOT CHANGED IN		OCT 10035
					# SERVICER.

		CA	EBAOG
		TS	EBANK

		EBANK=	AOG
REDOPOSE	EXTEND			# RE-STARTS COME HERE
		DCA	TEMPROLL
		DXCH	ROLL/180
		CA	TEMPBETA
		TS	BETA/180

		RELINT

		TC	INTPRET		# CANT TC DANZIG AFTER PHASCHNG.
CM/POSE3	VLOAD	ABVAL		# RETURN FROM CM/ATUP.	(RESTART)
			VN		# 2(-7) M/CS
		STORE	VMAGI		# FOR DISPLAY ON CALL.

		GOTO
			POSEXIT		# ENDEXIT, STARTENT, OR SCALEPOP.

CORANGOV	TS	L
		TC	Q
		INDEX	A
# Page 889
		CA	LIMITS
		ADS	L
		TC	Q		# COSTS 2 MCT TO USE.  SEE ANGOVCOR.

-KVSCALE	2DEC	-.81491944	# -12800/(2 VS .3048)

TCDU		DEC	.1		# TCDU = .1 SEC.

		EBANK=	AOG
REPOSADR	2CADR	REDOPOSE