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.1 2002/09/10 01:14:02 curt
56 Revision 1.5 2001/09/14 18:47:27 curt
57 More changes in support of UIUCModel.
59 Revision 1.4 2001/03/24 05:03:12 curt
62 Revision 1.3 2000/10/23 22:34:55 curt
64 LaRCsim c172 on-ground and in-air starts, reset: all work
65 UIUC Cessna172 on-ground and in-air starts work as expected, reset
66 results in an aircraft that is upside down but does not crash FG. I
67 don't know what it was like before, so it may well be no change.
68 JSBSim c172 and X15 in-air starts work fine, resets now work (and are
69 trimmed), on-ground starts do not -- the c172 ends up on its back. I
70 suspect this is no worse than before.
73 Balloon (the weather code returns nan's for the atmosphere data --this
74 is in the weather module and apparently is a linux only bug)
76 MagicCarpet (needs work yet)
77 External (don't know how)
80 LaRCsim c172 on-ground starts with a negative terrain altitude (this
81 happens at KPAO when the scenery is not present). The FDM inits to
82 about 50 feet AGL and the model falls to the ground. It does stay
83 upright, however, and seems to be fine once it settles out, FWIW.
86 --implement set_Model on the bus
87 --bring Christian's weather data into JSBSim
88 -- add default method to bus for updating things like the sin and cos of
89 latitude (for Balloon, MagicCarpet)
95 -- all data members now declared protected instead of private.
96 -- eliminated all but a small set of 'setters', no change to getters.
97 -- that small set is declared virtual, the default implementation
98 provided preserves the old behavior
99 -- all of the vector data members are now initialized.
100 -- added busdump() method -- SG_LOG's all the bus data when called,
101 useful for diagnostics.
104 -- bus data members now directly assigned to
107 -- bus data members now directly assigned to
108 -- changed V_equiv_kts to V_calibrated_kts
112 -- bus data members now directly assigned to
113 -- implemented the FGInterface virtual setters with JSBSim specific
115 -- changed the static FDMExec to a dynamic fdmex (needed so that the
116 JSBSim object can be deleted when a model change is called for)
117 -- implemented constructor and destructor, moved some of the logic
118 formerly in init() to constructor
119 -- added logic to bring up FGEngInterface objects and set the RPM and
124 -- bus data members now directly assigned to
125 -- implemented the FGInterface virtual setters with LaRCsim specific
126 logic, uses LaRCsimIC
127 -- implemented constructor and destructor, moved some of the logic
128 formerly in init() to constructor
129 -- moved default inertias to here from fg_init.cxx
130 -- eliminated the climb rate calculation. The equivalent, climb_rate =
131 -1*vdown, is now in copy_from_LaRCsim().
133 src/FDM/LaRCsimIC.cxx
134 src/FDM/LaRCsimIC.hxx
135 -- similar to FGInitialCondition, this class has all the logic needed to
136 turn data like Vc and Mach into the more fundamental quantities LaRCsim
138 -- put it in src/FDM since it is a class
140 src/FDM/MagicCarpet.cxx
141 -- bus data members now directly assigned to
144 -- adds LaRCsimIC.hxx and cxx
146 src/FDM/JSBSim/FGAtmosphere.h
147 src/FDM/JSBSim/FGDefs.h
148 src/FDM/JSBSim/FGInitialCondition.cpp
149 src/FDM/JSBSim/FGInitialCondition.h
150 src/FDM/JSBSim/JSBSim.cpp
151 -- changes to accomodate the new bus
153 src/FDM/LaRCsim/atmos_62.h
154 src/FDM/LaRCsim/ls_geodesy.h
155 -- surrounded prototypes with #ifdef __cplusplus ... #endif , functions
156 here are needed in LaRCsimIC
158 src/FDM/LaRCsim/c172_main.c
159 src/FDM/LaRCsim/cherokee_aero.c
160 src/FDM/LaRCsim/ls_aux.c
161 src/FDM/LaRCsim/ls_constants.h
162 src/FDM/LaRCsim/ls_geodesy.c
163 src/FDM/LaRCsim/ls_geodesy.h
164 src/FDM/LaRCsim/ls_step.c
165 src/FDM/UIUCModel/uiuc_betaprobe.cpp
166 -- changed PI to LS_PI, eliminates preprocessor naming conflict with
169 src/FDM/LaRCsim/ls_interface.c
170 src/FDM/LaRCsim/ls_interface.h
171 -- added function ls_set_model_dt()
174 -- eliminated calls that set the NED speeds to body components. They
175 are no longer needed and confuse the new bus.
178 -- eliminated calls that just brought the bus data up-to-date (e.g.
179 set_sin_cos_latitude). or set default values. The bus now handles the
180 defaults and updates itself when the setters are called (for LaRCsim and
181 JSBSim). A default method for doing this needs to be added to the bus.
182 -- added fgVelocityInit() to set the speed the user asked for. Both
183 JSBSim and LaRCsim can now be initialized using any of:
184 vc,mach, NED components, UVW components.
187 --eliminated call to fgFDMSetGroundElevation, this data is now 'pulled'
188 onto the bus every update()
192 -- added enum to keep track of the speed requested by the user
193 -- eliminated calls to set NED velocity properties to body speeds, they
194 are no longer needed.
195 -- added options for the NED components.
197 src/Network/garmin.cxx
199 --eliminated calls that just brought the bus data up-to-date (e.g.
200 set_sin_cos_latitude). The bus now updates itself when the setters are
201 called (for LaRCsim and JSBSim). A default method for doing this needs
202 to be added to the bus.
203 -- changed set_V_equiv_kts to set_V_calibrated_kts. set_V_equiv_kts no
204 longer exists ( get_V_equiv_kts still does, though)
206 src/WeatherCM/FGLocalWeatherDatabase.cpp
207 -- commented out the code to put the weather data on the bus, a
208 different scheme for this is needed.
210 Revision 1.2 1999/10/29 16:08:33 curt
211 Added flaps support to c172 model.
213 Revision 1.1.1.1 1999/06/17 18:07:33 curt
214 Start of 0.7.x branch
216 Revision 1.1.1.1 1999/04/05 21:32:45 curt
217 Start of 0.6.x branch.
219 Revision 1.4 1998/08/24 20:09:27 curt
220 Code optimization tweaks from Norman Vine.
222 Revision 1.3 1998/07/12 03:11:04 curt
223 Removed some printf()'s.
224 Fixed the autopilot integration so it should be able to update it's control
225 positions every time the internal flight model loop is run, and not just
226 once per rendered frame.
227 Added a routine to do the necessary stuff to force an arbitrary altitude
229 Gave the Navion engine just a tad more power.
231 Revision 1.2 1998/01/19 18:40:28 curt
232 Tons of little changes to clean up the code and to remove fatal errors
233 when building with the c++ compiler.
235 Revision 1.1 1997/05/29 00:09:59 curt
236 Initial Flight Gear revision.
238 * Revision 1.5 1995/03/02 20:24:13 bjax
239 * Added logic to avoid adding additional increment to V_east
240 * in case V_east already accounts for rotating earth. EBJ
242 * Revision 1.4 1995/02/07 20:52:21 bjax
243 * Added initialization of Alpha_dot and Beta_dot to zero on first
244 * pass; they get calculated by ls_aux on next pass... EBJ
246 * Revision 1.3 1994/01/11 19:01:12 bjax
247 * Changed from DATA to SCALAR type; also fixed header files (was ls_eom.h)
249 * Revision 1.2 1993/06/02 15:03:09 bjax
250 * Moved initialization of geocentric position to subroutine ls_geod_to_geoc.
252 * Revision 1.1 92/12/30 13:16:11 bjax
256 ----------------------------------------------------------------------------
260 [ 1] McFarland, Richard E.: "A Standard Kinematic Model
261 for Flight Simulation at NASA-Ames", NASA CR-2497,
264 [ 2] ANSI/AIAA R-004-1992 "Recommended Practice: Atmos-
265 pheric and Space Flight Vehicle Coordinate Systems",
269 ----------------------------------------------------------------------------
273 ----------------------------------------------------------------------------
277 ----------------------------------------------------------------------------
279 INPUTS: State derivatives
281 ----------------------------------------------------------------------------
285 --------------------------------------------------------------------------*/
287 #include <FDM/UIUCModel/uiuc_wrapper.h>
289 #include "ls_types.h"
290 #include "ls_constants.h"
291 #include "ls_generic.h"
292 #include "ls_accel.h"
294 #include "ls_model.h"
296 #include "ls_geodesy.h"
297 #include "ls_gravity.h"
298 /* #include "ls_sim_control.h" */
302 extern SCALAR Simtime; /* defined in ls_main.c */
304 void uiuc_init_vars() {
309 uiuc_init_aeromodel();
316 void ls_step( SCALAR dt, int Initialize ) {
317 static int inited = 0;
319 static SCALAR v_dot_north_past, v_dot_east_past, v_dot_down_past;
320 static SCALAR latitude_dot_past, longitude_dot_past, radius_dot_past;
321 static SCALAR p_dot_body_past, q_dot_body_past, r_dot_body_past;
322 SCALAR p_local_in_body, q_local_in_body, r_local_in_body;
323 SCALAR epsilon, inv_eps, local_gnd_veast;
324 SCALAR e_dot_0, e_dot_1, e_dot_2, e_dot_3;
325 static SCALAR e_0, e_1, e_2, e_3;
326 static SCALAR e_dot_0_past, e_dot_1_past, e_dot_2_past, e_dot_3_past;
327 SCALAR cos_Lat_geocentric, inv_Radius_to_vehicle;
329 /* I N I T I A L I Z A T I O N */
332 if ( (inited == 0) || (Initialize != 0) )
334 /* Set past values to zero */
335 v_dot_north_past = v_dot_east_past = v_dot_down_past = 0;
336 latitude_dot_past = longitude_dot_past = radius_dot_past = 0;
337 p_dot_body_past = q_dot_body_past = r_dot_body_past = 0;
338 e_dot_0_past = e_dot_1_past = e_dot_2_past = e_dot_3_past = 0;
340 /* Initialize geocentric position from geodetic latitude and altitude */
342 ls_geod_to_geoc( Latitude, Altitude, &Sea_level_radius, &Lat_geocentric);
343 Earth_position_angle = 0;
344 Lon_geocentric = Longitude;
345 Radius_to_vehicle = Altitude + Sea_level_radius;
347 /* Correct eastward velocity to account for earths' rotation, if necessary */
349 local_gnd_veast = OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric);
350 if( fabs(V_east - V_east_rel_ground) < 0.8*local_gnd_veast )
351 V_east = V_east + local_gnd_veast;
353 /* Initialize quaternions and transformation matrix from Euler angles */
354 if (current_model == UIUC && Simtime == 0) {
358 e_0 = cos(Psi*0.5)*cos(Theta*0.5)*cos(Phi*0.5)
359 + sin(Psi*0.5)*sin(Theta*0.5)*sin(Phi*0.5);
360 e_1 = cos(Psi*0.5)*cos(Theta*0.5)*sin(Phi*0.5)
361 - sin(Psi*0.5)*sin(Theta*0.5)*cos(Phi*0.5);
362 e_2 = cos(Psi*0.5)*sin(Theta*0.5)*cos(Phi*0.5)
363 + sin(Psi*0.5)*cos(Theta*0.5)*sin(Phi*0.5);
364 e_3 =-cos(Psi*0.5)*sin(Theta*0.5)*sin(Phi*0.5)
365 + sin(Psi*0.5)*cos(Theta*0.5)*cos(Phi*0.5);
366 T_local_to_body_11 = e_0*e_0 + e_1*e_1 - e_2*e_2 - e_3*e_3;
367 T_local_to_body_12 = 2*(e_1*e_2 + e_0*e_3);
368 T_local_to_body_13 = 2*(e_1*e_3 - e_0*e_2);
369 T_local_to_body_21 = 2*(e_1*e_2 - e_0*e_3);
370 T_local_to_body_22 = e_0*e_0 - e_1*e_1 + e_2*e_2 - e_3*e_3;
371 T_local_to_body_23 = 2*(e_2*e_3 + e_0*e_1);
372 T_local_to_body_31 = 2*(e_1*e_3 + e_0*e_2);
373 T_local_to_body_32 = 2*(e_2*e_3 - e_0*e_1);
374 T_local_to_body_33 = e_0*e_0 - e_1*e_1 - e_2*e_2 + e_3*e_3;
376 if (current_model == UIUC && Simtime == 0) {
380 /* Calculate local gravitation acceleration */
382 ls_gravity( Radius_to_vehicle, Lat_geocentric, &Gravity );
384 /* Initialize vehicle model */
389 /* Calculate initial accelerations */
393 /* Initialize auxiliary variables */
399 /* set flag; disable integrators */
409 Simtime = Simtime + dt;
411 /* L I N E A R V E L O C I T I E S */
413 /* Integrate linear accelerations to get velocities */
414 /* Using predictive Adams-Bashford algorithm */
416 V_north = V_north + dth*(3*V_dot_north - v_dot_north_past);
417 V_east = V_east + dth*(3*V_dot_east - v_dot_east_past );
418 V_down = V_down + dth*(3*V_dot_down - v_dot_down_past );
420 /* record past states */
422 v_dot_north_past = V_dot_north;
423 v_dot_east_past = V_dot_east;
424 v_dot_down_past = V_dot_down;
426 /* Calculate trajectory rate (geocentric coordinates) */
428 inv_Radius_to_vehicle = 1.0/Radius_to_vehicle;
429 cos_Lat_geocentric = cos(Lat_geocentric);
431 if ( cos_Lat_geocentric != 0) {
432 Longitude_dot = V_east/(Radius_to_vehicle*cos_Lat_geocentric);
435 Latitude_dot = V_north*inv_Radius_to_vehicle;
436 Radius_dot = -V_down;
438 /* 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 */
440 /* Integrate rotational accelerations to get velocities */
442 P_body = P_body + dth*(3*P_dot_body - p_dot_body_past);
443 Q_body = Q_body + dth*(3*Q_dot_body - q_dot_body_past);
444 R_body = R_body + dth*(3*R_dot_body - r_dot_body_past);
446 /* Save past states */
448 p_dot_body_past = P_dot_body;
449 q_dot_body_past = Q_dot_body;
450 r_dot_body_past = R_dot_body;
452 /* Calculate local axis frame rates due to travel over curved earth */
454 P_local = V_east*inv_Radius_to_vehicle;
455 Q_local = -V_north*inv_Radius_to_vehicle;
456 R_local = -V_east*tan(Lat_geocentric)*inv_Radius_to_vehicle;
458 /* Transform local axis frame rates to body axis rates */
460 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;
461 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;
462 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;
464 /* Calculate total angular rates in body axis */
466 P_total = P_body - p_local_in_body;
467 Q_total = Q_body - q_local_in_body;
468 R_total = R_body - r_local_in_body;
470 /* Transform to quaternion rates (see Appendix E in [2]) */
472 e_dot_0 = 0.5*( -P_total*e_1 - Q_total*e_2 - R_total*e_3 );
473 e_dot_1 = 0.5*( P_total*e_0 - Q_total*e_3 + R_total*e_2 );
474 e_dot_2 = 0.5*( P_total*e_3 + Q_total*e_0 - R_total*e_1 );
475 e_dot_3 = 0.5*( -P_total*e_2 + Q_total*e_1 + R_total*e_0 );
477 /* Integrate using trapezoidal as before */
479 e_0 = e_0 + dth*(e_dot_0 + e_dot_0_past);
480 e_1 = e_1 + dth*(e_dot_1 + e_dot_1_past);
481 e_2 = e_2 + dth*(e_dot_2 + e_dot_2_past);
482 e_3 = e_3 + dth*(e_dot_3 + e_dot_3_past);
484 /* calculate orthagonality correction - scale quaternion to unity length */
486 epsilon = sqrt(e_0*e_0 + e_1*e_1 + e_2*e_2 + e_3*e_3);
494 /* Save past values */
496 e_dot_0_past = e_dot_0;
497 e_dot_1_past = e_dot_1;
498 e_dot_2_past = e_dot_2;
499 e_dot_3_past = e_dot_3;
501 /* Update local to body transformation matrix */
503 T_local_to_body_11 = e_0*e_0 + e_1*e_1 - e_2*e_2 - e_3*e_3;
504 T_local_to_body_12 = 2*(e_1*e_2 + e_0*e_3);
505 T_local_to_body_13 = 2*(e_1*e_3 - e_0*e_2);
506 T_local_to_body_21 = 2*(e_1*e_2 - e_0*e_3);
507 T_local_to_body_22 = e_0*e_0 - e_1*e_1 + e_2*e_2 - e_3*e_3;
508 T_local_to_body_23 = 2*(e_2*e_3 + e_0*e_1);
509 T_local_to_body_31 = 2*(e_1*e_3 + e_0*e_2);
510 T_local_to_body_32 = 2*(e_2*e_3 - e_0*e_1);
511 T_local_to_body_33 = e_0*e_0 - e_1*e_1 - e_2*e_2 + e_3*e_3;
513 /* Calculate Euler angles */
515 Theta = asin( -T_local_to_body_13 );
517 if( T_local_to_body_11 == 0 )
520 Psi = atan2( T_local_to_body_12, T_local_to_body_11 );
522 if( T_local_to_body_33 == 0 )
525 Phi = atan2( T_local_to_body_23, T_local_to_body_33 );
527 /* Resolve Psi to 0 - 359.9999 */
529 if (Psi < 0 ) Psi = Psi + 2*LS_PI;
531 /* L I N E A R P O S I T I O N S */
533 /* Trapezoidal acceleration for position */
535 Lat_geocentric = Lat_geocentric + dth*(Latitude_dot + latitude_dot_past );
536 Lon_geocentric = Lon_geocentric + dth*(Longitude_dot + longitude_dot_past);
537 Radius_to_vehicle = Radius_to_vehicle + dth*(Radius_dot + radius_dot_past );
538 Earth_position_angle = Earth_position_angle + dt*OMEGA_EARTH;
540 /* Save past values */
542 latitude_dot_past = Latitude_dot;
543 longitude_dot_past = Longitude_dot;
544 radius_dot_past = Radius_dot;
548 /*************************************************************************/