# 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-061
# 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 EQUIVALENT 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.  THEREFORE, 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 WITH 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 SCALED 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