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