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 2003/07/25 17:53:47 mselig
54 UIUC code initilization mods to tidy things up a bit.
56 Revision 1.4 2003/06/20 19:53:56 ehofman
57 Get rid of a multiple defined symbol warning" src/FDM/LaRCsim/ls_step.c
60 Revision 1.3 2003/06/09 02:50:23 mselig
61 mods made to setup for some initializations in uiuc code
63 Revision 1.2 2003/05/25 12:14:46 ehofman
64 Rename some defines to prevent a namespace clash
66 Revision 1.1.1.1 2002/09/10 01:14:02 curt
67 Initial revision of FlightGear-0.9.0
69 Revision 1.5 2001/09/14 18:47:27 curt
70 More changes in support of UIUCModel.
72 Revision 1.4 2001/03/24 05:03:12 curt
75 Revision 1.3 2000/10/23 22:34:55 curt
77 LaRCsim c172 on-ground and in-air starts, reset: all work
78 UIUC Cessna172 on-ground and in-air starts work as expected, reset
79 results in an aircraft that is upside down but does not crash FG. I
80 don't know what it was like before, so it may well be no change.
81 JSBSim c172 and X15 in-air starts work fine, resets now work (and are
82 trimmed), on-ground starts do not -- the c172 ends up on its back. I
83 suspect this is no worse than before.
86 Balloon (the weather code returns nan's for the atmosphere data --this
87 is in the weather module and apparently is a linux only bug)
89 MagicCarpet (needs work yet)
90 External (don't know how)
93 LaRCsim c172 on-ground starts with a negative terrain altitude (this
94 happens at KPAO when the scenery is not present). The FDM inits to
95 about 50 feet AGL and the model falls to the ground. It does stay
96 upright, however, and seems to be fine once it settles out, FWIW.
99 --implement set_Model on the bus
100 --bring Christian's weather data into JSBSim
101 -- add default method to bus for updating things like the sin and cos of
102 latitude (for Balloon, MagicCarpet)
108 -- all data members now declared protected instead of private.
109 -- eliminated all but a small set of 'setters', no change to getters.
110 -- that small set is declared virtual, the default implementation
111 provided preserves the old behavior
112 -- all of the vector data members are now initialized.
113 -- added busdump() method -- SG_LOG's all the bus data when called,
114 useful for diagnostics.
117 -- bus data members now directly assigned to
120 -- bus data members now directly assigned to
121 -- changed V_equiv_kts to V_calibrated_kts
125 -- bus data members now directly assigned to
126 -- implemented the FGInterface virtual setters with JSBSim specific
128 -- changed the static FDMExec to a dynamic fdmex (needed so that the
129 JSBSim object can be deleted when a model change is called for)
130 -- implemented constructor and destructor, moved some of the logic
131 formerly in init() to constructor
132 -- added logic to bring up FGEngInterface objects and set the RPM and
137 -- bus data members now directly assigned to
138 -- implemented the FGInterface virtual setters with LaRCsim specific
139 logic, uses LaRCsimIC
140 -- implemented constructor and destructor, moved some of the logic
141 formerly in init() to constructor
142 -- moved default inertias to here from fg_init.cxx
143 -- eliminated the climb rate calculation. The equivalent, climb_rate =
144 -1*vdown, is now in copy_from_LaRCsim().
146 src/FDM/LaRCsimIC.cxx
147 src/FDM/LaRCsimIC.hxx
148 -- similar to FGInitialCondition, this class has all the logic needed to
149 turn data like Vc and Mach into the more fundamental quantities LaRCsim
151 -- put it in src/FDM since it is a class
153 src/FDM/MagicCarpet.cxx
154 -- bus data members now directly assigned to
157 -- adds LaRCsimIC.hxx and cxx
159 src/FDM/JSBSim/FGAtmosphere.h
160 src/FDM/JSBSim/FGDefs.h
161 src/FDM/JSBSim/FGInitialCondition.cpp
162 src/FDM/JSBSim/FGInitialCondition.h
163 src/FDM/JSBSim/JSBSim.cpp
164 -- changes to accomodate the new bus
166 src/FDM/LaRCsim/atmos_62.h
167 src/FDM/LaRCsim/ls_geodesy.h
168 -- surrounded prototypes with #ifdef __cplusplus ... #endif , functions
169 here are needed in LaRCsimIC
171 src/FDM/LaRCsim/c172_main.c
172 src/FDM/LaRCsim/cherokee_aero.c
173 src/FDM/LaRCsim/ls_aux.c
174 src/FDM/LaRCsim/ls_constants.h
175 src/FDM/LaRCsim/ls_geodesy.c
176 src/FDM/LaRCsim/ls_geodesy.h
177 src/FDM/LaRCsim/ls_step.c
178 src/FDM/UIUCModel/uiuc_betaprobe.cpp
179 -- changed PI to LS_PI, eliminates preprocessor naming conflict with
182 src/FDM/LaRCsim/ls_interface.c
183 src/FDM/LaRCsim/ls_interface.h
184 -- added function ls_set_model_dt()
187 -- eliminated calls that set the NED speeds to body components. They
188 are no longer needed and confuse the new bus.
191 -- eliminated calls that just brought the bus data up-to-date (e.g.
192 set_sin_cos_latitude). or set default values. The bus now handles the
193 defaults and updates itself when the setters are called (for LaRCsim and
194 JSBSim). A default method for doing this needs to be added to the bus.
195 -- added fgVelocityInit() to set the speed the user asked for. Both
196 JSBSim and LaRCsim can now be initialized using any of:
197 vc,mach, NED components, UVW components.
200 --eliminated call to fgFDMSetGroundElevation, this data is now 'pulled'
201 onto the bus every update()
205 -- added enum to keep track of the speed requested by the user
206 -- eliminated calls to set NED velocity properties to body speeds, they
207 are no longer needed.
208 -- added options for the NED components.
210 src/Network/garmin.cxx
212 --eliminated calls that just brought the bus data up-to-date (e.g.
213 set_sin_cos_latitude). The bus now updates itself when the setters are
214 called (for LaRCsim and JSBSim). A default method for doing this needs
215 to be added to the bus.
216 -- changed set_V_equiv_kts to set_V_calibrated_kts. set_V_equiv_kts no
217 longer exists ( get_V_equiv_kts still does, though)
219 src/WeatherCM/FGLocalWeatherDatabase.cpp
220 -- commented out the code to put the weather data on the bus, a
221 different scheme for this is needed.
223 Revision 1.2 1999/10/29 16:08:33 curt
224 Added flaps support to c172 model.
226 Revision 1.1.1.1 1999/06/17 18:07:33 curt
227 Start of 0.7.x branch
229 Revision 1.1.1.1 1999/04/05 21:32:45 curt
230 Start of 0.6.x branch.
232 Revision 1.4 1998/08/24 20:09:27 curt
233 Code optimization tweaks from Norman Vine.
235 Revision 1.3 1998/07/12 03:11:04 curt
236 Removed some printf()'s.
237 Fixed the autopilot integration so it should be able to update it's control
238 positions every time the internal flight model loop is run, and not just
239 once per rendered frame.
240 Added a routine to do the necessary stuff to force an arbitrary altitude
242 Gave the Navion engine just a tad more power.
244 Revision 1.2 1998/01/19 18:40:28 curt
245 Tons of little changes to clean up the code and to remove fatal errors
246 when building with the c++ compiler.
248 Revision 1.1 1997/05/29 00:09:59 curt
249 Initial Flight Gear revision.
251 * Revision 1.5 1995/03/02 20:24:13 bjax
252 * Added logic to avoid adding additional increment to V_east
253 * in case V_east already accounts for rotating earth. EBJ
255 * Revision 1.4 1995/02/07 20:52:21 bjax
256 * Added initialization of Alpha_dot and Beta_dot to zero on first
257 * pass; they get calculated by ls_aux on next pass... EBJ
259 * Revision 1.3 1994/01/11 19:01:12 bjax
260 * Changed from DATA to SCALAR type; also fixed header files (was ls_eom.h)
262 * Revision 1.2 1993/06/02 15:03:09 bjax
263 * Moved initialization of geocentric position to subroutine ls_geod_to_geoc.
265 * Revision 1.1 92/12/30 13:16:11 bjax
269 ----------------------------------------------------------------------------
273 [ 1] McFarland, Richard E.: "A Standard Kinematic Model
274 for Flight Simulation at NASA-Ames", NASA CR-2497,
277 [ 2] ANSI/AIAA R-004-1992 "Recommended Practice: Atmos-
278 pheric and Space Flight Vehicle Coordinate Systems",
282 ----------------------------------------------------------------------------
286 ----------------------------------------------------------------------------
290 ----------------------------------------------------------------------------
292 INPUTS: State derivatives
294 ----------------------------------------------------------------------------
298 --------------------------------------------------------------------------*/
300 #include "FDM/UIUCModel/uiuc_wrapper.h"
302 #include "ls_types.h"
303 #include "ls_constants.h"
304 #include "ls_generic.h"
305 #include "ls_accel.h"
307 #include "ls_model.h"
309 #include "ls_geodesy.h"
310 #include "ls_gravity.h"
311 #include "default_model_routines.h"
312 /* #include "ls_sim_control.h" */
315 extern Model current_model; /* defined in ls_model.c */
316 extern SCALAR Simtime; /* defined in ls_main.c */
318 void uiuc_init_vars() {
323 uiuc_init_aeromodel();
330 void ls_step( SCALAR dt, int Initialize ) {
331 static int inited = 0;
333 static SCALAR v_dot_north_past, v_dot_east_past, v_dot_down_past;
334 static SCALAR latitude_dot_past, longitude_dot_past, radius_dot_past;
335 static SCALAR p_dot_body_past, q_dot_body_past, r_dot_body_past;
336 SCALAR p_local_in_body, q_local_in_body, r_local_in_body;
337 SCALAR epsilon, inv_eps, local_gnd_veast;
338 SCALAR e_dot_0, e_dot_1, e_dot_2, e_dot_3;
339 static SCALAR e_0, e_1, e_2, e_3;
340 static SCALAR e_dot_0_past, e_dot_1_past, e_dot_2_past, e_dot_3_past;
341 SCALAR cos_Lat_geocentric, inv_Radius_to_vehicle;
343 /* I N I T I A L I Z A T I O N */
346 if ( (inited == 0) || (Initialize != 0) )
348 /* Set past values to zero */
349 v_dot_north_past = v_dot_east_past = v_dot_down_past = 0;
350 latitude_dot_past = longitude_dot_past = radius_dot_past = 0;
351 p_dot_body_past = q_dot_body_past = r_dot_body_past = 0;
352 e_dot_0_past = e_dot_1_past = e_dot_2_past = e_dot_3_past = 0;
354 /* Initialize geocentric position from geodetic latitude and altitude */
356 ls_geod_to_geoc( Latitude, Altitude, &Sea_level_radius, &Lat_geocentric);
357 Earth_position_angle = 0;
358 Lon_geocentric = Longitude;
359 Radius_to_vehicle = Altitude + Sea_level_radius;
361 /* Correct eastward velocity to account for earths' rotation, if necessary */
363 local_gnd_veast = OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric);
364 if( fabs(V_east - V_east_rel_ground) < 0.8*local_gnd_veast )
365 V_east = V_east + local_gnd_veast;
367 /* Initialize quaternions and transformation matrix from Euler angles */
368 // Initialize UIUC aircraft model
369 if (current_model == UIUC) {
370 uiuc_init_2_wrapper();
373 e_0 = cos(Psi*0.5)*cos(Theta*0.5)*cos(Phi*0.5)
374 + sin(Psi*0.5)*sin(Theta*0.5)*sin(Phi*0.5);
375 e_1 = cos(Psi*0.5)*cos(Theta*0.5)*sin(Phi*0.5)
376 - sin(Psi*0.5)*sin(Theta*0.5)*cos(Phi*0.5);
377 e_2 = cos(Psi*0.5)*sin(Theta*0.5)*cos(Phi*0.5)
378 + sin(Psi*0.5)*cos(Theta*0.5)*sin(Phi*0.5);
379 e_3 =-cos(Psi*0.5)*sin(Theta*0.5)*sin(Phi*0.5)
380 + sin(Psi*0.5)*cos(Theta*0.5)*cos(Phi*0.5);
381 T_local_to_body_11 = e_0*e_0 + e_1*e_1 - e_2*e_2 - e_3*e_3;
382 T_local_to_body_12 = 2*(e_1*e_2 + e_0*e_3);
383 T_local_to_body_13 = 2*(e_1*e_3 - e_0*e_2);
384 T_local_to_body_21 = 2*(e_1*e_2 - e_0*e_3);
385 T_local_to_body_22 = e_0*e_0 - e_1*e_1 + e_2*e_2 - e_3*e_3;
386 T_local_to_body_23 = 2*(e_2*e_3 + e_0*e_1);
387 T_local_to_body_31 = 2*(e_1*e_3 + e_0*e_2);
388 T_local_to_body_32 = 2*(e_2*e_3 - e_0*e_1);
389 T_local_to_body_33 = e_0*e_0 - e_1*e_1 - e_2*e_2 + e_3*e_3;
391 // Initialize local velocities (V_north, V_east, V_down)
392 // based on transformation matrix calculated above
393 if (current_model == UIUC) {
394 uiuc_local_vel_init();
397 /* Calculate local gravitation acceleration */
399 ls_gravity( Radius_to_vehicle, Lat_geocentric, &Gravity );
401 /* Initialize vehicle model */
406 /* Calculate initial accelerations */
410 /* Initialize auxiliary variables */
416 /* set flag; disable integrators */
426 Simtime = Simtime + dt;
428 /* L I N E A R V E L O C I T I E S */
430 /* Integrate linear accelerations to get velocities */
431 /* Using predictive Adams-Bashford algorithm */
433 V_north = V_north + dth*(3*V_dot_north - v_dot_north_past);
434 V_east = V_east + dth*(3*V_dot_east - v_dot_east_past );
435 V_down = V_down + dth*(3*V_dot_down - v_dot_down_past );
437 /* record past states */
439 v_dot_north_past = V_dot_north;
440 v_dot_east_past = V_dot_east;
441 v_dot_down_past = V_dot_down;
443 /* Calculate trajectory rate (geocentric coordinates) */
445 inv_Radius_to_vehicle = 1.0/Radius_to_vehicle;
446 cos_Lat_geocentric = cos(Lat_geocentric);
448 if ( cos_Lat_geocentric != 0) {
449 Longitude_dot = V_east/(Radius_to_vehicle*cos_Lat_geocentric);
452 Latitude_dot = V_north*inv_Radius_to_vehicle;
453 Radius_dot = -V_down;
455 /* 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 */
457 /* Integrate rotational accelerations to get velocities */
459 P_body = P_body + dth*(3*P_dot_body - p_dot_body_past);
460 Q_body = Q_body + dth*(3*Q_dot_body - q_dot_body_past);
461 R_body = R_body + dth*(3*R_dot_body - r_dot_body_past);
463 /* Save past states */
465 p_dot_body_past = P_dot_body;
466 q_dot_body_past = Q_dot_body;
467 r_dot_body_past = R_dot_body;
469 /* Calculate local axis frame rates due to travel over curved earth */
471 P_local = V_east*inv_Radius_to_vehicle;
472 Q_local = -V_north*inv_Radius_to_vehicle;
473 R_local = -V_east*tan(Lat_geocentric)*inv_Radius_to_vehicle;
475 /* Transform local axis frame rates to body axis rates */
477 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;
478 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;
479 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;
481 /* Calculate total angular rates in body axis */
483 P_total = P_body - p_local_in_body;
484 Q_total = Q_body - q_local_in_body;
485 R_total = R_body - r_local_in_body;
487 /* Transform to quaternion rates (see Appendix E in [2]) */
489 e_dot_0 = 0.5*( -P_total*e_1 - Q_total*e_2 - R_total*e_3 );
490 e_dot_1 = 0.5*( P_total*e_0 - Q_total*e_3 + R_total*e_2 );
491 e_dot_2 = 0.5*( P_total*e_3 + Q_total*e_0 - R_total*e_1 );
492 e_dot_3 = 0.5*( -P_total*e_2 + Q_total*e_1 + R_total*e_0 );
494 /* Integrate using trapezoidal as before */
496 e_0 = e_0 + dth*(e_dot_0 + e_dot_0_past);
497 e_1 = e_1 + dth*(e_dot_1 + e_dot_1_past);
498 e_2 = e_2 + dth*(e_dot_2 + e_dot_2_past);
499 e_3 = e_3 + dth*(e_dot_3 + e_dot_3_past);
501 /* calculate orthagonality correction - scale quaternion to unity length */
503 epsilon = sqrt(e_0*e_0 + e_1*e_1 + e_2*e_2 + e_3*e_3);
511 /* Save past values */
513 e_dot_0_past = e_dot_0;
514 e_dot_1_past = e_dot_1;
515 e_dot_2_past = e_dot_2;
516 e_dot_3_past = e_dot_3;
518 /* Update local to body transformation matrix */
520 T_local_to_body_11 = e_0*e_0 + e_1*e_1 - e_2*e_2 - e_3*e_3;
521 T_local_to_body_12 = 2*(e_1*e_2 + e_0*e_3);
522 T_local_to_body_13 = 2*(e_1*e_3 - e_0*e_2);
523 T_local_to_body_21 = 2*(e_1*e_2 - e_0*e_3);
524 T_local_to_body_22 = e_0*e_0 - e_1*e_1 + e_2*e_2 - e_3*e_3;
525 T_local_to_body_23 = 2*(e_2*e_3 + e_0*e_1);
526 T_local_to_body_31 = 2*(e_1*e_3 + e_0*e_2);
527 T_local_to_body_32 = 2*(e_2*e_3 - e_0*e_1);
528 T_local_to_body_33 = e_0*e_0 - e_1*e_1 - e_2*e_2 + e_3*e_3;
530 /* Calculate Euler angles */
532 Theta = asin( -T_local_to_body_13 );
534 if( T_local_to_body_11 == 0 )
537 Psi = atan2( T_local_to_body_12, T_local_to_body_11 );
539 if( T_local_to_body_33 == 0 )
542 Phi = atan2( T_local_to_body_23, T_local_to_body_33 );
544 /* Resolve Psi to 0 - 359.9999 */
546 if (Psi < 0 ) Psi = Psi + 2*LS_PI;
548 /* L I N E A R P O S I T I O N S */
550 /* Trapezoidal acceleration for position */
552 Lat_geocentric = Lat_geocentric + dth*(Latitude_dot + latitude_dot_past );
553 Lon_geocentric = Lon_geocentric + dth*(Longitude_dot + longitude_dot_past);
554 Radius_to_vehicle = Radius_to_vehicle + dth*(Radius_dot + radius_dot_past );
555 Earth_position_angle = Earth_position_angle + dt*OMEGA_EARTH;
557 /* Save past values */
559 latitude_dot_past = Latitude_dot;
560 longitude_dot_past = Longitude_dot;
561 radius_dot_past = Radius_dot;
565 /*************************************************************************/