Impressum FINDCDUW--GUIDAP_INTERFACE.agc   Interaktion und
PortierbarkeitApollo

# Copyright: Public domain.
# Filename: FINDCDUW--GUIDAP_INTERFACE.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: Hartmuth Gutsche <hgutsche@xplornet.com>.
# Website: www.ibiblio.org/apollo.
# Pages: 908-925
# Mod history: 2009-05-28 HG Transcribed from page images.
#
# 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 908
# PROGRAM NAME:  FINDCDUW
# MOD NUMBER:  1 68-07-15
# MOD AUTHOR:  KLUMPP
#
# OBJECTS OF MOD: 1. TO SUPPLY COMMANDED GIMBAL ANGLES FOR NOUN 22.
# 2. TO MAINTAIN CORRECT AND CURRENT THRUST
# DIRECTION DATA IN ALL MODES.  THIS IS DONE BY
# FETCHING FOR THE THRUST DIRECTION FILTER THE
# CDUD'S IN PNGCS-AUTO, THE CDU'S IN ALL OTHER
# MODES.
# 3. TO SUBSTITUDE A STOPRATE FOR THE NORMAL
# AUTOPILOT COMMANDS WHENEVER
# 1) NOT IN PNGCS-AUTO, OR
# 2) ENGINE IS OFF.
#
# FUNCTIONAL DESCRIPTION:
#
# FINDCDUW PROVIDES THE INTERFACES BETWEEN THE VARIOUS POWERED FLITE GUIDANCE PROGRAMS
# AND THE DIGITAL AUTOPILOT.  THE INPUTS TO FINDCDUW ARE THE THRUST COMMAND VECTOR
# AND THE WINDOW COMMAND VECTOR, AND THE OUTPUTS ARE THE GIMBAL ANGLE
# INCREMENTS, THE COMMANDED ATTITUDE ANGLE RATES, AND THE COMMANDED
# ATTITUDE LAG ANGLES (WHICH ACCOUNT FOR THE ANGLES BY WHICH THE BODY WILL
# LAG BEHIND A RAMP COMMAND IN ATTITUDE ANGLE DUE TO THE FINITE ANGULAR
# ACCELERATIONS AVAILABLE).
#
# FINDCDUW ALIGNS THE ESTIMATED THRUST VECTOR FROM THE THRUST DIRECTION
# FILTER WITH THE THRUST COMMAND VECTOR, AND, WHEN XDVINHIB SET,
# ALIGNS THE +Z HALF OF THE LM ZX PLANE WITH THE WINDOW COMMAND VECTOR.
#
# Page 909
# SPECIFICATIONS:
#
# INITIALIZATION: A SINGLE INTERPRETIVE CALL TO INITCDUW IS REQUIRED
# BEFORE EACH GUIDED MANEUVER USING FINDCDUW.
#
# CALL: INTERPRETIVE CALL TO FINDCDUW WITH THE THRUST COMMAND
# VECTOR IN MPAC.  INTERPRETIVE CALL TO FINDCDUW -2 WITH
# THE THRUST COMMAND VECTOR IN UNFC/2 AND NOT IN MPAC.
#
# RETURNS: NORMAL INTERPRETIVE IN ALL CASES
# 1. NORMALLY ALL AUTOPILOT CMDS ARE ISSUED.
# 2. IF NOT PNGCS AUTO, DO STOPRATE AND RETURN
# WITHOUT ISSUING AUTOPILOT CMDS.
# 3. IF ENGINE OFF, DO STOPRATE AND RETURN WITHOUT
# ISSUING AUTOPILOT CMDS.
#
# ALARMS: 00401 IF INPUTS DETERMINE AN ATTITUDE IN GIMBAL LOCK.
# FINDCDUW DRIVES CDUXD AND CDUYD TO THE RQD VALUES,
# BUT DRIVES CDUZD ONLY TO THE GIMBAL LOCK CONE.
#
# 00402 IF UNFC/2 OR UNWC/2 PRODUCE OVERFLOW WHEN
# UNITIZED USING NORMUNIT.  FINDCDUW ISSUES
# STOPRATE AS ONLY INPUT TO AUTOPILOT.
#
# INPUTS: UNFC/2 THRUST COMMAND VECTOR, NEED NOT BE SEMI-UNIT.
# UNWC/2 WINDOW COMMAND VECTOR, NEED NOT BE SEMI-UNIT.
# OGABIAS POSSIBLE BIAS FOR OUTER GIMBAL ANGLE (ZEROED IN INITCDUW), UNITS OF PI.
# XOVINHIB FLAG DENOTING X AXIS OVERRIDE INHIBITED.
# CSMDOCKD FLAG DENOTING CSM DOCKED.
# STEERSW FLAG DENOTING INSUFF THRUST FOR THRUST DIR FLTR.
#
# OUTPUTS: DELCDUX,Y,Z
# OMEGAPD,+1,+2
# DELPEROR,+1,+2
# CPHI,+1,+2 FOR NOUN22
#
# DEBRIS: FINDCDUW DESTROYS SINCDUX,Y,Z AND COSCDUX,Y,Z BY
# WRITING INTO THESE LOCATIONS THE SINES AND COSINES
# OF THE CDUD'S IN PNGCS-AUTO, OF THE CDU'S OTHERWISE.

