### FILE="Main.annotation"
## Copyright:	Public domain.
## Filename:	IMU_COMPENSATION_PACKAGE.agc
## Purpose:	Part of the source code for Solarium build 55. This
##		is for the Command Module's (CM) Apollo Guidance
##		Computer (AGC), for Apollo 6.
## Assembler:	yaYUL --block1
## Contact:	Jim Lawton <jim DOT lawton AT gmail DOT com>
## Website:	www.ibiblio.org/apollo/index.html
## Page Scans:	www.ibiblio.org/apollo/ScansForConversion/Solarium055/
## Mod history:	2009-09-29 JL	Created.
##		2016-08-18 RSB	Typos.
## 		2016-12-28 RSB	Proofed comment text using octopus/ProoferComments,
##				and fixed errors found.

## Page 214

#	THE FOLLWOING ROUTINE IS DESIGNED TO COMPENSATE FOR PIPA BIAS AND SCALE FACTOR ERROR. ATTHE SAME TIME,
# IT ACCUMULATES GYRO TORQUE COMMANDS NECESSARY TO COMPENSATE FOR THE ASSOCIATED BIAS AND ACCELERATION-CAUSED GYRO
# DRIFTS. THIS ROUTINE EXPECTS TO FIND PIPA READINGS IN THE MAJOR PARTS OF DELV (DELVX, DELVY, DELVZ), WITH THE
# MINOR PARTS IRRELEVANT. OUTPUT FROM 1/PIPA IS DOUBLE PRECISION COMPENSATED DATA, THE MINOR PART OF WHICH USUALLY
# CONTAINS ALL THE COMPENSATION. 1/PIPA MUST BE CALLED AT LEAST EVERY 2.56 SECONDS, DUE TO SCALING CONSIDERATIONS.
# SPECIFICALLY, THE CORRECTION IS:
#
#	PIPA  = PIPA (1 + SCALE FACTOR ERROR) - BIAS DELTAT
#	    C       I
#
# WHERE PIPA  IS THE COMPENSATED DATA OBTAINED FROM THE SAMPLED PIPA .
#           C                                                       I



		BANK	14
1/PIPA		CAF	FOUR		# LOOP TO PROCESS INPUT PIPA DATA.
 +1		TS	BUF +2
		INDEX	A
		CS	DELVX		# SAMPLED PIPA INTO MPAC
		TS	MPAC
		INDEX	BUF +2		# SUBTRACT BIAS TERM (LESS THAN 1 BIT).
		CS	PIPABIAS
		EXTEND
		MP	1/PIPADT	# SUPPLIED BY USER SCALED AT CS 2(-8),
		TS	MPAC +1

		CS	MPAC		# SHIFT THE PIPA READING LEFT THREE PLACES
		TS	MPAC
		EXTEND			# IN PREPARATION FOR SCALE FACTOR
		MP	BIT4		# CORRECTION. IF IT OVERFLOWS, SHIFT THE
		CCS	A		# SCALE FACTOR ERROR RIGHT 11 PLACES
		TC	PIPOVF		# TAKING A LOSS IN PRECISION.
		
		TC	+2		# THE ABOVE HAPPENS AT DELTA VS OF OVER
		TC	PIPOVF		# 12 G-SECS., RESULTING IN A SCALE FACTOR
		INDEX	BUF +2		# CORRECTION GOOD ONLY TO 60 PPM.
		CS	PIPASCF
		COM
		EXTEND
		MP	LP
		AD	MPAC +1		# ADD TO BIAS CORRECTION.
		INDEX	BUF +2
		TS	DELVX +1	# AND MAYBE SKIP.
		CAF	ZERO
		
		AD	MPAC
		INDEX	BUF +2
		TS	DELVX
## Page 215

PIPJUMP		CCS	BUF +2		# LOOP TO PROCESS NEXT COMPONENT.
		AD	NEG1
		TC	1/PIPA +1
LGCOMP		ADRES	GCOMP

## Page 216

