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