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 1998/01/19 18:40:28 curt
54 Tons of little changes to clean up the code and to remove fatal errors
55 when building with the c++ compiler.
57 Revision 1.1 1997/05/29 00:09:59 curt
58 Initial Flight Gear revision.
60 * Revision 1.5 1995/03/02 20:24:13 bjax
61 * Added logic to avoid adding additional increment to V_east
62 * in case V_east already accounts for rotating earth. EBJ
64 * Revision 1.4 1995/02/07 20:52:21 bjax
65 * Added initialization of Alpha_dot and Beta_dot to zero on first
66 * pass; they get calculated by ls_aux on next pass... EBJ
68 * Revision 1.3 1994/01/11 19:01:12 bjax
69 * Changed from DATA to SCALAR type; also fixed header files (was ls_eom.h)
71 * Revision 1.2 1993/06/02 15:03:09 bjax
72 * Moved initialization of geocentric position to subroutine ls_geod_to_geoc.
74 * Revision 1.1 92/12/30 13:16:11 bjax
78 ----------------------------------------------------------------------------
82 [ 1] McFarland, Richard E.: "A Standard Kinematic Model
83 for Flight Simulation at NASA-Ames", NASA CR-2497,
86 [ 2] ANSI/AIAA R-004-1992 "Recommended Practice: Atmos-
87 pheric and Space Flight Vehicle Coordinate Systems",
91 ----------------------------------------------------------------------------
95 ----------------------------------------------------------------------------
99 ----------------------------------------------------------------------------
101 INPUTS: State derivatives
103 ----------------------------------------------------------------------------
107 --------------------------------------------------------------------------*/
109 #include "ls_types.h"
110 #include "ls_constants.h"
111 #include "ls_generic.h"
112 #include "ls_accel.h"
114 #include "ls_model.h"
116 #include "ls_geodesy.h"
117 #include "ls_gravity.h"
118 /* #include "ls_sim_control.h" */
121 extern SCALAR Simtime; /* defined in ls_main.c */
123 void ls_step( SCALAR dt, int Initialize ) {
124 static int inited = 0;
126 static SCALAR v_dot_north_past, v_dot_east_past, v_dot_down_past;
127 static SCALAR latitude_dot_past, longitude_dot_past, radius_dot_past;
128 static SCALAR p_dot_body_past, q_dot_body_past, r_dot_body_past;
129 SCALAR p_local_in_body, q_local_in_body, r_local_in_body;
130 SCALAR epsilon, inv_eps, local_gnd_veast;
131 SCALAR e_dot_0, e_dot_1, e_dot_2, e_dot_3;
132 static SCALAR e_0, e_1, e_2, e_3;
133 static SCALAR e_dot_0_past, e_dot_1_past, e_dot_2_past, e_dot_3_past;
135 /* I N I T I A L I Z A T I O N */
138 if ( (inited == 0) || (Initialize != 0) )
140 /* Set past values to zero */
141 v_dot_north_past = v_dot_east_past = v_dot_down_past = 0;
142 latitude_dot_past = longitude_dot_past = radius_dot_past = 0;
143 p_dot_body_past = q_dot_body_past = r_dot_body_past = 0;
144 e_dot_0_past = e_dot_1_past = e_dot_2_past = e_dot_3_past = 0;
146 /* Initialize geocentric position from geodetic latitude and altitude */
148 ls_geod_to_geoc( Latitude, Altitude, &Sea_level_radius, &Lat_geocentric);
149 Earth_position_angle = 0;
150 Lon_geocentric = Longitude;
151 Radius_to_vehicle = Altitude + Sea_level_radius;
153 /* Correct eastward velocity to account for earths' rotation, if necessary */
155 local_gnd_veast = OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric);
156 if( fabs(V_east - V_east_rel_ground) < 0.8*local_gnd_veast )
157 V_east = V_east + local_gnd_veast;
159 /* Initialize quaternions and transformation matrix from Euler angles */
161 e_0 = cos(Psi*0.5)*cos(Theta*0.5)*cos(Phi*0.5)
162 + sin(Psi*0.5)*sin(Theta*0.5)*sin(Phi*0.5);
163 e_1 = cos(Psi*0.5)*cos(Theta*0.5)*sin(Phi*0.5)
164 - sin(Psi*0.5)*sin(Theta*0.5)*cos(Phi*0.5);
165 e_2 = cos(Psi*0.5)*sin(Theta*0.5)*cos(Phi*0.5)
166 + sin(Psi*0.5)*cos(Theta*0.5)*sin(Phi*0.5);
167 e_3 =-cos(Psi*0.5)*sin(Theta*0.5)*sin(Phi*0.5)
168 + sin(Psi*0.5)*cos(Theta*0.5)*cos(Phi*0.5);
169 T_local_to_body_11 = e_0*e_0 + e_1*e_1 - e_2*e_2 - e_3*e_3;
170 T_local_to_body_12 = 2*(e_1*e_2 + e_0*e_3);
171 T_local_to_body_13 = 2*(e_1*e_3 - e_0*e_2);
172 T_local_to_body_21 = 2*(e_1*e_2 - e_0*e_3);
173 T_local_to_body_22 = e_0*e_0 - e_1*e_1 + e_2*e_2 - e_3*e_3;
174 T_local_to_body_23 = 2*(e_2*e_3 + e_0*e_1);
175 T_local_to_body_31 = 2*(e_1*e_3 + e_0*e_2);
176 T_local_to_body_32 = 2*(e_2*e_3 - e_0*e_1);
177 T_local_to_body_33 = e_0*e_0 - e_1*e_1 - e_2*e_2 + e_3*e_3;
179 /* Calculate local gravitation acceleration */
181 ls_gravity( Radius_to_vehicle, Lat_geocentric, &Gravity );
183 /* Initialize vehicle model */
188 /* Calculate initial accelerations */
192 /* Initialize auxiliary variables */
198 /* set flag; disable integrators */
208 Simtime = Simtime + dt;
210 /* L I N E A R V E L O C I T I E S */
212 /* Integrate linear accelerations to get velocities */
213 /* Using predictive Adams-Bashford algorithm */
215 V_north = V_north + dth*(3*V_dot_north - v_dot_north_past);
216 V_east = V_east + dth*(3*V_dot_east - v_dot_east_past );
217 V_down = V_down + dth*(3*V_dot_down - v_dot_down_past );
219 /* record past states */
221 v_dot_north_past = V_dot_north;
222 v_dot_east_past = V_dot_east;
223 v_dot_down_past = V_dot_down;
225 /* Calculate trajectory rate (geocentric coordinates) */
227 if (cos(Lat_geocentric) != 0)
228 Longitude_dot = V_east/(Radius_to_vehicle*cos(Lat_geocentric));
230 Latitude_dot = V_north/Radius_to_vehicle;
231 Radius_dot = -V_down;
233 /* 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 */
235 /* Integrate rotational accelerations to get velocities */
237 P_body = P_body + dth*(3*P_dot_body - p_dot_body_past);
238 Q_body = Q_body + dth*(3*Q_dot_body - q_dot_body_past);
239 R_body = R_body + dth*(3*R_dot_body - r_dot_body_past);
241 /* Save past states */
243 p_dot_body_past = P_dot_body;
244 q_dot_body_past = Q_dot_body;
245 r_dot_body_past = R_dot_body;
247 /* Calculate local axis frame rates due to travel over curved earth */
249 P_local = V_east/Radius_to_vehicle;
250 Q_local = -V_north/Radius_to_vehicle;
251 R_local = -V_east*tan(Lat_geocentric)/Radius_to_vehicle;
253 /* Transform local axis frame rates to body axis rates */
255 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;
256 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;
257 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;
259 /* Calculate total angular rates in body axis */
261 P_total = P_body - p_local_in_body;
262 Q_total = Q_body - q_local_in_body;
263 R_total = R_body - r_local_in_body;
265 /* Transform to quaternion rates (see Appendix E in [2]) */
267 e_dot_0 = 0.5*( -P_total*e_1 - Q_total*e_2 - R_total*e_3 );
268 e_dot_1 = 0.5*( P_total*e_0 - Q_total*e_3 + R_total*e_2 );
269 e_dot_2 = 0.5*( P_total*e_3 + Q_total*e_0 - R_total*e_1 );
270 e_dot_3 = 0.5*( -P_total*e_2 + Q_total*e_1 + R_total*e_0 );
272 /* Integrate using trapezoidal as before */
274 e_0 = e_0 + dth*(e_dot_0 + e_dot_0_past);
275 e_1 = e_1 + dth*(e_dot_1 + e_dot_1_past);
276 e_2 = e_2 + dth*(e_dot_2 + e_dot_2_past);
277 e_3 = e_3 + dth*(e_dot_3 + e_dot_3_past);
279 /* calculate orthagonality correction - scale quaternion to unity length */
281 epsilon = sqrt(e_0*e_0 + e_1*e_1 + e_2*e_2 + e_3*e_3);
289 /* Save past values */
291 e_dot_0_past = e_dot_0;
292 e_dot_1_past = e_dot_1;
293 e_dot_2_past = e_dot_2;
294 e_dot_3_past = e_dot_3;
296 /* Update local to body transformation matrix */
298 T_local_to_body_11 = e_0*e_0 + e_1*e_1 - e_2*e_2 - e_3*e_3;
299 T_local_to_body_12 = 2*(e_1*e_2 + e_0*e_3);
300 T_local_to_body_13 = 2*(e_1*e_3 - e_0*e_2);
301 T_local_to_body_21 = 2*(e_1*e_2 - e_0*e_3);
302 T_local_to_body_22 = e_0*e_0 - e_1*e_1 + e_2*e_2 - e_3*e_3;
303 T_local_to_body_23 = 2*(e_2*e_3 + e_0*e_1);
304 T_local_to_body_31 = 2*(e_1*e_3 + e_0*e_2);
305 T_local_to_body_32 = 2*(e_2*e_3 - e_0*e_1);
306 T_local_to_body_33 = e_0*e_0 - e_1*e_1 - e_2*e_2 + e_3*e_3;
308 /* Calculate Euler angles */
310 Theta = asin( -T_local_to_body_13 );
312 if( T_local_to_body_11 == 0 )
315 Psi = atan2( T_local_to_body_12, T_local_to_body_11 );
317 if( T_local_to_body_33 == 0 )
320 Phi = atan2( T_local_to_body_23, T_local_to_body_33 );
322 /* Resolve Psi to 0 - 359.9999 */
324 if (Psi < 0 ) Psi = Psi + 2*PI;
326 /* L I N E A R P O S I T I O N S */
328 /* Trapezoidal acceleration for position */
330 Lat_geocentric = Lat_geocentric + dth*(Latitude_dot + latitude_dot_past );
331 Lon_geocentric = Lon_geocentric + dth*(Longitude_dot + longitude_dot_past);
332 Radius_to_vehicle = Radius_to_vehicle + dth*(Radius_dot + radius_dot_past );
333 Earth_position_angle = Earth_position_angle + dt*OMEGA_EARTH;
335 /* Save past values */
337 latitude_dot_past = Latitude_dot;
338 longitude_dot_past = Longitude_dot;
339 radius_dot_past = Radius_dot;
343 /*************************************************************************/