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