# Copyright: Public domain.
# Filename: IMU_COMPENSATION_PACKAGE.agc
# Purpose:  Part of the source code for Luminary 1A build 099.
# It is part of the source code for the Lunar Module's (LM)
# Apollo Guidance Computer (AGC), for Apollo 11.
# Assembler: yaYUL
# Contact: Ron Burkey <info@sandroid.org>.
# Website: www.ibiblio.org/apollo.
# Pages: 326-337
# Mod history: 2009-05-16 RSB Adapted from the corresponding
# Luminary131 file, using page
# images from Luminary 1A.
# 2009-06-07 RSB Corrected a typo.
#
# 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 001 of AGC program LMY99 by NASA 2021112-061
# 16:27 JULY 14, 1969

# Page 326
  BANK 7
  SETLOC IMUCOMP
  BANK

  EBANK= NBDX

  COUNT* $$/ICOMP
1/PIPA  CAF LGCOMP  # SAVE EBANK OF CALLING PROGRAM
  XCH EBANK
  TS MODE

  CCS GCOMPSW  # BYPASS IF GCOMPSW NEGATIVE
  TCF +3
  TCF +2
  TCF IRIG1  # RETURN

1/PIPA1  CAF FOUR  # PIPAZ, PIPAY, PIPAX
  TS BUF +2

  INDEX BUF +2
  CA PIPASCF  # (P.P.M.) X 2(-9)
  EXTEND
  INDEX BUF +2
  MP DELVX  # (PP) X 2(+14) NOW (PIPA PULSES) X 2(+5)
  TS Q  # SAVE MAJOR PART

  CA L  # MINOR PART
  EXTEND
  MP BIT6  # SCALE 2(+9)   SHIFT RIGHT 9
  INDEX  BUF +2
  TS DELVX +1 # FRACTIONAL PIPA PULSES SCALED 2(+14)

  CA Q  # MAJOR PART
  EXTEND
  MP BIT6  # SCALE 2(+9) SHIFT RIGHT 9
  INDEX BUF  +2
  DAS DELVX  # (PIPAI) + (PIPAI)(SFE)

  INDEX BUF +2
  CS PIPABIAS # (PIPA PULSES)/(CS) X 2(-5)  *
  EXTEND
  MP 1/PIPADT # (CS) X 2(+8)  NOW (PIPA PULSES) X 2(+3)*
  EXTEND
  MP BIT4  # SCALE 2(+11)  SHIFT RIGHT 11  *
  INDEX BUF +2
  DAS DELVX  # (PIPAI) + (PIPAI)(SFE) - (BIAS)(DELTAT)

  CCS BUF  +2 # PIPAZ, PIPAY, PIPAX
  AD NEG1
  TCF 1/PIPA1 +1
# Page 327
  NOOP   # LESS THAN ZERO IMPOSSIBLE

# Page 328
IRIGCOMP TS GCOMPSW  # INDICATE COMMANDS 2 PULSES OR LESS.
  TS BUF  # INDEX COUNTER .  IRIGX, IRIGY, IRIGZ.

  TC IRIGX  # COMPENSATE ACCELERATION TERMS

  CS NBDX  # (GYRO PULSES)/(CS) X 2(-5)
  TC DRIFTSUB # -(NBOX)(DELTAT)   (GYRO PULSES) X 2(+14)

  TC IRIGY  # COMPENSATE ACCELERATION TERMS

  CS NBDY  # (GYRO PULSES)/(CS) X 2(-5)
  TC DRIFTSUB # -(NBDY)(DELTAT)   (GYRO PULSES) X 2(+14)

  TC IRIGZ  # COMPENSATE ACCELERATION TERMS

  CA NBDZ  # (GYRO PULSES)/(CS) X 2(-5)
  TC DRIFTSUB # +(NBDZ)(DELTAT)   (GYRO PULSES) X 2(+14)

  CCS GCOMPSW  # ARE GYRO COMMANDS GREATER THAN 2 PULSES
  TCF +2  # YES  SEND OUT GYRO TORQUING COMMANDS.
  TCF IRIG1  # NO  RETURN

  CA PRIO21  # PRIO GREATER THAN SERVICER
  TC NOVAC  # SEND OUT GYRO TORQUING COMMANDS.
  EBANK= NBDX
  2CADR 1/GYRO

  RELINT
IRIG1  CA MODE  # RESTORE CALLERS EBANK
  TS EBANK
  TCF SWRETURN

