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.2 1999/10/29 16:08:33 curt
54 Added flaps support to c172 model.
56 Revision 1.1.1.1 1999/06/17 18:07:33 curt
59 Revision 1.1.1.1 1999/04/05 21:32:45 curt
60 Start of 0.6.x branch.
62 Revision 1.4 1998/08/24 20:09:27 curt
63 Code optimization tweaks from Norman Vine.
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
72 Gave the Navion engine just a tad more power.
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.
78 Revision 1.1 1997/05/29 00:09:59 curt
79 Initial Flight Gear revision.
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
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
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)
92 * Revision 1.2 1993/06/02 15:03:09 bjax
93 * Moved initialization of geocentric position to subroutine ls_geod_to_geoc.
95 * Revision 1.1 92/12/30 13:16:11 bjax
99 ----------------------------------------------------------------------------
103 [ 1] McFarland, Richard E.: "A Standard Kinematic Model
104 for Flight Simulation at NASA-Ames", NASA CR-2497,
107 [ 2] ANSI/AIAA R-004-1992 "Recommended Practice: Atmos-
108 pheric and Space Flight Vehicle Coordinate Systems",
112 ----------------------------------------------------------------------------
116 ----------------------------------------------------------------------------
120 ----------------------------------------------------------------------------
122 INPUTS: State derivatives
124 ----------------------------------------------------------------------------
128 --------------------------------------------------------------------------*/
130 #include "ls_types.h"
131 #include "ls_constants.h"
132 #include "ls_generic.h"
133 #include "ls_accel.h"
135 #include "ls_model.h"
137 #include "ls_geodesy.h"
138 #include "ls_gravity.h"
139 /* #include "ls_sim_control.h" */
142 extern SCALAR Simtime; /* defined in ls_main.c */
144 void ls_step( SCALAR dt, int Initialize ) {
145 static int inited = 0;
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;
157 /* I N I T I A L I Z A T I O N */
160 if ( (inited == 0) || (Initialize != 0) )
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;
168 /* Initialize geocentric position from geodetic latitude and altitude */
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;
175 /* Correct eastward velocity to account for earths' rotation, if necessary */
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;
181 /* Initialize quaternions and transformation matrix from Euler angles */
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;
201 /* Calculate local gravitation acceleration */
203 ls_gravity( Radius_to_vehicle, Lat_geocentric, &Gravity );
205 /* Initialize vehicle model */
210 /* Calculate initial accelerations */
214 /* Initialize auxiliary variables */
220 /* set flag; disable integrators */
230 Simtime = Simtime + dt;
232 /* L I N E A R V E L O C I T I E S */
234 /* Integrate linear accelerations to get velocities */
235 /* Using predictive Adams-Bashford algorithm */
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 );
241 /* record past states */
243 v_dot_north_past = V_dot_north;
244 v_dot_east_past = V_dot_east;
245 v_dot_down_past = V_dot_down;
247 /* Calculate trajectory rate (geocentric coordinates) */
249 inv_Radius_to_vehicle = 1.0/Radius_to_vehicle;
250 cos_Lat_geocentric = cos(Lat_geocentric);
252 if ( cos_Lat_geocentric != 0) {
253 Longitude_dot = V_east/(Radius_to_vehicle*cos_Lat_geocentric);
256 Latitude_dot = V_north*inv_Radius_to_vehicle;
257 Radius_dot = -V_down;
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 */
261 /* Integrate rotational accelerations to get velocities */
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);
267 /* Save past states */
269 p_dot_body_past = P_dot_body;
270 q_dot_body_past = Q_dot_body;
271 r_dot_body_past = R_dot_body;
273 /* Calculate local axis frame rates due to travel over curved earth */
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;
279 /* Transform local axis frame rates to body axis rates */
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;
285 /* Calculate total angular rates in body axis */
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;
291 /* Transform to quaternion rates (see Appendix E in [2]) */
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 );
298 /* Integrate using trapezoidal as before */
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);
305 /* calculate orthagonality correction - scale quaternion to unity length */
307 epsilon = sqrt(e_0*e_0 + e_1*e_1 + e_2*e_2 + e_3*e_3);
315 /* Save past values */
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;
322 /* Update local to body transformation matrix */
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;
334 /* Calculate Euler angles */
336 Theta = asin( -T_local_to_body_13 );
338 if( T_local_to_body_11 == 0 )
341 Psi = atan2( T_local_to_body_12, T_local_to_body_11 );
343 if( T_local_to_body_33 == 0 )
346 Phi = atan2( T_local_to_body_23, T_local_to_body_33 );
348 /* Resolve Psi to 0 - 359.9999 */
350 if (Psi < 0 ) Psi = Psi + 2*PI;
352 /* L I N E A R P O S I T I O N S */
354 /* Trapezoidal acceleration for position */
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;
361 /* Save past values */
363 latitude_dot_past = Latitude_dot;
364 longitude_dot_past = Longitude_dot;
365 radius_dot_past = Radius_dot;
369 /*************************************************************************/