# Page 910
# INITIALIZATION FOR FINDCDUW

  BANK 30
  SETLOC FCDUW
  BANK

  EBANK= ECDUW
  COUNT* $$/FCDUW

INITCDUW VLOAD
   UNITX
  STORE UNFV/2
  STORE UNWC/2
  RVQ

# FINDCDUW PRELIMINARIES

  VLOAD   # FINDCDUW -2:  ENTRY WHEN UNFC/2 PRE-STORD
   UNFC/2  # INPUT VECTORS NEED NOT BE SEMI-UNIT
FINDCDUW BOV SETPD  # FINDCDUW: ENTRY WHEN UNFC/2 IN MPAC
   FINDCDUW # INTERPRETER NOW INITIALIZED
   22  # LOCS 0 THRU 21 FOR DIRECTION COSINE MAT
  STQ EXIT
   QCDUWUSR # SAVE RETURN ADDRESS

# MORE HAUSKEEPING
  CA ECDUWL
  XCH EBANK  # SET EBANK
  TS ECDUWUSR # SAVE USER'S EBANK

  CA DAPBOOLS
  MASK CSMDOCKD # CSMDOCKD MUST NOT BE BIT15
  CCS A
  CA ONE  # INDEX IF CSM DOCKED
  TS NDXCDUW

  CA XOVINHIB # XOVINHIB MUST NOT BE BIT15
  TS FLPAUTNO # SET TO POS-NON-ZERO FLAG PNGCS AUTO NOT

  MASK DAPBOOLS
  TS FLAGOODW # FLAG0ODW = ANY PNZ NUMBER IF XOV INHIBTD

# Page 911
# FETCH BASIC DATA
  INHINT   # RELINT AT PAUTNO (TC INTPRET)

  CA CDUX  # FETCH CDUX,CDUY,CDUZ IN ALL CASES, BUT
  TS CDUSPOTX # REPLACE BELOW IF PNGCS AUTO
  CA CDUY
  TS CDUSPOTY
  CA CDUZ
  TS CDUSPOTZ

  CA BIT10  # PNGCS CONTROL BIT
  EXTEND
  RAND CHAN30
  CCS A
  TCF PAUTNO  # NOT PNGCS (BITS INVERTED)

  CA BIT14  # AUTO MODE BIT
  EXTEND
  RAND CHAN31
  CCS A
  TCF PAUTNO  # NOT AUTO (BITS INVERTED)

  TS FLPAUTNO # RESET FLAG PNGCS AUTO NOT

  CA CDUXD  # PNGCS AUTO:  FETCH CDUXD,CDUYD,CDUZD
  TS CDUSPOTX
  CA CDUYD
  TS CDUSPOTY
  CA CDUZD
  TS CDUSPOTZ