# Page 329
IRIGX  EXTEND
  QXCH MPAC +2         # SAVE Q
  EXTEND
  DCS DELVX  # (PIPA PULSES) X 2(+14)
  DXCH MPAC
  CA ADIAX  # (GYRO PULSES)/(PIPA PULSE) X 2(-6)     *
  TC GCOMPSUB # -(ADIAX)(PIPAX)   (GYRO PULSES) X 2(+14)

  EXTEND   #
  DCS DELVY  # (PIPA PULSES) X 2(+14)
  DXCH MPAC  #
  CS ADSRAX  # (GYRO PULSES)/(PIPA PULSE) X 2(-6)  *
  TC GCOMPSUB # +(ADSRAX)(PIPAY)  (GYRO PULSES) X 2(+14)

# EXTEND  # ***
# DCS DELVZ  # ***    (PIPA PULSES) X 2(+14)
# DXCH MPAC  # ***
# CA ADOAX  # ***    (GYRO PULSES)/(PIPA PULSE) X 2(-6)  *
# TC GCOMPSUB # ***    -(ADOAX)(PIPAZ)   (GYRO PULSES) X 2(+14)

  TC MPAC  +2

IRIGY  EXTEND
  QXCH MPAC  +2 # SAVE Q
  EXTEND
  DCS DELVY  # (PIPA PULSES) X 2(+14)
  DXCH MPAC
  CA ADIAY  # (GYRO PULSES)/(PIPA PULSE) X 2(-6)     *
  TC GCOMPSUB # -(ADIAY)(PIPAY)   (GYRO PULSES) X 2(+14)

  EXTEND
  DCS DELVZ  # (PIPA PULSES) X 2(+14)
  DXCH MPAC
  CS ADSRAY  # (GYRO PULSES)/(PIPA PULSE) X 2(-6)  *
  TC GCOMPSUB # +(ADSRAY)(PIPAZ)  (GYRO PULSES) X 2(+14)

# EXTEND  # ***
# DCS DELVX  # ***    (PIPA PULSES) X 2(+14)
# DXCH MPAC  # ***
# CA ADOAY  # ***    (GYRO PULSES)/(PIPA PULSE) X 2(-6)  *
# TC GCOMPSUB # ***    -(ADOAY)(PIPAX)   (GYRO PULSES) X 2(+14)

  TC MPAC +2

IRIGZ  EXTEND
  QXCH MPAC +2  # SAVE Q
  EXTEND
  DCS DELVY  # (PIPA PULSES) X 2(+14)
  DXCH MPAC
  CA ADSRAZ  # (GYRO PULSES)/(PIPA PULSE) X 2(-6)  *
# Page 330
  TC GCOMPSUB # -(ADSRAZ)(PIPAY)  (GYRO PULSES) X 2(+14)

  EXTEND
  DCS DELVZ  # (PIPA PULSES) X 2(+14)
  DXCH MPAC
  CA ADIAZ  # (GYRO PULSES)/(PIPA PULSE) X 2(-6)  *
  TC GCOMPSUB # -(ADIAZ)(PIPAZ)   (GYRO PULSES) X 2(+14)

# EXTEND  # ***
# DCS DELVX  # ***   (PIPA PULSE) X 2(+14)
# DXCH MPAC  # ***
# CS ADOAZ  # ***   (GYRO PULSES)/(PIPA PULSE) X 2(-6)     *
# TC GCOMPSUB # ***   +(ADOAZ)(PIPAX)   (GYRO PULSES) X 2(+14)

  TC MPAC +2

# Page 331
GCOMPSUB XCH MPAC  # ADIA OR ADSRA COEFFICIENT ARRIVES IN A
  EXTEND   # C(MPAC) = (PIPA PULSES) X 2(+14)
  MP MPAC  # (GYRO PULSES)/(PIPA PULSE) X 2(-6)  *
  DXCH VBUF  # NOW = (GYRO PULSES) X 2(+8)  *

  CA MPAC +1  # MINOR PART OF PIPA PULSES
  EXTEND
  MP MPAC  # ADIA OR ADSRA
  TS L
  CAF ZERO
  DAS VBUF  # NOW = (GYRO PULSES) X 2(+8)  *

  CA VBUF  # PARTIAL RESULT - MAJOR
  EXTEND
  MP BIT9  # SCALE 2(+6)    SHIFT RIGHT           *
  INDEX BUF  # RESULT = (GYRO PULSES) X 2(+14)
  DAS GCOMP  # HI(ADIA)(PIPAI)  OR  HI(ADSRA)(PIPAI)

  CA VBUF +1  # PARTIAL RESULT - MINOR
  EXTEND
  MP BIT9  # SCALE 2(+6) SHIFT RIGHT 6  *
  TS L
  CAF ZERO
  INDEX BUF  # RESULT = (GYRO PULSES) X 2(+14)
  DAS GCOMP  # (ADIA)(PIPAI) OR (ADSRA)(PIPAI)

  TC Q

