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