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