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.2  1998/01/19 18:40:28  curt
  54 Tons of little changes to clean up the code and to remove fatal errors
  55 when building with the c++ compiler.
  56
  57 Revision 1.1  1997/05/29 00:09:59  curt
  58 Initial Flight Gear revision.
  59
  60  * Revision 1.5  1995/03/02  20:24:13  bjax
  61  * Added logic to avoid adding additional increment to V_east
  62  * in case V_east already accounts for rotating earth. EBJ
  63  *
  64  * Revision 1.4  1995/02/07  20:52:21  bjax
  65  * Added initialization of Alpha_dot and Beta_dot to zero on first
  66  * pass; they get calculated by ls_aux on next pass...  EBJ
  67  *
  68  * Revision 1.3  1994/01/11  19:01:12  bjax
  69  * Changed from DATA to SCALAR type; also fixed header files (was ls_eom.h)
  70  *
  71  * Revision 1.2  1993/06/02  15:03:09  bjax
  72  * Moved initialization of geocentric position to subroutine ls_geod_to_geoc.
  73  *
  74  * Revision 1.1  92/12/30  13:16:11  bjax
  75  * Initial revision
  76  *
  77
  78 ----------------------------------------------------------------------------
  79
  80         REFERENCES:
  81
  82                 [ 1]    McFarland, Richard E.: "A Standard Kinematic Model
  83                         for Flight Simulation at NASA-Ames", NASA CR-2497,
  84                         January 1975
  85
  86                 [ 2]    ANSI/AIAA R-004-1992 "Recommended Practice: Atmos-
  87                         pheric and Space Flight Vehicle Coordinate Systems",
  88                         February 1992
  89
  90
  91 ----------------------------------------------------------------------------
  92
  93         CALLED BY:
  94
  95 ----------------------------------------------------------------------------
  96
  97         CALLS TO:       None.
  98
  99 ----------------------------------------------------------------------------
 100
 101         INPUTS: State derivatives
 102
 103 ----------------------------------------------------------------------------
 104
 105         OUTPUTS:        States
 106
 107 --------------------------------------------------------------------------*/
 108
 109 #include "ls_types.h"
 110 #include "ls_constants.h"
 111 #include "ls_generic.h"
 112 #include "ls_accel.h"
 113 #include "ls_aux.h"
 114 #include "ls_model.h"
 115 #include "ls_step.h"
 116 #include "ls_geodesy.h"
 117 #include "ls_gravity.h"
 118 /* #include "ls_sim_control.h" */
 119 #include <math.h>
 120
 121 extern SCALAR Simtime;          /* defined in ls_main.c */
 122
 123 void ls_step( SCALAR dt, int Initialize ) {
 124         static  int     inited = 0;
 125                 SCALAR  dth;
 126         static  SCALAR  v_dot_north_past, v_dot_east_past, v_dot_down_past;
 127         static  SCALAR  latitude_dot_past, longitude_dot_past, radius_dot_past;
 128         static  SCALAR  p_dot_body_past, q_dot_body_past, r_dot_body_past;
 129                 SCALAR  p_local_in_body, q_local_in_body, r_local_in_body;
 130                 SCALAR  epsilon, inv_eps, local_gnd_veast;
 131                 SCALAR  e_dot_0, e_dot_1, e_dot_2, e_dot_3;
 132         static  SCALAR  e_0, e_1, e_2, e_3;
 133         static  SCALAR  e_dot_0_past, e_dot_1_past, e_dot_2_past, e_dot_3_past;
 134
 135 /*  I N I T I A L I Z A T I O N   */
 136
 137
 138         if ( (inited == 0) || (Initialize != 0) )
 139         {
 140 /* Set past values to zero */
 141         v_dot_north_past = v_dot_east_past = v_dot_down_past      = 0;
 142         latitude_dot_past = longitude_dot_past = radius_dot_past  = 0;
 143         p_dot_body_past = q_dot_body_past = r_dot_body_past       = 0;
 144         e_dot_0_past = e_dot_1_past = e_dot_2_past = e_dot_3_past = 0;
 145
 146 /* Initialize geocentric position from geodetic latitude and altitude */
 147
 148         ls_geod_to_geoc( Latitude, Altitude, &Sea_level_radius, &Lat_geocentric);
 149         Earth_position_angle = 0;
 150         Lon_geocentric = Longitude;
 151         Radius_to_vehicle = Altitude + Sea_level_radius;
 152
 153 /* Correct eastward velocity to account for earths' rotation, if necessary */
 154
 155         local_gnd_veast = OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric);
 156         if( fabs(V_east - V_east_rel_ground) < 0.8*local_gnd_veast )
 157         V_east = V_east + local_gnd_veast;
 158
 159 /* Initialize quaternions and transformation matrix from Euler angles */
 160
 161             e_0 = cos(Psi*0.5)*cos(Theta*0.5)*cos(Phi*0.5)
 162                 + sin(Psi*0.5)*sin(Theta*0.5)*sin(Phi*0.5);
 163             e_1 = cos(Psi*0.5)*cos(Theta*0.5)*sin(Phi*0.5)
 164                 - sin(Psi*0.5)*sin(Theta*0.5)*cos(Phi*0.5);
 165             e_2 = cos(Psi*0.5)*sin(Theta*0.5)*cos(Phi*0.5)
 166                 + sin(Psi*0.5)*cos(Theta*0.5)*sin(Phi*0.5);
 167             e_3 =-cos(Psi*0.5)*sin(Theta*0.5)*sin(Phi*0.5)
 168                 + sin(Psi*0.5)*cos(Theta*0.5)*cos(Phi*0.5);
 169             T_local_to_body_11 = e_0*e_0 + e_1*e_1 - e_2*e_2 - e_3*e_3;
 170             T_local_to_body_12 = 2*(e_1*e_2 + e_0*e_3);
 171             T_local_to_body_13 = 2*(e_1*e_3 - e_0*e_2);
 172             T_local_to_body_21 = 2*(e_1*e_2 - e_0*e_3);
 173             T_local_to_body_22 = e_0*e_0 - e_1*e_1 + e_2*e_2 - e_3*e_3;
 174             T_local_to_body_23 = 2*(e_2*e_3 + e_0*e_1);
 175             T_local_to_body_31 = 2*(e_1*e_3 + e_0*e_2);
 176             T_local_to_body_32 = 2*(e_2*e_3 - e_0*e_1);
 177             T_local_to_body_33 = e_0*e_0 - e_1*e_1 - e_2*e_2 + e_3*e_3;
 178
 179 /*      Calculate local gravitation acceleration        */
 180
 181                 ls_gravity( Radius_to_vehicle, Lat_geocentric, &Gravity );
 182
 183 /*      Initialize vehicle model                        */
 184
 185                 ls_aux();
 186                 ls_model(0.0, 0);
 187
 188 /*      Calculate initial accelerations */
 189
 190                 ls_accel();
 191
 192 /* Initialize auxiliary variables */
 193
 194                 ls_aux();
 195                 Alpha_dot = 0.;
 196                 Beta_dot = 0.;
 197
 198 /* set flag; disable integrators */
 199
 200                 inited = -1;
 201                 dt = 0;
 202
 203         }
 204
 205 /* Update time */
 206
 207         dth = 0.5*dt;
 208         Simtime = Simtime + dt;
 209
 210 /*  L I N E A R   V E L O C I T I E S   */
 211
 212 /* Integrate linear accelerations to get velocities */
 213 /*    Using predictive Adams-Bashford algorithm     */
 214
 215     V_north = V_north + dth*(3*V_dot_north - v_dot_north_past);
 216     V_east  = V_east  + dth*(3*V_dot_east  - v_dot_east_past );
 217     V_down  = V_down  + dth*(3*V_dot_down  - v_dot_down_past );
 218
 219 /* record past states */
 220
 221     v_dot_north_past = V_dot_north;
 222     v_dot_east_past  = V_dot_east;
 223     v_dot_down_past  = V_dot_down;
 224
 225 /* Calculate trajectory rate (geocentric coordinates) */
 226
 227     if (cos(Lat_geocentric) != 0)
 228         Longitude_dot = V_east/(Radius_to_vehicle*cos(Lat_geocentric));
 229
 230     Latitude_dot = V_north/Radius_to_vehicle;
 231     Radius_dot = -V_down;
 232
 233 /*  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  */
 234
 235 /* Integrate rotational accelerations to get velocities */
 236
 237     P_body = P_body + dth*(3*P_dot_body - p_dot_body_past);
 238     Q_body = Q_body + dth*(3*Q_dot_body - q_dot_body_past);
 239     R_body = R_body + dth*(3*R_dot_body - r_dot_body_past);
 240
 241 /* Save past states */
 242
 243     p_dot_body_past = P_dot_body;
 244     q_dot_body_past = Q_dot_body;
 245     r_dot_body_past = R_dot_body;
 246
 247 /* Calculate local axis frame rates due to travel over curved earth */
 248
 249     P_local =  V_east/Radius_to_vehicle;
 250     Q_local = -V_north/Radius_to_vehicle;
 251     R_local = -V_east*tan(Lat_geocentric)/Radius_to_vehicle;
 252
 253 /* Transform local axis frame rates to body axis rates */
 254
 255     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;
 256     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;
 257     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;
 258
 259 /* Calculate total angular rates in body axis */
 260
 261     P_total = P_body - p_local_in_body;
 262     Q_total = Q_body - q_local_in_body;
 263     R_total = R_body - r_local_in_body;
 264
 265 /* Transform to quaternion rates (see Appendix E in [2]) */
 266
 267     e_dot_0 = 0.5*( -P_total*e_1 - Q_total*e_2 - R_total*e_3 );
 268     e_dot_1 = 0.5*(  P_total*e_0 - Q_total*e_3 + R_total*e_2 );
 269     e_dot_2 = 0.5*(  P_total*e_3 + Q_total*e_0 - R_total*e_1 );
 270     e_dot_3 = 0.5*( -P_total*e_2 + Q_total*e_1 + R_total*e_0 );
 271
 272 /* Integrate using trapezoidal as before */
 273
 274         e_0 = e_0 + dth*(e_dot_0 + e_dot_0_past);
 275         e_1 = e_1 + dth*(e_dot_1 + e_dot_1_past);
 276         e_2 = e_2 + dth*(e_dot_2 + e_dot_2_past);
 277         e_3 = e_3 + dth*(e_dot_3 + e_dot_3_past);
 278
 279 /* calculate orthagonality correction  - scale quaternion to unity length */
 280
 281         epsilon = sqrt(e_0*e_0 + e_1*e_1 + e_2*e_2 + e_3*e_3);
 282         inv_eps = 1/epsilon;
 283
 284         e_0 = inv_eps*e_0;
 285         e_1 = inv_eps*e_1;
 286         e_2 = inv_eps*e_2;
 287         e_3 = inv_eps*e_3;
 288
 289 /* Save past values */
 290
 291         e_dot_0_past = e_dot_0;
 292         e_dot_1_past = e_dot_1;
 293         e_dot_2_past = e_dot_2;
 294         e_dot_3_past = e_dot_3;
 295
 296 /* Update local to body transformation matrix */
 297
 298         T_local_to_body_11 = e_0*e_0 + e_1*e_1 - e_2*e_2 - e_3*e_3;
 299         T_local_to_body_12 = 2*(e_1*e_2 + e_0*e_3);
 300         T_local_to_body_13 = 2*(e_1*e_3 - e_0*e_2);
 301         T_local_to_body_21 = 2*(e_1*e_2 - e_0*e_3);
 302         T_local_to_body_22 = e_0*e_0 - e_1*e_1 + e_2*e_2 - e_3*e_3;
 303         T_local_to_body_23 = 2*(e_2*e_3 + e_0*e_1);
 304         T_local_to_body_31 = 2*(e_1*e_3 + e_0*e_2);
 305         T_local_to_body_32 = 2*(e_2*e_3 - e_0*e_1);
 306         T_local_to_body_33 = e_0*e_0 - e_1*e_1 - e_2*e_2 + e_3*e_3;
 307
 308 /* Calculate Euler angles */
 309
 310         Theta = asin( -T_local_to_body_13 );
 311
 312         if( T_local_to_body_11 == 0 )
 313         Psi = 0;
 314         else
 315         Psi = atan2( T_local_to_body_12, T_local_to_body_11 );
 316
 317         if( T_local_to_body_33 == 0 )
 318         Phi = 0;
 319         else
 320         Phi = atan2( T_local_to_body_23, T_local_to_body_33 );
 321
 322 /* Resolve Psi to 0 - 359.9999 */
 323
 324         if (Psi < 0 ) Psi = Psi + 2*PI;
 325
 326 /*  L I N E A R   P O S I T I O N S   */
 327
 328 /* Trapezoidal acceleration for position */
 329
 330         Lat_geocentric    = Lat_geocentric    + dth*(Latitude_dot  + latitude_dot_past );
 331         Lon_geocentric    = Lon_geocentric    + dth*(Longitude_dot + longitude_dot_past);
 332         Radius_to_vehicle = Radius_to_vehicle + dth*(Radius_dot    + radius_dot_past );
 333         Earth_position_angle = Earth_position_angle + dt*OMEGA_EARTH;
 334
 335 /* Save past values */
 336
 337         latitude_dot_past  = Latitude_dot;
 338         longitude_dot_past = Longitude_dot;
 339         radius_dot_past    = Radius_dot;
 340
 341 /* end of ls_step */
 342 }
 343 /*************************************************************************/
 344