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 "ls_sim_control.h" */
314 extern Model current_model; /* defined in ls_model.c */
315 extern SCALAR Simtime; /* defined in ls_main.c */
317 void uiuc_init_vars() {
322 uiuc_init_aeromodel();
329 void ls_step( SCALAR dt, int Initialize ) {
330 static int inited = 0;
332 static SCALAR v_dot_north_past, v_dot_east_past, v_dot_down_past;
333 static SCALAR latitude_dot_past, longitude_dot_past, radius_dot_past;
334 static SCALAR p_dot_body_past, q_dot_body_past, r_dot_body_past;
335 SCALAR p_local_in_body, q_local_in_body, r_local_in_body;
336 SCALAR epsilon, inv_eps, local_gnd_veast;
337 SCALAR e_dot_0, e_dot_1, e_dot_2, e_dot_3;
338 static SCALAR e_0, e_1, e_2, e_3;
339 static SCALAR e_dot_0_past, e_dot_1_past, e_dot_2_past, e_dot_3_past;
340 SCALAR cos_Lat_geocentric, inv_Radius_to_vehicle;
342 /* I N I T I A L I Z A T I O N */
345 if ( (inited == 0) || (Initialize != 0) )
347 /* Set past values to zero */
348 v_dot_north_past = v_dot_east_past = v_dot_down_past = 0;
349 latitude_dot_past = longitude_dot_past = radius_dot_past = 0;
350 p_dot_body_past = q_dot_body_past = r_dot_body_past = 0;
351 e_dot_0_past = e_dot_1_past = e_dot_2_past = e_dot_3_past = 0;
353 /* Initialize geocentric position from geodetic latitude and altitude */
355 ls_geod_to_geoc( Latitude, Altitude, &Sea_level_radius, &Lat_geocentric);
356 Earth_position_angle = 0;
357 Lon_geocentric = Longitude;
358 Radius_to_vehicle = Altitude + Sea_level_radius;
360 /* Correct eastward velocity to account for earths' rotation, if necessary */
362 local_gnd_veast = OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric);
363 if( fabs(V_east - V_east_rel_ground) < 0.8*local_gnd_veast )
364 V_east = V_east + local_gnd_veast;
366 /* Initialize quaternions and transformation matrix from Euler angles */
367 // Initialize UIUC aircraft model
368 if (current_model == UIUC) {
369 uiuc_init_2_wrapper();
372 e_0 = cos(Psi*0.5)*cos(Theta*0.5)*cos(Phi*0.5)
373 + sin(Psi*0.5)*sin(Theta*0.5)*sin(Phi*0.5);
374 e_1 = cos(Psi*0.5)*cos(Theta*0.5)*sin(Phi*0.5)
375 - sin(Psi*0.5)*sin(Theta*0.5)*cos(Phi*0.5);
376 e_2 = cos(Psi*0.5)*sin(Theta*0.5)*cos(Phi*0.5)
377 + sin(Psi*0.5)*cos(Theta*0.5)*sin(Phi*0.5);
378 e_3 =-cos(Psi*0.5)*sin(Theta*0.5)*sin(Phi*0.5)
379 + sin(Psi*0.5)*cos(Theta*0.5)*cos(Phi*0.5);
380 T_local_to_body_11 = e_0*e_0 + e_1*e_1 - e_2*e_2 - e_3*e_3;
381 T_local_to_body_12 = 2*(e_1*e_2 + e_0*e_3);
382 T_local_to_body_13 = 2*(e_1*e_3 - e_0*e_2);
383 T_local_to_body_21 = 2*(e_1*e_2 - e_0*e_3);
384 T_local_to_body_22 = e_0*e_0 - e_1*e_1 + e_2*e_2 - e_3*e_3;
385 T_local_to_body_23 = 2*(e_2*e_3 + e_0*e_1);
386 T_local_to_body_31 = 2*(e_1*e_3 + e_0*e_2);
387 T_local_to_body_32 = 2*(e_2*e_3 - e_0*e_1);
388 T_local_to_body_33 = e_0*e_0 - e_1*e_1 - e_2*e_2 + e_3*e_3;
390 // Initialize local velocities (V_north, V_east, V_down)
391 // based on transformation matrix calculated above
392 if (current_model == UIUC) {
393 uiuc_local_vel_init();
396 /* Calculate local gravitation acceleration */
398 ls_gravity( Radius_to_vehicle, Lat_geocentric, &Gravity );
400 /* Initialize vehicle model */
405 /* Calculate initial accelerations */
409 /* Initialize auxiliary variables */
415 /* set flag; disable integrators */
425 Simtime = Simtime + dt;
427 /* L I N E A R V E L O C I T I E S */
429 /* Integrate linear accelerations to get velocities */
430 /* Using predictive Adams-Bashford algorithm */
432 V_north = V_north + dth*(3*V_dot_north - v_dot_north_past);
433 V_east = V_east + dth*(3*V_dot_east - v_dot_east_past );
434 V_down = V_down + dth*(3*V_dot_down - v_dot_down_past );
436 /* record past states */
438 v_dot_north_past = V_dot_north;
439 v_dot_east_past = V_dot_east;
440 v_dot_down_past = V_dot_down;
442 /* Calculate trajectory rate (geocentric coordinates) */
444 inv_Radius_to_vehicle = 1.0/Radius_to_vehicle;
445 cos_Lat_geocentric = cos(Lat_geocentric);
447 if ( cos_Lat_geocentric != 0) {
448 Longitude_dot = V_east/(Radius_to_vehicle*cos_Lat_geocentric);
451 Latitude_dot = V_north*inv_Radius_to_vehicle;
452 Radius_dot = -V_down;
454 /* 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 */
456 /* Integrate rotational accelerations to get velocities */
458 P_body = P_body + dth*(3*P_dot_body - p_dot_body_past);
459 Q_body = Q_body + dth*(3*Q_dot_body - q_dot_body_past);
460 R_body = R_body + dth*(3*R_dot_body - r_dot_body_past);
462 /* Save past states */
464 p_dot_body_past = P_dot_body;
465 q_dot_body_past = Q_dot_body;
466 r_dot_body_past = R_dot_body;
468 /* Calculate local axis frame rates due to travel over curved earth */
470 P_local = V_east*inv_Radius_to_vehicle;
471 Q_local = -V_north*inv_Radius_to_vehicle;
472 R_local = -V_east*tan(Lat_geocentric)*inv_Radius_to_vehicle;
474 /* Transform local axis frame rates to body axis rates */
476 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;
477 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;
478 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;
480 /* Calculate total angular rates in body axis */
482 P_total = P_body - p_local_in_body;
483 Q_total = Q_body - q_local_in_body;
484 R_total = R_body - r_local_in_body;
486 /* Transform to quaternion rates (see Appendix E in [2]) */
488 e_dot_0 = 0.5*( -P_total*e_1 - Q_total*e_2 - R_total*e_3 );
489 e_dot_1 = 0.5*( P_total*e_0 - Q_total*e_3 + R_total*e_2 );
490 e_dot_2 = 0.5*( P_total*e_3 + Q_total*e_0 - R_total*e_1 );
491 e_dot_3 = 0.5*( -P_total*e_2 + Q_total*e_1 + R_total*e_0 );
493 /* Integrate using trapezoidal as before */
495 e_0 = e_0 + dth*(e_dot_0 + e_dot_0_past);
496 e_1 = e_1 + dth*(e_dot_1 + e_dot_1_past);
497 e_2 = e_2 + dth*(e_dot_2 + e_dot_2_past);
498 e_3 = e_3 + dth*(e_dot_3 + e_dot_3_past);
500 /* calculate orthagonality correction - scale quaternion to unity length */
502 epsilon = sqrt(e_0*e_0 + e_1*e_1 + e_2*e_2 + e_3*e_3);
510 /* Save past values */
512 e_dot_0_past = e_dot_0;
513 e_dot_1_past = e_dot_1;
514 e_dot_2_past = e_dot_2;
515 e_dot_3_past = e_dot_3;
517 /* Update local to body transformation matrix */
519 T_local_to_body_11 = e_0*e_0 + e_1*e_1 - e_2*e_2 - e_3*e_3;
520 T_local_to_body_12 = 2*(e_1*e_2 + e_0*e_3);
521 T_local_to_body_13 = 2*(e_1*e_3 - e_0*e_2);
522 T_local_to_body_21 = 2*(e_1*e_2 - e_0*e_3);
523 T_local_to_body_22 = e_0*e_0 - e_1*e_1 + e_2*e_2 - e_3*e_3;
524 T_local_to_body_23 = 2*(e_2*e_3 + e_0*e_1);
525 T_local_to_body_31 = 2*(e_1*e_3 + e_0*e_2);
526 T_local_to_body_32 = 2*(e_2*e_3 - e_0*e_1);
527 T_local_to_body_33 = e_0*e_0 - e_1*e_1 - e_2*e_2 + e_3*e_3;
529 /* Calculate Euler angles */
531 Theta = asin( -T_local_to_body_13 );
533 if( T_local_to_body_11 == 0 )
536 Psi = atan2( T_local_to_body_12, T_local_to_body_11 );
538 if( T_local_to_body_33 == 0 )
541 Phi = atan2( T_local_to_body_23, T_local_to_body_33 );
543 /* Resolve Psi to 0 - 359.9999 */
545 if (Psi < 0 ) Psi = Psi + 2*LS_PI;
547 /* L I N E A R P O S I T I O N S */
549 /* Trapezoidal acceleration for position */
551 Lat_geocentric = Lat_geocentric + dth*(Latitude_dot + latitude_dot_past );
552 Lon_geocentric = Lon_geocentric + dth*(Longitude_dot + longitude_dot_past);
553 Radius_to_vehicle = Radius_to_vehicle + dth*(Radius_dot + radius_dot_past );
554 Earth_position_angle = Earth_position_angle + dt*OMEGA_EARTH;
556 /* Save past values */
558 latitude_dot_past = Latitude_dot;
559 longitude_dot_past = Longitude_dot;
560 radius_dot_past = Radius_dot;
564 /*************************************************************************/