635 lines
12 KiB
ArmAsm
635 lines
12 KiB
ArmAsm
|
# 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
|
||
|
|
|