SUBROUTINE REALMAIN(Done)
*------------------------------------------------------------------------------
* Purpose:
* Serves as the main executive for the simulation
* in realtime mode.
*------------------------------------------------------------------------------
*
* Declaration Section
*
*------------------------------------------------------------------------------
IMPLICIT NONE
*------------------------------------------------------------------------------
* Local Variables
*------------------------------------------------------------------------------
INTEGER Max_Controls
PARAMETER (Max_Controls = 20)
INTEGER I, numlines
REAL DT, T, NITER, BatRun, RealRun, ITPW, RUNTIME
REAL RTOD, TERM_UPDATE_HZ, MODEL_DT, SPEEDUP, IO_DT
REAL P, Q, R, VT, ALFATRIM, BETATRIM, PHI, THETA, PSI, ALT, POW
REAL DUMT, MOMCMD(3), LMOM, MMOM, NMOM, MAXMOM(3)
INTEGER MULTILOOP, COUNT, ProcessIOString
DOUBLEPRECISION T1,T2,STEPSIZE(10000),READCLOCK(10000)
DOUBLEPRECISION T1A,T2A,T3,T4,T5,T6,T7,T8,T9,T10
LOGICAL Do_AGILEVU, Done, ReadStickFile
LOGICAL Do_Fmod, Do_Conallo, Do_Store, SaveXs, SaveXDs, SaveMoms
LOGICAL SaveCons, SaveSelect, Fexist, Do_Flapscd, SaveConallo
LOGICAL SaveInputs, SaveRates, SaveMisc, Do_Bogey_Input
LOGICAL MomScale, SaveTiming, DoTiming, DrawGraphics, EndOfRun
LOGICAL Do_Bogey_Output1, Do_Bogey_Output2
LOGICAL Do_All_Output_Files, Do_No_Output_Files, DisableStick
CHARACTER*4 Fileext, Default_ext, Output_ext, AV_ext, Filetype
PARAMETER (Default_ext = '.dat', AV_ext = '.flt.')
CHARACTER*16 Filename
CHARACTER*20 Header
CHARACTER*30 Filepath
CHARACTER*80 Dummy, HOMEDIR, Message(1), OutputFormat
REAL U(Max_Controls), UCMD(Max_Controls)
INTEGER IOStat
INTEGER ispace, bogeycount
REAL USAT(Max_Controls)
REAL TERM_UPDATE_HZ2, RUNTIME2
REAL TIME, X(30), XD(30)
*------------------------------------------------------------------------------
*
* (new) COMMON/DATA Section
*
*------------------------------------------------------------------------------
INCLUDE 'INCLUDES/a_cvars.com'
INCLUDE 'INCLUDES/simvars.com'
INCLUDE 'INCLUDES/simprs.com'
INCLUDE 'INCLUDES/flags.com'
INCLUDE 'INCLUDES/bogey.com'
INCLUDE 'INCLUDES/gencon.com'
INCLUDE 'INCLUDES/timing.com'
INCLUDE 'INCLUDES/allocat.com'
INCLUDE 'INCLUDES/filenames.com'
INCLUDE 'INCLUDES/fileunits.com'
INCLUDE 'INCLUDES/controlblock.com'
INCLUDE 'INCLUDES/xyzbogeyblock.com'
*------------------------------------------------------------------------------
*
* Initialization Section
*
*------------------------------------------------------------------------------
* Equivalences:
*--------------
*** <equivalences deleted>
*---------------------
ABORT = 0
T = 0.0
GEARDOWN=.FALSE.
BRAKEON=.FALSE.
EndOfRun = .FALSE.
IF (OutputFormat(1:6).EQ.'matlab') THEN
Output_ext = '.m'
ELSEIF (OutputFormat(1:12).EQ.'cricketgraph') THEN
Output_ext = '.dat'
ELSE
write (*,*) 'Sorry, file extension screwup'
stop
ENDIF
MODEL_DT= 1/TERM_UPDATE_HZ
STEPCOUNT=0 ! initialization
T1=0.0 ! initialization
T2=0.0 ! initialization
*------------------------------------------------------------------------------
*
* Execution Section
*
*------------------------------------------------------------------------------
Fexist = .TRUE.
Message(1) = 'Running Out-of-Window Simulation...'
IOStat = ProcessIOString(Message,1,0,0)
!----------------------------------------------------------
! Open and read data from input files (read data to arrays)
!----------------------------------------------------------
CALL PROCESS_INPUT_FILE
!--------------------------------
! Open and setup all output files
!--------------------------------
IF (Do_All_Output_Files) THEN
Do_Store = .TRUE.
SaveXs = .TRUE.
SaveXDs = .FALSE.
SaveMoms = .TRUE.
SaveCons = .TRUE.
SaveConallo = .TRUE.
SaveInputs = .TRUE.
SaveRates = .TRUE.
SaveMisc = .TRUE.
SaveTiming = .TRUE.
Do_AGILEVU = .TRUE.
Do_Bogey_Output1 = .TRUE.
Do_Bogey_Output2 = .TRUE.
ELSEIF (Do_No_Output_Files) THEN
SaveXs = .FALSE.
SaveXDs = .FALSE.
SaveMoms = .FALSE.
SaveCons = .FALSE.
SaveConallo = .FALSE.
SaveInputs = .TRUE.
SaveRates = .FALSE.
SaveMisc = .FALSE.
SaveTiming = .FALSE.
Do_AGILEVU = .FALSE.
Do_Bogey_Output1 = .TRUE.
Do_Bogey_Output2 = .FALSE.
ENDIF
IF (Do_Store) THEN
CALL PROCESS_OUTPUT_FILE('open')
CALL PROCESS_OUTPUT_FILE('init')
ENDIF
! Set READSTICKINPUT flag for use in gl_cockpit:
IF (Do_Conallo) THEN
IF (ReadStickFile.OR.DisableStick) THEN
READSTICKINPUT = 0
ELSE
READSTICKINPUT = 1
ENDIF
THTL = 0.0
LONGCON = 0.0
LATCON = 0.0
LATDIRCON = 0.0
IF (Do_Bogey_Input) THEN
DRAWBOGEYAC = 1
CALL GET_BOGEY_GEOMETRY
ELSE
DRAWBOGEYAC = 0
ENDIF
END IF
*-------------------
* Begin Simulation
*-------------------
DT = MODEL_DT
*------------------------
* Initialize simulation
*------------------------
! Enable moment scaling of realtime stick inputs:
MomScale = .TRUE.
! Initialize output data files
Fexist = .FALSE.
*----------------------
* Set up to read stick:
*----------------------
IF (DrawGraphics) THEN
CALL opencalltm ! call C routine which calls open_flybox
ENDIF
*---------------------------
* Schedule flaps if desired:
*---------------------------
IF (Do_Flapscd) THEN
CALL FLAPSCD
ENDIF
! note the call to Conallo.f is done via the above call to A_CCONTROL, which
! calls Conallo to perform stick logic (moment scaling) as well as to allocate
! controls. Therefore the following conditional structure calls CONALLO only
! if Do_Fmod is false
*-------------------
* Allocate controls:
*-------------------
! the following call to CONALLO is now performed in FMODELREAL
!
! IF (Do_Conallo) THEN
! CALL CONALLO
! ENDIF
*----------------------------
* t = 0.0 (initial conditions
*----------------------------
I = 1
CALL F(TIME,X,XD)
IF (ReadStickFile) THEN
LATCON = LATARRAY(I)
LONGCON = LONGARRAY(I)
LATDIRCON = LATDIRARRAY(I)
THTL = THTLARRAY(I)
ENDIF
IF (Do_Bogey_Input) THEN
X_BOGEY = X_BOGEY_ARRAY(I)
Y_BOGEY = Y_BOGEY_ARRAY(I)
Z_BOGEY = Z_BOGEY_ARRAY(I)
PHI_BOGEY = PHI_BOGEY_ARRAY(I)
THETA_BOGEY = THETA_BOGEY_ARRAY(I)
PSI_BOGEY = PSI_BOGEY_ARRAY(I)
ENDIF
IF (Do_Store) THEN
CALL PROCESS_OUTPUT_FILE('write')
ENDIF
*---------------------------------------
* Main realtime simulation loop:
*---------------------------------------
I = 1
CALL ms_time(T1)
STEPCOUNT = 0
DO WHILE (T.LT.(RUNTIME-.0005).AND.ALT.GE.0.0)
CALL ms_time(T1A)
CALL ms_time(T3)
CALL F(TIME,X,XD)
CALL ms_time(T4)
IF (ReadStickFile) THEN
LATCON = LATARRAY(I)
LONGCON = LONGARRAY(I)
LATDIRCON = LATDIRARRAY(I)
THTL = THTLARRAY(I)
ENDIF
IF (Do_Bogey_Input) THEN
X_BOGEY = X_BOGEY_ARRAY(I)
Y_BOGEY = Y_BOGEY_ARRAY(I)
Z_BOGEY = Z_BOGEY_ARRAY(I)
PHI_BOGEY = PHI_BOGEY_ARRAY(I)
THETA_BOGEY = THETA_BOGEY_ARRAY(I)
PSI_BOGEY = PSI_BOGEY_ARRAY(I)
ENDIF
IF (Do_Fmod) THEN
CALL FMODELREAL(T7,T8,T9,T10)
ENDIF
IF (Do_Flapscd) THEN
CALL FLAPSCD
ENDIF
STEPCOUNT=STEPCOUNT+1
IF (DrawGraphics) THEN
CALL ls_cockpit() ! C routine which draws graphics
ENDIF
*----------------------
* start of timing stuff
*----------------------
IF (SaveTiming) THEN
CALL ms_time(READCLOCK(STEPCOUNT))
CLOCKTIME = READCLOCK(STEPCOUNT)
IF (STEPCOUNT.NE.1) THEN
READCLK0 = READCLOCK(STEPCOUNT-1)
ELSE
READCLK0 = T1
ENDIF
READCLK1 = READCLOCK(STEPCOUNT)
IF (STEPCOUNT.NE.1) THEN
STEPSIZE(STEPCOUNT-1) =
. READCLOCK(STEPCOUNT) - READCLOCK(STEPCOUNT-1)
STEPSIZE0 = STEPSIZE(STEPCOUNT-1)
! only 'really' do realtime when DoTiming is .true.
IF (DoTiming) THEN
IF ( STEPSIZE(STEPCOUNT-1) .LT. DT) THEN
! *** pause ***
DOWHILE (CLOCKTIME .LT. (READCLOCK(STEPCOUNT)+
. (DT-STEPSIZE(STEPCOUNT-1))) )
CALL ms_time(CLOCKTIME)
ENDDO
ELSE
WRITE (*,*) 'I = ', I
WRITE (*,*) 'Sorry, calculations were too slow this'
WRITE (*,*) 'frame. '
WRITE (*,*) 'STEPCOUNT = ',STEPCOUNT
WRITE (*,*) 'DT = ',DT
WRITE (*,*) 'ACTUAL STEPSIZE = ',STEPSIZE(STEPCOUNT-1)
IF (SaveTiming) THEN
CALL PROCESS_OUTPUT_FILE('finish')
ENDIF
STOP
ENDIF
ENDIF ! endif (DoTiming)
ELSE
STEPSIZE0 = 0
ENDIF
ENDIF
*--------------------
* end of timing stuff
*--------------------
CALL ms_time(T5)
CALL RK4
CALL ms_time(T6)
CALL ms_time(T2A)
! calculate times for eom, rk4, ca:
EOMTIME = T4 - T3
RKTIME = T6 - T5
CATIME = T10 - T9
LOGICTIME = T8 - T7
REALTIME = CATIME + LOGICTIME
IF (EOMTIME .LE. 0.0) EOMTIME = 0.0
IF (RKTIME .LE. 0.0) RKTIME = 0.0
IF (CATIME .LE. 0.0. or. CATIME .GE. 1.0) CATIME = 0.0
IF (LOGICTIME .LE. 0.0) LOGICTIME = 0.0
IF (Do_Store) THEN
CALL PROCESS_OUTPUT_FILE('write')
ENDIF
IF (ABORT.EQ..TRUE.) GOTO 10
I = I+1
ENDDO ! DO WHILE (T.LT.(RUNTIME-.0005).AND.ALT.GE.0.0)
CALL ms_time(T2)
*---------------------------------
* End of Realtime Simulation Loop
*---------------------------------
*-----------------------------------------------
* Close data files and set ABORT and Done flags:
*-----------------------------------------------
IF (Do_Store) THEN
EndOfRun = .TRUE.
CALL PROCESS_OUTPUT_FILE('finish')
EndOfRun = .FALSE.
ENDIF
IF (ABORT .EQ. 1) THEN
Message(1) = 'ABORTED'
Done = .FALSE.
ELSE
Message(1) = 'Done'
Done = .TRUE.
END IF
IOStat = ProcessIOString(Message,1,0,0)
*-------------------
* Timing statistics:
*-------------------
10 CONTINUE
WRITE (6,*)
WRITE (6,*)
WRITE (6,*) 'Real time statistics: '
WRITE (6,*) 'Desired end time is ',RUNTIME,' sec'
WRITE (6,*) 'Desired step size is ',MODEL_DT,' sec'
WRITE (6,*) 'Number of multi-loop calls is ',MULTILOOP
WRITE (6,*)
WRITE (6,*) 'Actual end time was ',T2-T1,' sec'
WRITE (6,*) 'Actual step size was ',(T2-T1)/STEPCOUNT, 'sec'
IF (DrawGraphics) THEN
CALL closecalltm ! close joystick port (in openclose_tm.c)
ENDIF
*---------------------
* END OF SIMULATION
*---------------------
* Return to menus:
Done = .TRUE.
RETURN
* --------------------
* Format statements:
* --------------------
600 FORMAT(A)
601 FORMAT('Time:',A1,F6.2)
602 FORMAT(5X,5(F10.3,A1),/,5X,5(F10.3,A1))
!603 FORMAT(5X,5(F10.3,A1),/,5X,5(F10.3,A1),/,5X,5(F10.3,A1))
603 FORMAT(5X,5(F10.3,A1))
605 FORMAT(5X,3(F12.6,A1),/,5X,3(F12.6,A1))
700 FORMAT(5X,5(F10.3,A1))
1040 FORMAT ('Opened file ',I2,' named ',A80,' successfully')
1041 FORMAT ('Closed file ',I2,' named ',A80,' successfully')
1042 FORMAT ('Closed file ',I2,' successfully')
2000 FORMAT (13e5,3X)
END
*------------------------------------------------------------------------------
*
* End of Module REALMAIN
*
*------------------------------------------------------------------------------
C23456789012345678901234567890123456789012345678901234567890123456789012
!
! File: F18Funs_x_sl.f
!
SUBROUTINE FMODELREAL(T7,T8,T9,T10)
! ----------------------------------------------------------------------
!
! Function: ESTIMATES AERODYNAMIC PARAMETERS FROM AEROMOD AND
! FINDS THE REQUIRED MOMENTS NEEDED TO SATISFY THE PILOT
! COMMANDS SENT FROM MODULE GIF /*BY DYNAMIC INVERSION*/
!
! ----------------------------------------------------------------------
!
! MODIFICATIONS:
! Date Purpose By
! JUL 28 1995 Decreased LAMDAV,LAMDAW,LAMDAP,LAMDAQ,LAMDAR
! 50% JB
! SEPT 1 1995 moved the above parameters to the settings
! file JB
! JAN 29 1996 added logic for auto throttle JB
! JUL 03 1996 Added new COMMON and EQUIVALENCE Info for
! F-18 V1.2 JB
!
! ----------------------------------------------------------------------
!
! Glossary Section
!
! ----------------------------------------------------------------------
! Global Variables
!
! Name | Type | Description
*ALFA REAL Angle of attack (degrees)
*BETA REAL Sideslip angle (degrees)
*VT REAL Aircraft total velocity (Ft/sec)
*ALT REAL Altitude (ft)
*P REAL Body axis roll rate (sec^-1)
*Q REAL Body Axis pitch rate (sec^-1)
*R REAL Body axis yaw rate (sec^-1)
*DHTL REAL Left stabilator def. (deg)
*DHTR REAL Right stabilator def. (deg)
*DAL REAL Left aileron deflection (deg)
*DAR REAL Right aileron deflection (deg)
*DRL REAL Left rudder deflection (deg)
*DRR REAL Right rudder deflection (deg)
*THETA REAL Euler pitch angle (rad)
*PHI REAL Euler roll angle (rad)
*PSI REAL Euler heading angle (rad)
*LAMDAV REAL Control Law parameter
*LAMDAW REAL Control law parameter
*THTLTRIM REAL Trimmed Throttle position
*LAMDAP REAL Control law parameter
*LAMDAQ REAL Control law parameter
*LAMDAR REAL Control law parameter
*Do_Conallo LOGICAL Status of Control Allocation
*MomScale LOGICAL Flag used in Conallo (use global AMS)
*VTCMD REAL Velocity command (ft/sec)
*THTL REAL Throttle position (ND)
*LMOM REAL Output from Control allocation (Cl or dCl)
*MMOM REAL Output from control allocation (Cm or dCm)
*NMOM REAL Output from control allocation (Cn or dCn)
*MOMCMD() REAL Commanded control generated moments
*ALFACMD REAL ALFA Command (rad)
*BETACMD REAL BETA Command (rad)
*PCMD REAL Roll rate command (sec^-1)
*MAXMOM REAL Maximum moment in direction of desired moment;
* used in stick logic moment scaling
! ----------------------------------------------------------------------
IMPLICIT NONE
! ----------------------------------------------------------------------
!
! Declaration Section
!
! ----------------------------------------------------------------------
INTEGER Max_Controls
PARAMETER (Max_Controls = 20)
REAL ALFA, BETA, VT, ALT, P, Q, R, DHTL, DHTR, DAL, DAR, DRL,
. DRR, THETA, PHI, PSI, LAMDAV, LAMDAW, THTLTRIM, LAMDAP,
. LAMDAQ, LAMDAR, VTCMD, MOMCMD(3), LMOM, MMOM, NMOM,
. ALFACMD, BETACMD, PCMD
REAL U(Max_Controls),UCMD(Max_Controls)
REAL MAXMOM(3), STKSAT
REAL GetUEff
REAL TIME
LOGICAL Do_Conallo
DOUBLEPRECISION T7,T8,T9,T10
! -------------------------------Locals---------------------------------
INTEGER NN, I, J, k
PARAMETER (NN=30)
REAL T, XM(NN), XMD(NN)
REAL QCMD,RCMD,VDOTCMD,WDOTCMD,PDOTCMD,QDOTCMD,RDOTCMD
REAL CYX,CZX
REAL CD,CY,CL,CZ
REAL D2R,R2D
REAL U_,V,W
REAL ALPHA,AMACH,QBAR,PS
REAL CALP,SALP,CBTA,SBTA,STH,CTH,SPH,CPH,SPSI,CPSI,QSB,QSC,QSPH
REAL S,B,CBAR,RM,XCGR,G
REAL QS,RMQS,GCTH,IXX,IYY,IZZ,IXZ
REAL BETAR
REAL KTHTL
LOGICAL Do_U_Effects, MomScale
PARAMETER (KTHTL = 0.0375)
! ----------------------------------------------------------------------
!
! Common Section
!
! ----------------------------------------------------------------------
INCLUDE 'a_cvars.com'
INCLUDE 'simvars.com'
INCLUDE 'simprs.com'
INCLUDE 'gencon.com'
INCLUDE 'rateincs.com'
INCLUDE 'aerovars.com'
INCLUDE 'allocat.com'
INCLUDE 'usats.com'
INCLUDE 'timing.com'
! ----------------------------------------------------------------------
!
! Equivalence Section
!
! ----------------------------------------------------------------------
*** <equivalences deleted>
! ----------------------------------------------------------------------
DATA S,B,CBAR,RM,G,IXX,IYY,IZZ,IXZ/400,34.72,11.52,1.0E-3,32.174,
. 23168.0,123936.0,143239.0,-2970.75/
DATA R2D,D2R / 57.29578, 0.01745329/
DATA Do_U_Effects/ .TRUE./
! ----------------------------------------------------------------------
!
! Run Section
!
! ----------------------------------------------------------------------
ALPHA = ALFA*D2R
BETAR = BETA*D2R
CALL ADC(VT, ALT, AMACH, QBAR, PS)
CALL AEROMOD( CD, CY, CL, C1, CM, CN,
. ALFA, BETA, AMACH, P, Q, R,
. DHTR, DHTL, DAR, DAL, DRR, DRL,
. Do_U_Effects, .TRUE.)
CALP = COS(ALPHA)
SALP = SIN(ALPHA)
CBTA = COS(BETAR)
SBTA = SIN(BETAR)
U_= VT * CALP * CBTA
V= VT * SBTA
W= VT * SALP * CBTA
STH = SIN(THETA)
CTH = COS(THETA)
SPH = SIN(PHI)
CPH = COS(PHI)
SPSI = SIN(PSI)
CPSI = COS(PSI)
QS = QBAR * S
QSB = QS * B
QSC = QS * CBAR
RMQS = RM * QS
GCTH = G * CTH
QSPH = Q * SPH
CZ = -CD*SALP*CBTA-CL*CALP
CYX = CY-DCYDR*R
CZX = -(CD-DCDDQ*Q)*SALP*CBTA-(CL-DCLDQ*Q)*CALP
! INTERPRET PILOT COMMANDS
! MOMENT EQUATIONS (no control law):
! Given: LATCON, LONGCON, LATDIRCON (each -1 to 1)
MomScale = .TRUE. ! Flag used in Conallo.f
CALL ms_time(T7)
! Give initial desired moment direction so algorithm can find intersection
! with AMS:
MOMCMD(1) = -LATCON
MOMCMD(2) = -LONGCON
MOMCMD(3) = LATDIRCON
CALL CONALLO ! Find int of desired moment with global AMS for
! use in scaling; makes full stick deflection command
! full attainable moment in that particular direction
! (logic if SATM or STKSAT = 0)
IF (SATM.ne.0) THEN ! only allocate if saturation not negligible
! the following should make sure that these controls for max moment are
! on the AMS boundary:
DO 1045 J = 1,M
IF (SATM.LT.1.0) THEN
USAT(J) = UALLO_SL(J)/SATM
ELSE
USAT(J) = UALLO_SL(J)
ENDIF
1045 CONTINUE
! FIND MAX MOMENT
DO 1005 I = 1,3
MAXMOM(I) = 0.0
DO 1006 J = 1,M
BMAT(I,J) = GETUEFF (I, IU(J), 0.0)
MAXMOM(I) = MAXMOM(I) + BMAT(I,J)*USAT(J)
1006 CONTINUE
1005 CONTINUE
! calculate stick saturation:
IF ( ABS(LATCON) .GE. ABS(LONGCON) ) THEN
STKSAT = ABS(LATCON)
ELSE
STKSAT = ABS(LONGCON)
ENDIF
! finally, the moment inputs to the call to CONALLO for its intended purpose,
! allocating controls:
MOMCMD(1) = -STKSAT*MAXMOM(1)
MOMCMD(2) = -STKSAT*MAXMOM(2)
MOMCMD(3) = STKSAT*MAXMOM(3)
CALL ms_time(T8)
! call CONALLO again to calculate the actual controls (not max)
CALL ms_time(T9)
MomScale = .FALSE. ! Flag used in Conallo.f
CALL CONALLO
ELSE ! (else SATM does equal zero; don't allocate)
Do_Conallo = .false.
ENDIF
IF (.NOT. Do_Conallo) THEN
LMOM = MOMCMD(1)
MMOM = MOMCMD(2)
NMOM = MOMCMD(3)
END IF
A_CVARR(56) = THTL ! can't use EQUIVALENCE statement here
! since THTL is in another common block
CALL ms_time(T10)
! ----------------------------------------------------------------------
!
! End Of FMODEL
!
! ----------------------------------------------------------------------
RETURN
END
C23456789012345678901234567890123456789012345678901234567890123456789012
* File: Conallo.f: Control Allocation Software Version 4.0a
* Contents:
* CONALLO: Main Control Allocation executive module.
* ALLODIAGSOUT: Writes the ConAllo diagnostic output.
* RESTORE_U: Control restoring algorithms.
* GET_FACET: Facet searching algorithm.
* GET_MAT: Formulates the geometry defining matrix for a
* specific facet.
* GET_U: checks a facet and allocates controls if pos-
* sible.
* PINVB3: Pseudo-Inverse calculation of a 3xm matrix.
* PINVB4 Pseudo-Inverse calculation of a 4xm matrix.
* INVMAT3: 3X3 matrix inversion.
* INVMAT4: 4x4 matrix inversion.
* D3: Determinant of a 3X3 matrix
*
C23456789012345678901234567890123456789012345678901234567890123456789012
! ----------------------------------------------------------------------
!
! Module Name: CONALLO
! Called By: SHELL_CONALLO, TRIMMER, LINRIZE, SIMBATCH
! Calls to: A_CGETUEFF, GET_FACET, A_CGETCSTR
!
! ----------------------------------------------------------------------
SUBROUTINE CONALLO
! ----------------------------------------------------------------------
!
! Function: Main Control Allocation executive
!
!
! ----------------------------------------------------------------------
!
! Modifications:
! Date Purpose By
! JUL 09 1996 Created. (Based on major revisions to version
! 3.0) JB
! SEPT 11 1996 Added calls to a new aircraft-specifc module
! "A_CGETCSTR" to take care of all of the
! constraints, and the module "A_CTFORM" which takes
! care of any transformation tricks of BMAT JB
! OCT 11 1996 Removed calls to A_CTFORM, (not used anymore) JB
! JAN 11 1997 Added the diagnostics output subroutine ALLODIAGS-
! OUT. JB
! MAR 08 1997 changed the argument list that is sent to A_CGET-
! CSTR. Also made modifications so that Conallo cal-
! culates the control constraints based on position
! and rate info from A_CGETCSTR (which is where it
! was previously done). Finally, gains were added to
! the final commanded deflections during run time
! to overdrive the actuators so that max rate command
! equals max deflection rate. (yeah, confusing, but
! TRUST ME, IT WORKS) One more thing, I put some more
! logic in to bypass restoring (if enabled) when the
! rate saturation in moment space is greater than 1.
! This is done because the restoring algorithms gen-
! erally don't give maximum rate deflections which are
! required when RSAT > 1.0 JB
! MAR 23 1997 Changed the argument list to GETCSTR for the last
! time. JB
!
! ----------------------------------------------------------------------
!
! Glossary Section
!
! ----------------------------------------------------------------------
! Global Variables
!
! Name | Type | Description
*Trimming LOGICAL Status of Trimmer
*RealRun LOGICAL Realtime run status
*MomScale LOGICAL True if calling Conallo to scale desired moment
*Do_Linrize LOGICAL Status of Linearization Utility
*Initialized LOGICAL Initialization flag
*Do_Diags LOGICAL Diagnostics flag
*DT REAL Time step (sec)
*MOMCMD(1) REAL Commanded Cl (control generated)
*OLDMOM(1) REAL Actual Cl due to controls (previous iter.)
*LMOM REAL Input to Control allocation (Cl or dCl)
*MMOM REAL Input to control allocation (Cm or dCm)
*NMOM REAL Input to control allocation (Cn or dCn)
*U(1) REAL Control 1 deflection (deg)
*PSAT REAL Control Position saturation flag
*PSATOLD REAL % control rate saturation; used only for writing
* to an output data file
*RSAT REAL % control rate saturation
*NCTRLS INTEGER Number of configurable controls
*UCMD(1) REAL Control 1 def. Command (deg)
*IFAIL(1) INTEGER Failure Status: Control 1
*UTAU(1) REAL 1st order time constant U(1)
!
! CONALLO Globals
!
! Name | Type | Description
*MF INTEGER Number of failed controls
*M INTEGER Number of controls to allocate
*U1 INTEGER Number of 1st control defining facet
*U2 INTEGER Number of 2nd control defining facet
*IU(M) INTEGER UALLO(IU(M)) <-> U(M)
*RTYPE INTEGER Type of restoring logic to use
*
*
*URMIN(M) REAL The deflection rate limits (min. direction)
*URMAX(M) REAL The deflection rate limits (max. direction)
*UMIN(M) REAL The minimum control deflection constraints
*UMAX(M) REAL The maximum control deflection constraints
*BMAT(3,M) REAL Control power matrix (local or global)
*UALLO(M) REAL The allocated control deflections
*MOM(3) REAL Commanded moment inputs (deltas or global)
*SATM REAL saturation level (taken in moment space)
*RSATU REAL maximum rate saturation level (control space)
*
!
! Local Variables
!
! Name | Type | Description
*Allocated LOGICAL TRUE if allocation successful
*Isfailure LOGICAL TRUE if there is a failure present
*Use_Globals LOGICAL TRUE if global limits and eff. is to be used
*
*UFAIL(MF) REAL vector of failed controls
*IUF(MF) INTEGER UFAIL(IUF(MF)) <-> U(MF)
*MAXMOM(3) REAL maximum moment in direction of desired moment;
* used in stick logic moment scaling
*ACTMOM(3) REAL Actual moments produced
*MOLD(3) REAL Moments at previous iteration
*MDOT(3) REAL Moment rates
*UDOT(3) REAL Control deflection rates
*UOLD(3) REAL Control deflections at prev. iter
*NORMU REAL Norm of control deflections
! ----------------------------------------------------------------------
IMPLICIT NONE
! ----------------------------------------------------------------------
!
! Declaration Section
!
! ----------------------------------------------------------------------
INTEGER Max_Controls
PARAMETER (Max_Controls = 20)
LOGICAL Trimming, Do_Linrize, Initialized, Do_Diags, RealRun
LOGICAL MomScale
INTEGER IFAIL(Max_Controls)
INTEGER NCTRLS
REAL DT
REAL MOMCMD(3), OLDMOM(3), LMOM, MMOM, NMOM, PSAT, RSAT, PSATOLD
REAL RSATO, RSATUOLD
REAL U(Max_Controls), UCMD(Max_Controls), UTAU(Max_Controls)
REAL UOLD(Max_Controls), UDOT(Max_Controls), DU(Max_Controls)
REAL ACTMOM(3), MOLD(3), MDOT(3), NORMU, ACTMOM_DEL(3)
REAL MAXMOM(3)
! -------------------------------Locals---------------------------------
LOGICAL Allocated, Isfailure, Use_Globals
INTEGER I, J, K, MF, IUF(Max_Controls), ISTATUS
REAL UFAIL(Max_Controls),SECNDS
CHARACTER*80 MSG
REAL ETOTAU,GK
! ---------------------------Shared Library-----------------------------
REAL GETUEFF
! ----------------------------------------------------------------------
!
! Common Section
!
! ----------------------------------------------------------------------
INCLUDE 'INCLUDES/a_cvars.com'
INCLUDE 'INCLUDES/simvars.com'
INCLUDE 'INCLUDES/simprs.com'
INCLUDE 'INCLUDES/flags.com'
INCLUDE 'INCLUDES/allocat.com'
INCLUDE 'INCLUDES/allodiags.com'
! ----------------------------------------------------------------------
!
! Equivalence Section
!
! ----------------------------------------------------------------------
*** <equivalences deleted>
! ----------------------------------------------------------------------
!
! Initialization Section
!
! ----------------------------------------------------------------------
! ----------------------------------------------------------------------
!
! Run Section
!
! ----------------------------------------------------------------------
DG_ET = SECNDS(0.0)
MF = 0
Allocated = .FALSE.
Isfailure = .FALSE.
SATM = 0.
RSATU = 0.
ISTATUS = -99
MSG = ' '
! Get the approximate control generated moments for current frame using
! the slope at the origin method.
DO 1005 I = 1,3
OLDMOM(I) = 0.0
DO 1006 J = 1,NCTRLS
BMAT(I,J) = GETUEFF(I,J,0.0)
OLDMOM(I) = OLDMOM(I) + BMAT(I,J)*U(J)
1006 CONTINUE
1005 CONTINUE
! Use local effectiveness and contraints during RUNTIME, and use the
! slopes at the control origins and global constraints during
! TRIM, LINEARIZATION.
IF (Trimming .OR. Do_Linrize .OR. MomScale) THEN
Use_Globals = .TRUE.
ELSE
Use_Globals = .FALSE.
END IF
DG_U_Globals = Use_Globals
! See if all of the controls are working and reconfigure the controls
! to allocate if we have to.
J = 1
M = NCTRLS
DO 1020 I = 1,NCTRLS
IF (IFAIL(I) .NE. 0) THEN
Isfailure = .TRUE.
M = M - 1
MF = NCTRLS - M
IUF(MF) = I
UFAIL(MF) = U(I)
IF (U1 .GT. M .OR. U2 .GT. M) THEN
! the column for the control eff. of the control that worked last
! frame has changed. We better search from scratch.
U1 = 0
U2 = 0
END IF
ELSE
IU(J) = I
J = J + 1
END IF
1020 CONTINUE
DO 1025 I = 1,M
UALLO(I) = 0.0
1025 CONTINUE
! Can we still allocate? If not, get out.
IF (M .LT. 3) THEN
WRITE(6,'(1x,A)') 'Too few controls to allocate--EJECT! EJECT!'
ABORT = 1
RETURN
END IF
! Get the control power matrices for the controls (that work).
DO 1030 I = 1,3
DO 1031 J = 1,M
IF (Use_Globals) THEN
BMAT(I,J) = GETUEFF(I,IU(J),0.0)
ELSE
BMAT(I,J) = GETUEFF(I,IU(J),U(IU(J)))
END IF
1031 CONTINUE
1030 CONTINUE
! Set the control minimum and maximum contraints for allocation. This
! is done by getting the position limits and rate limits and then taking
! the most restrictive of either the position limit or the amount that
! a control can move in one frame (for run mode only, otherwise, we
! take the position limits)
CALL GETCSTR(M, IU, UMAX, UMIN, URMAX, URMIN)
IF (.NOT. Use_globals) THEN
DO 1035 I = 1,M
UMIN(I) = AMAX1((UMIN(I) - U(IU(I))),-URMIN(I)*DT)
UMAX(I) = AMIN1((UMAX(I) - U(IU(I))), URMAX(I)*DT)
1035 CONTINUE
END IF
! Input moment commands. (we use absolute moment commands when
! TRIMMING. For RUNMODE, we use "delta" moment commands.)
IF (.NOT.MomScale) THEN
IF (Use_Globals) THEN
MOM(1) = LMOM
MOM(2) = MMOM
MOM(3) = NMOM
ELSE
DO 1042 I = 1,3
MOM(I) = MOMCMD(I) - OLDMOM(I)
1042 CONTINUE
END IF
ENDIF
IF (MomScale) THEN
MOM(1) = MOMCMD(1)
MOM(2) = MOMCMD(2)
MOM(3) = MOMCMD(3)
ENDIF
! Check for any position saturation of controls
if (.NOT.MomScale) THEN
PSAT = 0.0
DO 1050 I = 1,M
IF (UMAX(I) .EQ. 0.0 .OR. UMIN(I) .EQ. 0.0) THEN
PSAT = 1.0
END IF
1050 CONTINUE
endif
! Check to see if we even need to allocate or not
IF (.NOT.MomScale) THEN
IF (MOM(1) .EQ. 0. .AND. MOM(2) .EQ. 0. .AND. MOM(3) .EQ. 0.) THEN
Allocated = .TRUE.
END IF
END IF
IF (.NOT. Allocated) THEN
! Start Control Allocation. We check the facet that worked last frame
! right now.
IF (U1 .NE. 0 .AND. U2 .NE. 0) THEN
if (MomScale) then
CALL GET_FACET(UALLO_SL, Allocated, SATM, ISTATUS, MSG,
. BMAT,U1, U2, UMIN, UMAX, MOM, M, MAXMOM, IU)
else
CALL GET_FACET(UALLO, Allocated, SATM, ISTATUS, MSG,
. BMAT,U1, U2, UMIN, UMAX, MOM, M, MAXMOM, IU)
endif
IF (Allocated) THEN
GO TO 1059
END IF
END IF
! Oh man! now we have to start searching from scratch.
DO 1051 U1 = 1,M-1
DO 1052 U2 = U1+1,M
if (MomScale) then
CALL GET_FACET(UALLO_SL, Allocated, SATM, ISTATUS,
. MSG,BMAT,U1, U2, UMIN, UMAX, MOM, M, MAXMOM, IU)
else
CALL GET_FACET(UALLO, Allocated, SATM, ISTATUS,
. MSG,BMAT,U1, U2, UMIN, UMAX, MOM, M, MAXMOM, IU)
endif
IF (Allocated) THEN
GO TO 1059
END IF
1052 CONTINUE
1051 CONTINUE
END IF
1059 CONTINUE ! we're done allocating
IF (.NOT. Use_Globals) THEN
RSAT = SATM ! Rate Saturation (moment space)
! Time for some control restoring algorithms
IF (RTYPE .GT. 0 .AND. RSAT .LT. 1.0) THEN
CALL RESTORE_U (MOMCMD, U, DU) ! returns DU
ELSE ! allocated delta DU = 0 since no restoring performed
K = 1
DO WHILE (K .LE. M)
DU(K) = 0.0
K = K+1
END DO
END IF
! Calculate the allocated controls (after restoring if selected):
RSATU = 0.
DO 1947 I=1,M
RSATO = RSATU
UALLO(I) = UALLO(I) + DU(I) ! UALLO is the restored control
IF (UALLO(I) .LT. 0.) THEN
RSATU = 1. - (UMIN(I)-UALLO(I))/UMIN(I)
ELSE
IF (UALLO(I) .GT. 0.) THEN
RSATU = 1. - (UMAX(I)-UALLO(I))/UMAX(I)
ELSE
RSATU = 0.
END IF
END IF
RSATU = AMAX1(RSATU,RSATO)
1947 CONTINUE
RSATUOLD = RSATU
! Calculate the actual moments produced (incrementally building up):
DO 1053 I = 1,3
ACTMOM(I) = 0.0
DO 1054 J = 1,M
ACTMOM(I) = ACTMOM(I) + BMAT(I,J)*UALLO(IU(J))
1054 CONTINUE
ACTMOM_DEL(I) = ACTMOM(I)
ACTMOM(I) = MOLD(I) + ACTMOM_DEL(I)
1053 CONTINUE
! Calculate the moment rates. The MOLDs are initilized to zero in BDInit
DO 1057 I = 1,3
MDOT(I) = (ACTMOM(I) - MOLD(I)) / DT
MOLD(I) = ACTMOM(I)
1057 CONTINUE
! Calculate the control rates. The UOLDs are initialized to zero in
! BDInit
DO 1055 I = 1,NCTRLS
UDOT(I) = (UALLO(I) - UOLD(I)) / DT
UOLD(I) = UALLO(I)
1055 CONTINUE
! Get allocated Control commands. (The UTAU()/DT factor is the gain re-
! quired to overdrive the control commands so that max rate command =
! max rate deflection
DO 1060 I = 1,M
UCMD(IU(I)) = U(IU(I)) + UTAU(IU(I))/DT*UALLO(I)
ELSE ! if (Use_Globals)
! Here we are dealing with global positions not deltas
IF (MomScale) THEN
PSAT = SATM ! Position saturation (moment space)
ENDIF
IF (.NOT.MomScale) THEN
DO 1070 I = 1,M
UCMD(IU(I)) = UALLO(I)
1070 CONTINUE
ENDIF
END IF
DG_ET = SECNDS(DG_ET)
DG_ET = AMAX1(0.0,DG_ET)
DG_ISTAT = ISTATUS
DG_IMSG = MSG
IF (Do_Diags) THEN
CALL ALLODIAGSOUT
END IF
! ----------------------------------------------------------------------
!
! End of CONALLO
!
! ----------------------------------------------------------------------
RETURN
END
C23456789012345678901234567890123456789012345678901234567890123456789012
! ----------------------------------------------------------------------
!
! Module Name: RESTORE_U
! Called by: CONALLO
! Calls to:
!
! ----------------------------------------------------------------------
SUBROUTINE RESTORE_U (MOMCMD, U, DU)
! ----------------------------------------------------------------------
!
! Function: Performs various control-restoring techniques
! according to RTYPE.
!
! RTYPE = 0 -> No Restoring
! RTYPE = 1 -> Restore towards the Minimum Norm solution.
! RTYPE = 2 -> Restore towards the min CD solution.
! RTYPE = 3 -> Restore towards the 'Maximum Norm' solution.
!
! ----------------------------------------------------------------------
!
! Modifications:
! Date Purpose By
! JUL 09 1996 Created, JB
! OCT 21 1996 Rewrote the minimum-norm restoring logic. Instead
! of using the null-space projection method of the
! pseudo inverse, we use a least squares approach
! by specifying a function of the the squares of the
! controls.
! MAR 23 1997 Removed the RSAT argument (Now defined in ALLOCAT
! Common block) JB
! ----------------------------------------------------------------------
IMPLICIT NONE
! ----------------------------------------------------------------------
!
! Declaration Section
!
! ----------------------------------------------------------------------
INTEGER Max_Controls
PARAMETER (Max_Controls = 20)
REAL CS_MINDRAG
! -------------------------------Locals---------------------------------
REAL UP(Max_Controls), DU(Max_Controls), U(Max_Controls),
. PMAT(Max_Controls,3), MOMCMD(3), RSAT, SC, SC1
INTEGER I,J
REAL P4(Max_Controls,4), ROW4(Max_Controls),
. B4MAT(4,Max_Controls), DELO(4)
REAL UPSEUDO(Max_Controls)
! ---------------------------Shared Library-----------------------------
REAL GETUEFF
! ----------------------------------------------------------------------
!
! Common Section
!
! ----------------------------------------------------------------------
INCLUDE 'INCLUDES/simvars.com'
INCLUDE 'INCLUDES/a_cvars.com'
INCLUDE 'INCLUDES/allocat.com'
! ----------------------------------------------------------------------
!
! Equivalence Section
!
! ----------------------------------------------------------------------
EQUIVALENCE (SIMVARR(172), CS_MINDRAG)
! ----------------------------------------------------------------------
!
! Run Section
!
! ----------------------------------------------------------------------
IF (RTYPE .EQ. 1) THEN
! ----------------------------------------------------------------------
! Minimum-Norm restoring
! ----------------------------------------------------------------------
! Mod to simplify this procedure wcd 2/17/98
! Get the pseudoinverse of BMAT. Ideally this would be the Global B,
! but we have to use the same B matrix (local) as the rest of the restoring
! algorithm is using, otherwise we are not moving in the null space.
! In any event BMAT -> BGLOBAL as U -> 0, which is where we are most
! interested in getting the min norm.
CALL PINVB3 (M, BMAT, PMAT)
DO 1941 I=1,M
UPSEUDO(I) = 0.0
1941 CONTINUE
DO 1943 I=1,M
DO 1942 J=1,3
UPSEUDO(I)=UPSEUDO(I)+PMAT(I,J)*MOMCMD(J)
1942 CONTINUE
1943 CONTINUE
! UPSEUDO is the control position that yields min norm
! for the desired moment (MOMCMD).
! Calculate DU.
! Vector U is old position, and UPSEUDO is where we'd like to
! get to if we can. We want U + UALLO + DU = UPSEUDO, but may
! have to scale DU to stay within rate constraints.
DO 1944 I = 1,M
DU(I) = UPSEUDO(I)-U(I)-UALLO(I)
1944 CONTINUE
! Now scale DU s.t. DU + UALLO does not
! violate local (frame-wise) limits.
SC = 1.0
DO 1945 I=1,M
SC1 = SC
IF (DU(I) .GT. (UMAX(I) - UALLO(I))) THEN
SC1 = (UMAX(I) - UALLO(I))/DU(I)
END IF
IF (DU(I) .LT. (UMIN(I) - UALLO(I))) THEN
SC1 = (UMIN(I) - UALLO(I))/DU(I)
END IF
SC = AMIN1(SC,SC1)
1945 CONTINUE
IF (SC.NE.1.0) THEN
DO 1946 I = 1,M
DU(I) = SC*DU(I)
1946 CONTINUE
ENDIF
END IF ! endif (RTYPE=1)
! ----------------------------------------------------------------------
! End of Minimum-norm Restoring
! ----------------------------------------------------------------------
! ----------------------------------------------------------------------
IF (RTYPE .EQ. 3) THEN
! ----------------------------------------------------------------------
! Maximum-Norm restoring
! ----------------------------------------------------------------------
! Get the 4th row of the B matrix (Y' = .001745*U)
DO 2012 I=1,3
DO 2013 J=1,M
B4MAT(I,J) = BMAT(I,J)
2013 CONTINUE
2012 CONTINUE
DO 2014 J=1,M
B4MAT(4,J) = 1.74533E-3*U(IU(J))
2014 CONTINUE
! make the objective vector (0,0,0,-1)^t
DELO(1) = 0.
DELO(2) = 0.
DELO(3) = 0.
DELO(4) = 1.0
CALL PINVB4(M, B4MAT, P4)
! find a solution u satisfying u = P4*DELO
DO 2015 I = 1,M
DU(I) = 0.
DO 2016 J = 1,4
DU(I) = DU(I) + P4(I,J)*DELO(J)
2016 CONTINUE
2015 CONTINUE
! now check that the pseudo inverse solution has not violated constraints
! and fix if necesary
SC = 1.
DO 2020 I=1,M
SC1 = SC
IF (DU(I) .GT. (UMAX(I) - UALLO(I))) THEN
SC1 = (UMAX(I) - UALLO(I))/DU(I)
END IF
IF (DU(I) .LT. (UMIN(I) - UALLO(I))) THEN
SC1 = (UMIN(I) - UALLO(I))/DU(I)
END IF
SC = AMIN1(SC,SC1)
2020 CONTINUE
! apply minimization factor to the scale factor also and scale controls
DO 2025 I = 1,M
DU(I) = 0.1*SC*DU(I)
2025 CONTINUE
END IF ! endif (RTYPE=3)
! ----------------------------------------------------------------------
! End of Maximum-norm Restoring
! ----------------------------------------------------------------------
IF (RTYPE .EQ. 2) THEN
! ----------------------------------------------------------------------
! Minimum drag restoring
! ----------------------------------------------------------------------
! Get the 4th row of the B matrix (corresponding to drag)
DO 1952 I=1,3
DO 1951 J=1,M
B4MAT(I,J) = BMAT(I,J)
1951 CONTINUE
1952 CONTINUE
DO 1953 J=1,M
B4MAT(4,J) = GETUEFF(4,IU(J),U(IU(J)))
1953 CONTINUE
! make the objective vector (0,0,0,-1)^t
DELO(1) = 0.
DELO(2) = 0.
DELO(3) = 0.
DELO(4) = -1.0
CALL PINVB4(M, B4MAT, P4)
! find a solution u satisfying u = P4*DELO
DO 1060 I = 1,M
DU(I) = 0.
DO 1061 J = 1,4
DU(I) = DU(I) + P4(I,J)*DELO(J)
1061 CONTINUE
1060 CONTINUE
! now check that the pseudo inverse solution has not violated constraints
! and fix if necesary
SC = 1.
DO 1066 I=1,M
SC1 = SC
IF (DU(I) .GT. (UMAX(I) - UALLO(I))) THEN
SC1 = (UMAX(I) - UALLO(I))/DU(I)
END IF
IF (DU(I) .LT. (UMIN(I) - UALLO(I))) THEN
SC1 = (UMIN(I) - UALLO(I))/DU(I)
END IF
SC = AMIN1(SC,SC1)
1066 CONTINUE
! apply minimization factor to the scale factor also and scale controls
DO 1067 I = 1,M
DU(I) = CS_MINDRAG*SC*DU(I)
1067 CONTINUE
! Calculate the restored controls and rate saturation
END IF ! endif(RTYPE=2)
! ----------------------------------------------------------------------
! End of minimum drag Restoring
! ----------------------------------------------------------------------
! ----------------------------------------------------------------------
!
! End of RESTORE_U
!
! ----------------------------------------------------------------------