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