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