# Page 912
# FETCH INPUTS
PAUTNO  TC INTPRET  # ENTERING THRUST CMD STILL IN MPAC
  RTB
   NORMUNIT
  STOVL UNX/2  # SEMI-UNIT THRUST CMD AS INITIAL UNX/2
   UNWC/2
  RTB RTB
   NORMUNIT
   QUICTRIG # ALWAYS RQD TO OBTAIN TRIGS OF CDUD'S
  STOVL UNZ/2  # SEMI-UNIT WINDOW CMD AS INITIAL UNZ/2
   DELV
  BOVB UNIT
   NOATTCNT # AT LEAST ONE ENTERING CMD VCT ZERO
  BOV CALL
   AFTRFLTR # IF UNIT DELV OVERFLOWS SKIP FILTER
   *SMNB*  # YIELDS UNIT(DELV) IN VEH COORDS FOR FLTR

# THRUST DIRECTION FILTER

  EXIT

  CA UNFVY/2  # FOR RESTARTS, UNFV/2 ALWAYS INTACT, MPAC
  LXCH MPAC +3 #  RENEWD AFTER RETURN FROM CALLER,
  TC FLTRSUB  # TWO FILTER UPDATES MAY BE DONE.
  TS UNFVY/2  # UNFV/2 NEED NOT BE EXACTLY SEMI-UNIT.

  CA UNFVZ/2
  LXCH MPAC +5
  TC FLTRSUB
  TS UNFVZ/2

  TC INTPRET  # COMPLETES FILTER

# Page 913
# FIND A SUITABLE WINDOW POINTING VECTOR

AFTRFLTR SLOAD BHIZ  # IF XOV NOT INHIBITED, GO FETCH ZNB
   FLAGOODW
   FETCHZNB
  VLOAD CALL
   UNZ/2
   UNWCTEST

FETCHZNB VLOAD
   ZNBPIP
  STCALL UNZ/2
   UNWCTEST

  VLOAD VCOMP  # Z AND -X CAN'T BOTH PARALLEL UNFC/2
   XNBPIP
  STORE UNZ/2

# COMPUTE THE REQUIRED DIRECTION COSINE MATRIX

DCMCL  VLOAD VXV
   UNZ/2
   UNX/2
  UNIT PUSH  # UNY/2 FIRST ITERATION
  VXV VSL1
   UNX/2
  STORE UNZ/2  # -UNZ/2 FIRST ITERATION
  VXSC PDVL  # EXCHANGE -UNFVZ/2 UNZ/2 FOR UNY/2
   UNFVZ/2  # MUST BE SMALL
  VXSC BVSU  # YIELDS -UNFVY/2 UNY/2-UNFVZ/2 UNZ/2
   UNFVY/2  # MUST BE SMALL
  VSL1 VAD
   UNX/2
  UNIT   # TOTALLY ELIMINATES THRUST POINTING ERROR
  STORE UNX/2  # UNX/2
  VXV VSL1
   UNZ/2  # -UNZ/2 WAS STORED HERE REMEMBER
  STORE UNY/2  # UNY/2
  VCOMP VXV
   UNX/2
  VSL1
  STORE UNZ/2  # UNZ/2

# Page 914
# COMPUTES THE REQUIRED GIMBAL ANGLES

  CALL
   NB2CDUSP # YIELDS THE RQD GIMBAL ANGLES, 2'S, PI
  EXIT

# LIMIT THE MIDDLE GIMBAL ANGLE & COMPUTE THE UNLIMITED GIMBAL ANGLE CHGS

  CA MPAC +2  # LIMIT THE MGA
  TS L  # CAN'T LXCH:  NEED UNLIMITED MGA FOR ALARM
  CA CDUZDLIM
  TC LIMITSUB # YIELDS LIMITED MGA.  1 BIT ERROR POSSIBLE
  XCH MPAC +2  #  BECAUSE USING 2'S COMP.  WHO CARES?
  EXTEND
  MSU MPAC +2  # THIS BETTER YIELD ZERO
  EXTEND
  BZF +2
  TCF ALARMMGA

