kjodle/Apollo-11
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
f1f0e58e2a
Apollo-11/Luminary099/ATTITUDE_MANEUVER_ROUTINE.agc

1028 lines
26 KiB
Plaintext
Raw Normal View History

Add remaining Apollo 11 source
2016-07-07 08:47:26 +00:00
# Copyright: Public domain.
# Filename: ATTITUDE_MANEUVER_ROUTINE.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: 342-363
# Mod history: 2009-05-16 RSB Adapted from the corresponding
# Luminary131 file, using page
# images from Luminary 1A.
#
# 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-61
# 16:27 JULY 14, 1969
# Page 342
# BLOCK 2 LGC ATTITUDE MANEUVER ROUTINE -- KALCMANU
#
# MOD 2 DATE 5/1/67 BY DON KEENE
#
# PROGRAM DESCRIPTION
#
# KALCMANU IS A ROUTINE WHICH GENERATES COMMANDS FOR THE LM DAP TO CHANGE THE ATTITUDE OF THE SPACECRAFT
# DURING FREE FALL. IT IS DESIGNED TO MANEUVER THE SPACECRAFT FROM ITS INITIAL ORIENTATION TO SOME DESIRED
# ORIENTATION SPECIFIED BY THE PROGRAM WHICH CALLS KALCMANU, AVOIDING GIMBAL LOCK IN THE PROCESS. IN THE
# MOD 2 VERSION, THIS DESIRED ATTITUDE IS SPECIFIED BY A SET OF OF THREE COMMANDED CDU ANGLES STORES AS 2'S COMPLEMENT
# SINGLE PRECISION ANGLES IN THE THREE CONSECUTIVE LOCATIONS, CPHI, CTHETA, CPSI, WHERE
#
# CPHI = COMMANDED OUTER GIMBAL ANGLE
# CTHETA = COMMANDED INNER GIMBAL ANGLE
# CPSI = COMMANDED MIDDLE GIMBAL ANGLE
#
# WHEN POINTING A SPACECRAFT AXIS (I.E., X, Y, Z, THE AOT, THRUST AXIS, ETC.) THE SUBROUTINE VECPOINT MAY BE
# USED TO GENERATE THIS SET OF DESIRED CDU ANGLES (SEE DESCRIPTION IN R60).
#
# WITH THIS INFORMATION KALCMANU DETERMINES THE DIRECTION OF THE SINGLE EQUIVALEN ROTATION (COF ALSO U) AND THE
# MAGNITUDE OF THE ROTATION (AM) TO BRING THE S/C FROM ITS INITIAL ORIENTATION TO ITS FINAL ORIENTATION.
# THIS DIRECTION REMAINS FIXED BOTH IN INERTIAL COORDINATES AND IN COMMANDED S/C AXES THROUGHOUT THE
# _
# MANEUVER. ONCE COF AND AM HAVE BEEN DETERMINED, KALCMANU THEN EXAMINES THE MANEUVER TO SEE IF IT WILL BRING
# _
# THE S/C THROUGH GIMBAL LOCK. IF SO, COF AND AM ARE READJUSTED SO THAT THE S/C WILL JUST SKIM THE GIMBAL
# LOCK ZONE AND ALIGN THE X-AXIS. IN GENERAL A FINAL YAW ABOUT X WILL BE NECESSARY TO COMPLETE THE MANEUVER.
# NEEDLESS TO SAY, NEITHER THE INITIAL NOR THE FINAL ORIENTATION CAN BE IN GIMBAL LOCK.
#
# FOR PROPER ATTITUDE CONTROL THE DIGITAL AUTOPILOT MUST BE GIVEN AN ATTITUDE REFERENCE WHICH IT CAN TRACK.
# KALCMANU DOES THIS BY GENERATING A REFERENCE OF DESIRED GIMBAL ANGLES (CDUXD, CDUYD, CDUZD) WHICH ARE UPDATED
# EVERY ONE SECOND DURING THE MANEUVER. TO ACHIEVE A SMOOTHER SEQUENCE OF COMMANDS BETWEEN SUCCESSIVE UPDATES,
# THE PROGRAM ALSO GENERATES A SET OF INCREMENTAL CDU ANGLES (DELDCDU) TO BE ADDED TO CDU DESIRED BY THE DIGITAL
# AUTOPILOT. KALCMANU ALSO CALCULATES THE COMPONENT MANEUVER RATES (OMEGAPD, OMEGAQD, OMEGARD), WHICH CAN
# _
# BE DETERMINED SIMPLY BY MULTIPLYING COF BY SOME SCALAR (ARATE) CORRESPONDING TO THE DESIRED ROTATIONAL RATE.
#
# AUTOMATIC MANEUVERS ARE TIMED WTH THE HELP OF WAITLIST SO THAT AFTER A SPECIFIED INTERVAL THE Y AND Z
# DESIRED RATES ARE SET TO ZERO AND THE DESIRED CDU ANGLES (CDUYD, CDUZD) ARE SET EQUAL TO THE FINAL DESIRED CDU
# ANGLES (CTHETA, CPSI). IF ANY YAW REMAINS DUE TO GIMBAL LOCK AVOIDANCE, THE FINAL YAW MANEUVER IS
# CALCULATED AND THE DESIRED YAW RATE SET TO SOME FIXED VALUE (ROLLRATE = + OR - 2 DEGREES PER SEC).
# IN THIS CASE ONLY AN INCREMENTAL CDUX ANGLE (DELFROLL) IS SUPPLIED TO THE DAP. AT THE END OF THE YAW
# MANEUVER OR IN THE EVENT THAT THERE WAS NO FINAL YAW, CDUXD IS SET EQUAL TO CPHI AND THE X-AXIS DESIRED
# RATE SET TO ZERO. THUS, UPON COMPLETION OF THE MANEUVER THE S/C WILL FINISH UP IN A LIMIT CYCLE ABOUT THE
# DESIRED GIMBAL ANGLES.
#
# PROGRAM LOGIC FLOW
#
# KALCMANU IS CALLED AS A HIGH PRIORITY JOB WITH ENTRY POINTS AT KALCMAN3 AND VECPOINT. IT FIRST PICKS
# UP THE CURRENT CDU ANGLES TO BE USED AS THE BASIS FOR ALL COMPUTATIONS INVOLVING THE INITIAL S/C ORIENTATION.
# Page 343
# IT THEN DETERMINES THE DIRECTION COSINE MATRICES RELATING BOTH THE INITIAL AND FINAL S/C ORIENTATION TO STABLE
# * * *
# MEMBER AXES (MIS,MFS). IT ALSO COMPUTES THE MATRIX RELATING FINAL S/C AXES TO INITIAL S/C AXES (MFI). THE
# ANGLE OF ROTATION (AM) IS THEN EXTRACTED FROM THIS MATRIX, AND TEST ARE MADE TO DETERMINE IF
#
# A) AM LESS THAN .25 DEGREES (MINANG)
# B) AM GREATER THAN 170 DEGREES (MAXANG)
#
# IF AM IS LESS THAN .25 DEGREES, NO COMPLICATED AUTOMATIC MANEUVERING IS NECESSARY. THREFORE, WE CAN SIMPLY
# SET CDU DESIRED EQUAL TO THE FINAL CDU DESIRED ANGLES AND TERMINATE THE JOB.
#
# IF AM IS GREATER THAN .25 DEGREES BUT LESS THAN 170 DEGREES THE AXES OF THE SINGLE EQUIVALENT ROTATION
# _ *
# (COF) IS EXTRACTED FROM THE SKEW SYMMETRIC COMPONENTS OF MFI.
# * *
# IF AM GREATER THAN 170 DEGREES AN ALTERNATE METHOD EMPLOYING THE SYMMETRIC PART OF MFI (MFISYM) IS USED
# _
# TO DETERMINE COF.
#
# THE PROGRAM THEN CHECKS TO SEE IF THE MANEUVER AS COMPUTED WILL BRING THE S/C THROUGH GIMBAL LOCK. IF
# SO, A NEW MANEUVER IS CALCULATED WHICH WILL JUST SKIM THE GIMBAL LOCK ZONE AND ALIGN THE S/C X-AXIS. THIS
# METHOD ASSURES THAT THE ADDITIONAL MANEUVERING TO AVOID GIMBAL LOCK WILL BE KEPT TO A MINIMUM. SINCE A FINAL
# P AXIS YAW WILL BE NECESSARY, A SWITCH IS RESET (STATE SWITCH 31) TO ALLOW FOR THE COMPUTATION OF THIS FINAL
# YAW.
#
# AS STATED PREVIOUSLY, KALCMANU GENERATES A SEQUENCE OF DESIRED GIMBAL ANGLES WHICH ARE UPDATED EVERY
# _
# SECOND. THIS IS ACCOMPLISHED BY A SMALL ROTATION OF THE DESIRED S/C FRAME ABOUT THE VECTOR COF. THE NEW
# DESIRED REFERENCE MATRIX IS THEN,
# * * *
# MIS = MIS DEL
# N+1 N
# *
# WHERE DEL IS THE MATRIX CORRESPONDING TO THIS SMALL ROTATION. THE NEW CDU ANGLES CAN THEN BE EXTRACTED
# *
# FROM MIS.
#
# AT THE BEGINNING OF THE MANEUVER THE AUTOPILOT DESIRED RATES (OMEGAPD, OMEGAQD, OMEGARD) AND THE
# MANEUVER TIMINGS ARE ESTABLISHED. ON THE FIRST PASS AND ON ALL SUBSEQUENT UPDATES THE CDU DESIRED
# ANGLES ARE LOADED WITH THE APPROPRIATE VALUES AND THE INCREMENTAL CDU ANGLES ARE COMPUTED. THE AGC CLOCKS
# (TIME1 AND TIME2) ARE THEN CHECKED TO SEE IF THE MANEUVER WILL TERMINATE BEFORE THE NEXT UPDATE. IF
# NOT, KALCMANU CALLS FOR ANOTHER UPDATE (RUN AS A JOB WITH PRIORITY TBD) IN ONE SECOND. ANY DELAYS IN THIS
# CALLING SEQUENCE ARE AUTOMATICALLY COMPENSATED IN CALLING FOR THE NEXT UPDATE.
#
# IF IT IS FOUND THAT THE MANEUVER IS TO TERMINATE BEFORE THE NEXT UPDATE A ROUTINE IS CALLED (AS A WAIT-
# LIST TASK) TO STOP THE MANEUVER AT THE APPROPRIATE TIME AS EXPLAINED ABOVE.
# Page 344
# CALLING SEQUENCE
#
# IN ORDER TO PERFORM A KALCMANU SUPERVISED MANEUVER, THE COMMANDED GIMBAL ANGLES MUST BE PRECOMPUTED AND
# STORED IN LOCATIONS CPHI, CTHETA, CPSI. THE USER'S PROGRAM MUST THEN CLEAR STATE SWITCH NO 33 TO ALLOW THE
# ATTITUDE MANEUVER ROUTINE TO PERFORM ANY FINAL P-AXIS YAW INCURRED BY AVOIDING GIMBAL LOCK. THE MANEUVER IS
# THEN INITIATED BY ESTABLISHING THE FOLLOWING EXECUTIVE JOB
# *
# CAF PRIO XX
# --
# INHINT
# TC FINDVAC
# 2CADR KALCMAN3
# RELINT
#
# THE USER'S PROGRAM MAY EITHER CONTINUE OR WAIT FOR THE TERMINATION OF THE MANEUVER. IF THE USER WISHES TO
# WAIT, HE MAY PUT HIS JOB TO SLEEP WTH THE FOLLOWING INSTRUCTIONS:
#
# L TC BANKCALL
# L+1 CADR ATTSTALL
# L+2 (BAD RETURN)
# L+3 (GOOD RETURN)
#
# UPON COMPLETION OF THE MANEUVER, THE PROGRAM WILL BE AWAKENED AT L+3 IF THE MANEUVER WAS COMPLETED
# SUCCESSFULLY, OR AT L+2 IF THE MANEUVER WAS ABORTED. THIS ABORT WOULD OCCUR IF THE INITIAL OR FINAL ATTITUDE
# WAS IN GIMBAL LOCK.
#
# *** NOTA BENE *** IF IT IS ASSUMED THAT THE DESIRED MANEUVERING RATE (0.5, 2, 5, 10 DEG/SEC) HAS BEEN SELECTED BY
# KEYBOARD ENTRY PRIOR TO THE EXECUTION OF KALCMANU.
#
# IT IS ALSO ASSUMED THAT THE AUTOPILOT IS IN THE AUTO MODE. IF THE MODE SWITCH IS CHANGED DURING THE
# MANEUVER, KALCMANU WILL TERMINATE VIA GOODEND WITHIN 1 SECOND SO THAT R60 MAY REQUEST A TRIM OF THE S/C ATTITUDE
# SUBROUTINES.
#
# KALCMANU USES A NUMBER OF INTERPRETIVE SUBROUTINES WHICH MAY BE OF GENERAL INTEREST. SINCE THESE ROUTINES
# WERE PROGRAMMED EXCLUSIVELY FOR KALCMANU, THEY ARE NOT, AS YET, GENERALLY AVAILABLE FOR USE BY OTHER PROGRAMS.
#
# MXM3
# ----
#
# THIS SUBROUTINE MULTIPLIES TWO 3X3 MATRICES AND LEAVES THE RESULT IN THE FIRST 18 LOCATIONS OF THE PUSH
# DOWN LIST, I.E.,
# [ M M M ]
# [ 0 1 2 ]
# * [ ] * *
# M = [ M M M ] = M1 X M2
# [ 3 4 5 ]
# [ ]
# [ M M M ]
# [ 6 7 8 ]
# Page 345
# *
# INDEX REGISTER X1 MUST BE LOADED WITH THE COMPLEMENT OF THE STARTING ADDRESS FOR M1, AND X2 MUST BE
# *
# LOADED WITH THE COMPLEMENT OF THE STARTING ADDRESS FOR M2. THE ROUTINE USES THE FIRST 20 LOCATIONS OF THE PUSH
# DOWN LIST. THE FIRST ELEMENT OF THE MATRIX APPEARS IN PDO. PUSH UP FOR M .
# 8
# TRANSPOS
# --------
#
# THIS ROUTINE TRANSPOSES A 3X3 MATRIX AND LEAVES THE RESULT IN THE PUSH DOWN LIST, I.E.,
#
# * * T
# M = M1
#
# INDEX REGISTER X1 MUST CONTAIN THE COMPLEMENT OF THE STARTING ADDRESS FOR M1. PUSH UP FOR THE FIRST AND SUB-
# *
# SEQUENT COMPONENTS OF M. THIS SUBROUTINE ALSO USES THE FIRST 20 LOCATIONS OF THE PUSH DOWN LIST.
#
# CDU TO DCM
# ----------
#
# THIS SUBROUTINE CONVERTS THREE CDU ANGLES IN T(MPAC) TO A DIRECTION COSINE MATRIX (SCALED BY 2) RELATING
# THE CORRESPONDING S/C ORIENTATIONS TO THE STABLE MEMBER FRAME. THE FORMULAS FOR THIS CONVERSION ARE
#
# M = COSY COSZ
# 0
#
# M = -COSY SINZ COSX + SINY SINX
# 1
#
# M = COSY SINZ SINX + SINY COSX
# 2
#
# M = SINZ
# 3
#
# M = COSZ COSX
# 4
#
# M = -COSZ SINX
# 5
#
# M = -SINY COSZ
# 6
#
# M = SINY SINZ COSX + COSY SINX
# 7
# Page 346
# M = -SINY SINZ SINX + COSY COSX
# 8
#
# WHERE X = OUTER GIMBAL ANGLE
# Y = INNER GIMBAL ANGLE
# Z = MIDDLE GIMBAL ANGLE
#
# THE INTERPRETATION OF THIS MATRIX IS AS FOLLOWS:
#
# IF A , A , A REPRESENT THE COMPONENTS OF A VECTOR IN S/C AXES THEN THE COMPONENTS OF THE SAME VECTOR IN
# X Y Z
# STABLE MEMBER AXES (B , B , B ) ARE
# X Y Z
#
# [ B ] [ A ]
# [ X ] [ X ]
# [ ] [ ]
# [ B ] * [ A ]
# [ Y ] = M [ Y ]
# [ ] [ ]
# [ B ] [ B ]
# [ Z ] [ Z ]
#
# THE SUBROUTINE WILL STORE THIS MATRIX IN SEQUENTIAL LOCATIONS OF ERASABLE MEMORY AS SPECIFIED BY THE CALLING
# *
# PROGRAM. TO DO THIS THE CALLING PROGRAM MUST FIRST LOAD X2 WITH THE COMPLEMENT OF THE STARTING ADDRESS FOR M.
#
# INTERNALLY, THE ROUTINE USES THE FIRST 16 LOCATIONS OF THE PUSH DOWN LIST, ALSO STEP REGISTER S1 AND INDEX
# REGISTER X2.
#
# DCM TO CDU
# ----------
# *
# THIS ROUTINE EXTRACTS THE CDU ANGLES FROM A DIRECTION COSINE MATRIX (M SCALED BY 2) RELATING S/C AXIS TO
# *
# STABLE MEMBER AXES. X1 MUST CONTAIN THE COMPLEMENT OF THE STARTING ADDRESS FOR M. THE SUBROUTINE LEAVES THE
# CORRESPONDING GIMBAL ANGLES IN V(MPAC) AS DOUBLE PRECISION 1'S COMPLEMENT ANGLES ACALED BY 2PI. THE FORMULAS
# FOR THIS CONVERSION ARE
#
# Z = ARCSIN (M )
# 3
#
# Y = ARCSIN (-M /COSZ)
# 6
#
# IF M IS NEGATIVE, Y IS REPLACED BY PI SGN Y - Y.
# 0
# Page 347
# X = ARCSIN (-M /COSZ)
# 5
#
# IF M IS NEGATIVE, X IS REPLACED BY PI SGN X - X.
# 4
#
# THIS ROUTINE DOES NOT SET THE PUSH DOWN POINTER, BUT USES THE NEXT 8 LOCATIONS OF THE PUSH DOWN LIST AND
# RETURNS THE POINTER TO ITS ORIGINAL SETTING. THIS PROCEDURE ALLOWS THE CALLER TO STORE THE MATRIX AT THE TOP OF
# THE PUSH DOWN LIST.
#
# DELCOMP
# -------
# *
# THIS ROUTINE COMPUTES THE DIRECTION COSINE MATRIX (DEL) RELATING ON
# _
# IS ROTATED WITH RESPECT TO THE FIRST BY AN ANGLE, A, ABOUT A UNIT VECTOR U. THE FORMULA FOR THIS MATRIX IS
#
# * * _ _T *
# DEL = I COSA + U U (1 - COSA) + V SINA
# X
#
# WHERE * [ 1 0 0 ]
# I = [ 0 1 0 ]
# [ 0 0 1 ]
#
# [ 2 ]
# [ U U U U U ]
# [ X X Y X Z ]
# [ ]
# _ _T [ 2 ]
# U U = [ U U U U U ]
# [ Y X Y Y Z ]
# [ ]
# [ 2 ]
# [ U U U U U ]
# [ Z X Z Y Z ]
#
#
# [ 0 -U U ]
# [ Z Y ]
# * [ ]
# V = [ U 0 -U ]
# X [ Z X ]
# [ ]
# [ -U U 0 ]
# [ Y X ]
#
# Page 348
# _
# U = UNIT ROTATION VECTOR RESOLVED INTO S/C AXES.
# A = ROTATION ANGLE
#
# *
# THE INTERPRETATION OF DEL IS AS FOLLOWS:
#
# IF A , A , A REPRESENT THE COMPONENTS OF A VECTOR IN THE ROTATED FRAME, THEN THE COMPONENTS OF THE SAME
# X Y Z
# VECTOR IN THE ORIGINAL S/C AXES (B , B , B ) ARE
# X Y Z
#
# [ B ] [ A ]
# [ X ] [ X ]
# [ ] [ ]
# [ B ] * [ A ]
# [ Y ] = DEL [ Y ]
# [ ] [ ]
# [ B ] [ B ]
# [ Z ] [ Z ]
#
# THE ROUTINE WILL STORE THIS MATRIX (SCALED UNITY) IN SEQUENTIAL LOCATIONS OF ERASABLE MEMORY BEGINNING WITH
# _
# THE LOCATION CALLED DEL. IN ORDER TO USE THE ROUTINE, THE CALLING PROGRAM MUST FIRST STORE U (A HALF UNIT
# DOUBLE PRECISION VECTOR) IN THE SET OF ERASABLE LOCATIONS BEGINNING WITH THE ADDRESS CALLED COF. THE ANGLE, A,
# MUST THEN BE LOADED INTO D(MPAC).
#
# INTERNALLY, THE PROGRAM ALSO USES THE FIRST 10 LOCATIONS OF THE PUSH DOWN LIST.
#
# READCDUK
# --------
#
# THIS BASIC LANGUAGE SUBROUTINE LOADS T(MPAC) WITH THE THREE CDU ANGLES.
#
# SIGNMPAC
# --------
#
# THIS IS A BASIC LANGUAGE SUBROUTINE WHICH LIMITS THE MAGNITUDE OF D(MPAC) TO + OR - DPOSMAX ON OVERFLOW.
#
# PROGRAM STORAGE ALLOCATION
#
# 1) FIXED MEMORY 1059 WORDS
# 2) ERASABLE MEMORY 98
# 3) STATE SWITCHES 3
# Page 349
# 4) FLAGS 1
#
# JOB PRIORITIES
#
# 1) KALCMANU TBD
# 2) ONE SECOND UPDATE TBD
#
# SUMMARY OF STATE SWITCHES AND FLAGWORDS USED BY KALCMANU.
#
# STATE FLAGWRD 2 SETTING MEANING
# SWITCH NO. BIT NO.
#
# *
# 31 14 0 MANEUVER WENT THROUGH GIMBAL LOCK
# 1 MANEUVER DID NOT GO THROUGH GIMBAL LOCK
# *
# 32 13 0 CONTINUE UPDATE PROCESS
# 1 START UPDATE PROCESS
#
# 33 12 0 PERFORM FINAL P AXIS YAW IF REQUIRED
# 1 IGNORE ANY FINAL P-AXIS YAW
#
# 34 11 0 SIGNAL END OF KALCMANU
# 1 KALCMANU IN PROCESS. USER MUST SET SWITCH BEFORE INITIATING
#
# * INTERNAL TO KALCMANU
#
# SUGGESTIONS FOR PROGRAM INTEGRATION
#
# THE FOLLOWING VARIABLES SHOULD BE ASSIGNED TO UNSWITCH ERASABLE:
#
# CPHI
# CTHETA
# CPSI
# POINTVSM +5
# SCAXIS +5
# DELDCDU
# DELDCDU1
# DELDCDU2
# RATEINDX
#
# THE FOLLOWING SUBROUTINES MAY BE PUT IN A DIFFERENT BANK
#
# MXM3
# Page 350
# TRANSPGS
# SIGNMPAC
# READCDUK
# CDUTODCM
# Page 351
BANK 15
SETLOC KALCMON1
BANK
EBANK= BCDU
# THE THREE DESIRED CDU ANGLES MUST BE STORED AS SINGLE PRECISION TWO'S COMPLEMENT ANGLES IN THE THREE SUCCESSIVE
# LOCATIONS, CPHI, CTHETA, CPSI.
COUNT* $$/KALC
KALCMAN3 TC INTPRET # PICK UP THE CURRENT CDU ANGLES AND
RTB # COMPUTE THE MATRIX FROM INITIAL S/C
READCDUK # AXES TO FINAL S/C AXES.
STORE BCDU # STORE INITIAL S/C ANGLES
SLOAD ABS # CHECK THE MAGNITUDE OF THE DESIRED
CPSI # MIDDLE GIMBAL ANGLE
DSU BPL
LOCKANGL # IF GREATER THAN 70 DEG ABORT MANEUVER
TOOBADF
AXC,2 TLOAD
MIS
BCDU
CALL # COMPUTE THE TRANSFORMATION FROM INITIAL
CDUTODCM # S/C AXES TO STABLE MEMBER AXES
AXC,2 TLOAD
MFS # PREPARE TO CALCULATE ARRAY MFS
CPHI
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
# Page 352
18D
TRNSPSPD
VLOAD STADR
STOVL TMFI +12D
STADR
STOVL TMFI +6
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
TLOAD # MANEUVER LESS THAN .25 DEGREES
CPHI # GO DIRECTLY INTO ATTITUDE HOLD
STCALL CDUXD # ABOUT COMMANDED ANGLES
TOOBADI # STOP RATE AND EXIT
CHECKMAX DLOAD DSU
AM
MAXANG
BPL VLOAD
ALTCALC # UNIT
COFSKEW # COFSKEW
UNIT
STORE COF # COF IS THE MANEUVER AXIS
# Page 353
GOTO # SEE IF MANEUVER GOES THRU GIMBAL LOCK
LOCSKIRT
ALTCALC VLOAD VAD # IF AM GREATER THAN 170 DEGREES
MFI
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 # PDO CAM $4
DPHALF
CAM
BOVB PDDL # PS2 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
# Page 354
COMP12
COF
DSU BMN
COF +4
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 garbled
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
# Page 355
STORE COF +4
GOTO
LOCSKIRT
METHOD3 DLOAD BPL # COFZ MAX
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 356
# MATRIX OPERATIONS
BANK 13
SETLOC KALCMON2
BANK
EBANK= BCDU
MXM3 SETPD VLOAD* # MXM3 MULTIPLIES 2 3X3 MATRICES
0 # AND LEAVES RESULT IN PD LIST
0,1 # AND MPAC
VXM* PDVL*
0,2
6,1
VXM* PDVL*
0,2
12D,1
VXM* PUSH
0,2
RVQ
# RETURN WITH MIXM2 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 EXIT # ENTER WITH MATRIX AT 0 IN PD LIST
INDEX FIXLOC
DXCH 12
INDEX FIXLOC
DXCH 16
INDEX FIXLOC
DXCH 12
INDEX FIXLOC
DXCH 14
INDEX FIXLOC
DXCH 4
INDEX FIXLOC
DXCH 14
INDEX FIXLOC
DXCH 2
INDEX FIXLOC
DXCH 6
INDEX FIXLOC
DXCH 2
# Page 357
TC INTPRET
RVQ
BANK 15
SETLOC KALCMON1
BANK
EBANK= BCDU
MINANG 2DEC 0.00069375
MAXANG 2DEC 0.472222222
# GIMBAL LOCK CONSTANTS
# D = MGA CORRESPONDING TO GIMBAL LOCK = 60 DEGREES
# NGL = BUFFER ANGLE (TO AVOID DIVISIONS BY ZERO) = 2 DEGREES
SD 2DEC .433015 # = SIN(D) $2
K3S1 2DEC .86603 # = SIN(D) $1
K4 2DEC -.25 # = -COS(D) $2
K4SQ 2DEC .125 # = COS(D)COS(D) $2
SNGLCD 2DEC .008725 # = SIN(NGL)COS(D) $2
CNGL 2DEC .499695 # COS(NGL) $2
LOCKANGL DEC .388889 # = 70 DEGREES
# INTERPRETIVE SUBROUTINE TO READ THE CDU ANGLES
READCDUK CA CDUZ # LOAD T(MPAC) WITH CDU ANGLES
TS MPAC +2
EXTEND
DCA CDUX # AND CHANGE MODE TO TRIPLE PRECISION
TCF TLOAD +6
CDUTODCM AXT,1 SSP
OCT 3
S1
OCT 1 # SET XR1, S1, AND PD FOR LOOP
STORE 7
SETPD
0
LOOPSIN SLOAD* RTB
10D,1
CDULOGIC
# Page 358
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 # C0 = COS(THETA)COS(PSI)
DLOAD DMP
4
0
PDDL DMP # (PD6 SIN(THETA)SIN(PHI))
6
8D
DMP SL1
2
BDSU SL1
12D
STORE 2,2 # C1=-COS(THETA)SIN(PSI)COS(PHI)
DLOAD DMP
2
4
PDDL DMP # (PD7 COS(PHI)SIN(THETA)) SCALED 4
6
8D
DMP SL1
0
DAD SL1
14D
STORE 4,2 # C2=COS(THETA)SIN(PSI)SIN(PHI)
DLOAD
8D
STORE 6,2 # C3=SIN(PSI)
DLOAD
10D
DMP SL1
2
STORE 8D,2 # C4=COS(PSI)COS(PHI)
DLOAD DMP
10D
0
DCOMP SL1
STORE 10D,2 # C5=-COS(PSI)SIN(PHI)
DLOAD DMP
4
10D
DCOMP SL1
STORE 12D,2 # C6=-SIN(THETA)COS(PSI)
# Page 359
DLOAD
DMP SL1 # (PUSH UP 7)
8D
PDDL DMP # (PD7 COS(PHI)SIN(THETA)SIN(PSI)) SCALE 4
6
0
DAD SL1 # (PUSH UP 7)
STADR # C7=COS(PHI)SIN(THETA)SIN(PSI)
STORE 14D,2 # +COS(THETA)SIN(PHI)
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)
# 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
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
DPHALF
SIGNMPAC
PDDL # PDA = 1-COS(A)
# COMPUTE THE DIAGONAL COMPONENTS OF DEL
COF
DSQ DMP
4
DAD SL3
2
BOVB
SIGNMPAC
# Page 360
STODL KEL # UX UX(1-COS(A)) +COS(A) $1
COF +2
DSQ DMP
4
DAD SL3
2
BOVB
SIGNMPAC
STODL KEL +8D # UY UY(1-COS(A)) +COS(A) $1
COF +4
DSQ DMP
4
DAD SL3
2
BOVB
SIGNMPAC
STORE KEL +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 KEL +6
BDSU SL2
BOVB
SIGNMPAC
STODL KEL +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 KEL +4 # UX UZ (1-COS(A))+UY SIN(A)
# Page 361
BDSU SL2
BOVB
SIGNMPAC
STODL KEL +12D # UX UZ (1-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)
0
DAD SL2
6
BOVB
SIGNMPAC
STODL KEL +14D # UY UZ(1-COS(A)) +UX SIN(A)
BDSU SL2
BOVB
SIGNMPAC
STORE KEL +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).
# LEAVE 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
#
# C = -SIN(THETA) SIN(PSI) SIN(PHI) + COS(THETA)COS(PHI)
# 8
# Page 362
#
# 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 363
# ROUTINES FOR TERMINATING THE AUTOMATIC MANEUVER AND RETURNING TO USER.
TOOBADF EXIT
TC ALARM
OCT 00401
TCF NOGO # DO NOT ZERO ATTITUDE ERRORS
TC BANKCALL
CADR ZATTEROR # ZERO ATTITUDE ERRORS
NOGO TC BANKCALL
CADR STOPRATE # STOP RATES
CAF TWO
INHINT # ALL RETURNS ARE NOW MADE VIA GOODEND
TC WAITLIST
EBANK= BCDU
2CADR GOODMANU
TCF ENDOFJOB
TOOBADI EXIT
TCF NOGO
Reference in New Issue Copy Permalink