C***** SOURCE FILENAME = STRIKE 06/23/00 C C C************************************************************** C***** ***** C***** STRIKE SUBROUTINE ***** C***** ***** C************************************************************** C C C SUBROUTINE STRIKE C C C C********************************************************************* C SUBROUTINE ABSTRACT & HIERARCHY C********************************************************************* C C *********************************************************** C * * * * * * C SIMULATION TRANSITION ROUTINE INCLUDING KINEMATIC EQUATIONS C * * * * * * C *********************************************************** C C ------------------------- C COMPUTER PORTABLE ROUTINE C ------------------------- C C THIS ROUTINE CALLS ATMOS (WHICH, IN TURN, CALLS ARDC62). C CALLS MADE IN BOTH I.C. AND OPERATE MODES. C IN FACT, TWO CALLS ARE MADE WHEN IN I.C. MODE. C THIS IS USED TO ELIMINATE ALGEBRAIC LOOPS. C C THIS ROUTINE ALWAYS NEEDS "BLOCK DATA BLOCK1" C ALSO, FOR REAL TIME WORK, IT NEEDS "BLOCK DATA BLOCK2" C (LOAD THE STRIKE SYSTEM'S BLOCK DATA ROUTINES BEFORE YOUR OWN...) C C SUBROUTINE STRIKE REPLACES SMART, BSETUP AND BLAND. C ----- ------ ----- C C COMMON DATA IS NEVER SET WITHIN ROUTINES IN THE STRIKE SYSTEM. C (SO DON'T LOOK) C C C C C********************************************************************* C CREATION & MODIFICATION LOG C********************************************************************* C DATE INIT DOC# DESCRIPTION C ------------------------------------------------------------------ C C 1986 R. E. MCFARLAND - NASA - CREATED C MAR 1987 REM - BETA VERSION C 11/30/87 REM - VERSION 1.3 - C 03/21/88 REM - VERSION 1.4 - ELIMINATES MULTIPLE PASSES C 02/21/89 REM - VERSION 1.5 - C 11/13/90 REM - VERSION 1.6 - CONSULTATION W/ALAN LEE C 11/15/90 REM - VERSION 1.61- CONTINUOUS ANGLES WITH IQUAT=1 C 11/28/90 REM - VERSION 1.62- CLEAN UP OF IF-THEN-ELSE LOGIC C 12/03/90 REM - VERSION 1.63- USES SUBROUTINE ATMOS, VERS. 1 C 12/13/90 REM - VERSION 1.64- UNIT CONVERSIONS COVERED C 02/14/91 REM - VERSION 1.65- WAITIC,XIXXIC, ETC. RENDERED C SUPERFLUOUS VIA 44444.0 DEFAULT C (MAY STILL BE USED, HOWEVER). C ALSO, PILOT VELOCITIES W/R/T C THE RUNWAY (FORMERLY IN PREDIK) C NOW ABSORBED HEREIN. C 02/28/91 REM - VERSION 1.66- ELIMINATED IETURB. ADDED VNWAKE, C VEWAKE AND VDWAKE. C 05/29/91 SCB A) REFORMATTED C B) NEW VARIABLES: 1) AXG, AYG, AZG C 2) AXPG, AYPG, AZPG C 3) GINV C 4) VGKT C C) OMEG REPLACED BY WEARTH C 03/05/92 SCB A) GRAVZ AND GRAVZI ADDED AND USED C B) IATMOS ADDED C C) EQUIVALENCE FOR TWOPI ADDED. C 09/29/92 MSS A) DELETED CALC. OF VEQ AND VRW IN I.C. C B) ADDED EQUIV FOR VARIABLES CALC. IN ATMOS C 07/20/93 KCD CHANGED QUATERNION SECTION BY: C 1) REMOVING TUNED INTEGRATION C 2) TIME-SYNCHING ALL EQUATIONS C 12/17/93 KCD REMOVED TIME-SYNCHING SINCE IT ADDED NO ACCURACY C 01/04/94 SCB CONVERT GAMV AND GAMH FROM RAD TO DEG C 01/20/94 KCD REMOVED EXTRA ADAMS-BASHFORTH VARIABLES. C 12/06/94 CJS FIXED DEC FORTRAN COMPILER 6.2 WARNING ERRORS C C 04/02/97 SCB MOVED ID CALCULATION TO ROUTINE BEND C 07/25/97 SL ADDED CALCULATION OF XCGD, YCGD, HCGD FOR PREDIKE C 06/16/98 SCB A) IATMOS LOCATED IN ICALCM(3) INSTEAD OF IA(142) C B) VOLATILES ADDED C 06/23/98 SCB ADDED GLOADN (A(344)) = -AZPG C C C C*********************************************************************** C C O M M O N S C*********************************************************************** C C COMMON / XFLOAT / A(500) / IFIXED / IA(250) C C COMMON/ ICALCOM / ICALCM(25) C C C C*********************************************************************** C D E C L A R A T I O N S C*********************************************************************** C C DIMENSION AWEEL(3), AWEELD(3), NGEAR(3), GEAR(9) DIMENSION FOLEO(3), FRICT(3), FSIDE(3), FRXP(3), FRYP(3), FRZP(3) C C C C*********************************************************************** C VOLATILES C*********************************************************************** C C VOLATILE AA, ATWD, ATWE, ATWN VOLATILE CB, CC, CGH, CGV, CPHIH, CPSIH, CTHEH VOLATILE DA, DB, DC, DELTI, DELZPE, DENO, DEPR, DEPRD VOLATILE DERD, DELTH, DHLST, DILST, DNPR, DNPRD, DNRD VOLATILE DTER, DTET, DTINV, DUM, DUM1, DUM2, DUMX VOLATILE IMODEP VOLATILE KTIME VOLATILE PAD, PBDP, PHICUR, PHIDEX, PHIDP, PHIRH, PHILST VOLATILE PROJ, PSICUR, PSIDEX, PSIDP, PSILST, PSIRH VOLATILE QBDP VOLATILE RAD, RAE, RAN, RBDP, RCLAT, RIUNDV, ROUNDV, RTVINV VOLATILE SGH, SGV, SPHIH, SPSIH, STHEH, SUMA, SUMB VOLATILE T11D, T12D, T13D, T21D, T22D, T23D, T31D, T32D, T33D VOLATILE TEMA, TEMG, TEMP, TEMP1, TEMPB, THEDP, THERH VOLATILE VDDP, VDP, VDWP, VEDP, VEWP, VNDP, VNWP, VRW2 VOLATILE WDBAND VOLATILE XDUM, XLATDP, XLONDP VOLATILE YDUM VOLATILE ZENO C C VOLATILE Q1N,Q2N,Q3N,Q4N, QNORM, Q1D,Q2D,Q3D,Q4D VOLATILE Q1DP,Q2DP,Q3DP,Q4DP C C C C*********************************************************************** C E Q U I V A L E N C E S C*********************************************************************** C C OUT ............................... EULER ROLL,PITCH AND C YAW ANGLES - DEG EQUIVALENCE (A( 1), PHI ) EQUIVALENCE (A( 2), THET ) EQUIVALENCE (A( 3), PSI ) C C OUT ............................... EULER ROLL, PITCH AND C YAW ANGLES - RAD EQUIVALENCE (A( 4), PHIR ) EQUIVALENCE (A( 5), THETR ) EQUIVALENCE (A( 6), PSIR ) C C OUT ............................... EULER ROLL, PITCH AND C YAW RATES - RAD/SEC EQUIVALENCE (A( 7), PHID ) EQUIVALENCE (A( 8), THED ) EQUIVALENCE (A( 9), PSID ) C C OUT ............................... SINE AND COSINE OF ROLL ANGLE EQUIVALENCE (A( 10), SPHI ) EQUIVALENCE (A( 11), CPHI ) C C OUT ............................... SINE AND COSINE OF PITCH ANGLE EQUIVALENCE (A( 12), STHT ) EQUIVALENCE (A( 13), CTHT ) C C OUT ............................... SINE AND COSINE OF YAW ANGLE EQUIVALENCE (A( 14), SPSI ) EQUIVALENCE (A( 15), CPSI ) C C OUT ............................... CTHT * CPSI EQUIVALENCE (A( 16), T11 ) C OUT ............................... SPHI*STHT*CPSI - CPHI*SPSI EQUIVALENCE (A( 17), T21 ) C OUT ............................... CPHI*STHT*CPSI + SPHI*SPSI EQUIVALENCE (A( 18), T31 ) C OUT ............................... CTHT*SPSI EQUIVALENCE (A( 19), T12 ) C OUT ............................... SPHI*STHT*SPSI + CPHI*CPSI EQUIVALENCE (A( 20), T22 ) C OUT ............................... CPHI*STHT*SPSI - SPHI*CPSI EQUIVALENCE (A( 21), T32 ) C OUT ............................... - STHT EQUIVALENCE (A( 22), T13 ) C OUT ............................... SPHI*CTHT EQUIVALENCE (A( 23), T23 ) C OUT ............................... CPHI*CTHT EQUIVALENCE (A( 24), T33 ) C C OUT ............................... ANGLES OF ATTACK AND C SIDESLIP - DEG EQUIVALENCE (A( 25), ALFA ) EQUIVALENCE (A( 26), BETA ) C C OUT ............................... ANGLES OF ATTACK AND C SIDESLIP - RAD EQUIVALENCE (A( 27), ALFAR ) EQUIVALENCE (A( 28), BETAR ) C C OUT ............................... DERIVATIVES OF ANGLE OF ATTACK C AND SIDESLIP - RAD/SEC EQUIVALENCE (A( 29), ALFD ) EQUIVALENCE (A( 30), BETD ) C C OUT ............................... SINE AND COSINE OF C ANGLE OF ATTACK EQUIVALENCE (A( 31), SALPH ) EQUIVALENCE (A( 32), CALPH ) C C OUT ............................... SIN AND COSINE OF C ANGLE OF SIDESLIP EQUIVALENCE (A( 33), SBETA ) EQUIVALENCE (A( 34), CBETA ) C C OUT ............................... FLIGHT PATH ANGLE C ABOVE HORIZON - RAD EQUIVALENCE (A( 35), GAMV ) C C OUT ............................... FLIGHT PATH ANGLE IN C HORIZONTAL PLANE - RAD EQUIVALENCE (A( 36), GAMH ) C C OUT ............................... BODY ROLL, PITCH AND YAW C VELOCITIES - RAD/SEC EQUIVALENCE (A( 37), PB ) EQUIVALENCE (A( 38), QB ) EQUIVALENCE (A( 39), RB ) C C OUT ............................... ROLL, PITCH AND YAW RATES OF C LOCAL FRAME DUE TO VEHICLE C MOTION, IN L-FRAME - RAD/SEC EQUIVALENCE (A( 40), PL ) EQUIVALENCE (A( 41), QL ) EQUIVALENCE (A( 42), RL ) C C OUT ............................... ROLL, PITCH AND YAW RATES OF C LOCAL FRAME DUE TO VEHICLE C MOTION, IN B-FRAME - RAD/SEC EQUIVALENCE (A( 43), PLB ) EQUIVALENCE (A( 44), QLB ) EQUIVALENCE (A( 45), RLB ) C C OUT ............................... BODY RATES WITHOUT L-FRAME C ROTATION, IN B-FRAME - RAD/SEC EQUIVALENCE (A( 46), PT ) EQUIVALENCE (A( 47), QT ) EQUIVALENCE (A( 48), RT ) C C OUT ............................... BODY RATES WITH TURB. - RAD/SEC EQUIVALENCE (A( 49), PBWN ) EQUIVALENCE (A( 50), QBWN ) EQUIVALENCE (A( 51), RBWN ) C C IN ------------------------------- ROLL,PITCH AND YAW TURBULENCE C COMPONENTS - RAD/SEC EQUIVALENCE (A( 52), PTURB ) EQUIVALENCE (A( 53), QTURB ) EQUIVALENCE (A( 54), RTURB ) C C OUT ............................... BODY ROLL,PITCH AND YAW C ACCELERATIONS - RAD/SEC2 EQUIVALENCE (A( 55), PBD ) EQUIVALENCE (A( 56), QBD ) EQUIVALENCE (A( 57), RBD ) C C OUT ............................... FORWARD, RIGHT AND DOWN C VELOCITIES, B-FRAME - FT/SEC EQUIVALENCE (A( 58), UB ) EQUIVALENCE (A( 59), VB ) EQUIVALENCE (A( 60), WB ) C C IN ------------------------------- TURBULENCE VELOCITY INPUTS C IN BODY FRAME - FT/SEC EQUIVALENCE (A( 61), UTURB ) EQUIVALENCE (A( 62), VTURB ) EQUIVALENCE (A( 63), WTURB ) C C OUT ............................... NORTHWARD, EASTWARD AND C DOWNWARD VELOCITY INCLUDING C EARTHS ROTATIONAL SPEED IN C THE L-FR. - FT/SEC EQUIVALENCE (A( 64), VN ) EQUIVALENCE (A( 65), VE ) EQUIVALENCE (A( 66), VD ) C C OUT ............................... EASTWARD VELOCITY W/R/T C EARTH'S SURFACE - FT/SEC EQUIVALENCE (A( 67), VEE ) C C OUT ............................... TOTAL VELOCITY IN THE C LOCAL FRAME - FT/SEC EQUIVALENCE (A( 68), VT ) C C OUT ............................... GROUND SPEED IN THE C LOCAL FRAME - FT/SEC EQUIVALENCE (A( 69), VG ) C C OUT ............................... VELOCITY W/R/T WIND - FT/SEC EQUIVALENCE (A( 70), VRW ) C C OUT ............................... NORTH, EAST AND DOWN C RELATIVE VELOCITIES, FT/SEC EQUIVALENCE (A( 72), VNR ) EQUIVALENCE (A( 73), VER ) EQUIVALENCE (A( 74), VDR ) C C IN ------------------------------- NORTH, EAST AND DOWN COMPONENTS C OF WIND VELOCITY - FT/SEC EQUIVALENCE (A( 76), VNW ) EQUIVALENCE (A( 77), VEW ) EQUIVALENCE (A( 78), VDW ) C C OUT ............................... ALTITUDE DERIVATIVE - FT/SEC EQUIVALENCE (A( 80), ALTD ) C C OUT ............................... LONGITUDE DERIVATIVE - FT/SEC EQUIVALENCE (A( 81), XLOND ) C C OUT ............................... LATITUDE DERIVATIVE - FT/SEC EQUIVALENCE (A( 82), XLATD ) C C OUT ............................... ALTITUDE ABOVE SEA LEVEL - FT EQUIVALENCE (A( 83), ALT ) C C OUT ............................... LONGITUDE AND LATITUDE OF C AIRCRAFT - RAD EQUIVALENCE (A( 84), XLON ) EQUIVALENCE (A( 85), XLAT ) C C OUT ............................... SINE AND COSINE OF LATITUDE C OF AIRCRAFT EQUIVALENCE (A( 86), SLAT ) EQUIVALENCE (A( 87), CLAT ) C C OUT ............................... AIRCRAFT NORTH, EAST AND DOWN- C WARD ACCELERATIONS - FT/SEC2 EQUIVALENCE (A( 88), VND ) EQUIVALENCE (A( 89), VED ) EQUIVALENCE (A( 90), VDD ) C C OUT ............................... AIRCRAFT C.G. BODY ACCELS. C - FT/SEC2 EQUIVALENCE (A( 91), AX ) EQUIVALENCE (A( 92), AY ) EQUIVALENCE (A( 93), AZ ) C C OUT ............................... PILOT ACCELS. - FT/SEC2 EQUIVALENCE (A( 94), AXP ) EQUIVALENCE (A( 95), AYP ) EQUIVALENCE (A( 96), AZP ) C C OUT ............................... GRAVITY, AT ALTITUDE - FT/SEC2 EQUIVALENCE (A( 97), G ) C C OUT ............................... INVERSE OF GRAVITY, AT ALTITUDE C -SEC2/FT EQUIVALENCE ( A( 98), GINV ) C C OUT ............................... VRW IN KNOTS EQUIVALENCE (A( 99), VRWKTS) C C OUT ............................... UB IN KNOTS EQUIVALENCE (A(100), UBKTS) C C FROM ATMOS............. CALIBRATED AIRSPEED(EQUAL TO VEQ AT SEA LEVEL C FOR A STANDARD DAY) KTS EQUIVALENCE (A(101), VCAL) C C OUT ............................... LANDING GEAR WHEEL HEIGHT C ABOVE RUNWAY - FT EQUIVALENCE (A(102), HWEEL) C C OUT ............................... X, Y AND H POSITIONS OF THE C PILOT W/R/T THE RUNWAY - FT EQUIVALENCE (A(103), XPR ) EQUIVALENCE (A(104), YPR ) EQUIVALENCE (A(105), HPR ) C C OUT ............................... NORTH AND EAST POSITIONS OF THE C AIRCRAFT C.G. W/R/T RUNWAY - FT EQUIVALENCE (A(106), DNR ) EQUIVALENCE (A(107), DER ) C C OUT ............................... EARTH RADIUS+RUNWAY HEIGHT - FT EQUIVALENCE (A(108), RR ) C C OUT ............................... RADIUS FROM EARTH'S CENTER C TO AIRCRAFT C.G. - FT EQUIVALENCE (A(109), RTV ) C C IN ------------------------------- CLOCKWISE ANGLE OF RUNWAY C FROM NORTH - DEG EQUIVALENCE (A(110), THETRR) C C IN ------------------------------- LAT. AND LONG. OF RUNWAY - RAD EQUIVALENCE (A(111), XLATR ) EQUIVALENCE (A(112), XLONR ) C C OUT ............................... COSINE OF THE RUNWAY LATITUDE EQUIVALENCE (A(113), CLATR ) C C OUT ............................... SINE AND COSINE OF RUNWAY ANGLE EQUIVALENCE (A(114), STHETR) EQUIVALENCE (A(115), CTHETR) C C VEHICLE XX, YY, ZZ, AND XZ C MOMENTS OF INERTIA - SLUG-FT2 C CONSULT NOTES BELOW. THESE ARE C INITIALLY SET TO I.C. VALUES C (SEE XIXXIC) C OUT ............................... IN I.C. MODE, AND OP MODE IF C THEY DO NOT VARY. C IN ------------------------------- IN OP MODE IF THEY VARY. EQUIVALENCE (A(116), XIXX ) EQUIVALENCE (A(117), XIYY ) EQUIVALENCE (A(118), XIZZ ) EQUIVALENCE (A(119), XIXZ ) C C OUT ............................... INTERTIA TERMS: C XMC1 = ((XIYY-XIZZ)*XIZZ-XIXZ**2)/(XIXX*XIZZ-XIXZ**2) C XMC2 = (XIXX-XIYY+XIZZ)*XIXZ/(XIXX*XIZZ-XIXZ**2) C XMC3 = XIZZ/(XIXX*XIZZ-XIXZ**2) C XMC4 = XIXZ/(XIXX*XIZZ-XIXZ**2) C XMC5 = (XIZZ-XIXX)/XIYY C XMC6 = XIXZ/XIYY C XMC7 = 1.0/XIYY C XMC8 = ((XIXX-XIYY)*XIXX+XIXZ**2)/(XIXX*XIZZ-XIXZ**2) C XMC9 = (XIYY-XIZZ-XIXX)*XIXZ/(XIXX*XIZZ-XIXZ**2) C XMC10 = XIXX/(XIXX*XIZZ-XIXZ**2) C EQUIVALENCE (A(120), XMC1 ) EQUIVALENCE (A(121), XMC2 ) EQUIVALENCE (A(122), XMC3 ) EQUIVALENCE (A(123), XMC4 ) EQUIVALENCE (A(124), XMC5 ) EQUIVALENCE (A(125), XMC6 ) EQUIVALENCE (A(126), XMC7 ) EQUIVALENCE (A(127), XMC8 ) EQUIVALENCE (A(128), XMC9 ) EQUIVALENCE (A(129), XMC10 ) C C OUT ............................... MASS OF VEHICLE INCLUDING FUEL C FUEL - SLUGS EQUIVALENCE (A(130), XMASS ) C C IN ------------------------------- AERODYNAMIC FORCES APPLICABLE C AT THE BEGINNING OF THE FRAME EQUIVALENCE (A(136), FAX ) EQUIVALENCE (A(137), FAY ) EQUIVALENCE (A(138), FAZ ) C C IN ------------------------------- ENGINE FORCES APPLICABLE AT C THE BEGINNING OF THE FRAME EQUIVALENCE (A(139), FEX ) EQUIVALENCE (A(140), FEY ) EQUIVALENCE (A(141), FEZ ) C C IN ------------------------------- GEAR FORCES APPLICABLE AT C THE BEGINNING OF THE FRAME EQUIVALENCE (A(142), FGX ) EQUIVALENCE (A(143), FGY ) EQUIVALENCE (A(144), FGZ ) C C OUT ............................... TOTALS OF ABOVE THREE FORCE C VECTORS - LBS EQUIVALENCE (A(145), FTX ) EQUIVALENCE (A(146), FTY ) EQUIVALENCE (A(147), FTZ ) C C OUT ............................... FORCE TOTALS RESOLVED INTO C NORTH, EAST AND DOWN - LBS EQUIVALENCE (A(148), FN ) EQUIVALENCE (A(149), FE ) EQUIVALENCE (A(150), FD ) C C OUT ............................... THE FORCE OF GRAVITY AT C ALTITUDE - LBS EQUIVALENCE (A(151), FG ) C C THE FOLLOWING MOMENTS ARE IN C BODY AXES - FT-LBS: C IN ------------------------------- AERODYNAMIC MOMENTS APPLICABLE C AT THE BEGINNING OF THE FRAME EQUIVALENCE (A(155), TAL ) EQUIVALENCE (A(156), TAM ) EQUIVALENCE (A(157), TAN ) C C IN ------------------------------- ENGINE MOMENTS APPLICABLE C AT THE BEGINNING OF THE FRAME EQUIVALENCE (A(158), TEL ) EQUIVALENCE (A(159), TEM ) EQUIVALENCE (A(160), TEN ) C C IN ------------------------------- GEAR MOMENTS APPLICABLE C AT THE BEGINNING OF THE FRAME EQUIVALENCE (A(161), TGL ) EQUIVALENCE (A(162), TGM ) EQUIVALENCE (A(163), TGN ) C C OUT ............................... TOTALS OF ABOVE THREE MOMENT C COMPONENTS - LBS EQUIVALENCE (A(164), TTL ) EQUIVALENCE (A(165), TTM ) EQUIVALENCE (A(166), TTN ) C C IN ------------------------------- THE CYCLE TIME - SEC EQUIVALENCE (A(168), DT2 ) C C IN ------------------------------- HEIGHT OF RUNWAY W/R/T C SEA LEVEL - FT EQUIVALENCE (A(170), HR ) C C IN ------------------------------- X, Y AND Z POSITIONS OF PILOT C W/R/T C.G. IN B-FRAME - FT EQUIVALENCE (A(171), XP ) EQUIVALENCE (A(172), YP ) EQUIVALENCE (A(173), ZP ) C C OUT ............................... X AND Y POSITIONS AND ALTITUDE C OF PILOT WITH RESPECT TO RUNWAY C THRESHHOLD - FT EQUIVALENCE (A(174), XCG ) EQUIVALENCE (A(175), YCG ) EQUIVALENCE (A(176), HCG ) C C THIS IS A SEA LEVEL QUANTITY. C OUT ............................... AIRCRAFT GROSS WEIGHT - LBS C FROM WAITIC IN I.C. MODE. C IN ------------------------------- IN OP MODE IF IT VARIES. EQUIVALENCE (A(177), WAIT ) C C FROM ATMOS ....................... AIR DENSITY - SLUG/FT3 EQUIVALENCE (A(183), RHO ) C C IN ------------------------------- X POS. OF TAIL WRT C.G. - FT EQUIVALENCE (A(184), XTAIL ) C C IN ------------------------------- Z POS. OF TAIL WRT C.G. - FT EQUIVALENCE (A(185), ZTAIL ) C C OUT ............................... HEIGHT OF TAIL ABOVE C RUNWAY - FT EQUIVALENCE (A(186), HTAIL ) C C IN ------------------------------- X, Y AND Z LOCATIONS OF C THE NOSE, RIGHT AND LEFT C GEARS IN BODY AXES - FT EQUIVALENCE (A(187), GEAR(1), XNG) EQUIVALENCE (A(188), GEAR(2), YNG) EQUIVALENCE (A(189), GEAR(3), ZNG) EQUIVALENCE (A(190), GEAR(4), XRG) EQUIVALENCE (A(191), GEAR(5), YRG) EQUIVALENCE (A(192), GEAR(6), ZRG) EQUIVALENCE (A(193), GEAR(7), XLG) EQUIVALENCE (A(194), GEAR(8), YLG) EQUIVALENCE (A(195), GEAR(9), ZLG) C C OUT ............................... RATE OF CHANGE OF TAIL C HEIGHT - FT/SEC EQUIVALENCE (A(202), HTAILD) C C OUT ............................... COMPRESSION OF NOSE, RIGHT C AND LEFT OLEOS - FT EQUIVALENCE (A(203),AWEEL(1), DSTN) EQUIVALENCE (A(204),AWEEL(2), DSTR) EQUIVALENCE (A(205),AWEEL(3), DSTL) C C OUT ............................... COMPRESSION RATES OF NOSE, C RIGHT AND LEFT OLEOS - FT/SEC EQUIVALENCE (A(206),AWEELD(1),DSTND) EQUIVALENCE (A(207),AWEELD(2),DSTRD) EQUIVALENCE (A(208),AWEELD(3),DSTLD) C C FROM ATMOS ........................... VELOCITY OF SOUND - FT/SEC EQUIVALENCE (A(209), SOUND ) C C IN ------------------------------- OLEO FORCES ON THE NOSE, C RIGHT AND LEFT WHEELS - LBS EQUIVALENCE (A(210),FOLEO(1),FOLEO1) EQUIVALENCE (A(211),FOLEO(2),FOLEO2) EQUIVALENCE (A(212),FOLEO(3),FOLEO3) C C IN ------------------------------- FRICTION FORCES ON THE NOSE, C RIGHT AND LEFT WHEELS - LBS EQUIVALENCE (A(213),FRICT(1),FRICT1) EQUIVALENCE (A(214),FRICT(2),FRICT2) EQUIVALENCE (A(215),FRICT(3),FRICT3) C C IN ------------------------------- SIDE FORCES ON THE NOSE, C RIGHT AND LEFT WHEELS - LBS EQUIVALENCE (A(216),FSIDE(1),FSIDE1) EQUIVALENCE (A(217),FSIDE(2),FSIDE2) EQUIVALENCE (A(218),FSIDE(3),FSIDE3) C C OUT ............................... X FORCES OF THE NOSE, RIGHT C AND LEFT GEARS - LBS EQUIVALENCE (A(219), FRXP(1),FRXP1) EQUIVALENCE (A(220), FRXP(2),FRXP2) EQUIVALENCE (A(221), FRXP(3),FRXP3) C C OUT ............................... Y FORCES OF THE NOSE, RIGHT C AND LEFT GEARS - LBS EQUIVALENCE (A(222), FRYP(1),FRYP1) EQUIVALENCE (A(223), FRYP(2),FRYP2) EQUIVALENCE (A(224), FRYP(3),FRYP3) C C OUT ............................... Z FORCES OF THE NOSE, RIGHT C AND LEFT GEARS - LBS EQUIVALENCE (A(225), FRZP(1),FRZP1) EQUIVALENCE (A(226), FRZP(2),FRZP2) EQUIVALENCE (A(227), FRZP(3),FRZP3) C C IN ------------------------------- EQUIVALENT OLEO AND DRAG C FRICTION FORCES ON TAIL - LBS EQUIVALENCE (A(228), FRTAIL) EQUIVALENCE (A(229), FRICTT) C C IN ------------------------------- INITIAL ROLL, PITCH AND YAW C EULER ANGLES W/R/T L-FRAME - DEG EQUIVALENCE (A(230), PHIIC ) EQUIVALENCE (A(231), THETIC) EQUIVALENCE (A(232), PSIIC ) C C IN ------------------------------- INITIAL FLIGHT PATH ANGLE ABOVE C HORIZON AND IN HORIZONTAL PLANE C CLOCKWISE FROM NORTH - DEG EQUIVALENCE (A(233), GAMVIC) EQUIVALENCE (A(234), GAMHIC) C C IN ------------------------------- IF IEULR = 0, PBIC, QBIC AND C RBIC ARE BODY AXIS RATES. C IF IEULR = 1, PBIC, QBIC AND C RBIC ARE EULER RATES. C THESE INPUT RATES ARE IN DEG/SEC EQUIVALENCE (A(235), PBIC ) EQUIVALENCE (A(236), QBIC ) EQUIVALENCE (A(237), RBIC ) C C INITIAL EQUIVALENT AIRSPEED WITH C RESPECT TO THE AIR MASS IN KNOTS C UNLESS THE AIRCRAFT IS ON THE C GROUND, IN WHICH CASE VEQIC IS C THE TAXI VELOCITY. C IN ------------------------------- IF IMACH = 1, VEQIC IS THE C INITIAL EQUIVALENT AIRSPEED C IN MACH NUMBERS. C IN ------------------------------- IF IMACH = 0, VEQIC IS THE C EQUIV. AIRSPEED IN KNOTS WHEN C AIRBORNE (IHIT = 0), OTHERWISE C (IHIT = 1) IT IS THE TAXI SPEED. C OUT ............................... IF IMACH = -1, VEQIC IS COMPUTED C FROM UBIC, VBIC AND WBIC EQUIVALENCE (A(238), VEQIC ) C C IN ------------------------------- IF ICG = 1, INITIAL POSITIONS OF C C. G. W/R/T RUNWAY - FT C IF ICG = 0, INITIAL POSITIONS OF C PILOT EYEPOINT W/R/T RUNWAY - FT EQUIVALENCE (A(239), XIC ) EQUIVALENCE (A(240), YIC ) EQUIVALENCE (A(241), HIC ) C C IN ------------------------------- INITIAL SEA LEVEL GROSS WEIGHT C OF AIRCRAFT - LBS EQUIVALENCE (A(242), WAITIC) C C IN ------------------------------- INITIAL XX, YY, ZZ AND XZ C MOMENTS OF INERTIA - SLUG-FT2 EQUIVALENCE (A(243), XIXXIC) EQUIVALENCE (A(244), XIYYIC) EQUIVALENCE (A(245), XIZZIC) EQUIVALENCE (A(246), XIXZIC) C C OUT ------------------------------- BODY AXIS RATES - DEG/SEC EQUIVALENCE (A(247), PBDEG ) EQUIVALENCE (A(248), QBDEG ) EQUIVALENCE (A(249), RBDEG ) C C OUT ------------------------------- BODY AXIS RATES - DEG/SEC EQUIVALENCE (A(250), PBDDEG) EQUIVALENCE (A(251), QBDDEG) EQUIVALENCE (A(252), RBDDEG) C C OUT ------------------------------- FLIGHT PATH ANGLE C ABOVE HORIZON - DEG EQUIVALENCE (A(253), GAMVDG) C C OUT ------------------------------- FLIGHT PATH ANGLE IN C HORIZONTAL PLANE - DEG EQUIVALENCE (A(254), GAMHDG) C C OUT ------------------------------- RATE OF CLIMB - FT/MIN EQUIVALENCE (A(262), HDOT ) C C OUT ------------------------------- LONGITUDE & LATITUDE - DEG EQUIVALENCE (A(285), XLONDG) EQUIVALENCE (A(286), XLATDG) C C OUT ............................... TIME AT END OF CYCLE SINCE C OPERATE MODE BEGAN - SEC EQUIVALENCE (A(303), TIME ) C C OUT ............................... GROUND SPEED IN KNOTS - KNOTS EQUIVALENCE (A(331), VGKT) C C IN ............................... INVERSE OF DT2 - SEC EQUIVALENCE (A(356), DT2I) C C IN ............................... DEGREES TO RADIANS CONVERSION FACTOR EQUIVALENCE (A(358), D2R) C C IN ............................... RADIANS TO DEGREES CONVERSION FACTOR EQUIVALENCE (A(359), R2D) C C IN ............................... RADIUS OF THE EARTH (FT) EQUIVALENCE (A(336), RE ) C C OUT ............................... INVERSE OF RADIUS OF THE EARTH C (FT) EQUIVALENCE ( A(337), REINV ) C C OUT ............................... C.G. ACCELERATION - G'S EQUIVALENCE ( A(338), AXG ) EQUIVALENCE ( A(339), AYG ) EQUIVALENCE ( A(340), AZG ) C----------------------------------------------------------------------- C C OUT ............................... PILOT STATION ACCELERATION - G'S EQUIVALENCE ( A(341), AXPG ) EQUIVALENCE ( A(342), AYPG ) EQUIVALENCE ( A(343), AZPG ) EQUIVALENCE ( A(344), GLOADN ) C C IN ............................... ROTATION RATE OF THE EARTH C - (RAD/SEC) EQUIVALENCE (A(347), WEARTH) C C IN ................................ SEA LEVEL DENSITY (STANDARD C DAY) - SLUG/FT3 EQUIVALENCE (A(364), RHOZ ) C C C OUT ............................... ROTATING ENGINE COEFFICIENTS C WITH UNITS OF 1/SEC C XMCC1 = XMC4*EXMX - XMC3*EXMZ C XMCC2 = XMC7*EXMX C XMCC3 = XMC7*EXMZ C XMCC4 = XMC10*EXMX - XMC4*EXMZ C XMCC5 = XMC3*EXMY C XMCC6 = XMC4*EXMY C XMCC7 = XMC10*EXMY EQUIVALENCE (A(375), XMCC1 ) EQUIVALENCE (A(376), XMCC2 ) EQUIVALENCE (A(377), XMCC3 ) EQUIVALENCE (A(378), XMCC4 ) EQUIVALENCE (A(379), XMCC5 ) EQUIVALENCE (A(380), XMCC6 ) EQUIVALENCE (A(381), XMCC7 ) C C IN ------------------------------- MOMENTS ABOUT X, Y AND Z AXES C OF ROTATING ENGINES - FT#SEC EQUIVALENCE (A(382), EXMX ) EQUIVALENCE (A(383), EXMY ) EQUIVALENCE (A(384), EXMZ ) C C C .................... DENSITY FOR A NON-STAND DAY FT/SEC EQUIVALENCE (A(389), RHOZNS) C C C OUT ............................... COMPONENT VELOCITY IN KNOTS EQUIVALENCE (A(404), VKT ) C C INITIAL X,Y AND Z BODY AXIS C VELOCITIES - FT/SEC C IN ------------------------------- IF IMACH.LT.0 C OUT ............................... IF IMACH.GE.0 EQUIVALENCE (A(406), UBIC ) EQUIVALENCE (A(407), VBIC ) EQUIVALENCE (A(408), WBIC ) C C OUT ............................... BODY AXIS FORWARD, RIGHT C SIDEWARD AND DOWNWARD C ACCELERATIONS - FT/SEC2 EQUIVALENCE (A(413), UBD ) EQUIVALENCE (A(414), VBD ) EQUIVALENCE (A(415), WBD ) C C OUT ............................... TOTAL WIND VELOCITY COMPONENTS C INCLUDING TURBULENCE IN THE C LOCAL FRAME - FT/SEC EQUIVALENCE (A(416), VTWN ) EQUIVALENCE (A(417), VTWE ) EQUIVALENCE (A(418), VTWD ) C C OUT ------------------------------- TOTAL TURBULENCE VELOCITIES IN- C CLUDING BODY AXIS CONTRIBUTIONS C (UTURB...) AND LOCAL FRAME C CONTRIBUTIONS (VNWAKE...) - FT/SEC EQUIVALENCE (A(419), VNTURB) EQUIVALENCE (A(420), VETURB) EQUIVALENCE (A(421), VDTURB) C C OUT ............................... TANGENT OF AIRCRAFT LATITUDE EQUIVALENCE (A(426), TLAT ) C C IN ............................... SHIP WAKE VELOCITIES COMPONENTS TO C VNTURB, VETURB AND VDTURB. THESE C ARE IN ADDITION TO THE STANDARD AIR C TURBULENCE GENERATED IN THE BODY FRAME. EQUIVALENCE (A(430), VNWAKE) EQUIVALENCE (A(431), VEWAKE) EQUIVALENCE (A(432), VDWAKE) C C OUT ............................... QUATERNION VARIABLES DEFINE THE C ORIENTATION AS A ROTATION THROUGH AN C ANGLE MU ABOUT AN AXIS WHICH MAKES C ANGLES A,B,C WITH THE X,Y,Z AXES. C Q1 = COS(MU/2) C Q2 = COS(C)SIN(MU/2) C Q3 = COS(B)SIN(MU/2) C Q4 = COS(A)SIN(MU/2) C THE QUATERNIONS ARE THEN USED TO C DEFINE THE TRANSITION MATRIX. THEY C DO NOT HAVE A SINGULAR VALUE LIKE C THE EULER ANGLES. EQUIVALENCE (A(440), Q1) EQUIVALENCE (A(441), Q2) EQUIVALENCE (A(442), Q3) EQUIVALENCE (A(443), Q4) C C OUT ............................... DERIVATIVES OF A/C POSITION C W/R/T THE RUNWAY (FT/SEC). FOR C USE BY THE ESIG SYSTEM CURRENTLY. EQUIVALENCE (A(444), XCGD ) EQUIVALENCE (A(445), YCGD ) EQUIVALENCE (A(446), HCGD ) C C OUT ............................... DERIVATIVES OF PILOT'S POSITION C W/R/T THE RUNWAY (FT/SEC). FOR C USE BY THE DIG SYSTEM CURRENTLY. EQUIVALENCE (A(453), XPRD ) EQUIVALENCE (A(454), YPRD ) EQUIVALENCE (A(455), ZPRD ) C C OUT ............................... EULER ROLL, PITCH AND YAW RATES C REPLICATED FROM PHID, THED AND C PSID (RAD/SEC) TO COMPLETE THE C SIX-VECTOR OF VELOCITIES FOR USE C BY THE DIG SYSTEM. EQUIVALENCE (A(456), PHDCGI) EQUIVALENCE (A(457), THDCGI) EQUIVALENCE (A(458), PSDCGI) C C IN................................ TWICE PI - N/D EQUIVALENCE ( A(479), TWOPI ) C C IN................................ SEA LEVEL GRAVITY CONSTANT - FT/SEC2 EQUIVALENCE (A(480), GRAVZ ) C C IN................................ INVERSE OF SEA LEVEL GRAVITY - SEC2/FT EQUIVALENCE (A(481), GRAVZI) C C IN .............................. PI, 3.141592654 (180 DEGREES) C EQUIVALENCE (A(491), PI ) C C IN .............................. Z DISTANCE (+) FROM EYEPOINT C TO MOTION POINT (TUMMY) - FT EQUIVALENCE (A(492), ZPE ) C C IN................................ 0.5924873, CONVERT FROM FT/SEC C TO KNOTS - KNOTS/(FT/SEC) EQUIVALENCE (A(495), FPS2KT) C C C C ********** S W I T C H E S ********** C C IN ------------------------------- MODE CONTROL INTEGER EQUIVALENCE (IA( 1), IMODE ) C C OUT ............................... DISCRETES INDICATING GROUND C CONTACT FOR THE NOSE, RIGHT, C LEFT GEARS AND THE TAIL EQUIVALENCE (IA( 2),NGEAR(1),INGT) EQUIVALENCE (IA( 3),NGEAR(2),IRGT) EQUIVALENCE (IA( 4),NGEAR(3),ILGT) EQUIVALENCE (IA( 5), ISTRIK) C C IN ------------------------------- FLAT EARTH OPTION WHEN = 1 EQUIVALENCE (IA( 6), IFLAT ) C C IN ------------------------------- IF IFFCI=-1, ZEROS ROT. MOTION, C IF IFFCI= 1, ZEROS TRANS.MOTION EQUIVALENCE (IA( 7), IFFCI ) C C OUT ............................... AT LEAST ONE WHEEL (OR TAIL) IN C CONTACT WITH THE GROUND WHEN =1 EQUIVALENCE (IA( 9), IHIT ) C C IN ------------------------------- TURNS OFF GEAR PROJECTION IF C SET HIGH (NOT RECOMMENDED) EQUIVALENCE (IA( 47), NOPROJ) C C IN ------------------------------- IF SET (1), THE USER IS C RESPONSIBLE FOR OLEO GEOMETRY C GEAR FORCES/MOMENTS. IF RESET C (0), GEOMETRY PROVIDED BY STRIKE. C ALSO, STRIKE TRANSFORMS FRICT, C FSIDE & FOLEO (WHEN RESET). EQUIVALENCE (IA( 48), NOBLAN) C C IN ------------------------------- POSITION INITIALIZATION CONTROL C (SEE XIC) EQUIVALENCE (IA( 64), ICG ) C C OUT ------------------------------ MODE REQUEST FLAG C EQUIVALENCE (IA( 66), IMREQ ) C C IN ------------------------------- VELOCITY INITIALIZATION CONTROL C (SEE VEQIC) EQUIVALENCE (IA(114), IMACH ) C C IN ------------------------------- INITIAL ANGULAR RATES SELECTION C (SEE PBIC) EQUIVALENCE (IA(184), IEULR ) C C IN ------------------------------- NULLS ALFD AND BETD IN I.C. EQUIVALENCE (IA(187), ITOMTR) C C IN ------------------------------- FREEZES BOTH TRANSLATIONAL AND C ROTATIONAL MOTION (SEE IFFCI) EQUIVALENCE (IA(200), IFREEZ) C C IN ------------------------------- FREEZE NORTH MOTION EQUIVALENCE (IA(203), IFREZN) C C IN ------------------------------- FREEZE EAST MOTION EQUIVALENCE (IA(204), IFREZE) C C IN ------------------------------- FREEZE VERTICAL MOTION EQUIVALENCE (IA(205), IFREZH) C C IN ------------------------------- FREEZE ROLL MOTION EQUIVALENCE (IA(206), IFREZP) C C IN ------------------------------- FREEZE PITCH MOTION EQUIVALENCE (IA(207), IFREZQ) C C IN ------------------------------- FREEZE YAW MOTION EQUIVALENCE (IA(208), IFREZR) C C IN ------------------------------- SELECTS QUATERNION OPTION EQUIVALENCE (IA(227), IQUAT ) C C IN ------------------------------- SELECTS ADAMS OR EULER C (0, 1) ALGORITHM FOR PREDICTOR C INTEGRATION (DEFAULT = 0) EQUIVALENCE (IA(249), INTALG) C C C C***** BASIC SUBROUTINE CALL FLAG EQUIVALENCES C EQUIVALENCE ( ICALCM( 3) , IATMOS ) C C C C*********************************************************************** C D A T A I N I T I A L I Z A T I O N C*********************************************************************** C C***** (LOCAL DATA ONLY) C***************************** C C C***** PAST VALUE OF IMODE C DATA IMODEP / -1 / C C C***** INITIALIZATION FOR DEC FORTRAN COMPILER 6.2 C DATA VNDP / 0.0 / DATA VEDP / 0.0 / DATA VDDP / 0.0 / DATA VDP / 0.0 / DATA XLONDP / 0.0 / DATA XLATDP / 0.0 / DATA PBDP / 0.0 / DATA QBDP / 0.0 / DATA RBDP / 0.0 / DATA PHIDP / 0.0 / DATA THEDP / 0.0 / DATA PSIDP / 0.0 / DATA VNWP / 0.0 / DATA VEWP / 0.0 / DATA VDWP / 0.0 / C C C***** FOR GEAR LOGIC: C***** THE PROJECTION DEAD BAND IS EQUIVALENT TO THE MAXIMUM COMPRESSION C***** STATISTICALLY POSSIBLE WITH A SINK RATE OF 8 FT/SEC (NOPROJ = 0). C***** BY -DEAD BAND- THIS MEANS THAT ANTICIPATION WILL NOT APPLY IF THE C***** INITIAL WAKE-UP SINK RATE IS LESS THAN THIS VALUE. C C ------------------------------------------------------------------- C THE COMMON FIXED-POINT SWITCHES ARE: C ------------------------------------------------------------------- C C IMODE - NEGATIVE VALUE FOR I.C. MODE, C ZERO VALUE FOR HOLD MODE, C POSITIVE VALUE FOR OPERATE MODE. C INGT - NOSE GEAR CONTACTING GROUND. C IRGT - RIGHT GEAR CONTACTING GROUND. C ILGT - LEFT GEAR CONTACTING GROUND. C ISTRIK - TAIL CONTACTING GROUND. C IFLAT - SETS FLAT EARTH OPTION RATHER THAN ROUND, ROTATING. C IFFCI - ZEROS ROTATIONAL MOTION IF -1, ZEROS TRANSLATIONAL IF 1. C IHIT - GROUND IMPACT DISCRETE. SOME GEAR OR TAIL ON GROUND. C ICG - POSITION INITIALIZATION W/R/T C.G. (OR PILOT IF ZERO). C IMACH - VELOCITY INITIALIZATION CONTROL. C *ICOND - SETS RHO AND SOUND TO THAT EVALUATED AT HRHOZ (SEE ATMOS). C IEULR - ANGULAR VELOCITY INITIALIZATION CONTROL. C IETURB - TURBULENCE GENERATED IN EARTH RATHER THAN BODY AXES. C ITOMTR - ZEROS ALFD AND BETD IN I.C. (HELPS TRIM PROCESS). C IFREEZ - ZEROS BOTH TRANSLATIONAL AND ROTATIONAL MOTION. C IQUAT - SETS QUATERNION INTEGRATION RATHER THAN EULER ANGLES. C NOPROJ - NO GEAR PROJECTION IF NOPROJ = 1 (NOT RECOMMENDED). C NOBLAN - SET THIS SWITCH IF NO NEED FOR BLAND (STD. GEAR) LOGIC. C INTALG - SET TO USE EULER INTEGRATION ALGORITHM INSTEAD OF C ADAMS FOR THE PREDICTOR. (DEFAULT = 0) C IFREZN - FREEZES LATITUDE (NORTH-SOUTH) MOTION. C IFREZE - FREEZES LONGITUDE (EAST-WEST) MOTION. C IFREZH - FREEZES H MOTION. C IFREZP - FREEZES P MOTION. C IFREZQ - FREEZES Q MOTION. C IFREZR - FREEZES R MOTION. C ------------------------------------------------------------------- C C * NOT USED IN THIS PROGRAM, USED IN SUBROUTINE ATMOS. C C C C C********************************************************************* C E X E C U T A B L E C O D E C********************************************************************* C C IF( IMODE ) 10, 9999, 100 C C C C 10 CONTINUE C C********************************************************************* C I N I T I A L C O N D I T I O N S ( I. C .) C********************************************************************* C C C***** THE PROJECTION DEAD BAND FOR GEAR EQUATIONS. C***** PROJECTION WILL NOT OCCUR IF THE INITIAL OLEO COMPRESSION C***** IS LESS THAN THIS VALUE. C WDBAND = 8.0*DT2 C C C***** INITIALIZE PSI,THET,PHI C PSI = PSIIC THET = THETIC PHI = PHIIC C C C***** EULER ANGLES: CONVERT TO RADIANS C----- PSIR, THETR, PHIR C PSIR = PSI*D2R THETR = THET*D2R PHIR = PHI*D2R C C C***** SINE AND COSINE OF EULER ANGLES C----- SPHI, SPSI, STHT, CPHI, CPSI, CTHT C SPHI = SIN(PHIR) SPSI = SIN(PSIR) STHT = SIN(THETR) C CPHI = COS(PHIR) CPSI = COS(PSIR) CTHT = COS(THETR) C C C***** INITIALIZE BODY TO INERTIAL TRANSFORMATION MATRIX: T11-T33 C T11 = CTHT*CPSI T21 = SPHI*STHT*CPSI - CPHI*SPSI T31 = CPHI*STHT*CPSI + SPHI*SPSI T12 = CTHT*SPSI T22 = SPHI*STHT*SPSI + CPHI*CPSI T32 = CPHI*STHT*SPSI - SPHI*CPSI T13 = - STHT T23 = SPHI*CTHT T33 = CPHI*CTHT C C C***** INITIALIZE SINES AND COSINES OF THETRR C----- STHETR, CTHETR C STHETR = SIN(THETRR*D2R) CTHETR = COS(THETRR*D2R) C C C***** REINV = INVERSE OF RADIUS OF THE EARTH (1/FT) C REINV = 1.0/RE C C C***** RR = RADIUS OF THE RUNWAY : CENTER OF EARTH TO RUNWAY (FT) C***** RTV = RADIUS OF THE A/C : CENTER OF EARTH TO A/C (FT) C RR = RE + HR RTV = RE + ALT C C C***** EQUATION LOOKS LIKE THIS BECAUSE ZPE IS FROM EYE TO MOTION C***** POINT (THIS IS CONST) C DELZPE = ZP - ZPE C C C***** INITIALIZE ALT, HPR, XDUM, YDUM C C***** ICG = 1 : INPUT POS. I.C.S AT C.G C***** ICG = 0 : INPUT POS. I.C.S AT PILOT'S EYE C IF(ICG.EQ.0) THEN C C-----ICG = 0 HPR = HIC ALT = HPR + HR + T13*XP + T23*YP + T33*DELZPE HCG = ALT - HR SUMA = T11*XP + T21*YP + T31*DELZPE SUMB = T12*XP + T22*YP + T32*DELZPE XDUM = XIC - CTHETR*SUMA - STHETR*SUMB YDUM = YIC - CTHETR*SUMB + STHETR*SUMA ELSE C C-----ICG = 1 HCG = HIC ALT = HCG + HR HPR = HCG - T13*XP - T23*YP - T33*DELZPE XDUM = XIC YDUM = YIC END IF C C C***** INITIALIZE CLATR, XLAT, XLON, SLAT, CLAT, TLAT C----- XLAT AND XLON ARE USED TO CALCULATE XCG & YCG C CLATR = COS(XLATR) XLAT = (XDUM*CTHETR - YDUM*STHETR)/RR + XLATR XLON = (XDUM*STHETR + YDUM*CTHETR)/(CLATR*RR) + XLONR SLAT = SIN(XLAT) CLAT = COS(XLAT) TLAT = SLAT/CLAT C C C C***** INITIALIZE PB, QB, RB, PHID, THED, PSID C C***** IEULR = 0 : PB, QB, AND RB COME FROM PBIC, QBIC AND C***** RBIC (DEG/SEC) AND PHID, THED, AND PSID C***** COME FROM PB, QB, AND RB. C***** IEULR = 1 : PHID, THED, AND, PSID COME FROM PBIC, QBIC, C***** AND RBIC (DEG/SEC) AND PB, QB, AND RB COME C***** FROM PHID, THED, AND PSID. C IF( IEULR .NE. 0 ) THEN C C----- ASSUMED EULER RATE SETUP: IEULR = 1 C PHID = PBIC*D2R THED = QBIC*D2R PSID = RBIC*D2R PB = PHID - PSID*STHT + PLB QB = THED*CPHI + PSID*SPHI*CTHT + QLB RB = PSID*CPHI*CTHT - THED*SPHI + RLB ELSE C C----- REGULAR INPUT: IEULR = 0 C PB = PBIC*D2R QB = QBIC*D2R RB = RBIC*D2R IF( ABS(CTHT) .LT. 0.00001 ) CTHT = SIGN( 0.00001, CTHT ) THED = QB*CPHI - RB*SPHI PSID = (QB*SPHI + RB*CPHI)/CTHT PHID = PB + PSID*STHT END IF C C C C***** CREATE ATMOSPHERIC TEMPS,PRESSURES, DENSITY, SPEED OF C***** SOUND, ETC... C IF( IATMOS .EQ. 1 ) CALL ATMOS C C C C C***** INITIALIZE VELOCITIES: VT, VG, VN, VE, VEE, VD, VNR, VER, VDR, UBIC, C***** VBIC, WBIC, VEQIC, GAMVIC, GAMHIC, GAMV, GAMH, SGV, CGV, SGH, CGH C C C***** IMACH = 0 : A) IF THE AIRCRAFT IS ON THE GROUND (IHIT=1), VEQIC IS C***** THE GROUND (TAXI) VELOCITY. C***** B) IF THE AIRCRAFT IS AIRBORNE (IHIT=0), VEQIC IS THE C***** EQUIVALENT VELOCITY (WITH RESPECT TO THE AIR MASS). C***** =-1 : THE VELOCITY IS INITIALIZED IN BODY AXES BY UBIC, C***** VBIC, AND WBIC C***** = 1 : THE INPUT VELOCITY VEQIC IS IN MACH NUMBER C C***** GAMVIC AND GAMHIC ARE THE INERTIAL VELOCITY VECTOR DIRECTIONAL ANGLES C***** AND, AS SUCH, IGNORE VARIATIONS IN THE WIND MAGNITUDE AND DIRECTION. C TEMA = 0.59249*SQRT(RHO/RHOZ) C C IF (IMACH.LT.0) GOTO 80 C C-----IMACH = 0 AND 1 C-------------------- C DUM = VEQIC*(SOUND*IMACH + (1 - IMACH)/TEMA) C C----- INITIALIZE SINES AND COSINES OF GAMV AND GAMH C GAMV = GAMVIC*D2R SGV = SIN(GAMV) CGV = COS(GAMV) GAMH = GAMHIC*D2R SGH = SIN(GAMH) CGH = COS(GAMH) C C IF( IHIT .EQ. 1 ) GOTO 60 C C----- AIRBORNE COMPUTATIONS : IHIT = 0 C CB = 2.0*(VDW*SGV - VEW*CGV*SGH - VNW*CGV*CGH) CC = VNW**2 + VEW**2 + VDW**2 - DUM**2 AA = CB**2 - 4.0*CC IF( AA .GT. 0.0 ) GOTO 50 VT = - 0.5*CB IF( VT .LT. 0.01 ) VT = 0.01 GOTO 70 C 50 VT = 0.5*(SQRT(AA) - CB) GOTO 70 C C----- ON THE GROUND : IHIT = 1 60 VT = DUM C C 70 VN = VT*CGV*CGH VEE = VT*CGV*SGH VD = - VT*SGV VNR = VN - VNW VER = VEE - VEW VDR = VD - VDW C UBIC = T11*VNR + T12*VER + T13*VDR VBIC = T21*VNR + T22*VER + T23*VDR WBIC = T31*VNR + T32*VER + T33*VDR GOTO 90 C C C-----IMACH = -1 C----- VELOCITY INITIALIZATION IN BODY AXES C--------------- C 80 VNR = T11*UBIC + T21*VBIC + T31*WBIC VER = T12*UBIC + T22*VBIC + T32*WBIC VDR = T13*UBIC + T23*VBIC + T33*WBIC C VN = VNR + VNW VEE = VER + VEW VD = VDR + VDW C VEQIC = TEMA*SQRT(VNR**2 + VER**2 + VDR**2) C PAD = VN**2 + VEE**2 VT = SQRT(PAD + VD**2) VG = SQRT(PAD) VGKT = VG*FPS2KT C IF( VT .LT. 0.0001 ) GOTO 90 GAMVIC = ATAN2(-VD,VG)*R2D GAMHIC = ATAN2(VEE,VN)*R2D GAMV = GAMVIC*D2R GAMH = GAMHIC*D2R C 90 CONTINUE C C-----END OF IMACH SECTION C C C C-----INITIALIZE VELOCITIES--- WRT WIND: VE, ALTD C WRT A/C : UB, VB, WB C WRT NED : VE, ALTD C UB = UBIC VB = VBIC WB = WBIC C C-----IFLAT = 1: FLAT EARTH VE = VEE C C-----IFLAT = 0: CURVED EARTH C C-----IF( IFLAT .NE. 1 ) VE = VEE + 0.72722E-4*RTV*CLAT IF( IFLAT .NE. 1 ) VE = VEE + WEARTH*RTV*CLAT C C C***** TIME ZERO'ED IN I.C. MODE C TIME = 0.0 KTIME = 0 C C***** INTEGRATION INTERVALS ZERO'ED IN I.C. MODE C C----- DELTI FOR ROTATIONAL INTEGRATIONS C DELTI = 0.0 C C----- DELTH FOR TRANSLATIONAL INTEGRATIONS C DELTH = 0.0 DTINV = 0.0 C C----- FOR EULER INTEGRATION, IF SELECTED C DTER = 0.0 DTET = 0.0 C C C IF( IQUAT .NE. 0 ) THEN C C***** QUATERNION INITIALIZATION SECTION C********************************************* C C----- PSIRH = 0.5*PSIR THERH = 0.5*THETR PHIRH = 0.5*PHIR C C----- CPSIH = COS(PSIRH) SPSIH = SIN(PSIRH) CTHEH = COS(THERH) STHEH = SIN(THERH) CPHIH = COS(PHIRH) SPHIH = SIN(PHIRH) C C----- Q1 = CPSIH*CTHEH*CPHIH + SPSIH*STHEH*SPHIH Q2 = SPSIH*CTHEH*CPHIH - CPSIH*STHEH*SPHIH Q3 = CPSIH*STHEH*CPHIH + SPSIH*CTHEH*SPHIH Q4 = CPSIH*CTHEH*SPHIH - SPSIH*STHEH*CPHIH C C----- INITIALIZATION, AS REQUIRED FOR CONTINUOUS PHIR AND PSIR C----- WHEN BACKING OUT OF QUATERNIONS. VERSION 1.61 PHILST = PHIR DHLST = 0.0 PSILST = PSIR DILST = 0.0 C END IF C C C C C C***** INITIALIZE WAIT, MASS AND INERTIA COEFFICIENTS C----- WAIT, FG, XMASS, XIXX, XIYY, XIZZ, XIXZ C----- XMC1 - XMC9, XMCC1 - XMCC7 C********************************************************** C C----- GRAVITY AT EARTH'S SURFACE FOR WEIGHT CONVERSION TO MASS IF( WAITIC .NE. 44444.0 ) WAIT = WAITIC FG = WAIT*(RE/RTV)**2 C C----- IN THE STRIKE SYSTEM YOU MAY IGNORE THE "IC" SET, IF YOU WISH, C----- AND SET (XIXX, XIYY, XIZZ, XIXZ) DIRECTLY. C IF( XIYYIC .NE. 44444.0 ) THEN XIXX = XIXXIC XIYY = XIYYIC XIZZ = XIZZIC XIXZ = XIXZIC ELSE XIXXIC = XIXX XIYYIC = XIYY XIZZIC = XIZZ XIXZIC = XIXZ END IF C C C C 100 CONTINUE C C********************************************************************* C O P E R A T E C********************************************************************* C C***** CALCULATE THE INERTIA COEFFICIENTS C***** REMOVES NEED TO CALCULATES THESE COEFFS IN ANOTHER C***** ROUTINE IF THE A/C MOMENTS OF INERTIA OR THE ENGINE C***** ROTATIONAL MOMENTS OF INERTIA CHANGE IN REAL-TIME. C ZENO = XIXZ**2 DENO = 1.0/(XIXX*XIZZ - ZENO) C XMC1 = ((XIYY - XIZZ)*XIZZ - ZENO)*DENO XMC2 = (XIXX - XIYY + XIZZ)*XIXZ*DENO XMC3 = XIZZ*DENO XMC4 = XIXZ*DENO XMC7 = 1.0/XIYY XMC5 = (XIZZ - XIXX)*XMC7 XMC6 = XIXZ*XMC7 XMC8 = ((XIXX - XIYY)*XIXX + ZENO)*DENO XMC9 = (XIYY - XIZZ - XIXX)*XIXZ*DENO XMC10 = XIXX*DENO C XMCC1 = XMC4*EXMX - XMC3*EXMZ XMCC2 = XMC7*EXMX XMCC3 = XMC7*EXMZ XMCC4 = XMC10*EXMX - XMC4*EXMZ XMCC5 = XMC3*EXMY XMCC6 = XMC4*EXMY XMCC7 = XMC10*EXMY C C C C********************************************** C***** LANDING GEAR SECTION ***** C********************************************** C C----- SAVE COMPUTATION TIME IF NO GEARS, BYPASS GEAR SECTION C----- OR THESE LANDING GEAR KINEMATIC EQUATIONS ARE NOT USED. C IF( NOBLAN .NE. 0 ) GOTO 230 C IF( IHIT .EQ. 0 ) THEN C----- GEAR CLEANUP LOGIC FGX = 0.0 FGY = 0.0 FGZ = 0.0 TGL = 0.0 TGM = 0.0 TGN = 0.0 GOTO 230 END IF C DO 200 I=1,3 IF( NGEAR(I) .NE. 0 ) THEN C C----- CALCULATE FORCE ON WHEELS FOR GEAR ON GROUND FRXP(I) = FRICT(I)*CTHT - FOLEO(I)*STHT FRYP(I) = FRICT(I)*SPHI*STHT + FOLEO(I)*T23 + FSIDE(I)*CPHI FRZP(I) = FRICT(I)*CPHI*STHT + FOLEO(I)*T33 - FSIDE(I)*SPHI ELSE C C----- PARTICULAR GEAR NOT ON GROUND. SET WHEEL FORCES TO ZERO. FRXP(I) = 0.0 FRYP(I) = 0.0 FRZP(I) = 0.0 END IF 200 CONTINUE C C C----- FOR TAIL NOT ON GROUND, SET FORCES ON TAIL TO ZERO C IF( ISTRIK .EQ. 0 ) THEN FRTAIL = 0.0 FRICTT = 0.0 END IF C C C----- SET AND CALCULATE LANDING GEAR MOMENT C----- COMPONENTS FOR TAIL ON GROUND (AND INITIALIZE DO LOOP) C TGL = 0.0 TGM = -XTAIL*FRTAIL TGN = 0.0 C C C----- CALCULATE LANDING GEAR MOMENT COMPONENTS C K = -2 DO 220 I=1,3 K = K + 3 IF( NGEAR(I) .NE. 0 ) THEN DUMX=AWEEL(I) + GEAR(K+2) TGL = TGL + FRZP(I)*GEAR(K+1) - FRYP(I)*DUMX TGM = TGM - FRZP(I)*GEAR(K) + FRXP(I)*DUMX TGN = TGN + FRYP(I)*GEAR(K) - FRXP(I)*GEAR(K+1) END IF 220 CONTINUE C C C----- CALCULATE LANDING GEAR FORCES C FGX = FRXP(1) + FRXP(2) + FRXP(3) + FRICTT FGY = FRYP(1) + FRYP(2) + FRYP(3) FGZ = FRZP(1) + FRZP(2) + FRZP(3) + FRTAIL C 230 CONTINUE C C C C C********************************************************************* C***** SUM FORCES AND MOMENTS TO CREATE TOTAL A/C FORCES AND MOMENTS C***** IN THE BODY FRAME, WHICH ARE APPLICABLE AT THE BEGINNING OF C***** THE FRAME. C********************************************************************* C C----- TOTAL A/C BODY AXES FORCES FTX = FAX + FEX + FGX FTY = FAY + FEY + FGY FTZ = FAZ + FEZ + FGZ C C----- TOTAL A/C BODY AXES MOMENTS TTL = TAL + TEL + TGL TTM = TAM + TEM + TGM TTN = TAN + TEN + TGN C C C***** TOTAL AIRCRAFT FORCE COMPONENTS IN EARTH FRAME C FN = T11*FTX + T21*FTY + T31*FTZ FE = T12*FTX + T22*FTY + T32*FTZ FD = T13*FTX + T23*FTY + T33*FTZ C C C***** CALCULATE MASS, FG, VARIOUS DISTANCES AND GRAVITY TERMS C***** WHICH CHANGE WITH ALTITUDE. C***** XMASS, RTV, RTVINV, RCLAT, FG, G AND GINV C XMASS = WAIT*GRAVZI TEMP = 1.0/XMASS RTV = ALT + RE RTVINV = 1.0/RTV RCLAT = RTV*CLAT TEMPB = (RE*RTVINV)**2 FG = WAIT*TEMPB G = GRAVZ*TEMPB GINV = 1.0/G C C C IF( IFLAT .EQ. 0 ) THEN C C***** ROTATING ROUND EARTH (IFLAT=0) C***** AIRCRAFT ACCELERATION COMPONENTS FROM ROTATING EARTH FRAME C***** NORTH, EAST AND DOWN ACCELERATIONS TEMP1 = RTVINV VND = FN*TEMP + (VN*VD - VE**2*TLAT)*TEMP1 VED = FE*TEMP + (VE*VD + VN*VE*TLAT)*TEMP1 VDD = (FD + FG)*TEMP - (VE**2 + VN**2)*TEMP1 C----- ROUNDV = - 0.72722E-4*RCLAT ROUNDV = -WEARTH*RCLAT ELSE C C***** NON-ROTATING FLAT EARTH (IFLAT=1) C***** AIRCRAFT ACCELERATION COMPONENT FROM FIXED, FLAT EARTH FRAME C***** NORTH, EAST AND DOWN ACCELERATIONS TEMP1 = 0.0 VND = FN*TEMP VED = FE*TEMP VDD = (FD + FG)*TEMP ROUNDV = 0.0 END IF C C IF( IMODE ) 310, 9999, 270 C 270 CONTINUE C C C C***** SETUP FOR FREEZING VARIOUS AXES FOR DEBUGGING PURPOSES. C***** C***** DELTH : TRANSLATIONAL - ADAMS/BASHFORTH INTEG: INTALG=0 C***** DELTHI : ROTATIONAL - ADAMS/BASHFORTH INTEG: INTALG=0 C***** DTET : TRANSLATIONAL - EULER INTEG: INTALG=1 C***** DTER : ROTATIONAL - EULER INTEG: INTALG=1 C***** C***** DEFAULT: INTALG=0, APPLIES FOR ACCELERATION TO C***** VELOCITY INTEGRATION ONLY. C***** C***** IFREEZ = 0 : NORMAL TRANSLATIONAL AND ROTATIONAL MOTION C***** ( IFFCI IS IN EFFECT IF IFREEZ=0 ) C***** 1 : FIXES TRANSLATIONAL AND ROTATIONAL MOTION C***** ( I.E. IFFCI IS IMMATERIAL IF IFREEZ=1 ) C***** C***** IFFCI = 0 : NORMAL TRANSLATIONAL AND ROTATIONAL MOTION C***** = 1 : FREEZES TRANSLATIONAL MOTION WITH NORMAL C***** ROTATIONAL MOTION C***** =-1 : FREEZES ROTATIONAL MOTION WITH NORMAL C***** TRANSLATIONAL MOTION C***** C IF( IFREEZ .EQ. 0 ) THEN C----- STANDARD MOTION IN ALL SIX AXES, IFREEZ=0 DELTI = 0.5*DT2 DTER = DT2 C------ DTINV = 1.0/DT2 ELSE C----- FREEZE ALL MOTION , IFREEZ=1 DELTI = 0.0 DTER=0.0 END IF C DELTH = DELTI DTET = DTER C C IF( IFFCI ) 280, 300, 290 C C----- FREEZE ROTATIONAL MOTION, IFFCI = - 1 280 DELTI = 0.0 DTER = 0.0 GOTO 300 C C----- FREEZE TRANSLATIONAL MOTION, IFFCI = 1 290 DELTH = 0.0 DTET = 0.0 C C C----- DOUBLE PRECISION NOT REQUIRED IN STRIKE SYSTEM. C 300 KTIME = KTIME + 1 TIME = KTIME*DT2 310 CONTINUE C C DTINV = DT2I C C C C C**************************************************************** C***** ********* C******* AIRCRAFT TRANSLATIONAL SECTION ********* C***** ********* C**************************************************************** C C***** INTEGRATE THE EARTH FRAME ACCELERATION COMPONENTS INTO C***** EARTH FRAME VELOCITIES USING EITHER: C***** C***** INTALG = 0 : ADAMS BASHFORTH INTEGRATION FOR THE PREDICTOR. C***** SUBROUTINE SMART USED INTALG=0 AS THE DEFAULT. C***** C***** = 1 : EULER INTEGRATION FOR THE PREDICTOR C***** C***** FIRST STEP FROM OPERATE ALWAYS USES EULER ALGORITHM. C***** THIS MAY BE REFERRED TO AS A "SMOOTH STARTING GATE", C***** REGARDLESS OF THE VALUE OF INTALG. C***** C***** NON-INITIALIZATION OF PREVIOUS VALUES NEVER CAUSES A PROBLEM IN C***** I.C. MODE BECAUSE OF THE TECHNIQUE OF ZEROING THE APPROPRIATE C***** CYCLIC UPDATE INTERVAL (E.G. DELTI). C C IF( IMODE .GT. IMODEP ) GOTO 315 IF( INTALG .EQ. 0 ) GOTO 320 C C----- EULER INTEGRATION FOR THE PREDICTOR (INTALG = 1) C 315 IF( IFREZN .NE. 1 ) VN = VN + DTET*VND IF( IFREZE .NE. 1 ) VE = VE + DTET*VED IF( IFREZH .NE. 1 ) VD = VD + DTET*VDD GOTO 325 C C----- ADAMS BASHFORTH INTEGRATION FOR THE PREDICTOR (INTALG=0) C 320 IF( IFREZN .NE. 1 ) VN = VN + DELTH*(3.0*VND - VNDP) IF( IFREZE .NE. 1 ) VE = VE + DELTH*(3.0*VED - VEDP) IF( IFREZH .NE. 1 ) VD = VD + DELTH*(3.0*VDD - VDDP) C 325 CONTINUE C C----- PAST VALUES VNDP = VND VEDP = VED VDDP = VDD C C C***** WIND ACCELERATION INCLUDES ONLY MEAN WIND C ATWN = (VNW - VNWP)*DTINV ATWE = (VEW - VEWP)*DTINV ATWD = (VDW - VDWP)*DTINV VNWP = VNW VEWP = VEW VDWP = VDW C C***** RELATIVE ACCELERATION OF THE A/C WRT THE WIND C RAN = VND - ATWN RAE = VED - ATWE RAD = VDD - ATWD C C***** CREATE AIRCRAFT VELOCITIES IN LATITUDE AND LONGITUDE AXES. C VEE = VE + ROUNDV XLATD = VN*RTVINV XLOND = VEE/RCLAT C C C***** INTEGRATE THE VELOCITIES IN THE LAT/LONG AXES INTO C***** LATITUDE AND LONGITUDE USING A TRAPEZOIDAL INTEGRATION C***** FOR POSITIONS AT END OF FRAME (CORRECTOR). C IF( IFREZN .NE. 1 ) XLAT = XLAT + DELTH*(XLATD + XLATDP) IF( IFREZE .NE. 1 ) XLON = XLON + DELTH*(XLOND + XLONDP) IF( IFREZH .NE. 1 ) ALT = ALT - DELTH*(VD + VDP) C XLONDP = XLOND XLATDP = XLATD VDP = VD C XLATDG = XLAT*R2D XLONDG = XLON*R2D C C C***** SINE, COSINE AND TANGENT OF LATITUDE C SLAT = SIN(XLAT) CLAT = COS(XLAT) TLAT = SLAT/CLAT C C C***** AIRCRAFT POSITION RELATIVE TO RUNWAY ORIGIN : C***** TRANSFORM FROM LATITUDE/LONGITUDE AXES TO NED AXES. C DNR = RR*(XLAT - XLATR) DER = RR*CLATR*(XLON - XLONR) C HCG = ALT - HR C ALTD = -VD HDOT = 60.0*ALTD C C C C C**************************************************************** C***** ********* C***** AIRCRAFT ROTATIONAL SECTION ********* C***** ********* C**************************************************************** C C C***** CREATE ANGULAR ACCELERATIONS AT BEGINNING OF FRAME C PBD = (XMC1*RB + XMC2*PB)*QB + XMC3*TTL + XMC4*TTN + XMCC1*QB 1 + XMCC5*RB - XMCC6*PB QBD = XMC5*RB*PB + XMC6*(RB**2 - PB**2) + XMC7*TTM - XMCC2*RB 1 + XMCC3*PB RBD = (XMC8*PB + XMC9*RB)*QB + XMC4*TTL + XMC10*TTN + XMCC4*QB 1 + XMCC6*RB - XMCC7*PB C PBDDEG = PBD*R2D QBDDEG = QBD*R2D RBDDEG = RBD*R2D C C C***** INTEGRATE THE BODY AXES ANGULAR ACCELERATIONS INTO BODY C***** AXES ANGULAR VELOCITIES USING: C***** C***** INTALG = 0 : ADAMS BASHFORTH INTEGRATION FOR THE PREDICTOR. C***** SUBROUTINE SMART USED INTALG=0 AS THE DEFAULT. C***** C***** = 1 : EULER INTEGRATION FOR THE PREDICTOR C***** C***** FIRST STEP FROM OPERATE ALWAYS USES EULER ALGORITHM. C***** THIS MAY BE REFERRED TO AS A "SMOOTH STARTING GATE", C***** REGARDLESS OF THE VALUE OF INTALG. C IF( IMODE .GT. IMODEP ) GOTO 330 IF( INTALG .EQ. 0 ) GOTO 335 C C----- EULER INTEGRATION FOR THE PREDICTOR (INTALG=1) C 330 IF( IFREZP .NE. 1 ) PB = PB + DTER*PBD IF( IFREZQ .NE. 1 ) QB = QB + DTER*QBD IF( IFREZR .NE. 1 ) RB = RB + DTER*RBD GOTO 340 C C----- ADAMS BASHFORTH INTEGRATION FOR THE PREDICTOR (INTALG=0) C 335 IF( IFREZP .NE. 1 ) PB = PB + DELTI*(3.0*PBD - PBDP) IF( IFREZQ .NE. 1 ) QB = QB + DELTI*(3.0*QBD - QBDP) IF( IFREZR .NE. 1 ) RB = RB + DELTI*(3.0*RBD - RBDP) C 340 CONTINUE C C KCD THE UPDATE FOR THE PAST VALUE OF IMODE HAS BEEN MOVED TO C KCD THE END OF STRIKE. C KCD IMODEP = IMODE C PBDEG = PB*R2D QBDEG = QB*R2D RBDEG = RB*R2D C PBDP = PBD QBDP = QBD RBDP = RBD C C C***** AIRCRAFT TOTAL ROTATIONAL RATE, EARTH FRAME C PT = PB - PLB QT = QB - QLB RT = RB - RLB C C C***** AIRCRAFT TOTAL ROTATIONAL RATES, BODY FRAME C PBWN = PB + PTURB QBWN = QB + QTURB RBWN = RB + RTURB C C C IF( IQUAT .EQ. 0 ) GOTO 380 C C C C**************************************************************** C***** ********* C***** QUATERNION ROTATIONAL SECTION ********* C***** ********* C**************************************************************** C C KCD793 THE QUATERNION SECTION WAS MODIFIED TO: C KCD793 1) CHANGE TUNED TO TRAPEZOIDAL INTEGRATION C KCD793 2) EXPLICITLY NOTE THE TIME INDEX OF EVERY C KCD793 EQUATION C KCD793 3) MAKE THE GENERAL ORDER OF THE COMPUTATION C KCD793 MORE OBVIOUS. C KCD793 MODIFIED BY: K.C.DUISENBERG AND D.E.FOUGHT C KCD793 THE OLD CODE HAS BEEN COMMENTED OUT IN BLOCKS WITH C KCD793 'KCD' PRECEDING IT. THE CODE REPLACING THOSE C KCD793 SECTIONS IMMEDIATELY FOLLOWS THE COMMENT BLOCKS. C KCD793 ANY ADDITIONS ARE PREFACED WITH 'KCD ADD'. C C C KCD1293 THE TIME-SYNCHING WAS FOUND TO NOT IMPROVE ACCURACY AND C KCD1293 WAS REMOVED. ALL OLD CODE WAS REMOVED. C C***** QUATERIONS IS AN ANGULAR SCHEME WHICH IS USES FOUR QUANTITIES C***** TO DEFINE THE ANGULAR POSITION OF AN OBJECT. R.E.MCFARLAND C***** HAS DEVELOPED A SYSTEM USING QUATERNIONS TO DO THE FOLLOWING: C***** A) TAKE THE A/C ROTATIONAL RATES AND INTEGRATE INTO C***** QUATERNION POSITIONS. C***** B) CREATE THE NEW T MATRIX FROM THE QUATERNION POSITIONS. C***** C) BACK OUT THE ROLL, PITCH AND YAW EULER ANGLES C***** (PHI, THET AND PSI) FROM THE T MATRIX. THE SET OF C***** QUATERNIONS USED CORRESPONDS TO THE ZYX ROTATION C***** AND RESULTS IN A SINGULARITY AT THETA=+-90 DEGREES. C***** C***** THE ADVANTAGE OF QUATERNIONS IS THAT THEY HAVE C***** NO SINGULARITIES (UNDEFINED REGIONS). SO YOU CAN C***** PERFORM AEROBATICS AND THE MOTION WILL BE TOTALLY VALID AND C***** NOT BE EFFECTED BY THE ERRORS INTRODUCED BY COMPENSATING FOR C***** THE EULER ANGLE SINGULARITIES. C C----- COMPUTE THE DERIVATIVES OF THE QUATERNIONS Q1D = (-0.5) * (RT*Q2 + QT*Q3 + PT*Q4) Q2D = (0.5) * (RT*Q1 - PT*Q3 + QT*Q4) Q3D = (0.5) * (QT*Q1 + PT*Q2 - RT*Q4) Q4D = (0.5) * (PT*Q1 - QT*Q2 + RT*Q3) C C----- ON THE FIRST RUN, USE EULER INTEGRATION IF (IMODE.GT.IMODEP) GOTO 345 C C----- OTHERWISE, USE TRAPEZOIDAL INTEGRATION ALWAYS Q1N = Q1 + (Q1D + Q1DP)*DELTI Q2N = Q2 + (Q2D + Q2DP)*DELTI Q3N = Q3 + (Q3D + Q3DP)*DELTI Q4N = Q4 + (Q4D + Q4DP)*DELTI GOTO 350 C 345 CONTINUE Q1N = Q1 + DTER*Q1D Q2N = Q2 + DTER*Q2D Q3N = Q3 + DTER*Q3D Q4N = Q4 + DTER*Q4D C 350 CONTINUE C----- NORMALIZATION (ORTHOGONAL MATRIX ONLY!) C----- THIS IS USED TO NORMALIZE OUT (OR REMOVE) SMALL C----- COMPUTATIONAL ERRORS. C QNORM = SQRT(Q1N**2 + Q2N**2 + Q3N**2 + Q4N**2) C Q1 = Q1N/QNORM Q2 = Q2N/QNORM Q3 = Q3N/QNORM Q4 = Q4N/QNORM C C----- FINALLY, SAVE THE PAST VALUES OF THE DERIVATIVES FOR NEXT C----- CYCLE. Q1DP = Q1D Q2DP = Q2D Q3DP = Q3D Q4DP = Q4D C C----- COMPUTE THE TRANSFORMATION ELEMENTS (LOCAL TO BODY T MATRIX) C T11 = Q1*Q1 - Q2*Q2 - Q3*Q3 + Q4*Q4 T21 = 2.0*(Q3*Q4 - Q1*Q2) T31 = 2.0*(Q1*Q3 + Q2*Q4) T12 = 2.0*(Q3*Q4 + Q1*Q2) T22 = Q1*Q1 - Q2*Q2 + Q3*Q3 - Q4*Q4 T32 = 2.0*(Q2*Q3 - Q1*Q4) T13 = 2.0*(Q2*Q4 - Q1*Q3) T23 = 2.0*(Q2*Q3 + Q1*Q4) T33 = Q1*Q1 + Q2*Q2 - Q3*Q3 - Q4*Q4 C C C----- BACK OUT THE EULER ANGLES FROM THE T MATRIX C----- NOTE - CONTINUOUS PHI AND PSI IN VERSION 1.61 C----- ATAN2 NECESSARY FOR SMALL COSINE. C C KCD ADD C KCD IF THE COSINE OF THETA IS TOO SMALL (THETA APPRX. 90), C KCD BACKING OUT THE ROLL AND YAW ANGLES CAN RESULT IN LARGE C KCD ERRORS. (FOR EXAMPLE, SPHI = T23/CTHT --> IF CTHT IS TOO C KCD SMALL, SPHI COULD BECOME TOO LARGE.) TO DEAL WITH THIS, C KCD THE YAW AND ROLL ANGLES ARE CALCULATED ONLY WHEN CTHT IS C KCD ABOVE A MINIMUM. C C----- PITCH EULER ANGLE STHT = - T13 CTHT = SQRT(T11*T11 + T12*T12) C KCD ADD IF (ABS(CTHT).LT.0.0001) CTHT=SIGN(0.0001,CTHT) THETR = ATAN2(STHT,CTHT) C C----- ROLL AND YAW EULER ANGLES IF (ABS(CTHT).GT.0.0001) THEN PHIR = ATAN2(T23,T33) SPHI = SIN(PHIR) CPHI = COS(PHIR) C PSIR = ATAN2(T12,T11) SPSI = SIN(PSIR) CPSI = COS(PSIR) ENDIF C C----- CREATE CONTINUOUS ROLL AND YAW ANGLES FROM ANGLES WHICH C----- ARE +/-180 DEG. PHI AND PSI OUTPUTS ARE +/-INFINITY. C----- C----- REPLACEMENT LOGIC FOR CONTINUOUS PHI AND PSI, VERS. 1.61 C----- THESE REPLACEMENTS ONLY OCCUR IN MULTIPLES OF 360 DEG, SO C----- DON'T GET EXCITED. C C----- UNWRAP PHIR FROM +/-180 DEG TO +/-INFINITY PHICUR = PHIR - DHLST PHIDEX = PHICUR - PHILST IF( ABS(PHIDEX) .GT. PI ) THEN DHLST = DHLST + SIGN( TWOPI, PHIDEX ) PHICUR = PHIR - DHLST END IF C PHIR = PHICUR PHILST = PHICUR C C----- UNWRAP PSIR FROM +/-180 DEG TO +/-INFINITY PSICUR = PSIR - DILST PSIDEX = PSICUR - PSILST IF( ABS(PSIDEX) .GT. PI ) THEN DILST = DILST + SIGN( TWOPI, PSIDEX ) PSICUR = PSIR - DILST END IF C PSIR = PSICUR PSILST = PSICUR C C KCD ADD C THE FOLLOWING CODE LINES ARE REPEATED IN THE EULER SECTION, C BUT RATHER THAN CONFUSE THINGS WITH OVERLAPPING LOOPS, THEY C ARE SIMPLY INCLUDED IN BOTH SECTIONS. C C***** AIRCRAFT ROTATIONAL RATES, EARTH FRAME. C***** ANGULAR RATES ARE IN COMMON WITH BOTH EULER AND QUATERNION C***** SECTIONS. C----- IQUAT = 1 : THED, PHID AND PSID ARE THE EULER RATES AT THE C----- END OF THE CYCLE TO BE USED ELSEWHERE. C----- NEW LOCAL RATES AND NEW SINE/COSINE OF EULER C----- ANGLES GIVES EULER RATES VALID AT THE END OF C----- THE CYCLE. C THED = QT*CPHI - RT*SPHI PSID = (QT*SPHI + RT*CPHI)/CTHT PHID = PT + PSID*STHT C C***** REDUNDANT STORES REQUIRED FOR MAKING A VECTOR OUT OF THE VELOCITIES C***** TRANSMITTED TO A CGI (DIG ONLY, CURRENTLY) C PHDCGI = PHID THDCGI = THED PSDCGI = PSID C 380 CONTINUE C C----- WE JUMP IF QUATERNIONS USED C IF( IQUAT .EQ. 1 ) GOTO 400 C C C***** INTEGRATE THE EARTH FRAME ANGULAR VELOCTIES INTO EULER ANGLES. C----- EULER ANGLES USED INSTEAD OF QUATERNIONS (ORIGINAL SYSTEM). C C KCD ADD C REPEAT OF ABOVE LINES FROM QUATERNION SECTION WITH IFREZX VARIABLES C APPLIED HERE INSTEAD OF ON THE ANGLE INTEGRATIONS. THE VARIABLES C USED ARE THE SAME TEMPORARY VARIABLES AS IN THE QUATERNION C SECTION. C C***** AIRCRAFT ROTATIONAL RATES, EARTH FRAME. C***** ANGULAR RATES ARE IN COMMON WITH BOTH EULER AND QUATERNION C***** SECTIONS. C----- IQUAT = 0 : THED, PHID AND PSID ARE THE EULER RATES C----- WHICH WILL BE INTEGRATED INTO EULER ANGLES. C----- NEW LOCAL ANGULAR RATES WITH OLD SINE AND C----- COSINE OF EULER ANGLES GIVES EULER RATES C----- WHICH CAN BE PROERLY INTEGRATED INTO EULER C----- ANGLES WHICH ARE VALID AT THE END OF THE CYCLE. C THED = QT*CPHI - RT*SPHI PSID = (QT*SPHI + RT*CPHI)/CTHT PHID = PT + PSID*STHT C PHDCGI = PHID THDCGI = THED PSDCGI = PSID C C----- INTEGRATE THE EARTH FRAME ANGULAR VELOCTIES INTO EULER C----- ANGLES USING A TRAPEZOIDAL CORRECTOR FORMULA. C C----- TRAPEZOIDAL INTEGRATION USED FOR THE CORRECTOR. C----- NOTE THAT THETA IS NOT RESTRICTED. PITCH TO 90 DEGREES WILL C----- CAUSE EXPONENT OVERFLOW IN PSID AND PHID RATE EQUATIONS. C IF( IFREZP .NE. 1 ) PHIR = PHIR + DELTI*(PHID + PHIDP) IF( IFREZQ .NE. 1 ) THETR = THETR + DELTI*(THED + THEDP) IF( IFREZR .NE. 1 ) PSIR = PSIR + DELTI*(PSID + PSIDP) C PHIDP = PHID THEDP = THED PSIDP = PSID C C----- NEW SINE & COSINE OF EULER ANGLES C SPHI = SIN(PHIR) CPHI = COS(PHIR) STHT = SIN(THETR) CTHT = COS(THETR) SPSI = SIN(PSIR) CPSI = COS(PSIR) C C----- NEW TRIGONOMETRIC MULTIPLIERS C T11 = CTHT*CPSI T21 = SPHI*STHT*CPSI - CPHI*SPSI T31 = CPHI*STHT*CPSI + SPHI*SPSI T12 = CTHT*SPSI T22 = SPHI*STHT*SPSI + CPHI*CPSI T32 = CPHI*STHT*SPSI - SPHI*CPSI T13 = - STHT T23 = SPHI*CTHT T33 = CPHI*CTHT C 400 CONTINUE C C************************************************************ C C C C***** PILOT EYEPOINT POSITION RELATIVE TO RUNWAY IN EARTH AXES C DNPR = DNR + T11*XP + T21*YP + T31*DELZPE DEPR = DER + T12*XP + T22*YP + T32*DELZPE C C C***** X,Y, AND H POSITION OF PILOT WRT RUNWAY IN RUNWAY AXES C***** TRANSFORM FROM NED AXES TO RUNWAY AXES. C XPR = DNPR*CTHETR + DEPR*STHETR YPR = -DNPR*STHETR + DEPR*CTHETR HPR = HCG - T13*XP - T23*YP - T33*DELZPE C C C***** T MATRIX DERIVATIVES USED FOR CGI COMPENSATION C***** DERIVATIVES DEVELOPED IN STRIKE FEB. 13, 91, REM C T11D = RT*T21 - QT*T31 T12D = RT*T22 - QT*T32 T13D = RT*T23 - QT*T33 T21D = PT*T31 - RT*T11 T22D = PT*T32 - RT*T12 T23D = PT*T33 - RT*T13 T31D = QT*T11 - PT*T21 T32D = QT*T12 - PT*T22 T33D = QT*T13 - PT*T23 C DNRD = RR*XLATD DERD = RR*CLATR*XLOND DNPRD = DNRD + T11D*XP + T21D*YP + T31D*DELZPE DEPRD = DERD + T12D*XP + T22D*YP + T32D*DELZPE C C----- BEGINNING OF THE 6-VECTOR (VELOCITIES) FOR CGI DRIVES, C----- START AT A(453) XPRD = DNPRD*CTHETR + DEPRD*STHETR YPRD = DEPRD*CTHETR - DNPRD*STHETR ZPRD = T13D*XP + T23D*YP + T33D*DELZPE - ALTD C C----- VECTOR COMPLETED USING PHDCGI, THDCGI AND PSDCGI (PHID,THED,PSID) C----------------------------------------------------------------------- C C C C***** AIRCRAFT C.G. RELATIVE TO RUNWAY IN RUNWAY AXES C***** TRANSFORM FROM NED AXES TO RUNWAY AXES. C XCG = DNR*CTHETR + DER*STHETR YCG = - DNR*STHETR + DER*CTHETR C XCGD = DNRD*CTHETR + DERD*STHETR YCGD = - DNRD*STHETR + DERD*CTHETR HCGD = ALTD C C C C***** TURBULENCE VELOCITY COMPONENTS C C***** CHANGE. ELIMINATED IETURB FEB. 28, 1991. TURBULENCE MAY NOW C***** BE INPUT IN THE EARTH FRAME BY USE OF (VNWAKE,VEWAKE,VDWAKE) C***** AND ALSO (STANDARD MIL SPEC) IN THE BODY FRAME (UTURB,VTURB,WTURB). C VNTURB = T11*UTURB + T21*VTURB + T31*WTURB + VNWAKE VETURB = T12*UTURB + T22*VTURB + T32*WTURB + VEWAKE VDTURB = T13*UTURB + T23*VTURB + T33*WTURB + VDWAKE C C C***** TOTAL WIND VELOCITIES C***** NOTE... VNW(+) MEANS THE AIR MASS IS TRAVELING NORTH... C VTWN = VNW + VNTURB VTWE = VEW + VETURB VTWD = VDW + VDTURB C C C***** AIRCRAFT VELOCITY WRT THE WIND C VNR = VN - VTWN VER = VEE - VTWE VDR = VD - VTWD C C C***** AIRCRAFT VELOCITY (BODY FRAME) WRT THE WIND C UB = T11*VNR + T12*VER + T13*VDR VB = T21*VNR + T22*VER + T23*VDR WB = T31*VNR + T32*VER + T33*VDR C UBKTS = UB*FPS2KT C C***** BODY AXIS ACCELERATIONS WRT THE WIND C UBD = RT*VB - QT*WB + T11*RAN + T12*RAE + T13*RAD VBD = PT*WB - RT*UB + T21*RAN + T22*RAE + T23*RAD WBD = QT*UB - PT*VB + T31*RAN + T32*RAE + T33*RAD C C C***** ANGLE OF ATTACK W/ SINE AND COSINE C IF( ABS(UB) .LT. 0.00001 ) UB=SIGN(0.00001,UB) C ALFAR = ATAN2(WB,UB) C SALPH = SIN(ALFAR) CALPH = COS(ALFAR) C C C***** SIDESLIP ANGLE W/SINE AND COSINE C DUM2 = UB**2 + WB**2 DUM = SQRT(DUM2) DUM1 = SIGN(DUM,UB) C BETAR = ATAN2(VB,DUM1) C SBETA = SIN(BETAR) CBETA = COS(BETAR) C C C***** VELOCITY IN XZ PLANE IN KNOTS C VKT = FPS2KT*DUM C C C***** AIRCRAFT VELOCITY WITH RESPECT TO WIND C VRW2 = DUM2 + VB**2 C VRW = SQRT(VRW2) C VRWKTS = VRW*FPS2KT C C C***** ALFA AND BETA RATE OF CHANGE: ALFD AND BETD C***** C***** IMODE = C***** ITOMTR = C IF( IMODE .GE. 0 ) GOTO 420 IF( ITOMTR .EQ. 0 ) GOTO 420 C C----- I.C. MODE AND ITOMTR=1 ALFD = 0.0 BETD = 0.0 GOTO 430 C 420 CONTINUE C C----- OPERATE MODE OR ITOMTR=0 ALFD = (UB*WBD - WB*UBD)/DUM2 BETD = (DUM2*VBD - (UB*UBD + WB*WBD)*VB)/(DUM1*VRW2) C 430 CONTINUE C C C***** A/C C.G. ACCELERATION, BODY FRAME (FT/SEC2) C AX = FTX*TEMP AY = FTY*TEMP AZ = FTZ*TEMP C C C***** A/C C.G. ACCELERATION, BODY FRAME (G'S) C AXG = AX*GINV AYG = AY*GINV AZG = AZ*GINV C C C***** PILOT STATION ACCELERATION, BODY FRAME (FT/SEC2) C AXP = AX - (RB**2+QB**2)*XP + (PB*QB-RBD)*YP + (PB*RB+QBD)*ZP AYP = AY + (PB*QB+RBD)*XP - (RB**2+PB**2)*YP + (QB*RB-PBD)*ZP AZP = AZ + (PB*RB-QBD)*XP + (QB*RB+PBD)*YP - (QB**2+PB**2)*ZP C C***** PILOT STATION ACCELERATION, BODY FRAME (G'S) C AXPG = AXP*GINV AYPG = AYP*GINV AZPG = AZP*GINV C GLOADN = -AZPG C C C***** EULER ANGLES, ALPHA AND BETA (DEG) C THET = THETR*R2D PHI = PHIR*R2D PSI = PSIR*R2D C ALFA = ALFAR*R2D BETA = BETAR*R2D C C C*****INSTANTANEOUS ROTATIONAL RATES (L-FRAME) C PL = VE*TEMP1 QL = -VN*TEMP1 RL = -PL*TLAT C C C*****INSTANTANEOUS ROTATIONAL RATES (BODY FRAME) C PLB = T11*PL + T12*QL + T13*RL QLB = T21*PL + T22*QL + T23*RL RLB = T31*PL + T32*QL + T33*RL C C C*****AIRCRAFT TOTAL VELOCITY & GROUND SPEED C PAD = VN**2 + VEE**2 VT = SQRT(PAD + VD**2) VG = SQRT(PAD) VGKT = VG*FPS2KT C C C*****FLIGHT PATH ANGLES C IF( VG .GE. 0.001 ) GAMV = ATAN2(-VD,VG) IF( ABS(VN) .GE. 0.001 ) GAMH = ATAN2(VEE,VN) C GAMVDG = GAMV*R2D GAMHDG = GAMH*R2D C C C C***** CREATE ATMOSPHERIC TEMPS,PRESSURES, DENSITY, SPEED OF C***** SOUND, ETC... C IF( IATMOS .EQ. 1 ) CALL ATMOS C C C C C******************************************************************* C***** SECOND AND LAST PART OF THE LANDING GEAR COMPUTATIONS ***** C******************************************************************* C C----- SAVE COMPUTATION TIME IF NO GEARS C IF( NOBLAN .EQ. 1 ) GOTO 9999 C C***** THE SUMMATION OF GEAR FORCES APPEARS AT THE BEGINNING OF THIS C***** ROUTINE, AND THE STRUT DEFLECTION COMPUTATIONS APPEAR HERE. C***** THINK ABOUT THIS. THIS IS THE PROPER ORDER. IT PERMITS YOUR C***** GEAR ROUTINE (CALLED BEFORE STRIKE) TO RECEIVE THE CORRECT C***** DEFLECTIONS, AND TO THEN PASS THE PROPER FORCES TO STRIKE. C C----- CHECK HEIGHT OF TAIL ABOVE RUNWAY C HTAIL = HCG + XTAIL*STHT - ZTAIL*CTHT C IF( HTAIL .LT. 0.0 ) THEN IHIT = 1 ISTRIK = 1 HTAILD = ALTD + THED*(XTAIL*CTHT + ZTAIL*STHT) ELSE IHIT = 0 ISTRIK = 0 HTAILD = 0.0 END IF C C----- CALCULATE MULTIPLIERS. THIS FORMULATION AVOIDS DIVISION PROBLEM C DA = QB*T33 - RB*T23 DB = RB*T13 - PB*T33 DC = PB*T23 - QB*T13 C K = - 2 DO 620 I=1,3 K = K + 3 C C----- CALCULATE VERTICAL HEIGHT OF GEAR C----- IN LOCAL AXES FROM BODY AXES C TEMG = HCG - T13*GEAR(K) - T23*GEAR(K+1) - T33*GEAR(K+2) C C----- FORGET GEARS MODEL FOR THIS GEAR IF IT IS ABOVE 20 FEET. C----- THIS AVOIDS THE DIVISION PROBLEM WHEN ACROBATICS ARE PERFORMED. C C----- START OUT WITH WHEEL HEIGHT IN LOCAL AXES AWEEL(I) = TEMG C IF( TEMG .GT. 20.0 ) GOTO 610 C C----- CALCULATE WHEEL STROKE (NO ACROBATICS BELOW 20 FEET, PLEASE) C----- BODY AXES AWEEL(I) = TEMG/T33 C C----- CALCULATE WHEEL STROKE RATE - BODY AXES AWEELD(I) = (ALTD + GEAR(K)*DA + GEAR(K+1)*DB 1 + (GEAR(K+2)+AWEEL(I))*DC)/T33 C C----- PROTECT STATICS IN I.C. MODE IF( IMODE .LT. 0 ) AWEELD(I) = 0.0 C C----- SEE IF GEAR IS COMPRESSED IF( AWEEL(I) .LT. 0.0 ) GOTO 600 C C----- LINEAR ANTICIPATION FOR SINGLE TRANSITION INTERVAL C----- NOTE THAT AWEEL IS POSITIVE HERE... C----- THIS USED TO BE CALLED PROJECTION. C IF( NOPROJ .EQ. 1 ) GOTO 610 PROJ = AWEEL(I) + DT2*AWEELD(I) C C----- ANTICIPATION OF COMPRESSION MUST BE GREATER THAN DEADBAND C----- FOR PROJECTION EQUATIONS TO APPLY. C IF( (PROJ+WDBAND) .GE. 0.0 ) GOTO 610 C C----- TAKE ONE HALF OF THE NEGATIVE DISPLACEMENT AWEEL(I) = 0.5*PROJ C 600 IHIT = 1 C C----- GEAR ON GROUND. INFORM YOUR GEAR ROUTINE. C NGEAR(I) = 1 GOTO 620 C C----- PARTICULAR GEAR NOT ON GROUND. C 610 NGEAR(I) = 0 C 620 CONTINUE C C----- SET LANDING WHEEL HEIGHT ABOVE RUNWAY (RIGHT GEAR) C HWEEL = DSTR*T33 C C C C 9999 CONTINUE C C KCD ADD IMODEP = IMODE C RETURN END