MGARET  INHINT   # RELINT AT TC INTPRET AFTER TCQCDUW

  ZL
  CA TWO
DELGMBLP TS TEM2

  CA L  # TO PREVENT FALSE STARTS ABOUT X, ZERO
  EXTEND   # FLAGOODW IF DELGMBZ OR Y TOO BIG.
  SQUARE
  AD HI5  # WITHIN 1 BIT OF -(45 DEG SQUARED)
  EXTEND
  BZMF +3
  CA ZERO
  TS FLAGOODW

  INDEX TEM2
  CA MPAC
  INDEX TEM2
  TS CPHI  # OUTPUTS TO NOUN22
  EXTEND
  INDEX TEM2
  MSU CDUXD  # NO MATTER THAT THESE SLIGHTLY DIFFERENT
  COM   # FROM WHEN WE INITIALLY FETCHED THEM
  INDEX TEM2
  TS -DELGMB  # -UNLIMITED GIMBAL ANGLE CHGS, 1'S, PI
  TS L  # FOR PRECEDING TEST ON NEXT LOOP PASS
  CCS TEM2
  TCF DELGMBLP

# Page 915
# BRANCHES TO NOATTCNT
  CCS FLPAUTNO
  TCF NOATTCNT +2 # NO PNGCS AUTO

  CA FLAGWRD5
  MASK ENGONBIT
  EXTEND
  BZF NOATTCNT +2 # ENGINE NOT ON

# Page 916
# LIMIT THE ATTITUDE ANGLE CHANGES
#
# THIS SECTION LIMITS THE ATTITUDE ANGLE CHANGES ABOUT A SET OF ORTHOGONAL VEHICLE AXES X,YPRIME,ZPRIME,
# THESE AXES COINCIDE WITH THE COMMANDED VEHICLE AXES IF AND ONLY IF CDUXD IS ZERO.  THE PRIME SYSTEM IS
# THE COMMANDED VEHICLE SYSTEM ROTATED ABOUT THE X AXIS TO BRING THE Z AXIS INTO ALIGNMENT WITH THE MIDDLE GIMBAL
# AXIS.  ATTITUDE ANGLE CHANGES IN THE PRIME SYSTEM ARE RELATED TO SMALL GIMBAL ANGLE CHANGES BY:
#
# [   -DELATTX    ]   [ 1     SIN(CDUZD)     0 ] [ -DELGMBX ]
# [               ]   [                        ] [          ]
# [ -DELATTYPRIME ] = [ 0     COS(CDUZD)     0 ] [ -DELGMBY ]
# [               ]   [                        ] [          ]
# [ -DELATTZPRIME ]   [ 0         0          1 ] [ -DELGMBZ ]

  LXCH -DELGMB +2 # SAME AS -DELATTZPRIME UNLIMITED
  INDEX NDXCDUW
  CA DAZMAX
  TC LIMITSUB
  TS -DELGMB +2 # -DELGMBZ

  CA -DELGMB +1
  EXTEND
  MP COSCDUZ  # YIELDS -DELATTYPRIME/2 UNLIMITED
  TS L
  INDEX NDXCDUW
  CA DAY/2MAX
  TC LIMITSUB
  EXTEND
  DV COSCDUZ
  XCH -DELGMB +1 # -DELGMBY, FETCHING UNLIMITED VALUE

  EXTEND
  MP SINCDUZ
  DDOUBL
  COM
  EXTEND   # YIELDS +DELATTX UNLIMITD, MAG < 180 DEG.
  MSU -DELGMB  # BASED ON UNLIMITED DELGMBV.
  TS L  # ONE BIT ERROR IF OPERANDS IN MSU
  INDEX NDXCDUW  # OF MIXED SIGNS.  WHO CARES?
  CA DAXMAX
  TC LIMITSUB
  TS -DELGMB  # SAVE LIMITED +DELATTX
  CCS FLAGOODW
  CS -DELGMB  # FETCH IT BACK CHGING SIGN IF WINDOW GOOD
  TS -DELGMB  # OTHERWISE USE ZERO FOR -DELATTX
  CS -DELGMB +1
  EXTEND
  MP SINCDUZ
  DDOUBL   # YIELDS -CNTRIB TO -DELATTX FROM -DELGMBY
  ADS -DELGMB  # -DELGMBX.  NO OVERFLOW SINCE LIMITED TO
     # 20DEG(1+SIN(70DEG)/COS(70DEG)) < 180DEG

