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