Apollo-11/ANGLFIND.s

635 lines
12 KiB
ArmAsm
Raw Normal View History

2016-07-07 08:47:26 +00:00
# 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.
#
# 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) $2
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 STARLE
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)
ENDOCM 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 SIGN(A)
COF +2
DMP DMP
COF +4
4
SL1 PDDL # D6 UY UZ (1-COS(A)) $ 4
COF
DMP PUSH # D6 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)+SIN(THETA)SIN(PHI)
# 1
#
# C =COS(THETA)SIN(PSI)SIN(PHI)+SIN(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)+COS(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 ZEROERROR
NOGO INHINT
TC STOPRATE
# TERMINATE MANEUVER
CAF TWO # NOTE: ALL RETURNS ARE NOW MADE VIA
TC WAITLIST # GOODEND
EBANK= BCDU
2CADR ENDMANU
TCF ENDOFJOB