Pre-launch Review
ATTITUDE MANEUVER ROUTINE
Scanned pages: 342-363
# 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* $$/KALCKALCMAN3 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 CDUTODCMSECAD 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 LOCSKIRTALTCALC 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 COFZCOMP12 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 COFU2POS DLOAD BPL MFISYM +2 # UX UY OKU21 DLOAD DCOMP # SIGN OF UX OPPOSITE garbled COF STORE COFOKU21 DLOAD BPL MFISYM +10D # UY UZ LOCSKIRT DLOAD DCOMP # SIGN OF UZ OPPOSITE TO UY COF +4 STORE COF +4 GOTO LOCSKIRTMETHOD1 DLOAD BPL # COFX MAX COFSKEW # UX U1POS VLOAD VCOMP COF STORE COFU1POS DLOAD BPL MFISYM +2 # UX UY OKU12 DLOAD DCOMP COF +2 # SIGN OF UY OPPOSITE TO UX STORE COF +2OKU12 DLOAD BPL MFISYM +4 # UX UZ LOCSKIRT DLOAD DCOMP # SIGN OF UZ OPPOSITE TO UY COF +4# Page 355 STORE COF +4 GOTO LOCSKIRTMETHOD3 DLOAD BPL # COFZ MAX COFSKEW +4 # UZ U3POS VLOAD VCOMP COF STORE COFU3POS DLOAD BPL MFISYM +4 # UX UZ OKU31 DLOAD DCOMP COF # SIGN OF UX OPPOSITE TO UZ STORE COFOKU31 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 PDTRNSPSPD 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 0LOOPSIN 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 CALCPHISUHALFA DSU DPHALFCALCPHI PUSHOKTHETA 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 VECOFANGSUHALFAP DSU GOTO DPHALF VECOFANGOKPHI DLOAD # PUSH UP PHIVECOFANG 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