# Page 332
DRIFTSUB EXTEND
  QXCH BUF +1

  EXTEND   # C(A) = NBD (GYRO PULSES)/(CS) X 2(-5)
  MP 1/PIPADT # (CS) X 2(+8)  NOW (GYRO PULSES) X 2(+3)
  LXCH MPAC +1         # SAVE FOR FRACTIONAL COMPENSATION
  EXTEND
  MP BIT4  # SCALE 2(+11)    SHIFT RIGHT 11
  INDEX BUF
  DAS GCOMP  # HI(NBD)(DELTAT)   (GYRO PULSES) X 2(+14)

  CA MPAC +1         # NOW MINOR PART
  EXTEND
  MP BIT4  # SCALE 2(+11)    SHIFT RIGHT 11
  TS L
  CAF ZERO
  INDEX BUF  # ADD IN FRACTIONAL COMPENSATION
  DAS GCOMP  # (NBD)(DELTAT)     (GYRO PULSES) X 2(+14)

DRFTSUB2 CAF TWO  # PIPAX, PIPAY, PIPAZ
  AD BUF
  XCH BUF
  INDEX A
  CCS GCOMP  # ARE GYRO COMMANDS 1 PULSE OR GREATER
  TCF +2  # YES
  TC BUF +1         # NO

  MASK COMPCHK  # DEC -1
  CCS A  # ARE GYRO COMMANDS GREATER THAN 2 PULSES
  TS GCOMPSW  # YES - SET GCOMPSW POSITIVE
  TC BUF +1         # NO

# Page 333
1/GYRO  CAF FOUR  # PIPAZ, PIPAY, PIPAX
  TS BUF

  INDEX BUF  # SCALE GYRO COMMANDS FOR IMUPULSE
  CA GCOMP  +1 # FRACTIONAL PULSES
  EXTEND
  MP BIT8  # SHIFT RIGHT 7
  INDEX BUF
  TS GCOMP  +1 # FRACTIONAL PULSES SCALED

  CAF ZERO  # SET GCOMP = 0 FOR DAS INSTRUCTION
  INDEX BUF
  XCH GCOMP  # GYRO PULSES
  EXTEND
  MP BIT8  # SHIFT RIGHT 7
  INDEX BUF
  DAS GCOMP  # ADD THESE TO FRACTIONAL PULSES ABOVE

  CCS BUF  # PIPAZ, PIPAY, PIPAX
  AD NEG1
  TCF 1/GYRO +1
LGCOMP  ECADR GCOMP  # LESS THAN ZERO IMPOSSIBLE

  CAF LGCOMP
  TC BANKCALL
  CADR IMUPULSE # CALL GYRO TORQUING ROUTINE
  TC BANKCALL
  CADR IMUSTALL # WAIT FOR PULSES TO GET OUT
  TCF ENDOFJOB # TEMPORARY

GCOMP1  CAF FOUR  # PIPAZ, PIPAY, PIPAX
  TS BUF

  INDEX BUF  # RESCALE
  CA GCOMP +1
  EXTEND
  MP BIT8  # SHIFT MINOR PART LEFT 7 - MAJOR PART = 0
  INDEX BUF
  LXCH GCOMP +1 # BITS 8-14 OF MINOR PART WERE = 0

  CCS BUF  # PIPAZ, PIPAY, PIPAX
  AD NEG1
  TCF GCOMP1 +1
COMPCHK  DEC -1  # LESS THAN ZERO IMPOSSIBLE
  TCF ENDOFJOB

# Page 334
NBDONLY  CCS GCOMPSW  # BYPASS IF GCOMPSW NEGATIVE
  TCF +3
  TCF +2
  TCF ENDOFJOB

  INHINT
  CCS FLAGWRD2 # PREREAD T3RUPT MAY COINCIDE
  TCF ENDOFJOB
  TCF ENDOFJOB
  TCF +1

  CA FLAGWRD8 # IF SURFACE FLAG IS SET, SET TEM1
  MASK BIT8  # POSITIVE SO THAT THE ACCELERATION TERMS
  TS TEM1  # WILL BE COMPENSATED.
  EXTEND
  BZF +3  # ARE WE ON THE SURFACE

  TC IBNKCALL # ON THE SURFACE
  CADR PIPASR +3 # READ PIPAS, BUT DO NOT SCALE THEM

  CA TIME1  # (CS) X 2(+14)
  XCH 1/PIPADT # PREVIOUS TIME
  RELINT
  COM
  AD 1/PIPADT # PRESENT TIME - PREVIOUS TIME
