Merge pull request #756 from zpeters/649-TRIM_GIMBAL_CNTROL_SYSTEM
Proof TRIM_GIMBAL_CNTROL_SYSTEM #649
This commit is contained in:
Chris Garry
GitHub
commit
5c10b6e1a6
@ -65,7 +65,7 @@ GTS CAF NEGONE # MAKE THE NEXT PASS THROUGH THE DAP BE
|
||||
AD A # FOR THE RIGHT SHIFT DUE TO EDITING.
|
||||
TS SAVESR
|
||||
|
||||
GTSGO+DN CAF TWO # SET INDEXER FOR R-AXIS CALCULATIONS.
|
||||
GTSGO+ON CAF TWO # SET INDEXER FOR R-AXIS CALCULATIONS.
|
||||
TCF GOQTRIMG +1
|
||||
|
||||
GOQTRIMG CAF ZERO # SET INDEXER FOR Q-AXIS CALCULATIONS
|
||||
@ -133,7 +133,7 @@ ALGORTHM EXTEND # Q(R)DIFF IS THETA (ERROR) SCALED AT PI.
|
||||
EXTEND
|
||||
# Page 1474
|
||||
DV KCENTRAL
|
||||
XCH A2CNTRAL +1 # QUOTIENT STORED AT 16*PI, D.P.
|
||||
XCH A2CNTRAL +1 # QUOTIENT STORED AT 16*PI , D.P.
|
||||
TCF HAVEQUOT
|
||||
|
||||
HUGEQUOT CA POSMAX
|
||||
@ -191,7 +191,7 @@ NEGFNCT2 EXTEND
|
||||
|
||||
FUNCT2 EXTEND
|
||||
DCA A2CNTRAL
|
||||
DAS K2CNTRAL # DEL*OMEGA + ALPHA(2)/(2*K) AT 16*PI, D.P.
|
||||
DAS K2CNTRAL # DEL*OMEGA + ALPHA(2)/(2*K) AT 16*PI,D.P.
|
||||
|
||||
FUNCT3 CA A2CNTRAL
|
||||
EXTEND
|
||||
@ -300,38 +300,38 @@ ACDT+C12 CS NEGUQ
|
||||
#
|
||||
# TIMEGMBL COMPUTES THE DRIVE TIME NEEDED FOR THE TRIM GIMBAL TO POSITION THE DESCENT ENGINE NOZZLE SO AS TO NULL
|
||||
# THE OFFSET ANGULAR ACCELERATION ABOUT THE Q (OR R) AXIS. INSTEAD OF USING AOSQ(R), TIMEGMBL USES .4*AOSQ(R),
|
||||
# SCALED AT PI/8. FOR EACH AXIS, THE DRIVE TIME IS COMPUTED AS ABS(ALPHA/ACCDOT). A ZERO
|
||||
# SCALED AT PI/8. FOR EACH AXIS, THE DRIVE TIME IS COMPUTED AS ABS(ALPHA/ACCDOT). A ZERO
|
||||
# ALPHA OR ACCDOT OR A ZERO QUOTIENT TURNS OFF THE GIMBAL DRIVE IMMEDIATELY. OTHERWISE, THE GIMBAL IS TURNED ON
|
||||
# DRIVING IN THE CORRECT DIRECTION. THE Q(R)GIMTIMR IS SET TO TERMINATE THE DRIVE AND Q(R)ACCDOT
|
||||
# IS STORED TO REFLECT THE NEW ACCELERATION DERIVATIVE. NEGUQ(R) WILL CONTAIN +1,+0,-1 FOR A Q(R)ACCDOT VALUE
|
||||
# WHICH IS NEGATIVE, ZERO, OR POSITIVE.
|
||||
#
|
||||
# INPUTS: AOSQ,AOSR, SCALED AT P1/2, AND ACCDOTQ, ACCDOTR AT PI/2(7). PI/2(7).
|
||||
# INPUTS: AOSQ,AOSR, SCALED AT P1/2, AND ACCDOTQ, ACCDOTR AT PI/2(7). PI/2(7).
|
||||
#
|
||||
# OUTPUTS: NEW GIMBAL DRIVE BITS IN CHANNEL 12, NEGUQ, NEGUR, QACCDOT, AND RACCDOT, THE LAST SCALED AT PI/2(7).
|
||||
# Q(R)GIMTIMR WILL BE SET TO TIME AND TERMINATE GIMBAL DRIVE(S).
|
||||
# OUTPUTS: NEW GIMBAL DRIVE BITS IN CHANNEL 12,NEGUQ,NEGUR,QACCDOT AND RACCDOT, THE LAST SCALED AT PI/2(7).
|
||||
# Q(R)GIMTIMR WILL BE SET TO TIME AND TERMINATE GIMBAL DRIVE(S)
|
||||
#
|
||||
# DEBRIS: A, L, Q, ITEMPS 2, 3, 6, AND RUPTREG2 AND ACDT+C12 DEBRIS.
|
||||
# DEBRIS: A,L,Q, ITEMPS 2,3,6, RUPTREG2 AND ACDT+C12 DEBRIS.
|
||||
#
|
||||
# EXITS: VIA TC Q.
|
||||
# EXITS: VIA TC Q.
|
||||
#
|
||||
# ALARMS, ABORTS: NONE.
|
||||
# ALARMS, ABORTS, : NONE
|
||||
#
|
||||
# SUBROUTINES: ACDT+C12, IBNKCALL
|
||||
# SUBROUTINES: ACDT+C12, IBNKCALL
|
||||
#
|
||||
# WARNING: THIS SUBROUTINE WRITES INTO CHANNEL 12 AND USES THE ITEMPS. THEREFORE IT MAY ONLY BE CALLED WITH
|
||||
# INTERRUPT INHIBITED.
|
||||
# WARNING: THIS SUBROUTINE WRITES INTO CHANNEL 12 AND USES THE ITEMPS. THEREFORE IT MAY ONLY BE CALLED WITH
|
||||
# INTERRUPT INHIBITED.
|
||||
#
|
||||
# ERASABLE STORAGE CONFIGURATION (NEEDED BY THE INDEXING METHODS):
|
||||
# NEGUQ ERASE +2 # NEGATIVE OF Q-AXIS GIMBAL DRIVE
|
||||
# (SPWORD) EQUALS NEGUQ +1 # ANY S.P. ERASABLE NUMBER, NOW THRSTCMD
|
||||
# NEGUR EQUALS NEGUQ +2 # NEGATIVE OF R-AXIS GIMBAL DRIVE
|
||||
# ACCDOTQ ERASE +2 # Q-JERK TERM SCALED AT PI/2(7) RAD/SEC(3)
|
||||
# (SPWORD) EQUALS ACCDOTQ +1 # ANY S.P. ERASABLE NUMBER NOW QACCDOT
|
||||
# ACCDOTR EQUALS ACCDOTQ +2 # R-JERK TERM SCALED AT PI/2(7) RAD/SEC(3)
|
||||
# # ACCDOTQ, ACCDOTR ARE MAGNITUDES.
|
||||
# AOSQ ERASE +4 # Q-AXIS ACC., D.P. AT PI/2 R/SEC(2)
|
||||
# AOSR EQUALS AOSQ +2 # R-AXIS ACCELERATION SCALED AT PI/2 R/S2
|
||||
# NEGUQ ERASE +2 # NEGATIVE OF Q-AXIS GIMBAL DRIVE
|
||||
# (SPWORD) EQUALS NEGUQ +1 # ANY S.P. ERASABLE NUMBER, NOW THRSTCMD
|
||||
# NEGUR EQUALS NEGUQ +2 # NEGATIVE OF R-AXIS GIMBAL DRIVE
|
||||
# ACCDOTQ ERASE +2 # Q-JERK TERM SCALED AT PI/2(7) RAD/SEC(3)
|
||||
# (SPWORD) EQUALS ACCDOTQ +1 # ANY S.P. ERASABLE NUMBER NOW QACCDOT
|
||||
# ACCDOTR EQUALS ACCDOTQ +2 # R-JERK TERM SCALED AT PI/2(7) RAD/SEC(3)
|
||||
# # ACCDOTQ, ACCDOTR ARE MAGNITUDES.
|
||||
# AOSQ ERASE +4 # Q-AXIS ACC., D.P. AT PI/2 R/SEC(2)
|
||||
# AOSR EQUALS AOSQ +2 # R-AXIS ACCELERATION SCALED AT PI/2 R/S2
|
||||
|
||||
QRNDXER EQUALS ITEMP6
|
||||
OCT23146 OCTAL 23146 # DECIMAL .6
|
||||
@ -365,7 +365,7 @@ ALPHATRY INDEX QRNDXER
|
||||
TS L # WAS THERE OVERFLOW?
|
||||
TCF SETNEGU # NO. COMPUTE DRIVE TIME.
|
||||
|
||||
CS A # RECOVER -SGN(AOS) IN THE A REGISTER.
|
||||
CS A # RECOVER -SGN(AOS) IN THE A REGISTER.
|
||||
INDEX QRNDXER # YES. START DRIVE WITHOUT WAITLIST.
|
||||
XCH NEGUQ
|
||||
TCF NOTALLOW # KNOCK DOWN THE ALLOWGTS FLAG.
|
||||
@ -374,10 +374,10 @@ SETNEGU EXTEND
|
||||
BZMF POSALPH
|
||||
|
||||
COM
|
||||
TS ITEMP2 # STORE -ABS(.4*AOS) SCALED AT PI/8.
|
||||
TS ITEMP2 # STORE -ABS(.4*AOS) SCALED AT PI/8.
|
||||
CS BIT1
|
||||
TCF POSALPH +2
|
||||
POSALPH TS ITEMP2 # STORE -ABS(.4*AOS) SCALED AT PI/8.
|
||||
POSALPH TS ITEMP2 # STORE -ABS(.4*AOS) SCALED AT PI/8.
|
||||
CA BIT1
|
||||
+2 INDEX QRNDXER # SGN(AOS) INTO NEGU
|
||||
TS NEGUQ # STORE SGN(ALPHA) AS NEGU
|
||||
@ -440,8 +440,8 @@ OCT00240 OCTAL 00240 # DECIMAL 10/1024
|
||||
# SQUARE ROOT OF THE FOURTEEN MOST SIGNIFICANT BITS OF THE ARGUMENT. ALSO, THE CELL SHFTFLAG CONTAINS A BINARY
|
||||
# EXPONENT S, SUCH THAT THE SQUARE ROOT (RETURNED IN THE A REGISTER) MUST BE SHIFTED RIGHT (MULTIPLIED BY 2 TO THE
|
||||
# POWER (-S)) IN ORDER TO BE THE TRUE SQUARE ROOT OF THE FOURTEEN MOST SIGNIFICANT BITS OF FUNCTION, FUNCTION +1.
|
||||
# SQUARE ROOT ERROR IS NOT MORE THAN 2 IN THE 14TH SIGNIFICANT BIT. CELLS CLOBBERED ARE A, L, SHFTFLAG, ININDEX,
|
||||
# HALFARG, SCRATCH, SR, FUNCTION, FUNCTION +1. GTSQRT IS CALLED BY TC GTSQRT AND RETURNS VIA TC Q OR TC FUNCTION +1.
|
||||
# SQUARE ROOT ERROR IS NOT MORE THAN 2 IN THE 14TH SIGNIFICANT BIT. CELLS CLOBBERED ARE A,L,SHFTFLAG.ININDEX,
|
||||
# HALFARG,SCRATCH,SR,FUNCTION, FUNCTION +1. GTSQRT IS CALLED BY TC GTSQRT AND RETURNS VIA TC Q OR TC FUNCTION +1.
|
||||
# ZERO OR NEGATIVE ARGUMENTS YIELD ZERO FOR SQUARE ROOTS.
|
||||
|
||||
GTSQRT CCS FUNCTION
|
||||
@ -506,7 +506,6 @@ SCALDONE EXTEND
|
||||
TC ROOTCYCL
|
||||
TC ROOTCYCL
|
||||
TC FUNCTION +1
|
||||
|
||||
# ***************************************************************************
|
||||
|
||||
RSTOFGTS TC GTSQRT
|
||||
@ -514,12 +513,12 @@ PRODUCT XCH K2CNTRAL
|
||||
EXTEND
|
||||
MP K2CNTRAL
|
||||
DXCH K2CNTRAL
|
||||
EXTEND # THE PRODUCT OF
|
||||
EXTEND # THE PRODUCT OF
|
||||
MP L # 1/2 2 1/2
|
||||
ADS K2CNTRAL +1 # K *(DEL*OMEGA + ALPHA /(2*K))
|
||||
TS L # AND
|
||||
TCF +2 # 2
|
||||
ADS K2CNTRAL # DEL*(DEL*OMEGA + ALPHA /(2*K)) NOW IN
|
||||
TS L # AND
|
||||
TCF +2 # 2
|
||||
ADS K2CNTRAL # DEL*(DEL*OMEGA + ALPHA /(2*K)) NOW IN
|
||||
# K2CNTRAL
|
||||
|
||||
DOSHIFT CA ININDEX
|
||||
@ -538,22 +537,22 @@ DOSHIFT CA ININDEX
|
||||
XCH K2CNTRAL
|
||||
EXTEND
|
||||
MP K2CNTRAL +1
|
||||
ADS K2THETA +1
|
||||
ADS K2THETA +1
|
||||
TS L
|
||||
TCF +2
|
||||
TCF +2
|
||||
ADS K2THETA
|
||||
|
||||
TCF NEGUSUM
|
||||
|
||||
ADDITIN EXTEND
|
||||
DCA K2CNTRAL
|
||||
DAS K2THETA # NO ADD IN THE K2THETA TERM.
|
||||
DAS K2THETA # NOW ADD IN THE K2THETA TERM.
|
||||
NEGUSUM CCS K2THETA # TEST SIGN OF HIGH ORDER PART.
|
||||
TCF NEGDRIVE
|
||||
TCF +2
|
||||
TCF +2
|
||||
TCF POSDRIVE
|
||||
|
||||
CCS K2THETA +1 # SIGN TEST FOR LOW ORDER PART.
|
||||
CCS K2THETA +1 # SIGN TEST FOR LOW ORDER PART.
|
||||
NEGDRIVE CA BIT1
|
||||
TCF +2 # STOP GIMBAL DRIVE FOR A ZERO NEGUSUM.
|
||||
POSDRIVE CS BIT1
|
||||
@ -598,16 +597,14 @@ CLOSEADR 2CADR CLOSEOUT # TERMINATE THE JASK.
|
||||
TWELVE EQUALS OCT14
|
||||
ROOTHALF OCTAL 26501 # SQUARE ROOT OF 1/2
|
||||
GMBLBITA OCTAL 01400 # INDEXED WRT GMBLBITB DO NOT MOVE *******
|
||||
OCT11276 OCTAL 11276 # POSMAX -- ROOTHALF
|
||||
OCT11276 OCTAL 11276 # POSMAX - ROOTHALF
|
||||
GMBLBITB OCTAL 06000 # INDEXED WRT GMBLBITA DO NOT MOVE *******
|
||||
|
||||
# SUBROUTINE ROOTCYCL: BY CRAIG WORK, 3 APRIL 68
|
||||
#
|
||||
# ROOTCYCL IS A SUBROUTINE WHICH EXECUTES ONE NEWTON SQUARE ALGORITHM ITERATION. THE INITIAL GUESS AT THE
|
||||
# SQUARE ROOT IS PRESUMED TO BE IN THE A REGISTER AND ONE-HALF THE SQUARE IS TAKEN FROM HALFARG. THE NEW APPROXIMATION
|
||||
# TO THE SQUARE ROOT IS RETURNED IN THE A REGISTER. DEBRIS: A, L, SR, SCRATCH. ROOTCYCL IS CALLED FROM
|
||||
# ROOTCYCL IS A SUBROUTINE WHICH EXECUTES ONE NEWTON SQUARE ROOT ALGORITHM ITERATION. THE INITIAL GUESS AT THE
|
||||
# SQUARE ROOT IS PRESUMED TO BE IN THE A REGISTER AND ONE-HALF THE SQUARE IS TAKEN FROM HALFARG. THE NEW APPROXI-
|
||||
# MATION TO THE SQUARE ROOT IS RETURNED IN THE A REGISTER. DEBRIS: A,L,SR,SCRATCH. ROOTCYCL IS CALLED FROM
|
||||
# LOCATION (LOC) BY A TC ROOTCYCL, AND RETURNS (TC Q) TO LOC +1.
|
||||
#
|
||||
# WARNING: IF THE INITIAL GUESS IS NOT GREATER THAN THE SQUARE, DIVIDE OR ADD OVERFLOW IS A REAL POSSIBILITY.
|
||||
|
||||
ROOTCYCL TS SCRATCH # STORE X
|
||||
@ -618,5 +615,3 @@ ROOTCYCL TS SCRATCH # STORE X
|
||||
DV SCRATCH # (ARG/X)/2
|
||||
AD SR # (X + ARG/X)/2 IN THE A REG
|
||||
TC Q
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user