]> git.mxchange.org Git - flightgear.git/blob - src/FDM/flight.cxx
5fd06bad9176f6ff56ac972e81b84eb46a641f37
[flightgear.git] / src / FDM / flight.cxx
1 // flight.cxx -- a general interface to the various flight models
2 //
3 // Written by Curtis Olson, started May 1997.
4 //
5 // Copyright (C) 1997  Curtis L. Olson  - http://www.flightgear.org/~curt
6 //
7 // This program is free software; you can redistribute it and/or
8 // modify it under the terms of the GNU General Public License as
9 // published by the Free Software Foundation; either version 2 of the
10 // License, or (at your option) any later version.
11 //
12 // This program is distributed in the hope that it will be useful, but
13 // WITHOUT ANY WARRANTY; without even the implied warranty of
14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
15 // General Public License for more details.
16 //
17 // You should have received a copy of the GNU General Public License
18 // along with this program; if not, write to the Free Software
19 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
20 //
21 // $Id$
22
23 #ifdef HAVE_CONFIG_H
24 #  include "config.h"
25 #endif
26
27 #include "flight.hxx"
28
29 #include <simgear/constants.h>
30 #include <simgear/debug/logstream.hxx>
31 #include <simgear/timing/timestamp.hxx>
32
33 #include <Scenery/scenery.hxx>
34 #include <Main/globals.hxx>
35 #include <Main/fg_props.hxx>
36 #include <FDM/groundcache.hxx>
37
38
39 static inline void assign(double* ptr, const SGVec3d& vec)
40 {
41   ptr[0] = vec[0];
42   ptr[1] = vec[1];
43   ptr[2] = vec[2];
44 }
45
46 // Constructor
47 FGInterface::FGInterface()
48 {
49     _setup();
50 }
51
52 FGInterface::FGInterface( double dt )
53 {
54     _setup();
55 }
56
57 // Destructor
58 FGInterface::~FGInterface() {
59     // unbind();                   // FIXME: should be called explicitly
60 }
61
62 int
63 FGInterface::_calc_multiloop (double dt)
64 {
65   // Since some time the simulation time increments we get here are
66   // already a multiple of the basic update frequency.
67   // So, there is no need to do our own multiloop rounding with all bad
68   // roundoff problems when we already have nearly accurate values.
69   // Only the speedup thing must be still handled here
70   int hz = fgGetInt("/sim/model-hz");
71   int multiloop = SGMiscd::roundToInt(dt*hz);
72   int speedup = fgGetInt("/sim/speed-up");
73   return multiloop * speedup;
74 }
75
76
77 /**
78  * Set default values for the state of the FDM.
79  *
80  * This method is invoked by the constructors.
81  */
82 void
83 FGInterface::_setup ()
84 {
85     inited = false;
86     bound = false;
87
88     d_cg_rp_body_v = SGVec3d::zeros();
89     v_dot_local_v = SGVec3d::zeros();
90     v_dot_body_v = SGVec3d::zeros();
91     a_cg_body_v = SGVec3d::zeros();
92     a_pilot_body_v = SGVec3d::zeros();
93     n_cg_body_v = SGVec3d::zeros();
94     v_local_v = SGVec3d::zeros();
95     v_local_rel_ground_v = SGVec3d::zeros();
96     v_local_airmass_v = SGVec3d::zeros();
97     v_wind_body_v = SGVec3d::zeros();
98     omega_body_v = SGVec3d::zeros();
99     euler_rates_v = SGVec3d::zeros();
100     geocentric_rates_v = SGVec3d::zeros();
101     geodetic_position_v = SGGeod::fromRadM(0, 0, 0);
102     cartesian_position_v = SGVec3d::fromGeod(geodetic_position_v);
103     geocentric_position_v = SGGeoc::fromCart(cartesian_position_v);
104     euler_angles_v = SGVec3d::zeros();
105     
106     nlf=0;
107     v_rel_wind=v_true_kts=0;
108     v_ground_speed=v_equiv_kts=0;
109     v_calibrated_kts=0;
110     alpha=beta=0;
111     gamma_vert_rad=0;
112     density=mach_number=0;
113     static_pressure=total_pressure=0;
114     dynamic_pressure=0;
115     static_temperature=total_temperature=0;
116     sea_level_radius=earth_position_angle=0;
117     runway_altitude=0;
118     climb_rate=0;
119     altitude_agl=0;
120     track=0;
121 }
122
123 void
124 FGInterface::init () {}
125
126 /**
127  * Initialize the state of the FDM.
128  *
129  * Subclasses of FGInterface may do their own, additional initialization,
130  * but there is some that is common to all.  Normally, they should call
131  * this before they begin their own init to make sure the basic structures
132  * are set up properly.
133  */
134 void
135 FGInterface::common_init ()
136 {
137     SG_LOG( SG_FLIGHT, SG_INFO, "Start common FDM init" );
138
139     set_inited( true );
140
141     ground_cache.set_cache_time_offset(globals->get_sim_time_sec());
142
143     // Set initial position
144     SG_LOG( SG_FLIGHT, SG_INFO, "...initializing position..." );
145     double lon = fgGetDouble("/sim/presets/longitude-deg")
146       * SGD_DEGREES_TO_RADIANS;
147     double lat = fgGetDouble("/sim/presets/latitude-deg")
148       * SGD_DEGREES_TO_RADIANS;
149     double alt_ft = fgGetDouble("/sim/presets/altitude-ft");
150     double alt_m = alt_ft * SG_FEET_TO_METER;
151     set_Longitude( lon );
152     set_Latitude( lat );
153     SG_LOG( SG_FLIGHT, SG_INFO, "Checking for lon = "
154             << lon*SGD_RADIANS_TO_DEGREES << "deg, lat = "
155             << lat*SGD_RADIANS_TO_DEGREES << "deg, alt = "
156             << alt_ft << "ft");
157
158     double ground_elev_m = get_groundlevel_m(lat, lon, alt_m);
159     double ground_elev_ft = ground_elev_m * SG_METER_TO_FEET;
160     _set_Runway_altitude ( ground_elev_ft );
161
162     // Set aircraft altitude
163     if ( fgGetBool("/sim/presets/onground") || alt_ft < ground_elev_ft ) {
164         fgSetDouble("/position/altitude-ft", ground_elev_ft + 0.1);
165         set_Altitude( ground_elev_ft + 0.1);
166     } else {
167         set_Altitude( alt_ft );
168     }
169
170     // Set ground elevation
171     SG_LOG( SG_FLIGHT, SG_INFO,
172             "...initializing ground elevation to " << ground_elev_ft
173             << "ft..." );
174
175     // Set sea-level radius
176     SG_LOG( SG_FLIGHT, SG_INFO, "...initializing sea-level radius..." );
177     SG_LOG( SG_FLIGHT, SG_INFO, " lat = "
178             << fgGetDouble("/sim/presets/latitude-deg")
179             << " alt = " << get_Altitude() );
180     double slr = SGGeodesy::SGGeodToSeaLevelRadius(geodetic_position_v);
181     _set_Sea_level_radius( slr * SG_METER_TO_FEET );
182
183     // Set initial Euler angles
184     SG_LOG( SG_FLIGHT, SG_INFO, "...initializing Euler angles..." );
185     set_Euler_Angles( fgGetDouble("/sim/presets/roll-deg")
186                         * SGD_DEGREES_TO_RADIANS,
187                       fgGetDouble("/sim/presets/pitch-deg")
188                         * SGD_DEGREES_TO_RADIANS,
189                       fgGetDouble("/sim/presets/heading-deg")
190                         * SGD_DEGREES_TO_RADIANS );
191
192     // Set initial velocities
193     SG_LOG( SG_FLIGHT, SG_INFO, "...initializing velocities..." );
194     if ( !fgHasNode("/sim/presets/speed-set") ) {
195         set_V_calibrated_kts(0.0);
196     } else {
197         const string speedset = fgGetString("/sim/presets/speed-set");
198         if ( speedset == "knots" || speedset == "KNOTS" ) {
199             set_V_calibrated_kts( fgGetDouble("/sim/presets/airspeed-kt") );
200         } else if ( speedset == "mach" || speedset == "MACH" ) {
201             set_Mach_number( fgGetDouble("/sim/presets/mach") );
202         } else if ( speedset == "UVW" || speedset == "uvw" ) {
203             set_Velocities_Wind_Body(
204                                      fgGetDouble("/sim/presets/uBody-fps"),
205                                      fgGetDouble("/sim/presets/vBody-fps"),
206                                      fgGetDouble("/sim/presets/wBody-fps") );
207         } else if ( speedset == "NED" || speedset == "ned" ) {
208             set_Velocities_Local(
209                                  fgGetDouble("/sim/presets/speed-north-fps"),
210                                  fgGetDouble("/sim/presets/speed-east-fps"),
211                                  fgGetDouble("/sim/presets/speed-down-fps") );
212         } else {
213             SG_LOG( SG_FLIGHT, SG_ALERT,
214                     "Unrecognized value for /sim/presets/speed-set: "
215                     << speedset);
216             set_V_calibrated_kts( 0.0 );
217         }
218     }
219
220     if ( fgHasNode("/sim/presets/glideslope-deg") )
221         set_Gamma_vert_rad( fgGetDouble("/sim/presets/glideslope-deg")
222                               * SGD_DEGREES_TO_RADIANS );
223     else if ( fgHasNode("/sim/presets/speed-set") &&
224               fgHasNode( "/sim/presets/vertical-speed-fps") )
225     {
226         set_Climb_Rate( fgGetDouble("/sim/presets/vertical-speed-fps") );
227     }
228
229     SG_LOG( SG_FLIGHT, SG_INFO, "End common FDM init" );
230 }
231
232
233 /**
234  * Bind getters and setters to properties.
235  *
236  * The bind() method will be invoked after init().  Note that unlike
237  * the usual implementations of FGSubsystem::bind(), this method does
238  * not automatically pick up existing values for the properties at
239  * bind time; instead, all values are set explicitly in the init()
240  * method.
241  */
242 void
243 FGInterface::bind ()
244 {
245   bound = true;
246
247   _tiedProperties.setRoot(globals->get_props());
248   // Aircraft position
249   _tiedProperties.Tie("/position/latitude-deg", this,
250                       &FGInterface::get_Latitude_deg,
251                       &FGInterface::set_Latitude_deg,
252                       false);
253   fgSetArchivable("/position/latitude-deg");
254   _tiedProperties.Tie("/position/longitude-deg", this,
255                       &FGInterface::get_Longitude_deg,
256                       &FGInterface::set_Longitude_deg,
257                       false);
258   fgSetArchivable("/position/longitude-deg");
259   _tiedProperties.Tie("/position/altitude-ft", this,
260                       &FGInterface::get_Altitude,
261                       &FGInterface::set_Altitude,
262                       false);
263   fgSetArchivable("/position/altitude-ft");
264   _tiedProperties.Tie("/position/altitude-agl-ft", this,
265                       &FGInterface::get_Altitude_AGL, &FGInterface::set_AltitudeAGL, false);
266   fgSetArchivable("/position/ground-elev-ft");
267   _tiedProperties.Tie("/position/ground-elev-ft", this,
268                       &FGInterface::get_Runway_altitude); // read-only
269   fgSetArchivable("/position/ground-elev-m");
270   _tiedProperties.Tie("/position/ground-elev-m", this,
271                       &FGInterface::get_Runway_altitude_m); // read-only
272   _tiedProperties.Tie("/environment/ground-elevation-m", this,
273                       &FGInterface::get_Runway_altitude_m); // read-only
274   fgSetArchivable("/position/sea-level-radius-ft");
275   _tiedProperties.Tie("/position/sea-level-radius-ft", this,
276                       &FGInterface::get_Sea_level_radius,
277                       &FGInterface::_set_Sea_level_radius, false);
278
279   // Orientation
280   _tiedProperties.Tie("/orientation/roll-deg", this,
281                       &FGInterface::get_Phi_deg,
282                       &FGInterface::set_Phi_deg, false);
283   fgSetArchivable("/orientation/roll-deg");
284   _tiedProperties.Tie("/orientation/pitch-deg", this,
285                       &FGInterface::get_Theta_deg,
286                       &FGInterface::set_Theta_deg, false);
287   fgSetArchivable("/orientation/pitch-deg");
288   _tiedProperties.Tie("/orientation/heading-deg", this,
289                       &FGInterface::get_Psi_deg,
290                       &FGInterface::set_Psi_deg, false);
291   fgSetArchivable("/orientation/heading-deg");
292   _tiedProperties.Tie("/orientation/track-deg", this,
293                       &FGInterface::get_Track); // read-only
294
295   // Body-axis "euler rates" (rotation speed, but in a funny
296   // representation).
297   _tiedProperties.Tie("/orientation/roll-rate-degps", this,
298                       &FGInterface::get_Phi_dot_degps,
299                       &FGInterface::set_Phi_dot_degps, false);
300   _tiedProperties.Tie("/orientation/pitch-rate-degps", this,
301                       &FGInterface::get_Theta_dot_degps,
302                       &FGInterface::set_Theta_dot_degps, false);
303   _tiedProperties.Tie("/orientation/yaw-rate-degps", this,
304                       &FGInterface::get_Psi_dot_degps,
305                       &FGInterface::set_Psi_dot_degps, false);
306
307   _tiedProperties.Tie("/orientation/p-body", this, &FGInterface::get_P_body); // read-only
308   _tiedProperties.Tie("/orientation/q-body", this, &FGInterface::get_Q_body); // read-only
309   _tiedProperties.Tie("/orientation/r-body", this, &FGInterface::get_R_body); // read-only
310
311   // Ground speed knots
312   _tiedProperties.Tie("/velocities/groundspeed-kt", this,
313                       &FGInterface::get_V_ground_speed_kt,
314                       &FGInterface::set_V_ground_speed_kt); // read-only
315
316   // Calibrated airspeed
317   _tiedProperties.Tie("/velocities/airspeed-kt", this,
318                       &FGInterface::get_V_calibrated_kts,
319                       &FGInterface::set_V_calibrated_kts,
320                       false);
321
322   _tiedProperties.Tie("/velocities/equivalent-kt", this,
323                       &FGInterface::get_V_equiv_kts); // read-only
324
325   // Mach number
326   _tiedProperties.Tie("/velocities/mach", this,
327                       &FGInterface::get_Mach_number,
328                       &FGInterface::set_Mach_number,
329                       false);
330
331   // Local velocities
332 //   _tiedProperties.Tie("/velocities/speed-north-fps", this,
333 //                       &FGInterface::get_V_north,
334 //                       &FGInterface::set_V_north);
335 //   fgSetArchivable("/velocities/speed-north-fps");
336 //   _tiedProperties.Tie("/velocities/speed-east-fps", this,
337 //                       &FGInterface::get_V_east,
338 //                       &FGInterface::set_V_east);
339 //   fgSetArchivable("/velocities/speed-east-fps");
340 //   _tiedProperties.Tie("/velocities/speed-down-fps", this,
341 //                       &FGInterface::get_V_down,
342 //                       &FGInterface::set_V_down);
343 //   fgSetArchivable("/velocities/speed-down-fps");
344
345   // FIXME: Temporarily read-only, until the
346   // incompatibilities between JSBSim and
347   // LaRCSim are fixed (LaRCSim adds the
348   // earth's rotation to the east velocity).
349   _tiedProperties.Tie("/velocities/speed-north-fps", this,
350                       &FGInterface::get_V_north, &FGInterface::set_V_north, false);
351   _tiedProperties.Tie("/velocities/speed-east-fps", this,
352                       &FGInterface::get_V_east, &FGInterface::set_V_east, false);
353   _tiedProperties.Tie("/velocities/speed-down-fps", this,
354                       &FGInterface::get_V_down, &FGInterface::set_V_down, false);
355
356   _tiedProperties.Tie("/velocities/north-relground-fps", this,
357                       &FGInterface::get_V_north_rel_ground); // read-only
358   _tiedProperties.Tie("/velocities/east-relground-fps", this,
359                       &FGInterface::get_V_east_rel_ground); // read-only
360   _tiedProperties.Tie("/velocities/down-relground-fps", this,
361                       &FGInterface::get_V_down_rel_ground); // read-only
362
363   // Relative wind
364   // FIXME: temporarily archivable, until the NED problem is fixed.
365   _tiedProperties.Tie("/velocities/uBody-fps", this,
366                       &FGInterface::get_uBody,
367                       &FGInterface::set_uBody,
368                       false);
369   fgSetArchivable("/velocities/uBody-fps");
370   _tiedProperties.Tie("/velocities/vBody-fps", this,
371                       &FGInterface::get_vBody,
372                       &FGInterface::set_vBody,
373                       false);
374   fgSetArchivable("/velocities/vBody-fps");
375   _tiedProperties.Tie("/velocities/wBody-fps", this,
376                       &FGInterface::get_wBody,
377                       &FGInterface::set_wBody,
378                       false);
379   fgSetArchivable("/velocities/wBody-fps");
380
381   // Climb and slip (read-only)
382   _tiedProperties.Tie("/velocities/vertical-speed-fps", this,
383                       &FGInterface::get_Climb_Rate,
384                       &FGInterface::set_Climb_Rate, false );
385   _tiedProperties.Tie("/velocities/glideslope", this,
386                       &FGInterface::get_Gamma_vert_rad,
387                       &FGInterface::set_Gamma_vert_rad, false );
388   _tiedProperties.Tie("/orientation/side-slip-rad", this,
389                       &FGInterface::get_Beta, &FGInterface::_set_Beta, false);
390   _tiedProperties.Tie("/orientation/side-slip-deg", this,
391                       &FGInterface::get_Beta_deg); // read-only
392   _tiedProperties.Tie("/orientation/alpha-deg", this,
393                       &FGInterface::get_Alpha_deg, &FGInterface::set_Alpha_deg, false);
394   _tiedProperties.Tie("/accelerations/nlf", this,
395                       &FGInterface::get_Nlf); // read-only
396
397   // NED accelerations
398   _tiedProperties.Tie("/accelerations/ned/north-accel-fps_sec",
399                       this, &FGInterface::get_V_dot_north); // read-only
400   _tiedProperties.Tie("/accelerations/ned/east-accel-fps_sec",
401                       this, &FGInterface::get_V_dot_east); // read-only
402   _tiedProperties.Tie("/accelerations/ned/down-accel-fps_sec",
403                       this, &FGInterface::get_V_dot_down); // read-only
404
405   // Pilot accelerations
406   _tiedProperties.Tie("/accelerations/pilot/x-accel-fps_sec",
407                       this, &FGInterface::get_A_X_pilot, &FGInterface::set_A_X_pilot, false);
408   _tiedProperties.Tie("/accelerations/pilot/y-accel-fps_sec",
409                       this, &FGInterface::get_A_Y_pilot, &FGInterface::set_A_Y_pilot, false);
410   _tiedProperties.Tie("/accelerations/pilot/z-accel-fps_sec",
411                       this, &FGInterface::get_A_Z_pilot, &FGInterface::set_A_Z_pilot, false);
412
413   _tiedProperties.Tie("/accelerations/n-z-cg-fps_sec",
414                       this, &FGInterface::get_N_Z_cg); // read-only
415 }
416
417
418 /**
419  * Unbind any properties bound to this FDM.
420  *
421  * This method allows the FDM to release properties so that a new
422  * FDM can bind them instead.
423  */
424 void
425 FGInterface::unbind ()
426 {
427   _tiedProperties.Untie();
428   bound = false;
429 }
430
431 /**
432  * Update the state of the FDM (i.e. run the equations of motion).
433  */
434 void
435 FGInterface::update (double dt)
436 {
437     SG_LOG(SG_FLIGHT, SG_ALERT, "dummy update() ... SHOULDN'T BE CALLED!");
438 }
439
440
441 void FGInterface::_updatePositionM(const SGVec3d& cartPos)
442 {
443     TrackComputer tracker( track, geodetic_position_v );
444     cartesian_position_v = cartPos;
445     geodetic_position_v = SGGeod::fromCart(cartesian_position_v);
446     geocentric_position_v = SGGeoc::fromCart(cartesian_position_v);
447     _set_Sea_level_radius( SGGeodesy::SGGeodToSeaLevelRadius(geodetic_position_v)*SG_METER_TO_FEET );
448     _update_ground_elev_at_pos();
449 }
450
451
452 void FGInterface::_updatePosition(const SGGeod& geod)
453 {
454     TrackComputer tracker( track, geodetic_position_v );
455     geodetic_position_v = geod;
456     cartesian_position_v = SGVec3d::fromGeod(geodetic_position_v);
457     geocentric_position_v = SGGeoc::fromCart(cartesian_position_v);
458
459     _set_Sea_level_radius( SGGeodesy::SGGeodToSeaLevelRadius(geodetic_position_v)*SG_METER_TO_FEET );
460     _update_ground_elev_at_pos();
461 }
462
463
464 void FGInterface::_updatePosition(const SGGeoc& geoc)
465 {
466     TrackComputer tracker( track, geodetic_position_v );
467     geocentric_position_v = geoc;
468     cartesian_position_v = SGVec3d::fromGeoc(geocentric_position_v);
469     geodetic_position_v = SGGeod::fromCart(cartesian_position_v);
470
471     _set_Sea_level_radius( SGGeodesy::SGGeodToSeaLevelRadius(geodetic_position_v)*SG_METER_TO_FEET );
472     _update_ground_elev_at_pos();
473 }
474
475
476 void FGInterface::_updateGeodeticPosition( double lat, double lon, double alt )
477 {
478     _updatePosition(SGGeod::fromRadFt(lon, lat, alt));
479 }
480
481
482 void FGInterface::_updateGeocentricPosition( double lat, double lon,
483                                              double alt )
484 {
485     _updatePosition(SGGeoc::fromRadFt(lon, lat, get_Sea_level_radius() + alt));
486 }
487
488 void FGInterface::_update_ground_elev_at_pos( void ) {
489     double groundlevel_m = get_groundlevel_m(geodetic_position_v);
490     _set_Runway_altitude( groundlevel_m * SG_METER_TO_FEET );
491 }
492
493 // Positions
494 void FGInterface::set_Latitude(double lat) {
495     geodetic_position_v.setLatitudeRad(lat);
496 }
497
498 void FGInterface::set_Longitude(double lon) {
499     geodetic_position_v.setLongitudeRad(lon);
500 }
501
502 void FGInterface::set_Altitude(double alt) {
503     geodetic_position_v.setElevationFt(alt);
504 }
505
506 void FGInterface::set_AltitudeAGL(double altagl) {
507     altitude_agl=altagl;
508 }
509
510 // Velocities
511 void FGInterface::set_V_calibrated_kts(double vc) {
512     v_calibrated_kts = vc;
513 }
514
515 void FGInterface::set_Mach_number(double mach) {
516     mach_number = mach;
517 }
518
519 void FGInterface::set_Velocities_Local( double north, 
520                                         double east,
521                                         double down ){
522     v_local_v[0] = north;
523     v_local_v[1] = east;
524     v_local_v[2] = down;
525 }
526
527 void FGInterface::set_Velocities_Wind_Body( double u, 
528                                             double v,
529                                             double w){
530     v_wind_body_v[0] = u;
531     v_wind_body_v[1] = v;
532     v_wind_body_v[2] = w;
533 }
534
535 // Euler angles 
536 void FGInterface::set_Euler_Angles( double phi, 
537                                     double theta,
538                                     double psi ) {
539     euler_angles_v[0] = phi;
540     euler_angles_v[1] = theta;
541     euler_angles_v[2] = psi;
542 }  
543
544 // Flight Path
545 void FGInterface::set_Climb_Rate( double roc) {
546     climb_rate = roc;
547 }
548
549 void FGInterface::set_Gamma_vert_rad( double gamma) {
550     gamma_vert_rad = gamma;
551 }
552
553 void FGInterface::set_Static_pressure(double p) { static_pressure = p; }
554 void FGInterface::set_Static_temperature(double T) { static_temperature = T; }
555 void FGInterface::set_Density(double rho) { density = rho; }
556
557 void FGInterface::set_Velocities_Local_Airmass (double wnorth, 
558                                                 double weast,
559                                                 double wdown ) {
560     v_local_airmass_v[0] = wnorth;
561     v_local_airmass_v[1] = weast;
562     v_local_airmass_v[2] = wdown;
563 }
564
565
566 void FGInterface::_busdump(void)
567 {
568     SG_LOG(SG_FLIGHT,SG_INFO,"d_cg_rp_body_v: " << d_cg_rp_body_v);
569     SG_LOG(SG_FLIGHT,SG_INFO,"v_dot_local_v: " << v_dot_local_v);
570     SG_LOG(SG_FLIGHT,SG_INFO,"v_dot_body_v: " << v_dot_body_v);
571     SG_LOG(SG_FLIGHT,SG_INFO,"a_cg_body_v: " << a_cg_body_v);
572     SG_LOG(SG_FLIGHT,SG_INFO,"a_pilot_body_v: " << a_pilot_body_v);
573     SG_LOG(SG_FLIGHT,SG_INFO,"n_cg_body_v: " << n_cg_body_v);
574     SG_LOG(SG_FLIGHT,SG_INFO,"v_local_v: " << v_local_v);
575     SG_LOG(SG_FLIGHT,SG_INFO,"v_local_rel_ground_v: " << v_local_rel_ground_v);
576     SG_LOG(SG_FLIGHT,SG_INFO,"v_local_airmass_v: " << v_local_airmass_v);
577     SG_LOG(SG_FLIGHT,SG_INFO,"v_wind_body_v: " << v_wind_body_v);
578     SG_LOG(SG_FLIGHT,SG_INFO,"omega_body_v: " << omega_body_v);
579     SG_LOG(SG_FLIGHT,SG_INFO,"euler_rates_v: " << euler_rates_v);
580     SG_LOG(SG_FLIGHT,SG_INFO,"geocentric_rates_v: " << geocentric_rates_v);
581     SG_LOG(SG_FLIGHT,SG_INFO,"geocentric_position_v: " << geocentric_position_v);
582     SG_LOG(SG_FLIGHT,SG_INFO,"geodetic_position_v: " << geodetic_position_v);
583     SG_LOG(SG_FLIGHT,SG_INFO,"euler_angles_v: " << euler_angles_v);
584
585     SG_LOG(SG_FLIGHT,SG_INFO,"nlf: " << nlf );
586     SG_LOG(SG_FLIGHT,SG_INFO,"v_rel_wind: " << v_rel_wind );
587     SG_LOG(SG_FLIGHT,SG_INFO,"v_true_kts: " << v_true_kts );
588     SG_LOG(SG_FLIGHT,SG_INFO,"v_ground_speed: " << v_ground_speed );
589     SG_LOG(SG_FLIGHT,SG_INFO,"v_equiv_kts: " << v_equiv_kts );
590     SG_LOG(SG_FLIGHT,SG_INFO,"v_calibrated_kts: " << v_calibrated_kts );
591     SG_LOG(SG_FLIGHT,SG_INFO,"alpha: " << alpha );
592     SG_LOG(SG_FLIGHT,SG_INFO,"beta: " << beta );
593     SG_LOG(SG_FLIGHT,SG_INFO,"gamma_vert_rad: " << gamma_vert_rad );
594     SG_LOG(SG_FLIGHT,SG_INFO,"density: " << density );
595     SG_LOG(SG_FLIGHT,SG_INFO,"mach_number: " << mach_number );
596     SG_LOG(SG_FLIGHT,SG_INFO,"static_pressure: " << static_pressure );
597     SG_LOG(SG_FLIGHT,SG_INFO,"total_pressure: " << total_pressure );
598     SG_LOG(SG_FLIGHT,SG_INFO,"dynamic_pressure: " << dynamic_pressure );
599     SG_LOG(SG_FLIGHT,SG_INFO,"static_temperature: " << static_temperature );
600     SG_LOG(SG_FLIGHT,SG_INFO,"total_temperature: " << total_temperature );
601     SG_LOG(SG_FLIGHT,SG_INFO,"sea_level_radius: " << sea_level_radius );
602     SG_LOG(SG_FLIGHT,SG_INFO,"earth_position_angle: " << earth_position_angle );
603     SG_LOG(SG_FLIGHT,SG_INFO,"runway_altitude: " << runway_altitude );
604     SG_LOG(SG_FLIGHT,SG_INFO,"climb_rate: " << climb_rate );
605     SG_LOG(SG_FLIGHT,SG_INFO,"altitude_agl: " << altitude_agl );
606 }
607
608 bool
609 FGInterface::prepare_ground_cache_m(double startSimTime, double endSimTime,
610                                     const double pt[3], double rad)
611 {
612   return ground_cache.prepare_ground_cache(startSimTime, endSimTime,
613                                            SGVec3d(pt), rad);
614 }
615
616 bool
617 FGInterface::prepare_ground_cache_ft(double startSimTime, double endSimTime,
618                                      const double pt[3], double rad)
619 {
620   // Convert units and do the real work.
621   SGVec3d pt_ft = SG_FEET_TO_METER*SGVec3d(pt);
622   return ground_cache.prepare_ground_cache(startSimTime, endSimTime,
623                                            pt_ft, rad*SG_FEET_TO_METER);
624 }
625
626 bool
627 FGInterface::is_valid_m(double *ref_time, double pt[3], double *rad)
628 {
629   SGVec3d _pt;
630   bool valid = ground_cache.is_valid(*ref_time, _pt, *rad);
631   assign(pt, _pt);
632   return valid;
633 }
634
635 bool FGInterface::is_valid_ft(double *ref_time, double pt[3], double *rad)
636 {
637   // Convert units and do the real work.
638   SGVec3d _pt;
639   bool found_ground = ground_cache.is_valid(*ref_time, _pt, *rad);
640   assign(pt, SG_METER_TO_FEET*_pt);
641   *rad *= SG_METER_TO_FEET;
642   return found_ground;
643 }
644
645 double
646 FGInterface::get_cat_m(double t, const double pt[3],
647                        double end[2][3], double vel[2][3])
648 {
649   SGVec3d _end[2], _vel[2];
650   double dist = ground_cache.get_cat(t, SGVec3d(pt), _end, _vel);
651   for (int k=0; k<2; ++k) {
652     assign( end[k], _end[k] );
653     assign( vel[k], _vel[k] );
654   }
655   return dist;
656 }
657
658 double
659 FGInterface::get_cat_ft(double t, const double pt[3],
660                         double end[2][3], double vel[2][3])
661 {
662   // Convert units and do the real work.
663   SGVec3d pt_m = SG_FEET_TO_METER*SGVec3d(pt);
664   SGVec3d _end[2], _vel[2];
665   double dist = ground_cache.get_cat(t, pt_m, _end, _vel);
666   for (int k=0; k<2; ++k) {
667     assign( end[k], SG_METER_TO_FEET*_end[k] );
668     assign( vel[k], SG_METER_TO_FEET*_vel[k] );
669   }
670   return dist*SG_METER_TO_FEET;
671 }
672
673 bool
674 FGInterface::get_body_m(double t, simgear::BVHNode::Id id,
675                         double bodyToWorld[16], double linearVel[3],
676                         double angularVel[3])
677 {
678   SGMatrixd _bodyToWorld;
679   SGVec3d _linearVel, _angularVel;
680   if (!ground_cache.get_body(t, _bodyToWorld, _linearVel, _angularVel, id))
681     return false;
682
683   assign(linearVel, _linearVel);
684   assign(angularVel, _angularVel);
685   for (unsigned i = 0; i < 16; ++i)
686       bodyToWorld[i] = _bodyToWorld.data()[i];
687
688   return true;
689 }
690
691 bool
692 FGInterface::get_agl_m(double t, const double pt[3], double max_altoff,
693                        double contact[3], double normal[3],
694                        double linearVel[3], double angularVel[3],
695                        SGMaterial const*& material, simgear::BVHNode::Id& id)
696 {
697   SGVec3d pt_m = SGVec3d(pt) - max_altoff*ground_cache.get_down();
698   SGVec3d _contact, _normal, _linearVel, _angularVel;
699   material = 0;
700   bool ret = ground_cache.get_agl(t, pt_m, _contact, _normal, _linearVel,
701                                   _angularVel, id, material);
702   // correct the linear velocity, since the line intersector delivers
703   // values for the start point and the get_agl function should
704   // traditionally deliver for the contact point
705   _linearVel += cross(_angularVel, _contact - pt_m);
706
707   assign(contact, _contact);
708   assign(normal, _normal);
709   assign(linearVel, _linearVel);
710   assign(angularVel, _angularVel);
711   return ret;
712 }
713
714 bool
715 FGInterface::get_agl_ft(double t, const double pt[3], double max_altoff,
716                         double contact[3], double normal[3],
717                         double linearVel[3], double angularVel[3],
718                         SGMaterial const*& material, simgear::BVHNode::Id& id)
719 {
720   // Convert units and do the real work.
721   SGVec3d pt_m = SGVec3d(pt) - max_altoff*ground_cache.get_down();
722   pt_m *= SG_FEET_TO_METER;
723   SGVec3d _contact, _normal, _linearVel, _angularVel;
724   material = 0;
725   bool ret = ground_cache.get_agl(t, pt_m, _contact, _normal, _linearVel,
726                                   _angularVel, id, material);
727   // correct the linear velocity, since the line intersector delivers
728   // values for the start point and the get_agl function should
729   // traditionally deliver for the contact point
730   _linearVel += cross(_angularVel, _contact - pt_m);
731
732   // Convert units back ...
733   assign( contact, SG_METER_TO_FEET*_contact );
734   assign( normal, _normal );
735   assign( linearVel, SG_METER_TO_FEET*_linearVel );
736   assign( angularVel, _angularVel );
737   return ret;
738 }
739
740 bool
741 FGInterface::get_nearest_m(double t, const double pt[3], double maxDist,
742                            double contact[3], double normal[3],
743                            double linearVel[3], double angularVel[3],
744                            SGMaterial const*& material,
745                            simgear::BVHNode::Id& id)
746 {
747   SGVec3d _contact, _linearVel, _angularVel;
748   if (!ground_cache.get_nearest(t, SGVec3d(pt), maxDist, _contact, _linearVel,
749                                 _angularVel, id, material))
750       return false;
751
752   assign(contact, _contact);
753   assign(linearVel, _linearVel);
754   assign(angularVel, _angularVel);
755   return true;
756 }
757
758 bool
759 FGInterface::get_nearest_ft(double t, const double pt[3], double maxDist,
760                             double contact[3], double normal[3],
761                             double linearVel[3], double angularVel[3],
762                             SGMaterial const*& material,
763                             simgear::BVHNode::Id& id)
764 {
765   SGVec3d _contact, _linearVel, _angularVel;
766   if (!ground_cache.get_nearest(t, SG_FEET_TO_METER*SGVec3d(pt),
767                                 SG_FEET_TO_METER*maxDist, _contact, _linearVel,
768                                 _angularVel, id, material))
769       return false;
770
771   assign(contact, SG_METER_TO_FEET*_contact);
772   assign(linearVel, SG_METER_TO_FEET*_linearVel);
773   assign(angularVel, _angularVel);
774   return true;
775 }
776
777 double
778 FGInterface::get_groundlevel_m(double lat, double lon, double alt)
779 {
780   return get_groundlevel_m(SGGeod::fromRadM(lon, lat, alt));
781 }
782
783 double
784 FGInterface::get_groundlevel_m(const SGGeod& geod)
785 {
786   // Compute the cartesian position of the given lat/lon/alt.
787   SGVec3d pos = SGVec3d::fromGeod(geod);
788
789   // FIXME: how to handle t - ref_time differences ???
790   SGVec3d cpos;
791   double ref_time = 0, radius;
792   // Prepare the ground cache for that position.
793   if (!is_valid_m(&ref_time, cpos.data(), &radius)) {
794     double startTime = ref_time;
795     double endTime = startTime + 1;
796     bool ok = prepare_ground_cache_m(startTime, endTime, pos.data(), 10);
797     /// This is most likely the case when the given altitude is
798     /// too low, try with a new altitude of 10000m, that should be
799     /// sufficient to find a ground level below everywhere on our planet
800     if (!ok) {
801       pos = SGVec3d::fromGeod(SGGeod::fromGeodM(geod, 10000));
802       /// If there is still no ground, return sea level radius
803       if (!prepare_ground_cache_m(startTime, endTime, pos.data(), 10))
804         return 0;
805     }
806   } else if (radius*radius <= distSqr(pos, cpos)) {
807     double startTime = ref_time;
808     double endTime = startTime + 1;
809
810     /// We reuse the old radius value, but only if it is at least 10 Meters ..
811     if (!(10 < radius)) // Well this strange compare is nan safe
812       radius = 10;
813
814     bool ok = prepare_ground_cache_m(startTime, endTime, pos.data(), radius);
815     /// This is most likely the case when the given altitude is
816     /// too low, try with a new altitude of 10000m, that should be
817     /// sufficient to find a ground level below everywhere on our planet
818     if (!ok) {
819       pos = SGVec3d::fromGeod(SGGeod::fromGeodM(geod, 10000));
820       /// If there is still no ground, return sea level radius
821       if (!prepare_ground_cache_m(startTime, endTime, pos.data(), radius))
822         return 0;
823     }
824   }
825   
826   double contact[3], normal[3], vel[3], angvel[3];
827   const SGMaterial* material;
828   simgear::BVHNode::Id id;
829   // Ignore the return value here, since it just tells us if
830   // the returns stem from the groundcache or from the coarse
831   // computations below the groundcache. The contact point is still something
832   // valid, the normals and the other returns just contain some defaults.
833   get_agl_m(ref_time, pos.data(), 2.0, contact, normal, vel, angvel,
834             material, id);
835   return SGGeod::fromCart(SGVec3d(contact)).getElevationM();
836 }
837   
838 bool
839 FGInterface::caught_wire_m(double t, const double pt[4][3])
840 {
841   SGVec3d pt_m[4];
842   for (int i=0; i<4; ++i)
843     pt_m[i] = SGVec3d(pt[i]);
844   
845   return ground_cache.caught_wire(t, pt_m);
846 }
847
848 bool
849 FGInterface::caught_wire_ft(double t, const double pt[4][3])
850 {
851   // Convert units and do the real work.
852   SGVec3d pt_m[4];
853   for (int i=0; i<4; ++i)
854     pt_m[i] = SG_FEET_TO_METER*SGVec3d(pt[i]);
855     
856   return ground_cache.caught_wire(t, pt_m);
857 }
858   
859 bool
860 FGInterface::get_wire_ends_m(double t, double end[2][3], double vel[2][3])
861 {
862   SGVec3d _end[2], _vel[2];
863   bool ret = ground_cache.get_wire_ends(t, _end, _vel);
864   for (int k=0; k<2; ++k) {
865     assign( end[k], _end[k] );
866     assign( vel[k], _vel[k] );
867   }
868   return ret;
869 }
870
871 bool
872 FGInterface::get_wire_ends_ft(double t, double end[2][3], double vel[2][3])
873 {
874   // Convert units and do the real work.
875   SGVec3d _end[2], _vel[2];
876   bool ret = ground_cache.get_wire_ends(t, _end, _vel);
877   for (int k=0; k<2; ++k) {
878     assign( end[k], SG_METER_TO_FEET*_end[k] );
879     assign( vel[k], SG_METER_TO_FEET*_vel[k] );
880   }
881   return ret;
882 }
883
884 void
885 FGInterface::release_wire(void)
886 {
887   ground_cache.release_wire();
888 }
889