# Page 917
# COMPUTE COMMANDED ATTITUDE RATES
#
# [ OMEGAPD ]   [ -2         -4 SINCDUZ              +0    ] [ -DELGMBZ ]
# [         ]   [                                          ] [          ]
# [ OMEGAQD ] = [ +0     -8 COSCDUZ COSCDUX     -4 SINCDUX ] [ -DELGMBY ]
# [         ]   [                                          ] [          ]
# [ OMEGARD ]   [ +0     +8 COSCDUZ SINCDUX     -4 COSCDUX ] [ -DELGMBZ ]
#
# ATTITUDE ANGLE RATES IN UNITS OF PI/4 RAD/SEC = K TRIG FCNS IN UNITS OF 2 X GIMBAL ANGLE RATES IN UNITS OF
# PI/2 RAD/SEC.  THE CONSTANTS ARE BASED ON DELGMB BEING THE GIMBAL ANGLE CHANGES IN UNITS OF PI RADIANS,
# AND 2 SECONDS BEING THE COMPUTATION PERIOD (THE PERIOD BETWEEN SUCCESSIVE PASSES THRU FINDCDUW).

  CS -DELGMB
  TS OMEGAPD
  CS -DELGMB +1
  EXTEND
  MP SINCDUZ
  DDOUBL
  ADS OMEGAPD
  ADS OMEGAPD

  CS -DELGMB +1
  EXTEND
  MP COSCDUX
  DDOUBL
  EXTEND
  MP COSCDUZ
  TS OMEGAQD
  CS -DELGMB +2
  EXTEND
  MP SINCDUX
  ADS OMEGAQD
  ADS OMEGAQD
  ADS OMEGAQD

  CA -DELGMB +1
  EXTEND
  MP SINCDUX
  DDOUBL
  EXTEND
  MP COSCDUZ
  TS OMEGARD
  CS -DELGMB +2
  EXTEND
  MP COSCDUX
  ADS OMEGARD
  ADS OMEGARD
  ADS OMEGARD

# Page 918
# FINAL TRANSFER

  CA TWO
CDUWXFR  TS TEM2
  INDEX TEM2
  CA -DELGMB
  EXTEND
  MP DT/DELT  # RATIO OF DAP INTERVAL TO CDUW INTERVAL
  TC ONESTO2S
  INDEX TEM2
  TS DELCDUX  # ANGLE INTERFACE

  INDEX TEM2
  CCS OMEGAPD
  AD ONE
  TCF +2
  AD ONE
  EXTEND   # WE NOW HAVE ABS(OMEGAPD,QD,RD)
  INDEX TEM2
  MP OMEGAPD
  EXTEND
  MP BIT11  # 1/16
  EXTEND
  INDEX TEM2  #                  2
  DV 1JACC  # UNITS PI/4 RAD/SEC
  TS L
  CA DELERLIM
  TC LIMITSUB
  INDEX TEM2
  TS DELPEROR # LAG ANGLE = OMEGA ABS(OMEGA)/2 ACCEL
  CCS TEM2
  TCF CDUWXFR

# HAUSKEEPING AND RETURN

TCQCDUW  CA ECDUWUSR
  TS EBANK  # RETURN USER'S EBANK

  TC INTPRET
  SETPD GOTO
   0
   QCDUWUSR # NORMAL AND ABNORMAL RETURN TO USER

