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