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