NBD2  AD HALF  # CORRECT FOR POSSIBLE TIME1 TICK
  AD HALF
  XCH L  # IF TIME1 DID NOT TICK, REMOVE RESULTING
  XCH L  # OVERFLOW.

NBD3  EXTEND   # C(A) = DELTAT    (CS) X 2(+14)
  MP BIT10  # SHIFT RIGHT 5
  DXCH VBUF +2

  CA ZERO
  TS GCOMPSW  # INDICATE COMMANDS 2 PULSES OR LESS.
  TS BUF  # INDEX  X, Y, Z.

  CCS TEM1  # IF SURFACE FLAG IS SET,
  TC IRIGX  # COMPENSATE ACCELERATION TERMS.

  EXTEND
  DCA VBUF +2
  DXCH MPAC  # DELTAT NOW SCALED (CS) X 2(+19)

  CS NBDX  # (GYRO PULSES)/(CS) X 2(-5)
  TC FBIASSUB # -(NBDX)(DELTAT)   (GYRO PULSES) X 2(+14)

  CCS TEM1  # IF SURFACE FLAG IS SET,
  TC IRIGY  # COMPENSATE ACCELERATION TERMS.
# Page 335
  EXTEND
  DCS VBUF +2
  DXCH MPAC  # DELTAT SCALED (CS) X 2(+19)
  CA NBDY  # (GYRO PULSES)/(CS) X 2(-5)
  TC FBIASSUB # -(NBDY)(DELTAT)   (GYRO PULSES) X 2(+14)

  CCS TEM1  # IF SURFACE FLAG IS SET.
  TC IRIGZ  # COMPENSATE ACCELERATION TERMS

  EXTEND
  DCS VBUF +2
  DXCH MPAC  # DELTAT SCALED (CS) X 2(+19)
  CS NBDZ  # (GYRO PULSES)/(CS) X 2(-5)
  TC FBIASSUB # +(NBDZ)(DELTAT)   (GYRO PULSES) X 2(+14)

  CCS GCOMPSW  # ARE GYRO COMMANDS GREATER THAN 2 PULSES
  TCF 1/GYRO  # YES
  TCF ENDOFJOB # NO

# Page 336
FBIASSUB XCH Q
  TS BUF +1

  CA Q  # NBD SCALED (GYRO PULSES)/(CS) X 2(-5)
  EXTEND
  MP MPAC  # DELTAT SCALED (CS) X 2(+19)
  INDEX BUF
  DAS GCOMP  # HI(NBD)(DELTAT)   (GYRO PULSES) X 2(+14)

  CA Q  # NOW FRACTIONAL PART
  EXTEND
  MP MPAC +1
  TS L
  CAF ZERO
  INDEX BUF
  DAS GCOMP  # (NBD)(DELTAT)     (GYRO PULSES) X 2(+14)

  TCF DRFTSUB2 # CHECK MAGNITUDE OF COMPENSATION

LASTBIAS TC BANKCALL
  CADR PIPUSE1

  CCS GCOMPSW
  TCF +3
  TCF +2
  TCF ENDOFJOB

  CA FLAGWRD8 # IF SURFACE FLAG IS SET, SET TEM1
  MASK SURFFBIT # POSITIVE SO THAT THE ACCELERATION TERMS
  TS TEM1  # WILL BE COMPENSATED.

  CAF PRIO31  # 2 SECONDS SCALED (CS) X 2(+8)
  XCH 1/PIPADT
  COM
  AD PIPTIME +1
  TCF NBD2

GCOMPZER CAF LGCOMP  # ROUTINE TO ZERO GCOMP BEFORE FIRST
  XCH EBANK  # CALL TO 1/PIPA
  TS MODE

  CAF ZERO
  TS GCOMPSW
  TS GCOMP
  TS GCOMP +1
  TS GCOMP +2
  TS GCOMP +3
  TS GCOMP +4
# Page 337
  TS GCOMP +5

  TCF IRIG1  # RESTORE EBANK AND RETURN