#	THE FOLLOWING ROUTINE COMPUTES THE GYRO TORQUES NECESSARY TO COMPENSATE FOR THE ACCELERATION-SENSITIVE
# DRIFTS DUE TO THE DATA JUST PROCESSED, AND SUMS THEM INTO THE GCOMP REGISTERS.

		TS	GCOMPSW		# SET GYRO TORQUE INDICATOR TO ZERO.
		TS	BUF		# RELATIVE ADDRESS OF PRESENT COMPONENT.
		
		CS	DELVX		# PROCESS X COMPONENT.
		TS	MPAC
		CS	DELVX +1
		TS	MPAC +1
		CS	ADIAX
		TC	GCOMPSUB -1
		
		CS	DELVY
		TS	MPAC
		CS	DELVY +1
		TS	MPAC +1
		CS	ADSRAX
		TC	GCOMPSUB
		
		CS	GBIASX		# ADD IN BIAS DRIFTS.
		TC	DRIFTSUB
		
		CS	DELVY		# COMPUTE Y GYRO COMPENSATION.
		TS	MPAC
		CS	DELVY +1
		TS	MPAC +1
		CS	ADIAY
		TC	GCOMPSUB -1
		
		CS	DELVZ
		TS	MPAC
		CS	DELVZ +1
		TS	MPAC +1
		CS	ADSRAY
		TC	GCOMPSUB
		
		CS	GBIASY
		TC	DRIFTSUB

## Page 217
		CS	DELVY		# CORRESPONDING PATTERN FOR Z GYRO.
		TS	MPAC
		CS	DELVY +1
		TS	MPAC +1
		CS	ADSRAZ
		TC	GCOMPSUB -1
		
		CS	DELVZ
		TS	MPAC
		CS	DELVZ +1
		TS	MPAC +1
		CS	ADIAZ
		TC	GCOMPSUB -1
		
		CS	GBIASZ
		TC	DRIFTSUB

		CCS	GCOMPSW		# NON-ZERO IF TIME TO PUT OUT COMPENSATION
		TC	+2
		TC	SWRETURN	# TORQUES NOT BIG ENOUGH YET.
		
		INHINT
		CAF	PRIO35		# REQUEST COMPENSATION JOB
		TC	NOVAC
		CADR	1/GYRO
		
		TC	MODEEXIT	# RELINT AND RETURN TO SWRETURN.

## Page 218

#	SUBROUTINES USED TO ACCUMULATE GYRO COMPENSATION COMMANDS.

 -1		COM			# (ENTERS HERE TO RE-COMPLEMENT INPUT).
GCOMPSUB	XCH	MPAC +1		# DOES DP BY SINGLE MULTIPLY WITH DP ADD
		EXTEND			# TO STORAGE.
		MP	MPAC +1
		XCH	MPAC +1
		EXTEND
		MP	MPAC
		TS	OVCTR
		XCH	LP
		AD	MPAC +1		# (AND MAYBE INCREMENT OVCTR).

BIASCOMP	INDEX	BUF		# BIAS COMPENSATION ENTERS HERE.
		AD	GCOMP +1
		INDEX	BUF
		XCH	GCOMP +1
		XCH	OVCTR
		INDEX	BUF
		AD	GCOMP
		INDEX	BUF
		TS	GCOMP
		
		TC	Q

## Page 219

#	SUBROUTINE WHICH ADDS IN GYRO BIAS DRIFT, TESTS TORQUE SIZE TO SEE IF COMMANDS SHOULD BE PUT OUT, AND
# ADVANCES GCOMP POINTER.

DRIFTSUB	XCH	Q
		TS	BUF +1
		XCH	Q
		EXTEND
		MP	1/PIPADT
		EXTEND			# RESULT WAS IN GYRO PULSES X 2(+2).
		MP	FOUR		# RE-SCALE TO ADD INTO GCOMP REGISTER.
		TS	OVCTR
		XCH	LP		# GO TO STANDARD INCREMENT ROUTINE.
DRFTSUB2	TC	BIASCOMP
		
		CAF	TWO		# INCREMENT POINTER AND
		AD	BUF
		XCH	BUF		# CHECK MAGNITUDE OF TORQUE COMMAND. IF
		INDEX	A		# SUFFICIENTLY LARGE, SET GCOMPSW PNZ TO
		CCS	GCOMP		# CALL FOR COMPENSATION JOB, 1/GYRO.
		TC	+2		# GETS DABS OF COMMAND.
		TC	BUF +1

		MASK	COMPCHK
		CCS	A
		TS	GCOMPSW
		TC	BUF +1		# RETURN.

## Page 220

#	(1/PIPA COMES HERE IF PIPA COUNTER SHOWED A DELTA V OF OVER 12 G-SEC.)