# Page 919
# THRUST VECTOR FILTER SUBROUTINE

FLTRSUB  EXTEND
  QXCH TEM2
  TS TEM3  # SAVE ORIGINAL OFFSET
  COM   # ONE MCT, NO WDS, CAN BE SAVED IF NEG OF
  AD L  # ORIG OFFSET ARRIVES IN A, BUT IT'S
  EXTEND   #  NOT WORTH THE INCREASED OBSCURITY.
  INDEX NDXCDUW
  MP GAINFLTR
  TS L  # INCR TO OFFSET, UNLIMITED
  CA DUNFVLIM # SAME LIMIT FOR Y AND Z
  TC LIMITSUB # YIELDS INCR TO OFFSET, LIMITED
  AD TEM3  # ORIGINAL OFFSET
  TS L  # TOTAL OFFSET, UNLIMITED
  CA UNFVLIM  # SAME LIMIT FOR Y AND Z
  TC LIMITSUB # YIELDS TOTAL OFFSET, LIMITED
  TC TEM2

# SUBR TO TEST THE ANGLE BETWEEN THE PROPOSED WINDOW AND THRUST CMD VCTS

UNWCTEST  DOT DSQ
   UNX/2
  DSU BMN
   DOTSWFMX
   DCMCL
  SSP RVQ  # RVQ FOR ALT CHOICE IF DOT MAGN TOO LARGE
   FLAGOODW #  ZEROING WINDOW GOOD FLAG
   0

# Page 920
# NB2CDUSP RETURNS THE 2'S COMPLEMENT, PI, SP CDU ANGLES X,Y,Z IN MPAC,+1,+2 GIVEN THE MATRIX WHOSE ROW VECTORS
# ARE THE SEMI-UNIT NAV BASE VECTORS X,Y,X EXPRESSED IN STABLE MEMBER COORDINATES, LOCATED AT 0 IN THE PUSH LIST.
# NB2CDUSP USES THE ARCTRGSP WHICH HAS A MAXIMUM ERROR OF +-4 BITS.

NB2CDUSP DLOAD DSQ
   2
  BDSU BPL
   DP1/4TH
   +3
  DLOAD
   ZEROVECS # IN CASE SIN WAS SLIGHTLY > 1/2
  SQRT EXIT  # YIELDS COS(CDUZ) IN UNITS OF 2

  EXTEND
  DCA MPAC
  DDOUBL
  TS TEM5
  TCF +3
  CA POSMAX  # OVERFLOW.  FETCH POSMAX, MPAC ALWAYS POS
  TS TEM5  # COS(CDUZ) IN TEM5, UNITS 1

  INDEX FIXLOC
  CA 2
  LXCH MPAC
  TC ARCTRGSP
  TS MPAC +2  # CDUZ

  CA ZERO
  TC DVBYCOSM
  CA FOUR
  TC DVBYCOSM
  CS TEM1
  TC ARCTRGSP
  TS MPAC +1  # CDUY

  CA BIT4
  TC DVBYCOSM
  CA 16OCT
  TC DVBYCOSM
  CS TEM1
  TC ARCTRGSP
  TS MPAC  # CDUX

  TC INTPRET
  RVQ

16OCT  OCT 16

# Page 921
# THE ELEMENTS OF THE NAV BASE MATRIX WHICH WE MUST DIVIDE BY COS(MGA)
# ALREADY CONTAIN COS(MGA)/2 AS A FACTOR. THEREFORE THE QUOTIENT SHOULD
# ORDINARILY NEVER EXCEED 1/2 IN MAGNITUDE.  BUT IF THE MGA IS NEAR PI/2
# THEN COS(MGA) IS NEAR ZERO, AND THERE MAY BE SOME CHAFF IN THE OTHER
# ELEMENTS OF THE MATRIX WHICH WOULD PRODUCE CHAOS UNDER DIVISION.
# BEFORE DIVIDING WE MAKE SURE COS(MGA) IS AT LEAST ONE BIT LARGER
# THAN THE MAGNITUDE OF THE HIGH ORDER PART OF THE OPERAND.
#
# IF ONE OR MORE DIVIDES CANNOT BE PERFORMED, THIS MEANS THAT THE
# REQUIRED MGA IS VERY NEARLY +-PI/2 AND THEREFORE THE OTHER GIMBAL
# ANGLES ARE INDETERMINATE.  THE INNER AND OUTER GIMBAL ANGLES RETURNED
# IN THIS CASE WILL BE RANDOM MULTIPLES OF PI/2.

