1 /***************************************************************************
2
3 TITLE: ls_step
4
5 ----------------------------------------------------------------------------
6
7 FUNCTION: Integration routine for equations of motion
8 (vehicle states)
9
10 ----------------------------------------------------------------------------
11
12 MODULE STATUS: developmental
13
14 ----------------------------------------------------------------------------
15
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.
20
21 ----------------------------------------------------------------------------
22
23 DESIGNED BY: Bruce Jackson
24
25 CODED BY: Bruce Jackson
26
27 MAINTAINED BY:
28
29 ----------------------------------------------------------------------------
30
31 MODIFICATION HISTORY:
32
33 DATE PURPOSE BY
34
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
38
39 940111 Changed from oldstyle include file ls_eom.h; also changed
40 from DATA to SCALAR type. EBJ
41
42 950207 Initialized Alpha_dot and Beta_dot to zero on first pass; calculated
43 thereafter. EBJ
44
45 950224 Added logic to avoid adding additional increment to V_east
46 in case V_east already accounts for rotating earth.
47 EBJ
48
49 CURRENT RCS HEADER:
50
51 $Header$
52 $Log$
53 Revision 1.5 2003/07/25 17:53:47 mselig
54 UIUC code initilization mods to tidy things up a bit.
55
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
58 "
59
60 Revision 1.3 2003/06/09 02:50:23 mselig
61 mods made to setup for some initializations in uiuc code
62
63 Revision 1.2 2003/05/25 12:14:46 ehofman
64 Rename some defines to prevent a namespace clash
65
66 Revision 1.1.1.1 2002/09/10 01:14:02 curt
67 Initial revision of FlightGear-0.9.0
68
69 Revision 1.5 2001/09/14 18:47:27 curt
70 More changes in support of UIUCModel.
71
72 Revision 1.4 2001/03/24 05:03:12 curt
73 SG-ified logstream.
74
75 Revision 1.3 2000/10/23 22:34:55 curt
76 I tested:
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.
84
85 I did not test:
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)
88 ADA (don't know how)
89 MagicCarpet (needs work yet)
90 External (don't know how)
91
92 known to be broken:
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.
97
98 To do:
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)
103 -- lots of cleanup
104
105 The files:
106 src/FDM/flight.cxx
107 src/FDM/flight.hxx
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.
115
116 src/FDM/ADA.cxx
117 -- bus data members now directly assigned to
118
119 src/FDM/Balloon.cxx
120 -- bus data members now directly assigned to
121 -- changed V_equiv_kts to V_calibrated_kts
122
123 src/FDM/JSBSim.cxx
124 src/FDM/JSBSim.hxx
125 -- bus data members now directly assigned to
126 -- implemented the FGInterface virtual setters with JSBSim specific
127 logic
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
133 throttle values.
134
135 src/FDM/LaRCsim.cxx
136 src/FDM/LaRCsim.hxx
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().
145
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
150 needs to initialize.
151 -- put it in src/FDM since it is a class
152
153 src/FDM/MagicCarpet.cxx
154 -- bus data members now directly assigned to
155
156 src/FDM/Makefile.am
157 -- adds LaRCsimIC.hxx and cxx
158
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
165
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
170
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
180 weather module
181
182 src/FDM/LaRCsim/ls_interface.c
183 src/FDM/LaRCsim/ls_interface.h
184 -- added function ls_set_model_dt()
185
186 src/Main/bfi.cxx
187 -- eliminated calls that set the NED speeds to body components. They
188 are no longer needed and confuse the new bus.
189
190 src/Main/fg_init.cxx
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.
198
199 src/Main/main.cxx
200 --eliminated call to fgFDMSetGroundElevation, this data is now 'pulled'
201 onto the bus every update()
202
203 src/Main/options.cxx
204 src/Main/options.hxx
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.
209
210 src/Network/garmin.cxx
211 src/Network/nmea.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)
218
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.
222
223 Revision 1.2 1999/10/29 16:08:33 curt
224 Added flaps support to c172 model.
225
226 Revision 1.1.1.1 1999/06/17 18:07:33 curt
227 Start of 0.7.x branch
228
229 Revision 1.1.1.1 1999/04/05 21:32:45 curt
230 Start of 0.6.x branch.
231
232 Revision 1.4 1998/08/24 20:09:27 curt
233 Code optimization tweaks from Norman Vine.
234
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
241 change.
242 Gave the Navion engine just a tad more power.
243
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.
247
248 Revision 1.1 1997/05/29 00:09:59 curt
249 Initial Flight Gear revision.
250
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
254 *
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
258 *
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)
261 *
262 * Revision 1.2 1993/06/02 15:03:09 bjax
263 * Moved initialization of geocentric position to subroutine ls_geod_to_geoc.
264 *
265 * Revision 1.1 92/12/30 13:16:11 bjax
266 * Initial revision
267 *
268
269 ----------------------------------------------------------------------------
270
271 REFERENCES:
272
273 [ 1] McFarland, Richard E.: "A Standard Kinematic Model
274 for Flight Simulation at NASA-Ames", NASA CR-2497,
275 January 1975
276
277 [ 2] ANSI/AIAA R-004-1992 "Recommended Practice: Atmos-
278 pheric and Space Flight Vehicle Coordinate Systems",
279 February 1992
280
281
282 ----------------------------------------------------------------------------
283
284 CALLED BY:
285
286 ----------------------------------------------------------------------------
287
288 CALLS TO: None.
289
290 ----------------------------------------------------------------------------
291
292 INPUTS: State derivatives
293
294 ----------------------------------------------------------------------------
295
296 OUTPUTS: States
297
298 --------------------------------------------------------------------------*/
299
300 #include "config.h"
301
302 #include "ls_types.h"
303 #include "ls_constants.h"
304 #include "ls_generic.h"
305 #include "ls_accel.h"
306 #include "ls_aux.h"
307 #include "ls_model.h"
308 #include "ls_step.h"
309 #include "ls_geodesy.h"
310 #include "ls_gravity.h"
311 #include "default_model_routines.h"
312 /* #include "ls_sim_control.h" */
313 #include <math.h>
314
315 extern Model current_model; /* defined in ls_model.c */
316 extern SCALAR Simtime; /* defined in ls_main.c */
317
318 #ifdef ENABLE_UIUC_MODEL
319 # include "FDM/UIUCModel/uiuc_wrapper.h"
320
uiuc_init_vars()321 void uiuc_init_vars() {
322 static int init = 0;
323
324 if (init==0) {
325 init=-1;
326 uiuc_init_aeromodel();
327 }
328
329 uiuc_initial_init();
330 }
331 #endif
332
ls_step(SCALAR dt,int Initialize)333 void ls_step( SCALAR dt, int Initialize ) {
334 static int inited = 0;
335 SCALAR dth;
336 static SCALAR v_dot_north_past, v_dot_east_past, v_dot_down_past;
337 static SCALAR latitude_dot_past, longitude_dot_past, radius_dot_past;
338 static SCALAR p_dot_body_past, q_dot_body_past, r_dot_body_past;
339 SCALAR p_local_in_body, q_local_in_body, r_local_in_body;
340 SCALAR epsilon, inv_eps, local_gnd_veast;
341 SCALAR e_dot_0, e_dot_1, e_dot_2, e_dot_3;
342 static SCALAR e_0, e_1, e_2, e_3;
343 static SCALAR e_dot_0_past, e_dot_1_past, e_dot_2_past, e_dot_3_past;
344 SCALAR cos_Lat_geocentric, inv_Radius_to_vehicle;
345
346 /* I N I T I A L I Z A T I O N */
347
348
349 if ( (inited == 0) || (Initialize != 0) )
350 {
351 /* Set past values to zero */
352 v_dot_north_past = v_dot_east_past = v_dot_down_past = 0;
353 latitude_dot_past = longitude_dot_past = radius_dot_past = 0;
354 p_dot_body_past = q_dot_body_past = r_dot_body_past = 0;
355 e_dot_0_past = e_dot_1_past = e_dot_2_past = e_dot_3_past = 0;
356
357 /* Initialize geocentric position from geodetic latitude and altitude */
358
359 ls_geod_to_geoc( Latitude, Altitude, &Sea_level_radius, &Lat_geocentric);
360 Earth_position_angle = 0;
361 Lon_geocentric = Longitude;
362 Radius_to_vehicle = Altitude + Sea_level_radius;
363
364 /* Correct eastward velocity to account for earths' rotation, if necessary */
365
366 local_gnd_veast = OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric);
367 if( fabs(V_east - V_east_rel_ground) < 0.8*local_gnd_veast )
368 V_east = V_east + local_gnd_veast;
369
370 /* Initialize quaternions and transformation matrix from Euler angles */
371 // Initialize UIUC aircraft model
372 #ifdef ENABLE_UIUC_MODEL
373 if (current_model == UIUC) {
374 uiuc_init_2_wrapper();
375 }
376 #endif
377 e_0 = cos(Psi*0.5)*cos(Theta*0.5)*cos(Phi*0.5)
378 + sin(Psi*0.5)*sin(Theta*0.5)*sin(Phi*0.5);
379 e_1 = cos(Psi*0.5)*cos(Theta*0.5)*sin(Phi*0.5)
380 - sin(Psi*0.5)*sin(Theta*0.5)*cos(Phi*0.5);
381 e_2 = cos(Psi*0.5)*sin(Theta*0.5)*cos(Phi*0.5)
382 + sin(Psi*0.5)*cos(Theta*0.5)*sin(Phi*0.5);
383 e_3 =-cos(Psi*0.5)*sin(Theta*0.5)*sin(Phi*0.5)
384 + sin(Psi*0.5)*cos(Theta*0.5)*cos(Phi*0.5);
385 T_local_to_body_11 = e_0*e_0 + e_1*e_1 - e_2*e_2 - e_3*e_3;
386 T_local_to_body_12 = 2*(e_1*e_2 + e_0*e_3);
387 T_local_to_body_13 = 2*(e_1*e_3 - e_0*e_2);
388 T_local_to_body_21 = 2*(e_1*e_2 - e_0*e_3);
389 T_local_to_body_22 = e_0*e_0 - e_1*e_1 + e_2*e_2 - e_3*e_3;
390 T_local_to_body_23 = 2*(e_2*e_3 + e_0*e_1);
391 T_local_to_body_31 = 2*(e_1*e_3 + e_0*e_2);
392 T_local_to_body_32 = 2*(e_2*e_3 - e_0*e_1);
393 T_local_to_body_33 = e_0*e_0 - e_1*e_1 - e_2*e_2 + e_3*e_3;
394
395 #ifdef ENABLE_UIUC_MODEL
396 // Initialize local velocities (V_north, V_east, V_down)
397 // based on transformation matrix calculated above
398 if (current_model == UIUC) {
399 uiuc_local_vel_init();
400 }
401 #endif
402
403 /* Calculate local gravitation acceleration */
404
405 ls_gravity( Radius_to_vehicle, Lat_geocentric, &Gravity );
406
407 /* Initialize vehicle model */
408
409 ls_aux();
410 ls_model(0.0, 0);
411
412 /* Calculate initial accelerations */
413
414 ls_accel();
415
416 /* Initialize auxiliary variables */
417
418 ls_aux();
419 Std_Alpha_dot = 0.;
420 Std_Beta_dot = 0.;
421
422 /* set flag; disable integrators */
423
424 inited = -1;
425 dt = 0.0;
426
427 }
428
429 /* Update time */
430
431 dth = 0.5*dt;
432 Simtime = Simtime + dt;
433
434 /* L I N E A R V E L O C I T I E S */
435
436 /* Integrate linear accelerations to get velocities */
437 /* Using predictive Adams-Bashford algorithm */
438
439 V_north = V_north + dth*(3*V_dot_north - v_dot_north_past);
440 V_east = V_east + dth*(3*V_dot_east - v_dot_east_past );
441 V_down = V_down + dth*(3*V_dot_down - v_dot_down_past );
442
443 /* record past states */
444
445 v_dot_north_past = V_dot_north;
446 v_dot_east_past = V_dot_east;
447 v_dot_down_past = V_dot_down;
448
449 /* Calculate trajectory rate (geocentric coordinates) */
450
451 inv_Radius_to_vehicle = 1.0/Radius_to_vehicle;
452 cos_Lat_geocentric = cos(Lat_geocentric);
453
454 if ( cos_Lat_geocentric != 0) {
455 Longitude_dot = V_east/(Radius_to_vehicle*cos_Lat_geocentric);
456 }
457
458 Latitude_dot = V_north*inv_Radius_to_vehicle;
459 Radius_dot = -V_down;
460
461 /* 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 */
462
463 /* Integrate rotational accelerations to get velocities */
464
465 P_body = P_body + dth*(3*P_dot_body - p_dot_body_past);
466 Q_body = Q_body + dth*(3*Q_dot_body - q_dot_body_past);
467 R_body = R_body + dth*(3*R_dot_body - r_dot_body_past);
468
469 /* Save past states */
470
471 p_dot_body_past = P_dot_body;
472 q_dot_body_past = Q_dot_body;
473 r_dot_body_past = R_dot_body;
474
475 /* Calculate local axis frame rates due to travel over curved earth */
476
477 P_local = V_east*inv_Radius_to_vehicle;
478 Q_local = -V_north*inv_Radius_to_vehicle;
479 R_local = -V_east*tan(Lat_geocentric)*inv_Radius_to_vehicle;
480
481 /* Transform local axis frame rates to body axis rates */
482
483 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;
484 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;
485 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;
486
487 /* Calculate total angular rates in body axis */
488
489 P_total = P_body - p_local_in_body;
490 Q_total = Q_body - q_local_in_body;
491 R_total = R_body - r_local_in_body;
492
493 /* Transform to quaternion rates (see Appendix E in [2]) */
494
495 e_dot_0 = 0.5*( -P_total*e_1 - Q_total*e_2 - R_total*e_3 );
496 e_dot_1 = 0.5*( P_total*e_0 - Q_total*e_3 + R_total*e_2 );
497 e_dot_2 = 0.5*( P_total*e_3 + Q_total*e_0 - R_total*e_1 );
498 e_dot_3 = 0.5*( -P_total*e_2 + Q_total*e_1 + R_total*e_0 );
499
500 /* Integrate using trapezoidal as before */
501
502 e_0 = e_0 + dth*(e_dot_0 + e_dot_0_past);
503 e_1 = e_1 + dth*(e_dot_1 + e_dot_1_past);
504 e_2 = e_2 + dth*(e_dot_2 + e_dot_2_past);
505 e_3 = e_3 + dth*(e_dot_3 + e_dot_3_past);
506
507 /* calculate orthagonality correction - scale quaternion to unity length */
508
509 epsilon = sqrt(e_0*e_0 + e_1*e_1 + e_2*e_2 + e_3*e_3);
510 inv_eps = 1/epsilon;
511
512 e_0 = inv_eps*e_0;
513 e_1 = inv_eps*e_1;
514 e_2 = inv_eps*e_2;
515 e_3 = inv_eps*e_3;
516
517 /* Save past values */
518
519 e_dot_0_past = e_dot_0;
520 e_dot_1_past = e_dot_1;
521 e_dot_2_past = e_dot_2;
522 e_dot_3_past = e_dot_3;
523
524 /* Update local to body transformation matrix */
525
526 T_local_to_body_11 = e_0*e_0 + e_1*e_1 - e_2*e_2 - e_3*e_3;
527 T_local_to_body_12 = 2*(e_1*e_2 + e_0*e_3);
528 T_local_to_body_13 = 2*(e_1*e_3 - e_0*e_2);
529 T_local_to_body_21 = 2*(e_1*e_2 - e_0*e_3);
530 T_local_to_body_22 = e_0*e_0 - e_1*e_1 + e_2*e_2 - e_3*e_3;
531 T_local_to_body_23 = 2*(e_2*e_3 + e_0*e_1);
532 T_local_to_body_31 = 2*(e_1*e_3 + e_0*e_2);
533 T_local_to_body_32 = 2*(e_2*e_3 - e_0*e_1);
534 T_local_to_body_33 = e_0*e_0 - e_1*e_1 - e_2*e_2 + e_3*e_3;
535
536 /* Calculate Euler angles */
537
538 Theta = asin( -T_local_to_body_13 );
539
540 if( T_local_to_body_11 == 0 )
541 Psi = 0;
542 else
543 Psi = atan2( T_local_to_body_12, T_local_to_body_11 );
544
545 if( T_local_to_body_33 == 0 )
546 Phi = 0;
547 else
548 Phi = atan2( T_local_to_body_23, T_local_to_body_33 );
549
550 /* Resolve Psi to 0 - 359.9999 */
551
552 if (Psi < 0 ) Psi = Psi + 2*LS_PI;
553
554 /* L I N E A R P O S I T I O N S */
555
556 /* Trapezoidal acceleration for position */
557
558 Lat_geocentric = Lat_geocentric + dth*(Latitude_dot + latitude_dot_past );
559 Lon_geocentric = Lon_geocentric + dth*(Longitude_dot + longitude_dot_past);
560 Radius_to_vehicle = Radius_to_vehicle + dth*(Radius_dot + radius_dot_past );
561 Earth_position_angle = Earth_position_angle + dt*OMEGA_EARTH;
562
563 /* Save past values */
564
565 latitude_dot_past = Latitude_dot;
566 longitude_dot_past = Longitude_dot;
567 radius_dot_past = Radius_dot;
568
569 /* end of ls_step */
570 }
571 /*************************************************************************/
572
573