PIPOVF		INDEX	BUF +2		# SHIFT THE SCALE FACTOR RIGHT 11 ON OVF.
		CS	PIPASCF
		COM
		EXTEND
		MP	BIT4
		EXTEND			# FORM DP INCREMENT TO DELV IN THIS CASE.
		MP	MPAC
		TS	OVCTR
		
		XCH	LP
		AD	MPAC +1		# MAYBE INCREMENTING OVCTR.
		INDEX	BUF +2
		XCH	DELVX +1	# NO SKIP SINCE OVCTR SET.
		XCH	OVCTR
		AD	MPAC
		INDEX	BUF +2
		XCH	DELVX
		TC	PIPJUMP		# JUMP ON LOOP COUNTER.

## Page 221

#	THE FOLLOWING JOB IS INITIATED BY 1/PIPA TO PUT OUT COMPENSATING GYRO TORQUE COMMANDS.

1/GYRO		CAF	BIT11		# IF MODE SWITCH IN PROGRESS, DONT PUT
		MASK	FLAGWRD1	# OUT COMPENSATION THIS TIME.
		CCS	A
		TC	ENDOFJOB
		
		TC	FLAG1UP		# SHOW IMU ACTIVITY
		OCT	02000		# FLAG TURNED OFF BY GYRO ROUTINES.
		
		CAF	LGCOMP		# DISABLE THE T4RUPT CDU DRIVE THEMSELVES.
		TC	BANKCALL
		CADR	GYROSPNT	# NO TWITCH (2+2-) ON ZERO INPUT.
		
		TC	BANKCALL
		CADR	IMUSTALL
		TC	CURTAINS
		
		CAF	ZERO		# ZERO GCOMP REGSITERS SINCE COMMANDS HAVE
		TS	GCOMP		# BEEN PUT OUT.
		TS	GCOMP +2
		TS	GCOMP +4
		
		TC	ENDOFJOB
		
COMPCHK		OCT	-37		# COMMANDS MUST BE AT LEAST 32 PLS.

## Page 222

#	THE FOLLOWING ROUTINE SUPPLIES GYRO DRIFT COMPENSATION DURING THOSE PORTIONS OF A MISSION WHICH DO NOT
# USE PIPA DATA. THIS JOB IS CALLED EVERY 81.93 SECONDS (WHEN APPROPRIATE) BY THE IDLE TASK.

BIASONLY	CS	TIME1		# COMPUTE TIME DIFFERENCE (ALMOST ALWAYS
		COM			# 81.93 SECS.).
		XCH	OLDBT1
		COM
		AD	OLDBT1
BONLY2		CCS	A		# LASTBIAS ENTERS HERE.
		AD	ONE
		TC	BONLY3
		TC	+2
		TC	ENDOFJOB	# MAY HAVE A COMPUTED DIFF = 0.
		
		COM
		AD	POSMAX
		
BONLY3		EXTEND			# FORM DELTA-T AT 2(+8) CS.
		MP	BIT9		# SHIFT RIGHT 6.
		TS	MPAC
		TS	TEM9
		XCH	LP
		TS	MPAC +1
		TS	TEM10
		CAF	ZERO
		TS	GCOMPSW
		TS	BUF
		
		CS	GBIASX		# DO X, Y, AND THEN Z.
		TC	FBIASSUB
		
		CS	TEM9		# RELOAD DT.
		TS	MPAC
		CS	TEM10
		TS	MPAC +1
		CS	GBIASY
		TC	FBIASSUB -1
		
		CS	TEM9
		TS	MPAC
		CS	TEM10
		TS	MPAC +1
		CS	GBIASZ
		TC	FBIASSUB -1
## Page 223
		CCS	GCOMPSW		# PUT OUT COMPENSATION IF CALLED FOR.
		TC	1/GYRO
		TC	ENDOFJOB
		
 -1		COM
FBIASSUB	XCH	Q
		TS	BUF +1
		CAF	ZERO
		TS	MPAC +2
		XCH	Q
		TC	SHORTMP
		XCH	MPAC
		TS	OVCTR
		XCH	MPAC +1
		TC	DRFTSUB2	# RETURN TO CALLER VIA BUF +1.
		
LASTBIAS	XCH	OLDBT1		# COMES HERE FOR TRANSITION TO PIPA READ-
		COM			# ING MODE. NEW VALUE OF 1/PIPADT ARRIVES
		AD	MPAC		# IN A WITH WITH TIME1 AT PIPA ZEROING IN
		TC	BONLY2		# MPAC. EXITS VIA ENDOFJOB.