DVBYCOSM AD FIXLOC
  TS ADDRWD  # ADRES OF OPERAND

  INDEX ADDRWD  # FETCH NEG ABS OF OPERAND, AD TEM5, AND
  CA 0  # SKIP DIVIDE IF RESULT NEG OR ZERO
  EXTEND
  BZMF +2
  COM
  AD TEM5  # C(A) ZERO OR NEG, C(TEM5) ZERO OR POS
  EXTEND
  BZMF TSL&TCQ  # DIFFERENCE ALWAYS SMALL IF BRANCH

  EXTEND   # TEM5 EXCEEDS ABS HIGH ORDER PART OF
  INDEX ADDRWD  # OPERAND BY AT LEAST ONE BIT.
  DCA 0  # THEREFORE IT EXCEEDS THE DP OPERAND
  EXTEND   #  AND DIVISION WILL ALWAYS SUCCEED.
  DV TEM5
TSL&TCQ  TS L
  LXCH TEM1
  TC Q

# Page 922
# ARCTRGSP RETURNS THE 2'S COMPLEMENT, PI, SP ANGLE IN THE A REGISTER GIVEN ITS SINE IN A AND ITS COSINE IN L IN
# UNITS OF 2.  THE RESULT IS AN UNAMBIGUOUS ANGLE ANYWHERE IN THE CIRCLE, WITH A MAXIMUM ERROR OF +-4 BITS.
# THE ERROR IS PRODUCED BY THE SUBROUTINE SPARCSIN WHICH IS USED ONLY IN THE REGION +-45 DEGREES.

ARCTRGSP EXTEND
  BZF SINZERO  # TO AVOID DIVIDING BY ZERO

  EXTEND
  QXCH TEM4
  TS TEM2
  CA L
  TS TEM3
  CA ZERO
  EXTEND
  DV TEM2
  EXTEND
  BZF USECOS

  CCS TEM3  # SIN IS SMALLER OR EQUAL
  CA ZERO
  TCF +4
  CS TEM2  # IF COS NEG, REVERSE SIGN OF SIN,
  TS TEM2  # ANGLE = PI-ARCSIN(SIN)
  CA NEGMAX  # PICK UP PI, 2'S COMPLEMENT
  TS TEM3  # WE NO LONGER NEED COS
  CA TEM2
  TC SPARCSIN -1
  TC ONESTO2S
  EXTEND
  MSU TEM3
1TO2&TCQ TC ONESTO2S
  TC TEM4

USECOS  CS TEM3  # COS IS SMALLER
  TC SPARCSIN -1 # ANGLE = SIGN(SIN)(FI/2-ARCSIN(COS))
  AD HALF
  TS TEM3  # WE NO LONGER NEED COS
  CCS TEM2
  CA TEM3
  TCF 1TO2&TCQ
  CS TEM3
  TCF 1TO2&TCQ

SINZERO  CCS L
  CA ZERO
  TC Q
  CA NEGMAX  # PI, 2'S COMP
  TC Q

# Page 923
# SPARCSIN TAKES AN ARGUMENT SCALED UNITY IN A AND RETURNS AN ANGLE SCALED
# 180 DEGREES IN A.  IT HAS BEEN UNIT TESTED IN THE REGION +-.94 (+-70
# DEGREES) AND THE MAXIMUM ERROR IS +-5 BITS WITH AN AVERAGE TIME OF
# 450 MICROSECONDS.  SPARCSIN -1 TAKES THE ARGUMENT SCALED TWO.  (BOB CRISP)

  DOUBLE
