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 "ls_sim_control.h" */
 312 #include <math.h>
 313
 314 extern Model current_model;     /* defined in ls_model.c */
 315 extern SCALAR Simtime;          /* defined in ls_main.c */
 316
 317 void uiuc_init_vars() {
 318     static int init = 0;
 319
 320     if (init==0) {
 321         init=-1;
 322         uiuc_init_aeromodel();
 323     }
 324
 325     uiuc_initial_init();
 326 }
 327
 328
 329 void ls_step( SCALAR dt, int Initialize ) {
 330         static  int     inited = 0;
 331                 SCALAR  dth;
 332         static  SCALAR  v_dot_north_past, v_dot_east_past, v_dot_down_past;
 333         static  SCALAR  latitude_dot_past, longitude_dot_past, radius_dot_past;
 334         static  SCALAR  p_dot_body_past, q_dot_body_past, r_dot_body_past;
 335                 SCALAR  p_local_in_body, q_local_in_body, r_local_in_body;
 336                 SCALAR  epsilon, inv_eps, local_gnd_veast;
 337                 SCALAR  e_dot_0, e_dot_1, e_dot_2, e_dot_3;
 338         static  SCALAR  e_0, e_1, e_2, e_3;
 339         static  SCALAR  e_dot_0_past, e_dot_1_past, e_dot_2_past, e_dot_3_past;
 340                 SCALAR  cos_Lat_geocentric, inv_Radius_to_vehicle;
 341
 342 /*  I N I T I A L I Z A T I O N   */
 343
 344
 345         if ( (inited == 0) || (Initialize != 0) )
 346         {
 347 /* Set past values to zero */
 348         v_dot_north_past = v_dot_east_past = v_dot_down_past      = 0;
 349         latitude_dot_past = longitude_dot_past = radius_dot_past  = 0;
 350         p_dot_body_past = q_dot_body_past = r_dot_body_past       = 0;
 351         e_dot_0_past = e_dot_1_past = e_dot_2_past = e_dot_3_past = 0;
 352
 353 /* Initialize geocentric position from geodetic latitude and altitude */
 354
 355         ls_geod_to_geoc( Latitude, Altitude, &Sea_level_radius, &Lat_geocentric);
 356         Earth_position_angle = 0;
 357         Lon_geocentric = Longitude;
 358         Radius_to_vehicle = Altitude + Sea_level_radius;
 359
 360 /* Correct eastward velocity to account for earths' rotation, if necessary */
 361
 362         local_gnd_veast = OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric);
 363         if( fabs(V_east - V_east_rel_ground) < 0.8*local_gnd_veast )
 364         V_east = V_east + local_gnd_veast;
 365
 366 /* Initialize quaternions and transformation matrix from Euler angles */
 367         // Initialize UIUC aircraft model
 368         if (current_model == UIUC) {
 369           uiuc_init_2_wrapper();
 370         }
 371
 372             e_0 = cos(Psi*0.5)*cos(Theta*0.5)*cos(Phi*0.5)
 373                 + sin(Psi*0.5)*sin(Theta*0.5)*sin(Phi*0.5);
 374             e_1 = cos(Psi*0.5)*cos(Theta*0.5)*sin(Phi*0.5)
 375                 - sin(Psi*0.5)*sin(Theta*0.5)*cos(Phi*0.5);
 376             e_2 = cos(Psi*0.5)*sin(Theta*0.5)*cos(Phi*0.5)
 377                 + sin(Psi*0.5)*cos(Theta*0.5)*sin(Phi*0.5);
 378             e_3 =-cos(Psi*0.5)*sin(Theta*0.5)*sin(Phi*0.5)
 379                 + sin(Psi*0.5)*cos(Theta*0.5)*cos(Phi*0.5);
 380             T_local_to_body_11 = e_0*e_0 + e_1*e_1 - e_2*e_2 - e_3*e_3;
 381             T_local_to_body_12 = 2*(e_1*e_2 + e_0*e_3);
 382             T_local_to_body_13 = 2*(e_1*e_3 - e_0*e_2);
 383             T_local_to_body_21 = 2*(e_1*e_2 - e_0*e_3);
 384             T_local_to_body_22 = e_0*e_0 - e_1*e_1 + e_2*e_2 - e_3*e_3;
 385             T_local_to_body_23 = 2*(e_2*e_3 + e_0*e_1);
 386             T_local_to_body_31 = 2*(e_1*e_3 + e_0*e_2);
 387             T_local_to_body_32 = 2*(e_2*e_3 - e_0*e_1);
 388             T_local_to_body_33 = e_0*e_0 - e_1*e_1 - e_2*e_2 + e_3*e_3;
 389
 390             // Initialize local velocities (V_north, V_east, V_down)
 391             // based on transformation matrix calculated above
 392             if (current_model == UIUC) {
 393               uiuc_local_vel_init();
 394             }
 395
 396 /*      Calculate local gravitation acceleration        */
 397
 398                 ls_gravity( Radius_to_vehicle, Lat_geocentric, &Gravity );
 399
 400 /*      Initialize vehicle model                        */
 401
 402                 ls_aux();
 403                 ls_model(0.0, 0);
 404
 405 /*      Calculate initial accelerations */
 406
 407                 ls_accel();
 408
 409 /* Initialize auxiliary variables */
 410
 411                 ls_aux();
 412                 Std_Alpha_dot = 0.;
 413                 Std_Beta_dot = 0.;
 414
 415 /* set flag; disable integrators */
 416
 417                 inited = -1;
 418                 dt = 0.0;
 419
 420         }
 421
 422 /* Update time */
 423
 424         dth = 0.5*dt;
 425         Simtime = Simtime + dt;
 426
 427 /*  L I N E A R   V E L O C I T I E S   */
 428
 429 /* Integrate linear accelerations to get velocities */
 430 /*    Using predictive Adams-Bashford algorithm     */
 431
 432     V_north = V_north + dth*(3*V_dot_north - v_dot_north_past);
 433     V_east  = V_east  + dth*(3*V_dot_east  - v_dot_east_past );
 434     V_down  = V_down  + dth*(3*V_dot_down  - v_dot_down_past );
 435
 436 /* record past states */
 437
 438     v_dot_north_past = V_dot_north;
 439     v_dot_east_past  = V_dot_east;
 440     v_dot_down_past  = V_dot_down;
 441
 442 /* Calculate trajectory rate (geocentric coordinates) */
 443
 444     inv_Radius_to_vehicle = 1.0/Radius_to_vehicle;
 445     cos_Lat_geocentric = cos(Lat_geocentric);
 446
 447     if ( cos_Lat_geocentric != 0) {
 448         Longitude_dot = V_east/(Radius_to_vehicle*cos_Lat_geocentric);
 449     }
 450
 451     Latitude_dot = V_north*inv_Radius_to_vehicle;
 452     Radius_dot = -V_down;
 453
 454 /*  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  */
 455
 456 /* Integrate rotational accelerations to get velocities */
 457
 458     P_body = P_body + dth*(3*P_dot_body - p_dot_body_past);
 459     Q_body = Q_body + dth*(3*Q_dot_body - q_dot_body_past);
 460     R_body = R_body + dth*(3*R_dot_body - r_dot_body_past);
 461
 462 /* Save past states */
 463
 464     p_dot_body_past = P_dot_body;
 465     q_dot_body_past = Q_dot_body;
 466     r_dot_body_past = R_dot_body;
 467
 468 /* Calculate local axis frame rates due to travel over curved earth */
 469
 470     P_local =  V_east*inv_Radius_to_vehicle;
 471     Q_local = -V_north*inv_Radius_to_vehicle;
 472     R_local = -V_east*tan(Lat_geocentric)*inv_Radius_to_vehicle;
 473
 474 /* Transform local axis frame rates to body axis rates */
 475
 476     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;
 477     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;
 478     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;
 479
 480 /* Calculate total angular rates in body axis */
 481
 482     P_total = P_body - p_local_in_body;
 483     Q_total = Q_body - q_local_in_body;
 484     R_total = R_body - r_local_in_body;
 485
 486 /* Transform to quaternion rates (see Appendix E in [2]) */
 487
 488     e_dot_0 = 0.5*( -P_total*e_1 - Q_total*e_2 - R_total*e_3 );
 489     e_dot_1 = 0.5*(  P_total*e_0 - Q_total*e_3 + R_total*e_2 );
 490     e_dot_2 = 0.5*(  P_total*e_3 + Q_total*e_0 - R_total*e_1 );
 491     e_dot_3 = 0.5*( -P_total*e_2 + Q_total*e_1 + R_total*e_0 );
 492
 493 /* Integrate using trapezoidal as before */
 494
 495         e_0 = e_0 + dth*(e_dot_0 + e_dot_0_past);
 496         e_1 = e_1 + dth*(e_dot_1 + e_dot_1_past);
 497         e_2 = e_2 + dth*(e_dot_2 + e_dot_2_past);
 498         e_3 = e_3 + dth*(e_dot_3 + e_dot_3_past);
 499
 500 /* calculate orthagonality correction  - scale quaternion to unity length */
 501
 502         epsilon = sqrt(e_0*e_0 + e_1*e_1 + e_2*e_2 + e_3*e_3);
 503         inv_eps = 1/epsilon;
 504
 505         e_0 = inv_eps*e_0;
 506         e_1 = inv_eps*e_1;
 507         e_2 = inv_eps*e_2;
 508         e_3 = inv_eps*e_3;
 509
 510 /* Save past values */
 511
 512         e_dot_0_past = e_dot_0;
 513         e_dot_1_past = e_dot_1;
 514         e_dot_2_past = e_dot_2;
 515         e_dot_3_past = e_dot_3;
 516
 517 /* Update local to body transformation matrix */
 518
 519         T_local_to_body_11 = e_0*e_0 + e_1*e_1 - e_2*e_2 - e_3*e_3;
 520         T_local_to_body_12 = 2*(e_1*e_2 + e_0*e_3);
 521         T_local_to_body_13 = 2*(e_1*e_3 - e_0*e_2);
 522         T_local_to_body_21 = 2*(e_1*e_2 - e_0*e_3);
 523         T_local_to_body_22 = e_0*e_0 - e_1*e_1 + e_2*e_2 - e_3*e_3;
 524         T_local_to_body_23 = 2*(e_2*e_3 + e_0*e_1);
 525         T_local_to_body_31 = 2*(e_1*e_3 + e_0*e_2);
 526         T_local_to_body_32 = 2*(e_2*e_3 - e_0*e_1);
 527         T_local_to_body_33 = e_0*e_0 - e_1*e_1 - e_2*e_2 + e_3*e_3;
 528
 529 /* Calculate Euler angles */
 530
 531         Theta = asin( -T_local_to_body_13 );
 532
 533         if( T_local_to_body_11 == 0 )
 534         Psi = 0;
 535         else
 536         Psi = atan2( T_local_to_body_12, T_local_to_body_11 );
 537
 538         if( T_local_to_body_33 == 0 )
 539         Phi = 0;
 540         else
 541         Phi = atan2( T_local_to_body_23, T_local_to_body_33 );
 542
 543 /* Resolve Psi to 0 - 359.9999 */
 544
 545         if (Psi < 0 ) Psi = Psi + 2*LS_PI;
 546
 547 /*  L I N E A R   P O S I T I O N S   */
 548
 549 /* Trapezoidal acceleration for position */
 550
 551         Lat_geocentric    = Lat_geocentric    + dth*(Latitude_dot  + latitude_dot_past );
 552         Lon_geocentric    = Lon_geocentric    + dth*(Longitude_dot + longitude_dot_past);
 553         Radius_to_vehicle = Radius_to_vehicle + dth*(Radius_dot    + radius_dot_past );
 554         Earth_position_angle = Earth_position_angle + dt*OMEGA_EARTH;
 555
 556 /* Save past values */
 557
 558         latitude_dot_past  = Latitude_dot;
 559         longitude_dot_past = Longitude_dot;
 560         radius_dot_past    = Radius_dot;
 561
 562 /* end of ls_step */
 563 }
 564 /*************************************************************************/
 565