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 2001/03/24 05:03:12 curt
56 Revision 1.3 2000/10/23 22:34:55 curt
58 LaRCsim c172 on-ground and in-air starts, reset: all work
59 UIUC Cessna172 on-ground and in-air starts work as expected, reset
60 results in an aircraft that is upside down but does not crash FG. I
61 don't know what it was like before, so it may well be no change.
62 JSBSim c172 and X15 in-air starts work fine, resets now work (and are
63 trimmed), on-ground starts do not -- the c172 ends up on its back. I
64 suspect this is no worse than before.
67 Balloon (the weather code returns nan's for the atmosphere data --this
68 is in the weather module and apparently is a linux only bug)
70 MagicCarpet (needs work yet)
71 External (don't know how)
74 LaRCsim c172 on-ground starts with a negative terrain altitude (this
75 happens at KPAO when the scenery is not present). The FDM inits to
76 about 50 feet AGL and the model falls to the ground. It does stay
77 upright, however, and seems to be fine once it settles out, FWIW.
80 --implement set_Model on the bus
81 --bring Christian's weather data into JSBSim
82 -- add default method to bus for updating things like the sin and cos of
83 latitude (for Balloon, MagicCarpet)
89 -- all data members now declared protected instead of private.
90 -- eliminated all but a small set of 'setters', no change to getters.
91 -- that small set is declared virtual, the default implementation
92 provided preserves the old behavior
93 -- all of the vector data members are now initialized.
94 -- added busdump() method -- SG_LOG's all the bus data when called,
95 useful for diagnostics.
98 -- bus data members now directly assigned to
101 -- bus data members now directly assigned to
102 -- changed V_equiv_kts to V_calibrated_kts
106 -- bus data members now directly assigned to
107 -- implemented the FGInterface virtual setters with JSBSim specific
109 -- changed the static FDMExec to a dynamic fdmex (needed so that the
110 JSBSim object can be deleted when a model change is called for)
111 -- implemented constructor and destructor, moved some of the logic
112 formerly in init() to constructor
113 -- added logic to bring up FGEngInterface objects and set the RPM and
118 -- bus data members now directly assigned to
119 -- implemented the FGInterface virtual setters with LaRCsim specific
120 logic, uses LaRCsimIC
121 -- implemented constructor and destructor, moved some of the logic
122 formerly in init() to constructor
123 -- moved default inertias to here from fg_init.cxx
124 -- eliminated the climb rate calculation. The equivalent, climb_rate =
125 -1*vdown, is now in copy_from_LaRCsim().
127 src/FDM/LaRCsimIC.cxx
128 src/FDM/LaRCsimIC.hxx
129 -- similar to FGInitialCondition, this class has all the logic needed to
130 turn data like Vc and Mach into the more fundamental quantities LaRCsim
132 -- put it in src/FDM since it is a class
134 src/FDM/MagicCarpet.cxx
135 -- bus data members now directly assigned to
138 -- adds LaRCsimIC.hxx and cxx
140 src/FDM/JSBSim/FGAtmosphere.h
141 src/FDM/JSBSim/FGDefs.h
142 src/FDM/JSBSim/FGInitialCondition.cpp
143 src/FDM/JSBSim/FGInitialCondition.h
144 src/FDM/JSBSim/JSBSim.cpp
145 -- changes to accomodate the new bus
147 src/FDM/LaRCsim/atmos_62.h
148 src/FDM/LaRCsim/ls_geodesy.h
149 -- surrounded prototypes with #ifdef __cplusplus ... #endif , functions
150 here are needed in LaRCsimIC
152 src/FDM/LaRCsim/c172_main.c
153 src/FDM/LaRCsim/cherokee_aero.c
154 src/FDM/LaRCsim/ls_aux.c
155 src/FDM/LaRCsim/ls_constants.h
156 src/FDM/LaRCsim/ls_geodesy.c
157 src/FDM/LaRCsim/ls_geodesy.h
158 src/FDM/LaRCsim/ls_step.c
159 src/FDM/UIUCModel/uiuc_betaprobe.cpp
160 -- changed PI to LS_PI, eliminates preprocessor naming conflict with
163 src/FDM/LaRCsim/ls_interface.c
164 src/FDM/LaRCsim/ls_interface.h
165 -- added function ls_set_model_dt()
168 -- eliminated calls that set the NED speeds to body components. They
169 are no longer needed and confuse the new bus.
172 -- eliminated calls that just brought the bus data up-to-date (e.g.
173 set_sin_cos_latitude). or set default values. The bus now handles the
174 defaults and updates itself when the setters are called (for LaRCsim and
175 JSBSim). A default method for doing this needs to be added to the bus.
176 -- added fgVelocityInit() to set the speed the user asked for. Both
177 JSBSim and LaRCsim can now be initialized using any of:
178 vc,mach, NED components, UVW components.
181 --eliminated call to fgFDMSetGroundElevation, this data is now 'pulled'
182 onto the bus every update()
186 -- added enum to keep track of the speed requested by the user
187 -- eliminated calls to set NED velocity properties to body speeds, they
188 are no longer needed.
189 -- added options for the NED components.
191 src/Network/garmin.cxx
193 --eliminated calls that just brought the bus data up-to-date (e.g.
194 set_sin_cos_latitude). The bus now updates itself when the setters are
195 called (for LaRCsim and JSBSim). A default method for doing this needs
196 to be added to the bus.
197 -- changed set_V_equiv_kts to set_V_calibrated_kts. set_V_equiv_kts no
198 longer exists ( get_V_equiv_kts still does, though)
200 src/WeatherCM/FGLocalWeatherDatabase.cpp
201 -- commented out the code to put the weather data on the bus, a
202 different scheme for this is needed.
204 Revision 1.2 1999/10/29 16:08:33 curt
205 Added flaps support to c172 model.
207 Revision 1.1.1.1 1999/06/17 18:07:33 curt
208 Start of 0.7.x branch
210 Revision 1.1.1.1 1999/04/05 21:32:45 curt
211 Start of 0.6.x branch.
213 Revision 1.4 1998/08/24 20:09:27 curt
214 Code optimization tweaks from Norman Vine.
216 Revision 1.3 1998/07/12 03:11:04 curt
217 Removed some printf()'s.
218 Fixed the autopilot integration so it should be able to update it's control
219 positions every time the internal flight model loop is run, and not just
220 once per rendered frame.
221 Added a routine to do the necessary stuff to force an arbitrary altitude
223 Gave the Navion engine just a tad more power.
225 Revision 1.2 1998/01/19 18:40:28 curt
226 Tons of little changes to clean up the code and to remove fatal errors
227 when building with the c++ compiler.
229 Revision 1.1 1997/05/29 00:09:59 curt
230 Initial Flight Gear revision.
232 * Revision 1.5 1995/03/02 20:24:13 bjax
233 * Added logic to avoid adding additional increment to V_east
234 * in case V_east already accounts for rotating earth. EBJ
236 * Revision 1.4 1995/02/07 20:52:21 bjax
237 * Added initialization of Alpha_dot and Beta_dot to zero on first
238 * pass; they get calculated by ls_aux on next pass... EBJ
240 * Revision 1.3 1994/01/11 19:01:12 bjax
241 * Changed from DATA to SCALAR type; also fixed header files (was ls_eom.h)
243 * Revision 1.2 1993/06/02 15:03:09 bjax
244 * Moved initialization of geocentric position to subroutine ls_geod_to_geoc.
246 * Revision 1.1 92/12/30 13:16:11 bjax
250 ----------------------------------------------------------------------------
254 [ 1] McFarland, Richard E.: "A Standard Kinematic Model
255 for Flight Simulation at NASA-Ames", NASA CR-2497,
258 [ 2] ANSI/AIAA R-004-1992 "Recommended Practice: Atmos-
259 pheric and Space Flight Vehicle Coordinate Systems",
263 ----------------------------------------------------------------------------
267 ----------------------------------------------------------------------------
271 ----------------------------------------------------------------------------
273 INPUTS: State derivatives
275 ----------------------------------------------------------------------------
279 --------------------------------------------------------------------------*/
281 #include "ls_types.h"
282 #include "ls_constants.h"
283 #include "ls_generic.h"
284 #include "ls_accel.h"
286 #include "ls_model.h"
288 #include "ls_geodesy.h"
289 #include "ls_gravity.h"
290 /* #include "ls_sim_control.h" */
293 extern SCALAR Simtime; /* defined in ls_main.c */
295 void ls_step( SCALAR dt, int Initialize ) {
296 static int inited = 0;
298 static SCALAR v_dot_north_past, v_dot_east_past, v_dot_down_past;
299 static SCALAR latitude_dot_past, longitude_dot_past, radius_dot_past;
300 static SCALAR p_dot_body_past, q_dot_body_past, r_dot_body_past;
301 SCALAR p_local_in_body, q_local_in_body, r_local_in_body;
302 SCALAR epsilon, inv_eps, local_gnd_veast;
303 SCALAR e_dot_0, e_dot_1, e_dot_2, e_dot_3;
304 static SCALAR e_0, e_1, e_2, e_3;
305 static SCALAR e_dot_0_past, e_dot_1_past, e_dot_2_past, e_dot_3_past;
306 SCALAR cos_Lat_geocentric, inv_Radius_to_vehicle;
308 /* I N I T I A L I Z A T I O N */
311 if ( (inited == 0) || (Initialize != 0) )
313 /* Set past values to zero */
314 v_dot_north_past = v_dot_east_past = v_dot_down_past = 0;
315 latitude_dot_past = longitude_dot_past = radius_dot_past = 0;
316 p_dot_body_past = q_dot_body_past = r_dot_body_past = 0;
317 e_dot_0_past = e_dot_1_past = e_dot_2_past = e_dot_3_past = 0;
319 /* Initialize geocentric position from geodetic latitude and altitude */
321 ls_geod_to_geoc( Latitude, Altitude, &Sea_level_radius, &Lat_geocentric);
322 Earth_position_angle = 0;
323 Lon_geocentric = Longitude;
324 Radius_to_vehicle = Altitude + Sea_level_radius;
326 /* Correct eastward velocity to account for earths' rotation, if necessary */
328 local_gnd_veast = OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric);
329 if( fabs(V_east - V_east_rel_ground) < 0.8*local_gnd_veast )
330 V_east = V_east + local_gnd_veast;
332 /* Initialize quaternions and transformation matrix from Euler angles */
334 e_0 = cos(Psi*0.5)*cos(Theta*0.5)*cos(Phi*0.5)
335 + sin(Psi*0.5)*sin(Theta*0.5)*sin(Phi*0.5);
336 e_1 = cos(Psi*0.5)*cos(Theta*0.5)*sin(Phi*0.5)
337 - sin(Psi*0.5)*sin(Theta*0.5)*cos(Phi*0.5);
338 e_2 = cos(Psi*0.5)*sin(Theta*0.5)*cos(Phi*0.5)
339 + sin(Psi*0.5)*cos(Theta*0.5)*sin(Phi*0.5);
340 e_3 =-cos(Psi*0.5)*sin(Theta*0.5)*sin(Phi*0.5)
341 + sin(Psi*0.5)*cos(Theta*0.5)*cos(Phi*0.5);
342 T_local_to_body_11 = e_0*e_0 + e_1*e_1 - e_2*e_2 - e_3*e_3;
343 T_local_to_body_12 = 2*(e_1*e_2 + e_0*e_3);
344 T_local_to_body_13 = 2*(e_1*e_3 - e_0*e_2);
345 T_local_to_body_21 = 2*(e_1*e_2 - e_0*e_3);
346 T_local_to_body_22 = e_0*e_0 - e_1*e_1 + e_2*e_2 - e_3*e_3;
347 T_local_to_body_23 = 2*(e_2*e_3 + e_0*e_1);
348 T_local_to_body_31 = 2*(e_1*e_3 + e_0*e_2);
349 T_local_to_body_32 = 2*(e_2*e_3 - e_0*e_1);
350 T_local_to_body_33 = e_0*e_0 - e_1*e_1 - e_2*e_2 + e_3*e_3;
352 /* Calculate local gravitation acceleration */
354 ls_gravity( Radius_to_vehicle, Lat_geocentric, &Gravity );
356 /* Initialize vehicle model */
361 /* Calculate initial accelerations */
365 /* Initialize auxiliary variables */
371 /* set flag; disable integrators */
381 Simtime = Simtime + dt;
383 /* L I N E A R V E L O C I T I E S */
385 /* Integrate linear accelerations to get velocities */
386 /* Using predictive Adams-Bashford algorithm */
388 V_north = V_north + dth*(3*V_dot_north - v_dot_north_past);
389 V_east = V_east + dth*(3*V_dot_east - v_dot_east_past );
390 V_down = V_down + dth*(3*V_dot_down - v_dot_down_past );
392 /* record past states */
394 v_dot_north_past = V_dot_north;
395 v_dot_east_past = V_dot_east;
396 v_dot_down_past = V_dot_down;
398 /* Calculate trajectory rate (geocentric coordinates) */
400 inv_Radius_to_vehicle = 1.0/Radius_to_vehicle;
401 cos_Lat_geocentric = cos(Lat_geocentric);
403 if ( cos_Lat_geocentric != 0) {
404 Longitude_dot = V_east/(Radius_to_vehicle*cos_Lat_geocentric);
407 Latitude_dot = V_north*inv_Radius_to_vehicle;
408 Radius_dot = -V_down;
410 /* 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 */
412 /* Integrate rotational accelerations to get velocities */
414 P_body = P_body + dth*(3*P_dot_body - p_dot_body_past);
415 Q_body = Q_body + dth*(3*Q_dot_body - q_dot_body_past);
416 R_body = R_body + dth*(3*R_dot_body - r_dot_body_past);
418 /* Save past states */
420 p_dot_body_past = P_dot_body;
421 q_dot_body_past = Q_dot_body;
422 r_dot_body_past = R_dot_body;
424 /* Calculate local axis frame rates due to travel over curved earth */
426 P_local = V_east*inv_Radius_to_vehicle;
427 Q_local = -V_north*inv_Radius_to_vehicle;
428 R_local = -V_east*tan(Lat_geocentric)*inv_Radius_to_vehicle;
430 /* Transform local axis frame rates to body axis rates */
432 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;
433 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;
434 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;
436 /* Calculate total angular rates in body axis */
438 P_total = P_body - p_local_in_body;
439 Q_total = Q_body - q_local_in_body;
440 R_total = R_body - r_local_in_body;
442 /* Transform to quaternion rates (see Appendix E in [2]) */
444 e_dot_0 = 0.5*( -P_total*e_1 - Q_total*e_2 - R_total*e_3 );
445 e_dot_1 = 0.5*( P_total*e_0 - Q_total*e_3 + R_total*e_2 );
446 e_dot_2 = 0.5*( P_total*e_3 + Q_total*e_0 - R_total*e_1 );
447 e_dot_3 = 0.5*( -P_total*e_2 + Q_total*e_1 + R_total*e_0 );
449 /* Integrate using trapezoidal as before */
451 e_0 = e_0 + dth*(e_dot_0 + e_dot_0_past);
452 e_1 = e_1 + dth*(e_dot_1 + e_dot_1_past);
453 e_2 = e_2 + dth*(e_dot_2 + e_dot_2_past);
454 e_3 = e_3 + dth*(e_dot_3 + e_dot_3_past);
456 /* calculate orthagonality correction - scale quaternion to unity length */
458 epsilon = sqrt(e_0*e_0 + e_1*e_1 + e_2*e_2 + e_3*e_3);
466 /* Save past values */
468 e_dot_0_past = e_dot_0;
469 e_dot_1_past = e_dot_1;
470 e_dot_2_past = e_dot_2;
471 e_dot_3_past = e_dot_3;
473 /* Update local to body transformation matrix */
475 T_local_to_body_11 = e_0*e_0 + e_1*e_1 - e_2*e_2 - e_3*e_3;
476 T_local_to_body_12 = 2*(e_1*e_2 + e_0*e_3);
477 T_local_to_body_13 = 2*(e_1*e_3 - e_0*e_2);
478 T_local_to_body_21 = 2*(e_1*e_2 - e_0*e_3);
479 T_local_to_body_22 = e_0*e_0 - e_1*e_1 + e_2*e_2 - e_3*e_3;
480 T_local_to_body_23 = 2*(e_2*e_3 + e_0*e_1);
481 T_local_to_body_31 = 2*(e_1*e_3 + e_0*e_2);
482 T_local_to_body_32 = 2*(e_2*e_3 - e_0*e_1);
483 T_local_to_body_33 = e_0*e_0 - e_1*e_1 - e_2*e_2 + e_3*e_3;
485 /* Calculate Euler angles */
487 Theta = asin( -T_local_to_body_13 );
489 if( T_local_to_body_11 == 0 )
492 Psi = atan2( T_local_to_body_12, T_local_to_body_11 );
494 if( T_local_to_body_33 == 0 )
497 Phi = atan2( T_local_to_body_23, T_local_to_body_33 );
499 /* Resolve Psi to 0 - 359.9999 */
501 if (Psi < 0 ) Psi = Psi + 2*LS_PI;
503 /* L I N E A R P O S I T I O N S */
505 /* Trapezoidal acceleration for position */
507 Lat_geocentric = Lat_geocentric + dth*(Latitude_dot + latitude_dot_past );
508 Lon_geocentric = Lon_geocentric + dth*(Longitude_dot + longitude_dot_past);
509 Radius_to_vehicle = Radius_to_vehicle + dth*(Radius_dot + radius_dot_past );
510 Earth_position_angle = Earth_position_angle + dt*OMEGA_EARTH;
512 /* Save past values */
514 latitude_dot_past = Latitude_dot;
515 longitude_dot_past = Longitude_dot;
516 radius_dot_past = Radius_dot;
520 /*************************************************************************/