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