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