src/FDM/LaRCsim/ls_step.c

   1 /***************************************************************************
   2
   3         TITLE:  ls_step
   4
   5 ----------------------------------------------------------------------------
   6
   7         FUNCTION:       Integration routine for equations of motion
   8                         (vehicle states)
   9
  10 ----------------------------------------------------------------------------
  11
  12         MODULE STATUS:  developmental
  13
  14 ----------------------------------------------------------------------------
  15
  16         GENEALOGY:      Written 920802 by Bruce Jackson.  Based upon equations
  17                         given in reference [1] and a Matrix-X/System Build block
  18                         diagram model of equations of motion coded by David Raney
  19                         at NASA-Langley in June of 1992.
  20
  21 ----------------------------------------------------------------------------
  22
  23         DESIGNED BY:    Bruce Jackson
  24
  25         CODED BY:       Bruce Jackson
  26
  27         MAINTAINED BY:
  28
  29 ----------------------------------------------------------------------------
  30
  31         MODIFICATION HISTORY:
  32
  33         DATE    PURPOSE                                         BY
  34
  35         921223  Modified calculation of Phi and Psi to use the "atan2" routine
  36                 rather than the "atan" to allow full circular angles.
  37                 "atan" limits to +/- pi/2.                      EBJ
  38
  39         940111  Changed from oldstyle include file ls_eom.h; also changed
  40                 from DATA to SCALAR type.                       EBJ
  41
  42         950207  Initialized Alpha_dot and Beta_dot to zero on first pass; calculated
  43                 thereafter.                                     EBJ
  44
  45         950224  Added logic to avoid adding additional increment to V_east
  46                 in case V_east already accounts for rotating earth.
  47                                                                 EBJ
  48
  49         CURRENT RCS HEADER:
  50
  51 $Header$
  52 $Log$
  53 Revision 1.4  2001/03/24 05:03:12  curt
  54 SG-ified logstream.
  55
  56 Revision 1.3  2000/10/23 22:34:55  curt
  57 I tested:
  58 LaRCsim c172 on-ground and in-air starts, reset: all work
  59 UIUC Cessna172 on-ground and in-air starts work as expected, reset
  60 results in an aircraft that is upside down but does not crash FG.   I
  61 don't know what it was like before, so it may well be no change.
  62 JSBSim c172 and X15 in-air starts work fine, resets now work (and are
  63 trimmed), on-ground starts do not -- the c172 ends up on its back.  I
  64 suspect this is no worse than before.
  65
  66 I did not test:
  67 Balloon (the weather code returns nan's for the atmosphere data --this
  68 is in the weather module and apparently is a linux only bug)
  69 ADA (don't know how)
  70 MagicCarpet  (needs work yet)
  71 External (don't know how)
  72
  73 known to be broken:
  74 LaRCsim c172 on-ground starts with a negative terrain altitude (this
  75 happens at KPAO when the scenery is not present).   The FDM inits to
  76 about 50 feet AGL and the model falls to the ground.  It does stay
  77 upright, however, and seems to be fine once it settles out, FWIW.
  78
  79 To do:
  80 --implement set_Model on the bus
  81 --bring Christian's weather data into JSBSim
  82 -- add default method to bus for updating things like the sin and cos of
  83 latitude (for Balloon, MagicCarpet)
  84 -- lots of cleanup
  85
  86 The files:
  87 src/FDM/flight.cxx
  88 src/FDM/flight.hxx
  89 -- all data members now declared protected instead of private.
  90 -- eliminated all but a small set of 'setters', no change to getters.
  91 -- that small set is declared virtual, the default implementation
  92 provided preserves the old behavior
  93 -- all of the vector data members are now initialized.
  94 -- added busdump() method -- SG_LOG's  all the bus data when called,
  95 useful for diagnostics.
  96
  97 src/FDM/ADA.cxx
  98 -- bus data members now directly assigned to
  99
 100 src/FDM/Balloon.cxx
 101 -- bus data members now directly assigned to
 102 -- changed V_equiv_kts to V_calibrated_kts
 103
 104 src/FDM/JSBSim.cxx
 105 src/FDM/JSBSim.hxx
 106 -- bus data members now directly assigned to
 107 -- implemented the FGInterface virtual setters with JSBSim specific
 108 logic
 109 -- changed the static FDMExec to a dynamic fdmex (needed so that the
 110 JSBSim object can be deleted when a model change is called for)
 111 -- implemented constructor and destructor, moved some of the logic
 112 formerly in init() to constructor
 113 -- added logic to bring up FGEngInterface objects and set the RPM and
 114 throttle values.
 115
 116 src/FDM/LaRCsim.cxx
 117 src/FDM/LaRCsim.hxx
 118 -- bus data members now directly assigned to
 119 -- implemented the FGInterface virtual setters with LaRCsim specific
 120 logic, uses LaRCsimIC
 121 -- implemented constructor and destructor, moved some of the logic
 122 formerly in init() to constructor
 123 -- moved default inertias to here from fg_init.cxx
 124 -- eliminated the climb rate calculation.  The equivalent, climb_rate =
 125 -1*vdown, is now in copy_from_LaRCsim().
 126
 127 src/FDM/LaRCsimIC.cxx
 128 src/FDM/LaRCsimIC.hxx
 129 -- similar to FGInitialCondition, this class has all the logic needed to
 130 turn data like Vc and Mach into the more fundamental quantities LaRCsim
 131 needs to initialize.
 132 -- put it in src/FDM since it is a class
 133
 134 src/FDM/MagicCarpet.cxx
 135  -- bus data members now directly assigned to
 136
 137 src/FDM/Makefile.am
 138 -- adds LaRCsimIC.hxx and cxx
 139
 140 src/FDM/JSBSim/FGAtmosphere.h
 141 src/FDM/JSBSim/FGDefs.h
 142 src/FDM/JSBSim/FGInitialCondition.cpp
 143 src/FDM/JSBSim/FGInitialCondition.h
 144 src/FDM/JSBSim/JSBSim.cpp
 145 -- changes to accomodate the new bus
 146
 147 src/FDM/LaRCsim/atmos_62.h
 148 src/FDM/LaRCsim/ls_geodesy.h
 149 -- surrounded prototypes with #ifdef __cplusplus ... #endif , functions
 150 here are needed in LaRCsimIC
 151
 152 src/FDM/LaRCsim/c172_main.c
 153 src/FDM/LaRCsim/cherokee_aero.c
 154 src/FDM/LaRCsim/ls_aux.c
 155 src/FDM/LaRCsim/ls_constants.h
 156 src/FDM/LaRCsim/ls_geodesy.c
 157 src/FDM/LaRCsim/ls_geodesy.h
 158 src/FDM/LaRCsim/ls_step.c
 159 src/FDM/UIUCModel/uiuc_betaprobe.cpp
 160 -- changed PI to LS_PI, eliminates preprocessor naming conflict with
 161 weather module
 162
 163 src/FDM/LaRCsim/ls_interface.c
 164 src/FDM/LaRCsim/ls_interface.h
 165 -- added function ls_set_model_dt()
 166
 167 src/Main/bfi.cxx
 168 -- eliminated calls that set the NED speeds to body components.  They
 169 are no longer needed and confuse the new bus.
 170
 171 src/Main/fg_init.cxx
 172 -- eliminated calls that just brought the bus data up-to-date (e.g.
 173 set_sin_cos_latitude). or set default values.   The bus now handles the
 174 defaults and updates itself when the setters are called (for LaRCsim and
 175 JSBSim).  A default method for doing this needs to be added to the bus.
 176 -- added fgVelocityInit() to set the speed the user asked for.  Both
 177 JSBSim and LaRCsim can now be initialized using any of:
 178 vc,mach, NED components, UVW components.
 179
 180 src/Main/main.cxx
 181 --eliminated call to fgFDMSetGroundElevation, this data is now 'pulled'
 182 onto the bus every update()
 183
 184 src/Main/options.cxx
 185 src/Main/options.hxx
 186 -- added enum to keep track of the speed requested by the user
 187 -- eliminated calls to set NED velocity properties to body speeds, they
 188 are no longer needed.
 189 -- added options for the NED components.
 190
 191 src/Network/garmin.cxx
 192 src/Network/nmea.cxx
 193 --eliminated calls that just brought the bus data up-to-date (e.g.
 194 set_sin_cos_latitude).  The bus now updates itself when the setters are
 195 called (for LaRCsim and JSBSim).  A default method for doing this needs
 196 to be added to the bus.
 197 -- changed set_V_equiv_kts to set_V_calibrated_kts.  set_V_equiv_kts no
 198 longer exists ( get_V_equiv_kts still does, though)
 199
 200 src/WeatherCM/FGLocalWeatherDatabase.cpp
 201 -- commented out the code to put the weather data on the bus, a
 202 different scheme for this is needed.
 203
 204 Revision 1.2  1999/10/29 16:08:33  curt
 205 Added flaps support to c172 model.
 206
 207 Revision 1.1.1.1  1999/06/17 18:07:33  curt
 208 Start of 0.7.x branch
 209
 210 Revision 1.1.1.1  1999/04/05 21:32:45  curt
 211 Start of 0.6.x branch.
 212
 213 Revision 1.4  1998/08/24 20:09:27  curt
 214 Code optimization tweaks from Norman Vine.
 215
 216 Revision 1.3  1998/07/12 03:11:04  curt
 217 Removed some printf()'s.
 218 Fixed the autopilot integration so it should be able to update it's control
 219   positions every time the internal flight model loop is run, and not just
 220   once per rendered frame.
 221 Added a routine to do the necessary stuff to force an arbitrary altitude
 222   change.
 223 Gave the Navion engine just a tad more power.
 224
 225 Revision 1.2  1998/01/19 18:40:28  curt
 226 Tons of little changes to clean up the code and to remove fatal errors
 227 when building with the c++ compiler.
 228
 229 Revision 1.1  1997/05/29 00:09:59  curt
 230 Initial Flight Gear revision.
 231
 232  * Revision 1.5  1995/03/02  20:24:13  bjax
 233  * Added logic to avoid adding additional increment to V_east
 234  * in case V_east already accounts for rotating earth. EBJ
 235  *
 236  * Revision 1.4  1995/02/07  20:52:21  bjax
 237  * Added initialization of Alpha_dot and Beta_dot to zero on first
 238  * pass; they get calculated by ls_aux on next pass...  EBJ
 239  *
 240  * Revision 1.3  1994/01/11  19:01:12  bjax
 241  * Changed from DATA to SCALAR type; also fixed header files (was ls_eom.h)
 242  *
 243  * Revision 1.2  1993/06/02  15:03:09  bjax
 244  * Moved initialization of geocentric position to subroutine ls_geod_to_geoc.
 245  *
 246  * Revision 1.1  92/12/30  13:16:11  bjax
 247  * Initial revision
 248  *
 249
 250 ----------------------------------------------------------------------------
 251
 252         REFERENCES:
 253
 254                 [ 1]    McFarland, Richard E.: "A Standard Kinematic Model
 255                         for Flight Simulation at NASA-Ames", NASA CR-2497,
 256                         January 1975
 257
 258                 [ 2]    ANSI/AIAA R-004-1992 "Recommended Practice: Atmos-
 259                         pheric and Space Flight Vehicle Coordinate Systems",
 260                         February 1992
 261
 262
 263 ----------------------------------------------------------------------------
 264
 265         CALLED BY:
 266
 267 ----------------------------------------------------------------------------
 268
 269         CALLS TO:       None.
 270
 271 ----------------------------------------------------------------------------
 272
 273         INPUTS: State derivatives
 274
 275 ----------------------------------------------------------------------------
 276
 277         OUTPUTS:        States
 278
 279 --------------------------------------------------------------------------*/
 280
 281 #include "ls_types.h"
 282 #include "ls_constants.h"
 283 #include "ls_generic.h"
 284 #include "ls_accel.h"
 285 #include "ls_aux.h"
 286 #include "ls_model.h"
 287 #include "ls_step.h"
 288 #include "ls_geodesy.h"
 289 #include "ls_gravity.h"
 290 /* #include "ls_sim_control.h" */
 291 #include <math.h>
 292
 293 extern SCALAR Simtime;          /* defined in ls_main.c */
 294
 295 void ls_step( SCALAR dt, int Initialize ) {
 296         static  int     inited = 0;
 297                 SCALAR  dth;
 298         static  SCALAR  v_dot_north_past, v_dot_east_past, v_dot_down_past;
 299         static  SCALAR  latitude_dot_past, longitude_dot_past, radius_dot_past;
 300         static  SCALAR  p_dot_body_past, q_dot_body_past, r_dot_body_past;
 301                 SCALAR  p_local_in_body, q_local_in_body, r_local_in_body;
 302                 SCALAR  epsilon, inv_eps, local_gnd_veast;
 303                 SCALAR  e_dot_0, e_dot_1, e_dot_2, e_dot_3;
 304         static  SCALAR  e_0, e_1, e_2, e_3;
 305         static  SCALAR  e_dot_0_past, e_dot_1_past, e_dot_2_past, e_dot_3_past;
 306                 SCALAR  cos_Lat_geocentric, inv_Radius_to_vehicle;
 307
 308 /*  I N I T I A L I Z A T I O N   */
 309
 310
 311         if ( (inited == 0) || (Initialize != 0) )
 312         {
 313 /* Set past values to zero */
 314         v_dot_north_past = v_dot_east_past = v_dot_down_past      = 0;
 315         latitude_dot_past = longitude_dot_past = radius_dot_past  = 0;
 316         p_dot_body_past = q_dot_body_past = r_dot_body_past       = 0;
 317         e_dot_0_past = e_dot_1_past = e_dot_2_past = e_dot_3_past = 0;
 318
 319 /* Initialize geocentric position from geodetic latitude and altitude */
 320
 321         ls_geod_to_geoc( Latitude, Altitude, &Sea_level_radius, &Lat_geocentric);
 322         Earth_position_angle = 0;
 323         Lon_geocentric = Longitude;
 324         Radius_to_vehicle = Altitude + Sea_level_radius;
 325
 326 /* Correct eastward velocity to account for earths' rotation, if necessary */
 327
 328         local_gnd_veast = OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric);
 329         if( fabs(V_east - V_east_rel_ground) < 0.8*local_gnd_veast )
 330         V_east = V_east + local_gnd_veast;
 331
 332 /* Initialize quaternions and transformation matrix from Euler angles */
 333
 334             e_0 = cos(Psi*0.5)*cos(Theta*0.5)*cos(Phi*0.5)
 335                 + sin(Psi*0.5)*sin(Theta*0.5)*sin(Phi*0.5);
 336             e_1 = cos(Psi*0.5)*cos(Theta*0.5)*sin(Phi*0.5)
 337                 - sin(Psi*0.5)*sin(Theta*0.5)*cos(Phi*0.5);
 338             e_2 = cos(Psi*0.5)*sin(Theta*0.5)*cos(Phi*0.5)
 339                 + sin(Psi*0.5)*cos(Theta*0.5)*sin(Phi*0.5);
 340             e_3 =-cos(Psi*0.5)*sin(Theta*0.5)*sin(Phi*0.5)
 341                 + sin(Psi*0.5)*cos(Theta*0.5)*cos(Phi*0.5);
 342             T_local_to_body_11 = e_0*e_0 + e_1*e_1 - e_2*e_2 - e_3*e_3;
 343             T_local_to_body_12 = 2*(e_1*e_2 + e_0*e_3);
 344             T_local_to_body_13 = 2*(e_1*e_3 - e_0*e_2);
 345             T_local_to_body_21 = 2*(e_1*e_2 - e_0*e_3);
 346             T_local_to_body_22 = e_0*e_0 - e_1*e_1 + e_2*e_2 - e_3*e_3;
 347             T_local_to_body_23 = 2*(e_2*e_3 + e_0*e_1);
 348             T_local_to_body_31 = 2*(e_1*e_3 + e_0*e_2);
 349             T_local_to_body_32 = 2*(e_2*e_3 - e_0*e_1);
 350             T_local_to_body_33 = e_0*e_0 - e_1*e_1 - e_2*e_2 + e_3*e_3;
 351
 352 /*      Calculate local gravitation acceleration        */
 353
 354                 ls_gravity( Radius_to_vehicle, Lat_geocentric, &Gravity );
 355
 356 /*      Initialize vehicle model                        */
 357
 358                 ls_aux();
 359                 ls_model(0.0, 0);
 360
 361 /*      Calculate initial accelerations */
 362
 363                 ls_accel();
 364
 365 /* Initialize auxiliary variables */
 366
 367                 ls_aux();
 368                 Alpha_dot = 0.;
 369                 Beta_dot = 0.;
 370
 371 /* set flag; disable integrators */
 372
 373                 inited = -1;
 374                 dt = 0.0;
 375
 376         }
 377
 378 /* Update time */
 379
 380         dth = 0.5*dt;
 381         Simtime = Simtime + dt;
 382
 383 /*  L I N E A R   V E L O C I T I E S   */
 384
 385 /* Integrate linear accelerations to get velocities */
 386 /*    Using predictive Adams-Bashford algorithm     */
 387
 388     V_north = V_north + dth*(3*V_dot_north - v_dot_north_past);
 389     V_east  = V_east  + dth*(3*V_dot_east  - v_dot_east_past );
 390     V_down  = V_down  + dth*(3*V_dot_down  - v_dot_down_past );
 391
 392 /* record past states */
 393
 394     v_dot_north_past = V_dot_north;
 395     v_dot_east_past  = V_dot_east;
 396     v_dot_down_past  = V_dot_down;
 397
 398 /* Calculate trajectory rate (geocentric coordinates) */
 399
 400     inv_Radius_to_vehicle = 1.0/Radius_to_vehicle;
 401     cos_Lat_geocentric = cos(Lat_geocentric);
 402
 403     if ( cos_Lat_geocentric != 0) {
 404         Longitude_dot = V_east/(Radius_to_vehicle*cos_Lat_geocentric);
 405     }
 406
 407     Latitude_dot = V_north*inv_Radius_to_vehicle;
 408     Radius_dot = -V_down;
 409
 410 /*  A N G U L A R   V E L O C I T I E S   A N D   P O S I T I O N S  */
 411
 412 /* Integrate rotational accelerations to get velocities */
 413
 414     P_body = P_body + dth*(3*P_dot_body - p_dot_body_past);
 415     Q_body = Q_body + dth*(3*Q_dot_body - q_dot_body_past);
 416     R_body = R_body + dth*(3*R_dot_body - r_dot_body_past);
 417
 418 /* Save past states */
 419
 420     p_dot_body_past = P_dot_body;
 421     q_dot_body_past = Q_dot_body;
 422     r_dot_body_past = R_dot_body;
 423
 424 /* Calculate local axis frame rates due to travel over curved earth */
 425
 426     P_local =  V_east*inv_Radius_to_vehicle;
 427     Q_local = -V_north*inv_Radius_to_vehicle;
 428     R_local = -V_east*tan(Lat_geocentric)*inv_Radius_to_vehicle;
 429
 430 /* Transform local axis frame rates to body axis rates */
 431
 432     p_local_in_body = T_local_to_body_11*P_local + T_local_to_body_12*Q_local + T_local_to_body_13*R_local;
 433     q_local_in_body = T_local_to_body_21*P_local + T_local_to_body_22*Q_local + T_local_to_body_23*R_local;
 434     r_local_in_body = T_local_to_body_31*P_local + T_local_to_body_32*Q_local + T_local_to_body_33*R_local;
 435
 436 /* Calculate total angular rates in body axis */
 437
 438     P_total = P_body - p_local_in_body;
 439     Q_total = Q_body - q_local_in_body;
 440     R_total = R_body - r_local_in_body;
 441
 442 /* Transform to quaternion rates (see Appendix E in [2]) */
 443
 444     e_dot_0 = 0.5*( -P_total*e_1 - Q_total*e_2 - R_total*e_3 );
 445     e_dot_1 = 0.5*(  P_total*e_0 - Q_total*e_3 + R_total*e_2 );
 446     e_dot_2 = 0.5*(  P_total*e_3 + Q_total*e_0 - R_total*e_1 );
 447     e_dot_3 = 0.5*( -P_total*e_2 + Q_total*e_1 + R_total*e_0 );
 448
 449 /* Integrate using trapezoidal as before */
 450
 451         e_0 = e_0 + dth*(e_dot_0 + e_dot_0_past);
 452         e_1 = e_1 + dth*(e_dot_1 + e_dot_1_past);
 453         e_2 = e_2 + dth*(e_dot_2 + e_dot_2_past);
 454         e_3 = e_3 + dth*(e_dot_3 + e_dot_3_past);
 455
 456 /* calculate orthagonality correction  - scale quaternion to unity length */
 457
 458         epsilon = sqrt(e_0*e_0 + e_1*e_1 + e_2*e_2 + e_3*e_3);
 459         inv_eps = 1/epsilon;
 460
 461         e_0 = inv_eps*e_0;
 462         e_1 = inv_eps*e_1;
 463         e_2 = inv_eps*e_2;
 464         e_3 = inv_eps*e_3;
 465
 466 /* Save past values */
 467
 468         e_dot_0_past = e_dot_0;
 469         e_dot_1_past = e_dot_1;
 470         e_dot_2_past = e_dot_2;
 471         e_dot_3_past = e_dot_3;
 472
 473 /* Update local to body transformation matrix */
 474
 475         T_local_to_body_11 = e_0*e_0 + e_1*e_1 - e_2*e_2 - e_3*e_3;
 476         T_local_to_body_12 = 2*(e_1*e_2 + e_0*e_3);
 477         T_local_to_body_13 = 2*(e_1*e_3 - e_0*e_2);
 478         T_local_to_body_21 = 2*(e_1*e_2 - e_0*e_3);
 479         T_local_to_body_22 = e_0*e_0 - e_1*e_1 + e_2*e_2 - e_3*e_3;
 480         T_local_to_body_23 = 2*(e_2*e_3 + e_0*e_1);
 481         T_local_to_body_31 = 2*(e_1*e_3 + e_0*e_2);
 482         T_local_to_body_32 = 2*(e_2*e_3 - e_0*e_1);
 483         T_local_to_body_33 = e_0*e_0 - e_1*e_1 - e_2*e_2 + e_3*e_3;
 484
 485 /* Calculate Euler angles */
 486
 487         Theta = asin( -T_local_to_body_13 );
 488
 489         if( T_local_to_body_11 == 0 )
 490         Psi = 0;
 491         else
 492         Psi = atan2( T_local_to_body_12, T_local_to_body_11 );
 493
 494         if( T_local_to_body_33 == 0 )
 495         Phi = 0;
 496         else
 497         Phi = atan2( T_local_to_body_23, T_local_to_body_33 );
 498
 499 /* Resolve Psi to 0 - 359.9999 */
 500
 501         if (Psi < 0 ) Psi = Psi + 2*LS_PI;
 502
 503 /*  L I N E A R   P O S I T I O N S   */
 504
 505 /* Trapezoidal acceleration for position */
 506
 507         Lat_geocentric    = Lat_geocentric    + dth*(Latitude_dot  + latitude_dot_past );
 508         Lon_geocentric    = Lon_geocentric    + dth*(Longitude_dot + longitude_dot_past);
 509         Radius_to_vehicle = Radius_to_vehicle + dth*(Radius_dot    + radius_dot_past );
 510         Earth_position_angle = Earth_position_angle + dt*OMEGA_EARTH;
 511
 512 /* Save past values */
 513
 514         latitude_dot_past  = Latitude_dot;
 515         longitude_dot_past = Longitude_dot;
 516         radius_dot_past    = Radius_dot;
 517
 518 /* end of ls_step */
 519 }
 520 /*************************************************************************/
 521