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