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.4 2003/06/20 19:53:56 ehofman
54 Get rid of a multiple defined symbol warning" src/FDM/LaRCsim/ls_step.c
57 Revision 1.3 2003/06/09 02:50:23 mselig
58 mods made to setup for some initializations in uiuc code
60 Revision 1.2 2003/05/25 12:14:46 ehofman
61 Rename some defines to prevent a namespace clash
63 Revision 1.1.1.1 2002/09/10 01:14:02 curt
64 Initial revision of FlightGear-0.9.0
66 Revision 1.5 2001/09/14 18:47:27 curt
67 More changes in support of UIUCModel.
69 Revision 1.4 2001/03/24 05:03:12 curt
72 Revision 1.3 2000/10/23 22:34:55 curt
74 LaRCsim c172 on-ground and in-air starts, reset: all work
75 UIUC Cessna172 on-ground and in-air starts work as expected, reset
76 results in an aircraft that is upside down but does not crash FG. I
77 don't know what it was like before, so it may well be no change.
78 JSBSim c172 and X15 in-air starts work fine, resets now work (and are
79 trimmed), on-ground starts do not -- the c172 ends up on its back. I
80 suspect this is no worse than before.
83 Balloon (the weather code returns nan's for the atmosphere data --this
84 is in the weather module and apparently is a linux only bug)
86 MagicCarpet (needs work yet)
87 External (don't know how)
90 LaRCsim c172 on-ground starts with a negative terrain altitude (this
91 happens at KPAO when the scenery is not present). The FDM inits to
92 about 50 feet AGL and the model falls to the ground. It does stay
93 upright, however, and seems to be fine once it settles out, FWIW.
96 --implement set_Model on the bus
97 --bring Christian's weather data into JSBSim
98 -- add default method to bus for updating things like the sin and cos of
99 latitude (for Balloon, MagicCarpet)
105 -- all data members now declared protected instead of private.
106 -- eliminated all but a small set of 'setters', no change to getters.
107 -- that small set is declared virtual, the default implementation
108 provided preserves the old behavior
109 -- all of the vector data members are now initialized.
110 -- added busdump() method -- SG_LOG's all the bus data when called,
111 useful for diagnostics.
114 -- bus data members now directly assigned to
117 -- bus data members now directly assigned to
118 -- changed V_equiv_kts to V_calibrated_kts
122 -- bus data members now directly assigned to
123 -- implemented the FGInterface virtual setters with JSBSim specific
125 -- changed the static FDMExec to a dynamic fdmex (needed so that the
126 JSBSim object can be deleted when a model change is called for)
127 -- implemented constructor and destructor, moved some of the logic
128 formerly in init() to constructor
129 -- added logic to bring up FGEngInterface objects and set the RPM and
134 -- bus data members now directly assigned to
135 -- implemented the FGInterface virtual setters with LaRCsim specific
136 logic, uses LaRCsimIC
137 -- implemented constructor and destructor, moved some of the logic
138 formerly in init() to constructor
139 -- moved default inertias to here from fg_init.cxx
140 -- eliminated the climb rate calculation. The equivalent, climb_rate =
141 -1*vdown, is now in copy_from_LaRCsim().
143 src/FDM/LaRCsimIC.cxx
144 src/FDM/LaRCsimIC.hxx
145 -- similar to FGInitialCondition, this class has all the logic needed to
146 turn data like Vc and Mach into the more fundamental quantities LaRCsim
148 -- put it in src/FDM since it is a class
150 src/FDM/MagicCarpet.cxx
151 -- bus data members now directly assigned to
154 -- adds LaRCsimIC.hxx and cxx
156 src/FDM/JSBSim/FGAtmosphere.h
157 src/FDM/JSBSim/FGDefs.h
158 src/FDM/JSBSim/FGInitialCondition.cpp
159 src/FDM/JSBSim/FGInitialCondition.h
160 src/FDM/JSBSim/JSBSim.cpp
161 -- changes to accomodate the new bus
163 src/FDM/LaRCsim/atmos_62.h
164 src/FDM/LaRCsim/ls_geodesy.h
165 -- surrounded prototypes with #ifdef __cplusplus ... #endif , functions
166 here are needed in LaRCsimIC
168 src/FDM/LaRCsim/c172_main.c
169 src/FDM/LaRCsim/cherokee_aero.c
170 src/FDM/LaRCsim/ls_aux.c
171 src/FDM/LaRCsim/ls_constants.h
172 src/FDM/LaRCsim/ls_geodesy.c
173 src/FDM/LaRCsim/ls_geodesy.h
174 src/FDM/LaRCsim/ls_step.c
175 src/FDM/UIUCModel/uiuc_betaprobe.cpp
176 -- changed PI to LS_PI, eliminates preprocessor naming conflict with
179 src/FDM/LaRCsim/ls_interface.c
180 src/FDM/LaRCsim/ls_interface.h
181 -- added function ls_set_model_dt()
184 -- eliminated calls that set the NED speeds to body components. They
185 are no longer needed and confuse the new bus.
188 -- eliminated calls that just brought the bus data up-to-date (e.g.
189 set_sin_cos_latitude). or set default values. The bus now handles the
190 defaults and updates itself when the setters are called (for LaRCsim and
191 JSBSim). A default method for doing this needs to be added to the bus.
192 -- added fgVelocityInit() to set the speed the user asked for. Both
193 JSBSim and LaRCsim can now be initialized using any of:
194 vc,mach, NED components, UVW components.
197 --eliminated call to fgFDMSetGroundElevation, this data is now 'pulled'
198 onto the bus every update()
202 -- added enum to keep track of the speed requested by the user
203 -- eliminated calls to set NED velocity properties to body speeds, they
204 are no longer needed.
205 -- added options for the NED components.
207 src/Network/garmin.cxx
209 --eliminated calls that just brought the bus data up-to-date (e.g.
210 set_sin_cos_latitude). The bus now updates itself when the setters are
211 called (for LaRCsim and JSBSim). A default method for doing this needs
212 to be added to the bus.
213 -- changed set_V_equiv_kts to set_V_calibrated_kts. set_V_equiv_kts no
214 longer exists ( get_V_equiv_kts still does, though)
216 src/WeatherCM/FGLocalWeatherDatabase.cpp
217 -- commented out the code to put the weather data on the bus, a
218 different scheme for this is needed.
220 Revision 1.2 1999/10/29 16:08:33 curt
221 Added flaps support to c172 model.
223 Revision 1.1.1.1 1999/06/17 18:07:33 curt
224 Start of 0.7.x branch
226 Revision 1.1.1.1 1999/04/05 21:32:45 curt
227 Start of 0.6.x branch.
229 Revision 1.4 1998/08/24 20:09:27 curt
230 Code optimization tweaks from Norman Vine.
232 Revision 1.3 1998/07/12 03:11:04 curt
233 Removed some printf()'s.
234 Fixed the autopilot integration so it should be able to update it's control
235 positions every time the internal flight model loop is run, and not just
236 once per rendered frame.
237 Added a routine to do the necessary stuff to force an arbitrary altitude
239 Gave the Navion engine just a tad more power.
241 Revision 1.2 1998/01/19 18:40:28 curt
242 Tons of little changes to clean up the code and to remove fatal errors
243 when building with the c++ compiler.
245 Revision 1.1 1997/05/29 00:09:59 curt
246 Initial Flight Gear revision.
248 * Revision 1.5 1995/03/02 20:24:13 bjax
249 * Added logic to avoid adding additional increment to V_east
250 * in case V_east already accounts for rotating earth. EBJ
252 * Revision 1.4 1995/02/07 20:52:21 bjax
253 * Added initialization of Alpha_dot and Beta_dot to zero on first
254 * pass; they get calculated by ls_aux on next pass... EBJ
256 * Revision 1.3 1994/01/11 19:01:12 bjax
257 * Changed from DATA to SCALAR type; also fixed header files (was ls_eom.h)
259 * Revision 1.2 1993/06/02 15:03:09 bjax
260 * Moved initialization of geocentric position to subroutine ls_geod_to_geoc.
262 * Revision 1.1 92/12/30 13:16:11 bjax
266 ----------------------------------------------------------------------------
270 [ 1] McFarland, Richard E.: "A Standard Kinematic Model
271 for Flight Simulation at NASA-Ames", NASA CR-2497,
274 [ 2] ANSI/AIAA R-004-1992 "Recommended Practice: Atmos-
275 pheric and Space Flight Vehicle Coordinate Systems",
279 ----------------------------------------------------------------------------
283 ----------------------------------------------------------------------------
287 ----------------------------------------------------------------------------
289 INPUTS: State derivatives
291 ----------------------------------------------------------------------------
295 --------------------------------------------------------------------------*/
297 #include <FDM/UIUCModel/uiuc_wrapper.h>
299 #include "ls_types.h"
300 #include "ls_constants.h"
301 #include "ls_generic.h"
302 #include "ls_accel.h"
304 #include "ls_model.h"
306 #include "ls_geodesy.h"
307 #include "ls_gravity.h"
308 /* #include "ls_sim_control.h" */
311 extern Model current_model; /* defined in ls_model.c */
312 extern SCALAR Simtime; /* defined in ls_main.c */
314 void uiuc_init_vars() {
319 uiuc_init_aeromodel();
326 void ls_step( SCALAR dt, int Initialize ) {
327 static int inited = 0;
329 static SCALAR v_dot_north_past, v_dot_east_past, v_dot_down_past;
330 static SCALAR latitude_dot_past, longitude_dot_past, radius_dot_past;
331 static SCALAR p_dot_body_past, q_dot_body_past, r_dot_body_past;
332 SCALAR p_local_in_body, q_local_in_body, r_local_in_body;
333 SCALAR epsilon, inv_eps, local_gnd_veast;
334 SCALAR e_dot_0, e_dot_1, e_dot_2, e_dot_3;
335 static SCALAR e_0, e_1, e_2, e_3;
336 static SCALAR e_dot_0_past, e_dot_1_past, e_dot_2_past, e_dot_3_past;
337 SCALAR cos_Lat_geocentric, inv_Radius_to_vehicle;
339 /* I N I T I A L I Z A T I O N */
342 if ( (inited == 0) || (Initialize != 0) )
344 /* Set past values to zero */
345 v_dot_north_past = v_dot_east_past = v_dot_down_past = 0;
346 latitude_dot_past = longitude_dot_past = radius_dot_past = 0;
347 p_dot_body_past = q_dot_body_past = r_dot_body_past = 0;
348 e_dot_0_past = e_dot_1_past = e_dot_2_past = e_dot_3_past = 0;
350 /* Initialize geocentric position from geodetic latitude and altitude */
352 ls_geod_to_geoc( Latitude, Altitude, &Sea_level_radius, &Lat_geocentric);
353 Earth_position_angle = 0;
354 Lon_geocentric = Longitude;
355 Radius_to_vehicle = Altitude + Sea_level_radius;
357 /* Correct eastward velocity to account for earths' rotation, if necessary */
359 local_gnd_veast = OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric);
360 if( fabs(V_east - V_east_rel_ground) < 0.8*local_gnd_veast )
361 V_east = V_east + local_gnd_veast;
363 /* Initialize quaternions and transformation matrix from Euler angles */
364 if (current_model == UIUC && Simtime == 0) {
366 uiuc_defaults_inits();
371 e_0 = cos(Psi*0.5)*cos(Theta*0.5)*cos(Phi*0.5)
372 + sin(Psi*0.5)*sin(Theta*0.5)*sin(Phi*0.5);
373 e_1 = cos(Psi*0.5)*cos(Theta*0.5)*sin(Phi*0.5)
374 - sin(Psi*0.5)*sin(Theta*0.5)*cos(Phi*0.5);
375 e_2 = cos(Psi*0.5)*sin(Theta*0.5)*cos(Phi*0.5)
376 + sin(Psi*0.5)*cos(Theta*0.5)*sin(Phi*0.5);
377 e_3 =-cos(Psi*0.5)*sin(Theta*0.5)*sin(Phi*0.5)
378 + sin(Psi*0.5)*cos(Theta*0.5)*cos(Phi*0.5);
379 T_local_to_body_11 = e_0*e_0 + e_1*e_1 - e_2*e_2 - e_3*e_3;
380 T_local_to_body_12 = 2*(e_1*e_2 + e_0*e_3);
381 T_local_to_body_13 = 2*(e_1*e_3 - e_0*e_2);
382 T_local_to_body_21 = 2*(e_1*e_2 - e_0*e_3);
383 T_local_to_body_22 = e_0*e_0 - e_1*e_1 + e_2*e_2 - e_3*e_3;
384 T_local_to_body_23 = 2*(e_2*e_3 + e_0*e_1);
385 T_local_to_body_31 = 2*(e_1*e_3 + e_0*e_2);
386 T_local_to_body_32 = 2*(e_2*e_3 - e_0*e_1);
387 T_local_to_body_33 = e_0*e_0 - e_1*e_1 - e_2*e_2 + e_3*e_3;
389 if (current_model == UIUC && Simtime == 0) {
393 /* Calculate local gravitation acceleration */
395 ls_gravity( Radius_to_vehicle, Lat_geocentric, &Gravity );
397 /* Initialize vehicle model */
402 /* Calculate initial accelerations */
406 /* Initialize auxiliary variables */
412 /* set flag; disable integrators */
422 Simtime = Simtime + dt;
424 /* L I N E A R V E L O C I T I E S */
426 /* Integrate linear accelerations to get velocities */
427 /* Using predictive Adams-Bashford algorithm */
429 V_north = V_north + dth*(3*V_dot_north - v_dot_north_past);
430 V_east = V_east + dth*(3*V_dot_east - v_dot_east_past );
431 V_down = V_down + dth*(3*V_dot_down - v_dot_down_past );
433 /* record past states */
435 v_dot_north_past = V_dot_north;
436 v_dot_east_past = V_dot_east;
437 v_dot_down_past = V_dot_down;
439 /* Calculate trajectory rate (geocentric coordinates) */
441 inv_Radius_to_vehicle = 1.0/Radius_to_vehicle;
442 cos_Lat_geocentric = cos(Lat_geocentric);
444 if ( cos_Lat_geocentric != 0) {
445 Longitude_dot = V_east/(Radius_to_vehicle*cos_Lat_geocentric);
448 Latitude_dot = V_north*inv_Radius_to_vehicle;
449 Radius_dot = -V_down;
451 /* 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 */
453 /* Integrate rotational accelerations to get velocities */
455 P_body = P_body + dth*(3*P_dot_body - p_dot_body_past);
456 Q_body = Q_body + dth*(3*Q_dot_body - q_dot_body_past);
457 R_body = R_body + dth*(3*R_dot_body - r_dot_body_past);
459 /* Save past states */
461 p_dot_body_past = P_dot_body;
462 q_dot_body_past = Q_dot_body;
463 r_dot_body_past = R_dot_body;
465 /* Calculate local axis frame rates due to travel over curved earth */
467 P_local = V_east*inv_Radius_to_vehicle;
468 Q_local = -V_north*inv_Radius_to_vehicle;
469 R_local = -V_east*tan(Lat_geocentric)*inv_Radius_to_vehicle;
471 /* Transform local axis frame rates to body axis rates */
473 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;
474 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;
475 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;
477 /* Calculate total angular rates in body axis */
479 P_total = P_body - p_local_in_body;
480 Q_total = Q_body - q_local_in_body;
481 R_total = R_body - r_local_in_body;
483 /* Transform to quaternion rates (see Appendix E in [2]) */
485 e_dot_0 = 0.5*( -P_total*e_1 - Q_total*e_2 - R_total*e_3 );
486 e_dot_1 = 0.5*( P_total*e_0 - Q_total*e_3 + R_total*e_2 );
487 e_dot_2 = 0.5*( P_total*e_3 + Q_total*e_0 - R_total*e_1 );
488 e_dot_3 = 0.5*( -P_total*e_2 + Q_total*e_1 + R_total*e_0 );
490 /* Integrate using trapezoidal as before */
492 e_0 = e_0 + dth*(e_dot_0 + e_dot_0_past);
493 e_1 = e_1 + dth*(e_dot_1 + e_dot_1_past);
494 e_2 = e_2 + dth*(e_dot_2 + e_dot_2_past);
495 e_3 = e_3 + dth*(e_dot_3 + e_dot_3_past);
497 /* calculate orthagonality correction - scale quaternion to unity length */
499 epsilon = sqrt(e_0*e_0 + e_1*e_1 + e_2*e_2 + e_3*e_3);
507 /* Save past values */
509 e_dot_0_past = e_dot_0;
510 e_dot_1_past = e_dot_1;
511 e_dot_2_past = e_dot_2;
512 e_dot_3_past = e_dot_3;
514 /* Update local to body transformation matrix */
516 T_local_to_body_11 = e_0*e_0 + e_1*e_1 - e_2*e_2 - e_3*e_3;
517 T_local_to_body_12 = 2*(e_1*e_2 + e_0*e_3);
518 T_local_to_body_13 = 2*(e_1*e_3 - e_0*e_2);
519 T_local_to_body_21 = 2*(e_1*e_2 - e_0*e_3);
520 T_local_to_body_22 = e_0*e_0 - e_1*e_1 + e_2*e_2 - e_3*e_3;
521 T_local_to_body_23 = 2*(e_2*e_3 + e_0*e_1);
522 T_local_to_body_31 = 2*(e_1*e_3 + e_0*e_2);
523 T_local_to_body_32 = 2*(e_2*e_3 - e_0*e_1);
524 T_local_to_body_33 = e_0*e_0 - e_1*e_1 - e_2*e_2 + e_3*e_3;
526 /* Calculate Euler angles */
528 Theta = asin( -T_local_to_body_13 );
530 if( T_local_to_body_11 == 0 )
533 Psi = atan2( T_local_to_body_12, T_local_to_body_11 );
535 if( T_local_to_body_33 == 0 )
538 Phi = atan2( T_local_to_body_23, T_local_to_body_33 );
540 /* Resolve Psi to 0 - 359.9999 */
542 if (Psi < 0 ) Psi = Psi + 2*LS_PI;
544 /* L I N E A R P O S I T I O N S */
546 /* Trapezoidal acceleration for position */
548 Lat_geocentric = Lat_geocentric + dth*(Latitude_dot + latitude_dot_past );
549 Lon_geocentric = Lon_geocentric + dth*(Longitude_dot + longitude_dot_past);
550 Radius_to_vehicle = Radius_to_vehicle + dth*(Radius_dot + radius_dot_past );
551 Earth_position_angle = Earth_position_angle + dt*OMEGA_EARTH;
553 /* Save past values */
555 latitude_dot_past = Latitude_dot;
556 longitude_dot_past = Longitude_dot;
557 radius_dot_past = Radius_dot;
561 /*************************************************************************/