744 lines
18 KiB
Plaintext
744 lines
18 KiB
Plaintext
|
# 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-61
|
||
|
|
# 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
|
||
|
|
# INCRMENTS, 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 SLIGHLTY 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 ***
|