ANGLFIND.agc
### FILE="Main.annotation"
## Copyright: Public domain.
## Filename: ANGLFIND.agc
## Purpose: Part of the source code for Artemis (i.e., Colossus 3),
## build 072. This is for the Command Module's (CM)
## Apollo Guidance Computer (AGC), for
## Apollo 15-17.
## Assembler: yaYUL
## Contact: Sergio Navarro <sergionavarrog@gmail.com>
## Website: www.ibiblio.org/apollo/index.html
## Page Scans: www.ibiblio.org/apollo/ScansForConversion/Artemis072/
## Mod history: 2009-08-19 SN Adapted from corresponding Comanche 055 file.
## 2009-09-04 JL Fixed page number comment. Fixed bad octal number in NOGO.
## Fixed typos. Added missing code at end.
## 2010-02-20 RSB Un-##'d this header.
## 2017-01-21 RSB Proofed comment text by diff'ing vs Comanche 55
## and corrected errors found.
## 2021-05-30 ABS ENDOCM -> ENDDCM
## Page 403
SETLOC KALCMON1
BANK
EBANK= BCDU
COUNT* $$/KALC
KALCMAN3 TC INTPRET
+1 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
STADR
STORE TMFI # TMFI = TRANSPOSE (MFI) SCALED BY 4
## Page 404
#
# 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
TMFI
VSR1
## Page 405
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
METHOD3 # COFZ G COFX OR COFY
GOTO
## Page 406
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
COFSKEW +4 # UZ
U3POS
## Page 407
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 408
# 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 409
14D
STODL 10D
STADR
STORE 14D
RVQ # RETURN WITH TRANSPOSED MATRIX IN PD LIST
MINANG DEC .00069375
MAXANG DEC .472222
READCDUK INHINT # LOAD T(MPAC) WITH THE CURRENT CDU ANGLES
CA CDUZ
TS MPAC +2
EXTEND
DCA CDUX
RELINT
TCF TLOAD +6
SETLOC KALCMON2
BANK
COUNT* $$/KALC
CDUTODCM AXT,1 SSP # SUBROUTINE TO COMPUTE DIRECTION COSINE
OCT 3 # MATRIX RELATING S/C AXES TO STABLE
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
STORE 0,2
DLOAD
4
DMP PDDL
0 # (PD6 SIN(THETA)SIN(PHI))
6
DMP DMP
8D
2
SL1 BDSU
12D
SL1
## Page 410
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)) SCALE4
6
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)) SCALE4
6
2
DSU SL1 # (PUSH UP 6)
STADR
## Page 411
STORE 16D,2 # C8=-SIN(THETA)SIN(PHI)SIN(PSI)
RVQ # +COS(THETA)COS(PHI)
ENDDCM EQUALS
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* $$/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
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
## Page 412
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
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 SIN(A)
COF +2
DMP DMP
COF +4
4
SL1 PDDL # D6 UY UZ (1-COS(A)) $ 4
COF
DMP PUSH # D8 UX SIN(A)
## Page 413
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)+SI (THETA)SIN(PHI)
# 1
# C =COS(THETA)SIN(PSI)SIN(PHI) + S N(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
# C =-SIN(THETA)SIN(PSI)SIN(PHI)+CO (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
## Page 414
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 415
# ROUTINE FOR TERMINATING AUTOMATIC MANEUVERS
NOGOM3 EXIT
NOGOM2 INHINT # THIS LOCATION ACCESSED BY A BZMF NOGO -2
TC BANKCALL
CADR ZEROEROR
NOGO INHINT
TC STOPRATE
CAF R67BIT
MASK FLAGWRD8
EXTEND
BZF NOTR67ND
CAF 1SEC # WAITLIST TASK IN 1 SECON:
TC WAITLIST # RETURN TO UPDTCALL
EBANK= BCDU
2CADR R67
TC PHASCHNG
OCT 00111
1P11SPT2 = 1.11SPOT
TC ENDOFJOB
NOTR67ND CAF TWO # RETURN VIA
TC WAITLIST # GOODEND
EBANK= BCDU
2CADR ENDMANU
TCF ENDOFJOB