SPARCSIN TS SR
  TCF +4
  INDEX A
  CS LIMITS
  TS SR
  EXTEND
  MP A
  TS TEM1
  EXTEND
  MP DPL9
  AD DPL7
  EXTEND
  MP TEM1
  AD DPL5
  EXTEND
  MP TEM1
  AD DPL3
  EXTEND
  MP TEM1
  AD DPL1
  EXTEND
  MP SR
  TC Q
DPL1  DEC 10502
DPL3  DEC 432
DPL5  DEC 7300
DPL7  DEC -11803
DPL9  DEC 8397

# Page 924
# LIMITSUB LIMITS THE MAGNITUDE OF THE POSITIVE OR NEGATIVE VARIABLE
# ARRIVING IN L TO THE POSITIVE LIMIT ARRIVING IN A.
# THE SIGNED LIMITED VARIABLE IS RETURNED IN A.
#
# VERSION COUTESY HUGH BLAIR-SMITH

LIMITSUB TS TEM1
  CA ZERO
  EXTEND
  DV TEM1
  CCS A
  LXCH TEM1
  TCF +2
  TCF +3
  CA L
  TC Q
  CS TEM1
  TC Q

# SUBROUTINE TO CONVERT 1'S COMP SP TO 2'S COMP

ONESTO2S CCS A
  AD ONE
  TC Q
  CS A
  TC Q

# NO ATTITUDE CONTROL

NOATTCNT TC ALARM
  OCT 00402  # NO ATTITUDE CONTROL

 +2  INHINT   # COME HERE FOR NOATTCNT WITHOUT ALARM
   TC IBNKCALL # RELINT AT TC INTPRET AFTER TCQCDUW
  FCADR STOPRATE
  TCF TCQCDUW  # RETURN TO USER SKIPPING AUTOPILOT CMDS

# MIDDLE GIMBAL ANGLE ALARM

ALARMMGA TC ALARM
  OCT 00401
  TCF MGARET

# Page 925
#******************************************************************
# CONSTANTS
#******************************************************************

# ADDRESS CONSTANTS

ECDUWL  ECADR ECDUW

# THRUST DIRECTION FILTER CONSTANTS

GAINFLTR DEC .2  # GAIN FILTER SANS CSM
  DEC .1  # GAIN FILTER WITH CSM

DUNFVLIM DEC .007 B-1 # 7 MR MAX CHG IN F DIR IN VEH IN 2 SECS.
     # THIS DOES NOT ALLOW FOR S/C ROT RATE.

UNFVLIM  DEC .129 B-1 # 129 MR MAX THRUST OFFSET.  105 MR TRAVEL
     # +10MR DEFL+5MR MECH MOUNT+9MR ABLATION.

# CONSTANT RELATED TO GIMBAL ANGLE COMPUTATIONS

DOTSWFMX DEC .93302 B-4 # LIM COLNRTY OF UNWC/2 & UNFC/2 TO 85 DEG
     # LOWER PART COMES FROM NEXT CONSTANT

DAXMAX  DEC .11111111111 # DELATTX LIM TO 20 DEG IN 2 SECS, 1'S, PI
  DEC .0111111111 # 2 DEG WHEN CSM DOCKED

DAY/2MAX DEC .05555555555 # LIKEWISE FOR DELATTY
  DEC .0055555555

DAZMAX  = DAXMAX  # LIKEWISE FOR DELATTZ

CDUZDLIM DEC .3888888888 # 70 DEG LIMIT FOR MGA, 1'S, PI

# CONSTANTS FOR DATA TRANSFER

DT/DELT  DEC .05  # .1 SEC/2 SEC WHICH IS THE AUTOPILOT
     # CONTROL SAMPLE PERIOD/COMPUTATION PERIOD

DELERLIM = DAY/2MAX #  0 DEG LIMIT FOR LAG ANGLES, 1'S, PI

# *** END OF FLY .132 ***