### FILE="Main.annotation"
## 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.
##		2016-12-21 RSB	Proofed comment text using octopus/ProoferComments
##				and corrected the errors found.
##
## 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)/4
			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.

# SPACER
## 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