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.5 2001/09/14 18:47:27 curt
54 More changes in support of UIUCModel.
56 Revision 1.4 2001/03/24 05:03:12 curt
59 Revision 1.3 2000/10/23 22:34:55 curt
61 LaRCsim c172 on-ground and in-air starts, reset: all work
62 UIUC Cessna172 on-ground and in-air starts work as expected, reset
63 results in an aircraft that is upside down but does not crash FG. I
64 don't know what it was like before, so it may well be no change.
65 JSBSim c172 and X15 in-air starts work fine, resets now work (and are
66 trimmed), on-ground starts do not -- the c172 ends up on its back. I
67 suspect this is no worse than before.
70 Balloon (the weather code returns nan's for the atmosphere data --this
71 is in the weather module and apparently is a linux only bug)
73 MagicCarpet (needs work yet)
74 External (don't know how)
77 LaRCsim c172 on-ground starts with a negative terrain altitude (this
78 happens at KPAO when the scenery is not present). The FDM inits to
79 about 50 feet AGL and the model falls to the ground. It does stay
80 upright, however, and seems to be fine once it settles out, FWIW.
83 --implement set_Model on the bus
84 --bring Christian's weather data into JSBSim
85 -- add default method to bus for updating things like the sin and cos of
86 latitude (for Balloon, MagicCarpet)
92 -- all data members now declared protected instead of private.
93 -- eliminated all but a small set of 'setters', no change to getters.
94 -- that small set is declared virtual, the default implementation
95 provided preserves the old behavior
96 -- all of the vector data members are now initialized.
97 -- added busdump() method -- SG_LOG's all the bus data when called,
98 useful for diagnostics.
101 -- bus data members now directly assigned to
104 -- bus data members now directly assigned to
105 -- changed V_equiv_kts to V_calibrated_kts
109 -- bus data members now directly assigned to
110 -- implemented the FGInterface virtual setters with JSBSim specific
112 -- changed the static FDMExec to a dynamic fdmex (needed so that the
113 JSBSim object can be deleted when a model change is called for)
114 -- implemented constructor and destructor, moved some of the logic
115 formerly in init() to constructor
116 -- added logic to bring up FGEngInterface objects and set the RPM and
121 -- bus data members now directly assigned to
122 -- implemented the FGInterface virtual setters with LaRCsim specific
123 logic, uses LaRCsimIC
124 -- implemented constructor and destructor, moved some of the logic
125 formerly in init() to constructor
126 -- moved default inertias to here from fg_init.cxx
127 -- eliminated the climb rate calculation. The equivalent, climb_rate =
128 -1*vdown, is now in copy_from_LaRCsim().
130 src/FDM/LaRCsimIC.cxx
131 src/FDM/LaRCsimIC.hxx
132 -- similar to FGInitialCondition, this class has all the logic needed to
133 turn data like Vc and Mach into the more fundamental quantities LaRCsim
135 -- put it in src/FDM since it is a class
137 src/FDM/MagicCarpet.cxx
138 -- bus data members now directly assigned to
141 -- adds LaRCsimIC.hxx and cxx
143 src/FDM/JSBSim/FGAtmosphere.h
144 src/FDM/JSBSim/FGDefs.h
145 src/FDM/JSBSim/FGInitialCondition.cpp
146 src/FDM/JSBSim/FGInitialCondition.h
147 src/FDM/JSBSim/JSBSim.cpp
148 -- changes to accomodate the new bus
150 src/FDM/LaRCsim/atmos_62.h
151 src/FDM/LaRCsim/ls_geodesy.h
152 -- surrounded prototypes with #ifdef __cplusplus ... #endif , functions
153 here are needed in LaRCsimIC
155 src/FDM/LaRCsim/c172_main.c
156 src/FDM/LaRCsim/cherokee_aero.c
157 src/FDM/LaRCsim/ls_aux.c
158 src/FDM/LaRCsim/ls_constants.h
159 src/FDM/LaRCsim/ls_geodesy.c
160 src/FDM/LaRCsim/ls_geodesy.h
161 src/FDM/LaRCsim/ls_step.c
162 src/FDM/UIUCModel/uiuc_betaprobe.cpp
163 -- changed PI to LS_PI, eliminates preprocessor naming conflict with
166 src/FDM/LaRCsim/ls_interface.c
167 src/FDM/LaRCsim/ls_interface.h
168 -- added function ls_set_model_dt()
171 -- eliminated calls that set the NED speeds to body components. They
172 are no longer needed and confuse the new bus.
175 -- eliminated calls that just brought the bus data up-to-date (e.g.
176 set_sin_cos_latitude). or set default values. The bus now handles the
177 defaults and updates itself when the setters are called (for LaRCsim and
178 JSBSim). A default method for doing this needs to be added to the bus.
179 -- added fgVelocityInit() to set the speed the user asked for. Both
180 JSBSim and LaRCsim can now be initialized using any of:
181 vc,mach, NED components, UVW components.
184 --eliminated call to fgFDMSetGroundElevation, this data is now 'pulled'
185 onto the bus every update()
189 -- added enum to keep track of the speed requested by the user
190 -- eliminated calls to set NED velocity properties to body speeds, they
191 are no longer needed.
192 -- added options for the NED components.
194 src/Network/garmin.cxx
196 --eliminated calls that just brought the bus data up-to-date (e.g.
197 set_sin_cos_latitude). The bus now updates itself when the setters are
198 called (for LaRCsim and JSBSim). A default method for doing this needs
199 to be added to the bus.
200 -- changed set_V_equiv_kts to set_V_calibrated_kts. set_V_equiv_kts no
201 longer exists ( get_V_equiv_kts still does, though)
203 src/WeatherCM/FGLocalWeatherDatabase.cpp
204 -- commented out the code to put the weather data on the bus, a
205 different scheme for this is needed.
207 Revision 1.2 1999/10/29 16:08:33 curt
208 Added flaps support to c172 model.
210 Revision 1.1.1.1 1999/06/17 18:07:33 curt
211 Start of 0.7.x branch
213 Revision 1.1.1.1 1999/04/05 21:32:45 curt
214 Start of 0.6.x branch.
216 Revision 1.4 1998/08/24 20:09:27 curt
217 Code optimization tweaks from Norman Vine.
219 Revision 1.3 1998/07/12 03:11:04 curt
220 Removed some printf()'s.
221 Fixed the autopilot integration so it should be able to update it's control
222 positions every time the internal flight model loop is run, and not just
223 once per rendered frame.
224 Added a routine to do the necessary stuff to force an arbitrary altitude
226 Gave the Navion engine just a tad more power.
228 Revision 1.2 1998/01/19 18:40:28 curt
229 Tons of little changes to clean up the code and to remove fatal errors
230 when building with the c++ compiler.
232 Revision 1.1 1997/05/29 00:09:59 curt
233 Initial Flight Gear revision.
235 * Revision 1.5 1995/03/02 20:24:13 bjax
236 * Added logic to avoid adding additional increment to V_east
237 * in case V_east already accounts for rotating earth. EBJ
239 * Revision 1.4 1995/02/07 20:52:21 bjax
240 * Added initialization of Alpha_dot and Beta_dot to zero on first
241 * pass; they get calculated by ls_aux on next pass... EBJ
243 * Revision 1.3 1994/01/11 19:01:12 bjax
244 * Changed from DATA to SCALAR type; also fixed header files (was ls_eom.h)
246 * Revision 1.2 1993/06/02 15:03:09 bjax
247 * Moved initialization of geocentric position to subroutine ls_geod_to_geoc.
249 * Revision 1.1 92/12/30 13:16:11 bjax
253 ----------------------------------------------------------------------------
257 [ 1] McFarland, Richard E.: "A Standard Kinematic Model
258 for Flight Simulation at NASA-Ames", NASA CR-2497,
261 [ 2] ANSI/AIAA R-004-1992 "Recommended Practice: Atmos-
262 pheric and Space Flight Vehicle Coordinate Systems",
266 ----------------------------------------------------------------------------
270 ----------------------------------------------------------------------------
274 ----------------------------------------------------------------------------
276 INPUTS: State derivatives
278 ----------------------------------------------------------------------------
282 --------------------------------------------------------------------------*/
284 #include <FDM/UIUCModel/uiuc_wrapper.h>
286 #include "ls_types.h"
287 #include "ls_constants.h"
288 #include "ls_generic.h"
289 #include "ls_accel.h"
291 #include "ls_model.h"
293 #include "ls_geodesy.h"
294 #include "ls_gravity.h"
295 /* #include "ls_sim_control.h" */
299 extern SCALAR Simtime; /* defined in ls_main.c */
301 void uiuc_init_vars() {
306 uiuc_init_aeromodel();
313 void ls_step( SCALAR dt, int Initialize ) {
314 static int inited = 0;
316 static SCALAR v_dot_north_past, v_dot_east_past, v_dot_down_past;
317 static SCALAR latitude_dot_past, longitude_dot_past, radius_dot_past;
318 static SCALAR p_dot_body_past, q_dot_body_past, r_dot_body_past;
319 SCALAR p_local_in_body, q_local_in_body, r_local_in_body;
320 SCALAR epsilon, inv_eps, local_gnd_veast;
321 SCALAR e_dot_0, e_dot_1, e_dot_2, e_dot_3;
322 static SCALAR e_0, e_1, e_2, e_3;
323 static SCALAR e_dot_0_past, e_dot_1_past, e_dot_2_past, e_dot_3_past;
324 SCALAR cos_Lat_geocentric, inv_Radius_to_vehicle;
326 /* I N I T I A L I Z A T I O N */
329 if ( (inited == 0) || (Initialize != 0) )
331 /* Set past values to zero */
332 v_dot_north_past = v_dot_east_past = v_dot_down_past = 0;
333 latitude_dot_past = longitude_dot_past = radius_dot_past = 0;
334 p_dot_body_past = q_dot_body_past = r_dot_body_past = 0;
335 e_dot_0_past = e_dot_1_past = e_dot_2_past = e_dot_3_past = 0;
337 /* Initialize geocentric position from geodetic latitude and altitude */
339 ls_geod_to_geoc( Latitude, Altitude, &Sea_level_radius, &Lat_geocentric);
340 Earth_position_angle = 0;
341 Lon_geocentric = Longitude;
342 Radius_to_vehicle = Altitude + Sea_level_radius;
344 /* Correct eastward velocity to account for earths' rotation, if necessary */
346 local_gnd_veast = OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric);
347 if( fabs(V_east - V_east_rel_ground) < 0.8*local_gnd_veast )
348 V_east = V_east + local_gnd_veast;
350 /* Initialize quaternions and transformation matrix from Euler angles */
351 if (current_model == UIUC && Simtime == 0) {
355 e_0 = cos(Psi*0.5)*cos(Theta*0.5)*cos(Phi*0.5)
356 + sin(Psi*0.5)*sin(Theta*0.5)*sin(Phi*0.5);
357 e_1 = cos(Psi*0.5)*cos(Theta*0.5)*sin(Phi*0.5)
358 - sin(Psi*0.5)*sin(Theta*0.5)*cos(Phi*0.5);
359 e_2 = cos(Psi*0.5)*sin(Theta*0.5)*cos(Phi*0.5)
360 + sin(Psi*0.5)*cos(Theta*0.5)*sin(Phi*0.5);
361 e_3 =-cos(Psi*0.5)*sin(Theta*0.5)*sin(Phi*0.5)
362 + sin(Psi*0.5)*cos(Theta*0.5)*cos(Phi*0.5);
363 T_local_to_body_11 = e_0*e_0 + e_1*e_1 - e_2*e_2 - e_3*e_3;
364 T_local_to_body_12 = 2*(e_1*e_2 + e_0*e_3);
365 T_local_to_body_13 = 2*(e_1*e_3 - e_0*e_2);
366 T_local_to_body_21 = 2*(e_1*e_2 - e_0*e_3);
367 T_local_to_body_22 = e_0*e_0 - e_1*e_1 + e_2*e_2 - e_3*e_3;
368 T_local_to_body_23 = 2*(e_2*e_3 + e_0*e_1);
369 T_local_to_body_31 = 2*(e_1*e_3 + e_0*e_2);
370 T_local_to_body_32 = 2*(e_2*e_3 - e_0*e_1);
371 T_local_to_body_33 = e_0*e_0 - e_1*e_1 - e_2*e_2 + e_3*e_3;
373 if (current_model == UIUC && Simtime == 0) {
377 /* Calculate local gravitation acceleration */
379 ls_gravity( Radius_to_vehicle, Lat_geocentric, &Gravity );
381 /* Initialize vehicle model */
386 /* Calculate initial accelerations */
390 /* Initialize auxiliary variables */
396 /* set flag; disable integrators */
406 Simtime = Simtime + dt;
408 /* L I N E A R V E L O C I T I E S */
410 /* Integrate linear accelerations to get velocities */
411 /* Using predictive Adams-Bashford algorithm */
413 V_north = V_north + dth*(3*V_dot_north - v_dot_north_past);
414 V_east = V_east + dth*(3*V_dot_east - v_dot_east_past );
415 V_down = V_down + dth*(3*V_dot_down - v_dot_down_past );
417 /* record past states */
419 v_dot_north_past = V_dot_north;
420 v_dot_east_past = V_dot_east;
421 v_dot_down_past = V_dot_down;
423 /* Calculate trajectory rate (geocentric coordinates) */
425 inv_Radius_to_vehicle = 1.0/Radius_to_vehicle;
426 cos_Lat_geocentric = cos(Lat_geocentric);
428 if ( cos_Lat_geocentric != 0) {
429 Longitude_dot = V_east/(Radius_to_vehicle*cos_Lat_geocentric);
432 Latitude_dot = V_north*inv_Radius_to_vehicle;
433 Radius_dot = -V_down;
435 /* 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 */
437 /* Integrate rotational accelerations to get velocities */
439 P_body = P_body + dth*(3*P_dot_body - p_dot_body_past);
440 Q_body = Q_body + dth*(3*Q_dot_body - q_dot_body_past);
441 R_body = R_body + dth*(3*R_dot_body - r_dot_body_past);
443 /* Save past states */
445 p_dot_body_past = P_dot_body;
446 q_dot_body_past = Q_dot_body;
447 r_dot_body_past = R_dot_body;
449 /* Calculate local axis frame rates due to travel over curved earth */
451 P_local = V_east*inv_Radius_to_vehicle;
452 Q_local = -V_north*inv_Radius_to_vehicle;
453 R_local = -V_east*tan(Lat_geocentric)*inv_Radius_to_vehicle;
455 /* Transform local axis frame rates to body axis rates */
457 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;
458 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;
459 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;
461 /* Calculate total angular rates in body axis */
463 P_total = P_body - p_local_in_body;
464 Q_total = Q_body - q_local_in_body;
465 R_total = R_body - r_local_in_body;
467 /* Transform to quaternion rates (see Appendix E in [2]) */
469 e_dot_0 = 0.5*( -P_total*e_1 - Q_total*e_2 - R_total*e_3 );
470 e_dot_1 = 0.5*( P_total*e_0 - Q_total*e_3 + R_total*e_2 );
471 e_dot_2 = 0.5*( P_total*e_3 + Q_total*e_0 - R_total*e_1 );
472 e_dot_3 = 0.5*( -P_total*e_2 + Q_total*e_1 + R_total*e_0 );
474 /* Integrate using trapezoidal as before */
476 e_0 = e_0 + dth*(e_dot_0 + e_dot_0_past);
477 e_1 = e_1 + dth*(e_dot_1 + e_dot_1_past);
478 e_2 = e_2 + dth*(e_dot_2 + e_dot_2_past);
479 e_3 = e_3 + dth*(e_dot_3 + e_dot_3_past);
481 /* calculate orthagonality correction - scale quaternion to unity length */
483 epsilon = sqrt(e_0*e_0 + e_1*e_1 + e_2*e_2 + e_3*e_3);
491 /* Save past values */
493 e_dot_0_past = e_dot_0;
494 e_dot_1_past = e_dot_1;
495 e_dot_2_past = e_dot_2;
496 e_dot_3_past = e_dot_3;
498 /* Update local to body transformation matrix */
500 T_local_to_body_11 = e_0*e_0 + e_1*e_1 - e_2*e_2 - e_3*e_3;
501 T_local_to_body_12 = 2*(e_1*e_2 + e_0*e_3);
502 T_local_to_body_13 = 2*(e_1*e_3 - e_0*e_2);
503 T_local_to_body_21 = 2*(e_1*e_2 - e_0*e_3);
504 T_local_to_body_22 = e_0*e_0 - e_1*e_1 + e_2*e_2 - e_3*e_3;
505 T_local_to_body_23 = 2*(e_2*e_3 + e_0*e_1);
506 T_local_to_body_31 = 2*(e_1*e_3 + e_0*e_2);
507 T_local_to_body_32 = 2*(e_2*e_3 - e_0*e_1);
508 T_local_to_body_33 = e_0*e_0 - e_1*e_1 - e_2*e_2 + e_3*e_3;
510 /* Calculate Euler angles */
512 Theta = asin( -T_local_to_body_13 );
514 if( T_local_to_body_11 == 0 )
517 Psi = atan2( T_local_to_body_12, T_local_to_body_11 );
519 if( T_local_to_body_33 == 0 )
522 Phi = atan2( T_local_to_body_23, T_local_to_body_33 );
524 /* Resolve Psi to 0 - 359.9999 */
526 if (Psi < 0 ) Psi = Psi + 2*LS_PI;
528 /* L I N E A R P O S I T I O N S */
530 /* Trapezoidal acceleration for position */
532 Lat_geocentric = Lat_geocentric + dth*(Latitude_dot + latitude_dot_past );
533 Lon_geocentric = Lon_geocentric + dth*(Longitude_dot + longitude_dot_past);
534 Radius_to_vehicle = Radius_to_vehicle + dth*(Radius_dot + radius_dot_past );
535 Earth_position_angle = Earth_position_angle + dt*OMEGA_EARTH;
537 /* Save past values */
539 latitude_dot_past = Latitude_dot;
540 longitude_dot_past = Longitude_dot;
541 radius_dot_past = Radius_dot;
545 /*************************************************************************/