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