1 /***************************************************************************
5 ----------------------------------------------------------------------------
7 FUNCTION: Integration routine for equations of motion
10 ----------------------------------------------------------------------------
12 MODULE STATUS: developmental
14 ----------------------------------------------------------------------------
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.
21 ----------------------------------------------------------------------------
23 DESIGNED BY: Bruce Jackson
25 CODED BY: Bruce Jackson
29 ----------------------------------------------------------------------------
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
39 940111 Changed from oldstyle include file ls_eom.h; also changed
40 from DATA to SCALAR type. EBJ
42 950207 Initialized Alpha_dot and Beta_dot to zero on first pass; calculated
45 950224 Added logic to avoid adding additional increment to V_east
46 in case V_east already accounts for rotating earth.
53 Revision 1.1 1999/04/05 21:32:45 curt
56 Revision 1.4 1998/08/24 20:09:27 curt
57 Code optimization tweaks from Norman Vine.
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
66 Gave the Navion engine just a tad more power.
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.
72 Revision 1.1 1997/05/29 00:09:59 curt
73 Initial Flight Gear revision.
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
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
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)
86 * Revision 1.2 1993/06/02 15:03:09 bjax
87 * Moved initialization of geocentric position to subroutine ls_geod_to_geoc.
89 * Revision 1.1 92/12/30 13:16:11 bjax
93 ----------------------------------------------------------------------------
97 [ 1] McFarland, Richard E.: "A Standard Kinematic Model
98 for Flight Simulation at NASA-Ames", NASA CR-2497,
101 [ 2] ANSI/AIAA R-004-1992 "Recommended Practice: Atmos-
102 pheric and Space Flight Vehicle Coordinate Systems",
106 ----------------------------------------------------------------------------
110 ----------------------------------------------------------------------------
114 ----------------------------------------------------------------------------
116 INPUTS: State derivatives
118 ----------------------------------------------------------------------------
122 --------------------------------------------------------------------------*/
124 #include "ls_types.h"
125 #include "ls_constants.h"
126 #include "ls_generic.h"
127 #include "ls_accel.h"
129 #include "ls_model.h"
131 #include "ls_geodesy.h"
132 #include "ls_gravity.h"
133 /* #include "ls_sim_control.h" */
136 extern SCALAR Simtime; /* defined in ls_main.c */
138 void ls_step( SCALAR dt, int Initialize ) {
139 static int inited = 0;
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;
151 /* I N I T I A L I Z A T I O N */
154 if ( (inited == 0) || (Initialize != 0) )
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;
162 /* Initialize geocentric position from geodetic latitude and altitude */
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;
169 /* Correct eastward velocity to account for earths' rotation, if necessary */
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;
175 /* Initialize quaternions and transformation matrix from Euler angles */
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;
195 /* Calculate local gravitation acceleration */
197 ls_gravity( Radius_to_vehicle, Lat_geocentric, &Gravity );
199 /* Initialize vehicle model */
204 /* Calculate initial accelerations */
208 /* Initialize auxiliary variables */
214 /* set flag; disable integrators */
224 Simtime = Simtime + dt;
226 /* L I N E A R V E L O C I T I E S */
228 /* Integrate linear accelerations to get velocities */
229 /* Using predictive Adams-Bashford algorithm */
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 );
235 /* record past states */
237 v_dot_north_past = V_dot_north;
238 v_dot_east_past = V_dot_east;
239 v_dot_down_past = V_dot_down;
241 /* Calculate trajectory rate (geocentric coordinates) */
243 inv_Radius_to_vehicle = 1.0/Radius_to_vehicle;
244 cos_Lat_geocentric = cos(Lat_geocentric);
246 if ( cos_Lat_geocentric != 0) {
247 Longitude_dot = V_east/(Radius_to_vehicle*cos_Lat_geocentric);
250 Latitude_dot = V_north*inv_Radius_to_vehicle;
251 Radius_dot = -V_down;
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 */
255 /* Integrate rotational accelerations to get velocities */
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);
261 /* Save past states */
263 p_dot_body_past = P_dot_body;
264 q_dot_body_past = Q_dot_body;
265 r_dot_body_past = R_dot_body;
267 /* Calculate local axis frame rates due to travel over curved earth */
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;
273 /* Transform local axis frame rates to body axis rates */
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;
279 /* Calculate total angular rates in body axis */
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;
285 /* Transform to quaternion rates (see Appendix E in [2]) */
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 );
292 /* Integrate using trapezoidal as before */
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);
299 /* calculate orthagonality correction - scale quaternion to unity length */
301 epsilon = sqrt(e_0*e_0 + e_1*e_1 + e_2*e_2 + e_3*e_3);
309 /* Save past values */
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;
316 /* Update local to body transformation matrix */
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;
328 /* Calculate Euler angles */
330 Theta = asin( -T_local_to_body_13 );
332 if( T_local_to_body_11 == 0 )
335 Psi = atan2( T_local_to_body_12, T_local_to_body_11 );
337 if( T_local_to_body_33 == 0 )
340 Phi = atan2( T_local_to_body_23, T_local_to_body_33 );
342 /* Resolve Psi to 0 - 359.9999 */
344 if (Psi < 0 ) Psi = Psi + 2*PI;
346 /* L I N E A R P O S I T I O N S */
348 /* Trapezoidal acceleration for position */
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;
355 /* Save past values */
357 latitude_dot_past = Latitude_dot;
358 longitude_dot_past = Longitude_dot;
359 radius_dot_past = Radius_dot;
363 /*************************************************************************/