1 // flight.hxx -- define shared flight model parameters 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 24 #ifndef _FLIGHT_HXX 25 #define _FLIGHT_HXX 26 27 28 #ifndef __cplusplus 29 # error This library requires C++ 30 #endif 31 32 33 /* Required get_() 34 35 `FGInterface::get_Longitude ()' 36 `FGInterface::get_Latitude ()' 37 `FGInterface::get_Altitude ()' 38 `FGInterface::get_Phi ()' 39 `FGInterface::get_Theta ()' 40 `FGInterface::get_Psi ()' 41 `FGInterface::get_V_equiv_kts ()' 42 43 `FGInterface::get_V_north ()' 44 `FGInterface::get_V_east ()' 45 `FGInterface::get_V_down ()' 46 47 `FGInterface::get_P_Body ()' 48 `FGInterface::get_Q_Body ()' 49 `FGInterface::get_R_Body ()' 50 51 `FGInterface::get_Gamma_vert_rad ()' 52 `FGInterface::get_Climb_Rate ()' 53 `FGInterface::get_Alpha ()' 54 `FGInterface::get_Beta ()' 55 56 `FGInterface::get_Runway_altitude ()' 57 58 `FGInterface::get_Lon_geocentric ()' 59 `FGInterface::get_Lat_geocentric ()' 60 `FGInterface::get_Sea_level_radius ()' 61 `FGInterface::get_Earth_position_angle ()' 62 63 `FGInterface::get_Latitude_dot()' 64 `FGInterface::get_Longitude_dot()' 65 `FGInterface::get_Radius_dot()' 66 67 `FGInterface::get_Dx_cg ()' 68 `FGInterface::get_Dy_cg ()' 69 `FGInterface::get_Dz_cg ()' 70 71 `FGInterface::get_Radius_to_vehicle ()' 72 73 */ 74 75 76 #include <cmath> 77 78 #include <simgear/compiler.h> 79 #include <simgear/constants.h> 80 #include <simgear/structure/subsystem_mgr.hxx> 81 #include <simgear/props/tiedpropertylist.hxx> 82 #include <FDM/groundcache.hxx> 83 #include <FDM/AIWake/AIWakeGroup.hxx> 84 85 namespace simgear { 86 class BVHMaterial; 87 } 88 89 class SGIOChannel; 90 class FGAIAircraft; 91 92 /** 93 * A little helper class to update the track if 94 * the position has changed. In the constructor, 95 * create a copy of the current position and store 96 * references to the position object and the track 97 * variable to update. 98 * The destructor, called at TrackComputer's end of 99 * life/visibility, computes the track if the 100 * position has changed. 101 */ 102 class TrackComputer 103 { 104 public: TrackComputer(double & track,double & path,const SGGeod & position)105 inline TrackComputer( double & track, double & path, const SGGeod & position ) : 106 _track( track ), 107 _path( path ), 108 _position( position ), 109 _prevPosition( position ) { 110 } 111 ~TrackComputer()112 inline ~TrackComputer() { 113 if( _prevPosition == _position ) return; 114 // _track = SGGeodesy::courseDeg( _prevPosition, _position ); 115 double d = .0; 116 double distance = .0; 117 if( SGGeodesy::inverse( _prevPosition, _position, _track, d, distance ) ) { 118 d = _position.getElevationM() - _prevPosition.getElevationM(); 119 _path = atan2( d, distance ) * SGD_RADIANS_TO_DEGREES; 120 } 121 } 122 123 private: 124 double & _track; 125 double & _path; 126 const SGGeod & _position; 127 const SGGeod _prevPosition; 128 }; 129 130 // This is based heavily on LaRCsim/ls_generic.h 131 class FGInterface : public SGSubsystem 132 { 133 // Has the init() method been called. This is used to delay 134 // initialization until scenery can be loaded and we know the true 135 // ground elevation. 136 bool inited; 137 138 // Have we bound to the property system 139 bool bound; 140 141 // periodic update management variable. This is a scheme to run 142 // the fdm with a fixed delta-t. We control how many iteration of 143 // the fdm to run with the fixed dt based on the elapsed time from 144 // the last update. This allows us to maintain sync with the real 145 // time clock, even though each frame could take a random amount 146 // of time. Since "dt" is unlikely to divide evenly into the 147 // elapse time, we keep track of the remainder and add it into the 148 // next elapsed time. This yields a small amount of temporal 149 // jitter ( < dt ) but in practice seems to work well. 150 151 /** 152 * encapsulate primary flight state. This is packaged so it can be 153 * (unfortunately) sent directly over the wire by the 'native' FDM 154 * protocol. 155 */ 156 struct FlightState 157 { 158 // CG position w.r.t. ref. point 159 SGVec3d d_cg_rp_body_v; 160 161 // Accelerations 162 SGVec3d v_dot_local_v; 163 SGVec3d v_dot_body_v; 164 SGVec3d a_cg_body_v; 165 SGVec3d a_pilot_body_v; 166 SGVec3d n_cg_body_v; 167 SGVec3d omega_dot_body_v; 168 169 // Velocities 170 SGVec3d v_local_v; 171 SGVec3d v_local_rel_ground_v; // V rel w.r.t. earth surface 172 SGVec3d v_local_airmass_v; // velocity of airmass (steady winds) 173 SGVec3d v_body_v; // ECEF velocities in body axis 174 175 SGVec3d omega_body_v; // Angular B rates 176 SGVec3d euler_rates_v; 177 SGVec3d geocentric_rates_v; // Geocentric linear velocities 178 179 // Positions 180 SGGeod geodetic_position_v; 181 SGVec3d cartesian_position_v; 182 SGGeoc geocentric_position_v; 183 SGVec3d euler_angles_v; 184 185 // Normal Load Factor 186 double nlf; 187 188 // Velocities 189 double v_rel_wind, v_true_kts; 190 double v_ground_speed, v_equiv_kts; 191 double v_calibrated_kts; 192 193 // Miscellaneious Quantities 194 double alpha, beta; // in radians 195 double gamma_vert_rad; // Flight path angles 196 double density, mach_number; 197 double static_pressure, total_pressure; 198 double dynamic_pressure; 199 double static_temperature, total_temperature; 200 double sea_level_radius, earth_position_angle; 201 double runway_altitude; 202 double climb_rate; // in feet per second 203 double altitude_agl; 204 double track; 205 double path; 206 }; 207 208 FlightState _state; 209 210 simgear::TiedPropertyList _tiedProperties; 211 212 // the ground cache object itself. 213 FGGroundCache ground_cache; 214 215 AIWakeGroup wake_group; 216 set_A_X_pilot(double x)217 void set_A_X_pilot(double x) 218 { _set_Accels_Pilot_Body(x, _state.a_pilot_body_v[1], _state.a_pilot_body_v[2]); } 219 set_A_Y_pilot(double y)220 void set_A_Y_pilot(double y) 221 { _set_Accels_Pilot_Body(_state.a_pilot_body_v[0], y, _state.a_pilot_body_v[2]); } 222 set_A_Z_pilot(double z)223 void set_A_Z_pilot(double z) 224 { _set_Accels_Pilot_Body(_state.a_pilot_body_v[0], _state.a_pilot_body_v[1], z); } 225 226 protected: 227 int _calc_multiloop (double dt); 228 229 230 // deliberately not virtual so that 231 // FGInterface constructor will call 232 // the right version 233 void _setup(); 234 235 void _busdump(void); 236 void _updatePositionM(const SGVec3d& cartPos); _updatePositionFt(const SGVec3d & cartPos)237 void _updatePositionFt(const SGVec3d& cartPos) { 238 _updatePositionM(SG_FEET_TO_METER*cartPos); 239 } 240 void _updatePosition(const SGGeod& geod); 241 void _updatePosition(const SGGeoc& geoc); 242 243 void _updateGeodeticPosition( double lat, double lon, double alt ); 244 void _updateGeocentricPosition( double lat_geoc, double lon, double alt ); 245 void _update_ground_elev_at_pos( void ); 246 _set_CG_Position(double dx,double dy,double dz)247 inline void _set_CG_Position( double dx, double dy, double dz ) { 248 _state.d_cg_rp_body_v[0] = dx; 249 _state.d_cg_rp_body_v[1] = dy; 250 _state.d_cg_rp_body_v[2] = dz; 251 } _set_Accels_Local(double north,double east,double down)252 inline void _set_Accels_Local( double north, double east, double down ) { 253 _state.v_dot_local_v[0] = north; 254 _state.v_dot_local_v[1] = east; 255 _state.v_dot_local_v[2] = down; 256 } _set_Accels_Body(double u,double v,double w)257 inline void _set_Accels_Body( double u, double v, double w ) { 258 _state.v_dot_body_v[0] = u; 259 _state.v_dot_body_v[1] = v; 260 _state.v_dot_body_v[2] = w; 261 } _set_Accels_CG_Body(double x,double y,double z)262 inline void _set_Accels_CG_Body( double x, double y, double z ) { 263 _state.a_cg_body_v[0] = x; 264 _state.a_cg_body_v[1] = y; 265 _state.a_cg_body_v[2] = z; 266 } _set_Accels_Pilot_Body(double x,double y,double z)267 inline void _set_Accels_Pilot_Body( double x, double y, double z ) { 268 _state.a_pilot_body_v[0] = x; 269 _state.a_pilot_body_v[1] = y; 270 _state.a_pilot_body_v[2] = z; 271 } _set_Accels_CG_Body_N(double x,double y,double z)272 inline void _set_Accels_CG_Body_N( double x, double y, double z ) { 273 _state.n_cg_body_v[0] = x; 274 _state.n_cg_body_v[1] = y; 275 _state.n_cg_body_v[2] = z; 276 } _set_Nlf(double n)277 void _set_Nlf(double n) { _state.nlf=n; } _set_Velocities_Local(double north,double east,double down)278 inline void _set_Velocities_Local( double north, double east, double down ){ 279 _state.v_local_v[0] = north; 280 _state.v_local_v[1] = east; 281 _state.v_local_v[2] = down; 282 } _set_Velocities_Ground(double north,double east,double down)283 inline void _set_Velocities_Ground(double north, double east, double down) { 284 _state.v_local_rel_ground_v[0] = north; 285 _state.v_local_rel_ground_v[1] = east; 286 _state.v_local_rel_ground_v[2] = down; 287 } _set_Velocities_Local_Airmass(double north,double east,double down)288 inline void _set_Velocities_Local_Airmass( double north, double east, 289 double down) 290 { 291 _state.v_local_airmass_v[0] = north; 292 _state.v_local_airmass_v[1] = east; 293 _state.v_local_airmass_v[2] = down; 294 } _set_Velocities_Body(double u,double v,double w)295 inline void _set_Velocities_Body( double u, double v, double w) { 296 _state.v_body_v[0] = u; 297 _state.v_body_v[1] = v; 298 _state.v_body_v[2] = w; 299 } _set_V_rel_wind(double vt)300 inline void _set_V_rel_wind(double vt) { _state.v_rel_wind = vt; } _set_V_ground_speed(double v)301 inline void _set_V_ground_speed( double v) { _state.v_ground_speed = v; } _set_V_equiv_kts(double kts)302 inline void _set_V_equiv_kts( double kts ) { _state.v_equiv_kts = kts; } _set_V_calibrated_kts(double kts)303 inline void _set_V_calibrated_kts( double kts ) { _state.v_calibrated_kts = kts; } _set_Omega_Body(double p,double q,double r)304 inline void _set_Omega_Body( double p, double q, double r ) { 305 _state.omega_body_v[0] = p; 306 _state.omega_body_v[1] = q; 307 _state.omega_body_v[2] = r; 308 } _set_Euler_Rates(double phi,double theta,double psi)309 inline void _set_Euler_Rates( double phi, double theta, double psi ) { 310 _state.euler_rates_v[0] = phi; 311 _state.euler_rates_v[1] = theta; 312 _state.euler_rates_v[2] = psi; 313 } 314 set_Phi_dot_degps(double x)315 void set_Phi_dot_degps(double x) 316 { 317 _state.euler_rates_v[0] = x * SGD_DEGREES_TO_RADIANS; 318 } 319 set_Theta_dot_degps(double x)320 void set_Theta_dot_degps(double x) 321 { 322 _state.euler_rates_v[1] = x * SGD_DEGREES_TO_RADIANS; 323 } 324 set_Psi_dot_degps(double x)325 void set_Psi_dot_degps(double x) 326 { 327 _state.euler_rates_v[2] = x * SGD_DEGREES_TO_RADIANS; 328 } 329 _set_Geocentric_Rates(double lat,double lon,double rad)330 inline void _set_Geocentric_Rates( double lat, double lon, double rad ) { 331 _state.geocentric_rates_v[0] = lat; 332 _state.geocentric_rates_v[1] = lon; 333 _state.geocentric_rates_v[2] = rad; 334 } _set_Geocentric_Position(double lat,double lon,double rad)335 inline void _set_Geocentric_Position( double lat, double lon, double rad ) { 336 _state.geocentric_position_v.setLatitudeRad(lat); 337 _state.geocentric_position_v.setLongitudeRad(lon); 338 _state.geocentric_position_v.setRadiusFt(rad); 339 } 340 /* Don't call _set_L[at|ong]itude() directly, use _set_Geodetic_Position() instead. 341 These methods can't update the track. 342 * 343 inline void _set_Latitude(double lat) { 344 geodetic_position_v.setLatitudeRad(lat); 345 } 346 inline void _set_Longitude(double lon) { 347 geodetic_position_v.setLongitudeRad(lon); 348 } 349 */ _set_Altitude(double altitude)350 inline void _set_Altitude(double altitude) { 351 _state.geodetic_position_v.setElevationFt(altitude); 352 } _set_Altitude_AGL(double agl)353 inline void _set_Altitude_AGL(double agl) { 354 _state.altitude_agl = agl; 355 } _set_Geodetic_Position(double lat,double lon)356 inline void _set_Geodetic_Position( double lat, double lon ) { 357 _set_Geodetic_Position( lat, lon, _state.geodetic_position_v.getElevationFt()); 358 } _set_Geodetic_Position(double lat,double lon,double alt)359 inline void _set_Geodetic_Position( double lat, double lon, double alt ) { 360 TrackComputer tracker( _state.track, _state.path, _state.geodetic_position_v ); 361 _state.geodetic_position_v.setLatitudeRad(lat); 362 _state.geodetic_position_v.setLongitudeRad(lon); 363 _state.geodetic_position_v.setElevationFt(alt); 364 } _set_Euler_Angles(double phi,double theta,double psi)365 inline void _set_Euler_Angles( double phi, double theta, double psi ) { 366 _state.euler_angles_v[0] = phi; 367 _state.euler_angles_v[1] = theta; 368 _state.euler_angles_v[2] = psi; 369 } 370 // FIXME, for compatibility with JSBSim _set_T_Local_to_Body(int i,int j,double value)371 inline void _set_T_Local_to_Body( int i, int j, double value) { } _set_Alpha(double a)372 inline void _set_Alpha( double a ) { _state.alpha = a; } _set_Beta(double b)373 inline void _set_Beta( double b ) { _state.beta = b; } 374 set_Alpha_deg(double a)375 inline void set_Alpha_deg( double a ) { _state.alpha = a * SGD_DEGREES_TO_RADIANS; } 376 _set_Gamma_vert_rad(double gv)377 inline void _set_Gamma_vert_rad( double gv ) { _state.gamma_vert_rad = gv; } _set_Density(double d)378 inline void _set_Density( double d ) { _state.density = d; } _set_Mach_number(double m)379 inline void _set_Mach_number( double m ) { _state.mach_number = m; } _set_Static_pressure(double sp)380 inline void _set_Static_pressure( double sp ) { _state.static_pressure = sp; } _set_Static_temperature(double t)381 inline void _set_Static_temperature( double t ) { _state.static_temperature = t; } _set_Total_temperature(double tat)382 inline void _set_Total_temperature( double tat ) { _state.total_temperature = tat; } //JW _set_Sea_level_radius(double r)383 inline void _set_Sea_level_radius( double r ) { _state.sea_level_radius = r; } _set_Earth_position_angle(double a)384 inline void _set_Earth_position_angle(double a) {_state.earth_position_angle = a; } _set_Runway_altitude(double alt)385 inline void _set_Runway_altitude( double alt ) { _state.runway_altitude = alt; } _set_Climb_Rate(double rate)386 inline void _set_Climb_Rate(double rate) { _state.climb_rate = rate; } 387 388 public: 389 FGInterface(); 390 FGInterface( double dt ); 391 virtual ~FGInterface(); 392 393 // Subsystem API. 394 void bind() override; 395 void init() override; 396 void unbind() override; 397 void update(double dt) override; 398 ToggleDataLogging(bool state)399 virtual bool ToggleDataLogging(bool state) { return false; } ToggleDataLogging(void)400 virtual bool ToggleDataLogging(void) { return false; } 401 402 bool readState(SGIOChannel* io); 403 bool writeState(SGIOChannel* io); 404 405 // Define the various supported flight models (many not yet implemented) 406 enum { 407 // Magic Carpet mode 408 FG_MAGICCARPET = 0, 409 410 // The NASA LaRCsim (Navion) flight model 411 FG_LARCSIM = 1, 412 413 // Jon S. Berndt's new FDM written from the ground up in C++ 414 FG_JSBSIM = 2, 415 416 // Christian's hot air balloon simulation 417 FG_BALLOONSIM = 3, 418 419 // Aeronautical DEvelopment AGEncy, Bangalore India 420 FG_ADA = 4, 421 422 // The following aren't implemented but are here to spark 423 // thoughts and discussions, and maybe even action. 424 FG_ACM = 5, 425 FG_SUPER_SONIC = 6, 426 FG_HELICOPTER = 7, 427 FG_AUTOGYRO = 8, 428 FG_PARACHUTE = 9, 429 430 // Driven externally via a serial port, net, file, etc. 431 FG_EXTERNAL = 10 432 }; 433 434 // initialization get_inited() const435 inline bool get_inited() const { return inited; } set_inited(bool value)436 inline void set_inited( bool value ) { inited = value; } 437 get_bound() const438 inline bool get_bound() const { return bound; } 439 440 //perform initializion that is common to all FDM's 441 void common_init(); 442 443 // Positions 444 virtual void set_Latitude(double lat); // geocentric 445 virtual void set_Longitude(double lon); 446 virtual void set_Altitude(double alt); // triggers re-calc of AGL altitude 447 virtual void set_AltitudeAGL(double altagl); // and vice-versa set_Latitude_deg(double lat)448 virtual void set_Latitude_deg (double lat) { 449 set_Latitude(lat * SGD_DEGREES_TO_RADIANS); 450 } set_Longitude_deg(double lon)451 virtual void set_Longitude_deg (double lon) { 452 set_Longitude(lon * SGD_DEGREES_TO_RADIANS); 453 } 454 455 // Speeds -- setting any of these will trigger a re-calc of the rest 456 virtual void set_V_calibrated_kts(double vc); 457 virtual void set_Mach_number(double mach); 458 virtual void set_Velocities_Local( double north, double east, double down ); set_V_north(double north)459 inline void set_V_north (double north) { 460 set_Velocities_Local(north, _state.v_local_v[1], _state.v_local_v[2]); 461 } set_V_east(double east)462 inline void set_V_east (double east) { 463 set_Velocities_Local(_state.v_local_v[0], east, _state.v_local_v[2]); 464 } set_V_down(double down)465 inline void set_V_down (double down) { 466 set_Velocities_Local(_state.v_local_v[0], _state.v_local_v[1], down); 467 } 468 virtual void set_Velocities_Body( double u, double v, double w); set_uBody(double uBody)469 virtual void set_uBody (double uBody) { 470 set_Velocities_Body(uBody, _state.v_body_v[1], _state.v_body_v[2]); 471 } set_vBody(double vBody)472 virtual void set_vBody (double vBody) { 473 set_Velocities_Body(_state.v_body_v[0], vBody, _state.v_body_v[2]); 474 } set_wBody(double wBody)475 virtual void set_wBody (double wBody) { 476 set_Velocities_Body(_state.v_body_v[0], _state.v_body_v[1], wBody); 477 } 478 479 // Euler angles 480 virtual void set_Euler_Angles( double phi, double theta, double psi ); set_Phi(double phi)481 virtual void set_Phi (double phi) { 482 set_Euler_Angles(phi, get_Theta(), get_Psi()); 483 } set_Theta(double theta)484 virtual void set_Theta (double theta) { 485 set_Euler_Angles(get_Phi(), theta, get_Psi()); 486 } set_Psi(double psi)487 virtual void set_Psi (double psi) { 488 set_Euler_Angles(get_Phi(), get_Theta(), psi); 489 } set_Phi_deg(double phi)490 virtual void set_Phi_deg (double phi) { 491 set_Phi(phi * SGD_DEGREES_TO_RADIANS); 492 } set_Theta_deg(double theta)493 virtual void set_Theta_deg (double theta) { 494 set_Theta(theta * SGD_DEGREES_TO_RADIANS); 495 } set_Psi_deg(double psi)496 virtual void set_Psi_deg (double psi) { 497 set_Psi(psi * SGD_DEGREES_TO_RADIANS); 498 } 499 500 // Flight Path 501 virtual void set_Climb_Rate( double roc); 502 virtual void set_Gamma_vert_rad( double gamma); 503 504 // Earth 505 506 virtual void set_Static_pressure(double p); 507 virtual void set_Static_temperature(double T); 508 virtual void set_Density(double rho); 509 510 virtual void set_Velocities_Local_Airmass (double wnorth, 511 double weast, 512 double wdown ); 513 514 // ========== Mass properties and geometry values ========== 515 516 // CG position w.r.t. ref. point get_Dx_cg() const517 inline double get_Dx_cg() const { return _state.d_cg_rp_body_v[0]; } get_Dy_cg() const518 inline double get_Dy_cg() const { return _state.d_cg_rp_body_v[1]; } get_Dz_cg() const519 inline double get_Dz_cg() const { return _state.d_cg_rp_body_v[2]; } 520 521 // ========== Accelerations ========== 522 get_V_dot_north() const523 inline double get_V_dot_north() const { return _state.v_dot_local_v[0]; } get_V_dot_east() const524 inline double get_V_dot_east() const { return _state.v_dot_local_v[1]; } get_V_dot_down() const525 inline double get_V_dot_down() const { return _state.v_dot_local_v[2]; } 526 get_U_dot_body() const527 inline double get_U_dot_body() const { return _state.v_dot_body_v[0]; } get_V_dot_body() const528 inline double get_V_dot_body() const { return _state.v_dot_body_v[1]; } get_W_dot_body() const529 inline double get_W_dot_body() const { return _state.v_dot_body_v[2]; } 530 get_A_X_cg() const531 inline double get_A_X_cg() const { return _state.a_cg_body_v[0]; } get_A_Y_cg() const532 inline double get_A_Y_cg() const { return _state.a_cg_body_v[1]; } get_A_Z_cg() const533 inline double get_A_Z_cg() const { return _state.a_cg_body_v[2]; } 534 get_A_X_pilot() const535 inline double get_A_X_pilot() const { return _state.a_pilot_body_v[0]; } get_A_Y_pilot() const536 inline double get_A_Y_pilot() const { return _state.a_pilot_body_v[1]; } get_A_Z_pilot() const537 inline double get_A_Z_pilot() const { return _state.a_pilot_body_v[2]; } 538 get_N_X_cg() const539 inline double get_N_X_cg() const { return _state.n_cg_body_v[0]; } get_N_Y_cg() const540 inline double get_N_Y_cg() const { return _state.n_cg_body_v[1]; } get_N_Z_cg() const541 inline double get_N_Z_cg() const { return _state.n_cg_body_v[2]; } 542 get_Nlf(void) const543 inline double get_Nlf(void) const { return _state.nlf; } 544 545 // ========== Velocities ========== 546 get_V_north() const547 inline double get_V_north() const { return _state.v_local_v[0]; } get_V_east() const548 inline double get_V_east() const { return _state.v_local_v[1]; } get_V_down() const549 inline double get_V_down() const { return _state.v_local_v[2]; } get_uBody() const550 inline double get_uBody () const { return _state.v_body_v[0]; } get_vBody() const551 inline double get_vBody () const { return _state.v_body_v[1]; } get_wBody() const552 inline double get_wBody () const { return _state.v_body_v[2]; } 553 554 // Please dont comment these out. fdm=ada uses these (see 555 // cockpit.cxx) ---> get_V_north_rel_ground() const556 inline double get_V_north_rel_ground() const { 557 return _state.v_local_rel_ground_v[0]; 558 } get_V_east_rel_ground() const559 inline double get_V_east_rel_ground() const { 560 return _state.v_local_rel_ground_v[1]; 561 } get_V_down_rel_ground() const562 inline double get_V_down_rel_ground() const { 563 return _state.v_local_rel_ground_v[2]; 564 } 565 // <--- fdm=ada uses these (see cockpit.cxx) 566 get_V_north_airmass() const567 inline double get_V_north_airmass() const { return _state.v_local_airmass_v[0]; } get_V_east_airmass() const568 inline double get_V_east_airmass() const { return _state.v_local_airmass_v[1]; } get_V_down_airmass() const569 inline double get_V_down_airmass() const { return _state.v_local_airmass_v[2]; } 570 get_U_body() const571 inline double get_U_body() const { return _state.v_body_v[0]; } get_V_body() const572 inline double get_V_body() const { return _state.v_body_v[1]; } get_W_body() const573 inline double get_W_body() const { return _state.v_body_v[2]; } 574 get_V_rel_wind() const575 inline double get_V_rel_wind() const { return _state.v_rel_wind; } 576 get_V_true_kts() const577 inline double get_V_true_kts() const { return _state.v_true_kts; } 578 get_V_ground_speed() const579 inline double get_V_ground_speed() const { return _state.v_ground_speed; } get_V_ground_speed_kt() const580 inline double get_V_ground_speed_kt() const { return _state.v_ground_speed * SG_FEET_TO_METER * 3600 * SG_METER_TO_NM; } set_V_ground_speed_kt(double ground_speed)581 inline void set_V_ground_speed_kt(double ground_speed) { _state.v_ground_speed = ground_speed / ( SG_FEET_TO_METER * 3600 * SG_METER_TO_NM); } 582 get_V_equiv_kts() const583 inline double get_V_equiv_kts() const { return _state.v_equiv_kts; } 584 get_V_calibrated_kts() const585 inline double get_V_calibrated_kts() const { return _state.v_calibrated_kts; } 586 get_P_body() const587 inline double get_P_body() const { return _state.omega_body_v[0]; } get_Q_body() const588 inline double get_Q_body() const { return _state.omega_body_v[1]; } get_R_body() const589 inline double get_R_body() const { return _state.omega_body_v[2]; } 590 get_Phi_dot() const591 inline double get_Phi_dot() const { return _state.euler_rates_v[0]; } get_Theta_dot() const592 inline double get_Theta_dot() const { return _state.euler_rates_v[1]; } get_Psi_dot() const593 inline double get_Psi_dot() const { return _state.euler_rates_v[2]; } get_Phi_dot_degps() const594 inline double get_Phi_dot_degps() const { return _state.euler_rates_v[0] * SGD_RADIANS_TO_DEGREES; } get_Theta_dot_degps() const595 inline double get_Theta_dot_degps() const { return _state.euler_rates_v[1] * SGD_RADIANS_TO_DEGREES; } get_Psi_dot_degps() const596 inline double get_Psi_dot_degps() const { return _state.euler_rates_v[2] * SGD_RADIANS_TO_DEGREES; } 597 get_Latitude_dot() const598 inline double get_Latitude_dot() const { return _state.geocentric_rates_v[0]; } get_Longitude_dot() const599 inline double get_Longitude_dot() const { return _state.geocentric_rates_v[1]; } get_Radius_dot() const600 inline double get_Radius_dot() const { return _state.geocentric_rates_v[2]; } 601 602 // ========== Positions ========== 603 get_Lat_geocentric() const604 inline double get_Lat_geocentric() const { 605 return _state.geocentric_position_v.getLatitudeRad(); 606 } get_Lon_geocentric() const607 inline double get_Lon_geocentric() const { 608 return _state.geocentric_position_v.getLongitudeRad(); 609 } get_Radius_to_vehicle() const610 inline double get_Radius_to_vehicle() const { 611 return _state.geocentric_position_v.getRadiusFt(); 612 } 613 getPosition() const614 const SGGeod& getPosition() const { return _state.geodetic_position_v; } getGeocPosition() const615 const SGGeoc& getGeocPosition() const { return _state.geocentric_position_v; } getCartPosition() const616 const SGVec3d& getCartPosition() const { return _state.cartesian_position_v; } 617 get_Latitude() const618 inline double get_Latitude() const { 619 return _state.geodetic_position_v.getLatitudeRad(); 620 } get_Longitude() const621 inline double get_Longitude() const { 622 return _state.geodetic_position_v.getLongitudeRad(); 623 } get_Altitude() const624 inline double get_Altitude() const { 625 return _state.geodetic_position_v.getElevationFt(); 626 } get_Altitude_AGL(void) const627 inline double get_Altitude_AGL(void) const { return _state.altitude_agl; } get_Track(void) const628 inline double get_Track(void) const { return _state.track; } get_Path(void) const629 inline double get_Path(void) const { return _state.path; } 630 get_Latitude_deg() const631 inline double get_Latitude_deg () const { 632 return _state.geodetic_position_v.getLatitudeDeg(); 633 } get_Longitude_deg() const634 inline double get_Longitude_deg () const { 635 return _state.geodetic_position_v.getLongitudeDeg(); 636 } 637 get_Phi() const638 inline double get_Phi() const { return _state.euler_angles_v[0]; } get_Theta() const639 inline double get_Theta() const { return _state.euler_angles_v[1]; } get_Psi() const640 inline double get_Psi() const { return _state.euler_angles_v[2]; } get_Phi_deg() const641 inline double get_Phi_deg () const { return get_Phi() * SGD_RADIANS_TO_DEGREES; } get_Theta_deg() const642 inline double get_Theta_deg () const { return get_Theta() * SGD_RADIANS_TO_DEGREES; } get_Psi_deg() const643 inline double get_Psi_deg () const { return get_Psi() * SGD_RADIANS_TO_DEGREES; } 644 645 646 // ========== Miscellaneous quantities ========== 647 get_Alpha() const648 inline double get_Alpha() const { return _state.alpha; } get_Alpha_deg() const649 inline double get_Alpha_deg() const { return _state.alpha * SGD_RADIANS_TO_DEGREES; } get_Beta() const650 inline double get_Beta() const { return _state.beta; } get_Beta_deg() const651 inline double get_Beta_deg() const { return _state.beta * SGD_RADIANS_TO_DEGREES; } get_Gamma_vert_rad() const652 inline double get_Gamma_vert_rad() const { return _state.gamma_vert_rad; } 653 get_Density() const654 inline double get_Density() const { return _state.density; } get_Mach_number() const655 inline double get_Mach_number() const { return _state.mach_number; } 656 get_Static_pressure() const657 inline double get_Static_pressure() const { return _state.static_pressure; } get_Total_pressure() const658 inline double get_Total_pressure() const { return _state.total_pressure; } get_Dynamic_pressure() const659 inline double get_Dynamic_pressure() const { return _state.dynamic_pressure; } 660 get_Static_temperature() const661 inline double get_Static_temperature() const { return _state.static_temperature; } get_Total_temperature() const662 inline double get_Total_temperature() const { return _state.total_temperature; } 663 get_Sea_level_radius() const664 inline double get_Sea_level_radius() const { return _state.sea_level_radius; } get_Earth_position_angle() const665 inline double get_Earth_position_angle() const { 666 return _state.earth_position_angle; 667 } 668 get_Runway_altitude() const669 inline double get_Runway_altitude() const { return _state.runway_altitude; } get_Runway_altitude_m() const670 inline double get_Runway_altitude_m() const { return SG_FEET_TO_METER * _state.runway_altitude; } 671 get_Climb_Rate() const672 inline double get_Climb_Rate() const { return _state.climb_rate; } 673 674 // Note that currently this is the "same" value runway altitude... get_ground_elev_ft() const675 inline double get_ground_elev_ft() const { return _state.runway_altitude; } 676 677 678 ////////////////////////////////////////////////////////////////////////// 679 // Ground handling routines 680 ////////////////////////////////////////////////////////////////////////// 681 682 // Prepare the ground cache for the wgs84 position pt_*. 683 // That is take all vertices in the ball with radius rad around the 684 // position given by the pt_* and store them in a local scene graph. 685 bool prepare_ground_cache_m(double startSimTime, double endSimTime, 686 const double pt[3], double rad); 687 bool prepare_ground_cache_ft(double startSimTime, double endSimTime, 688 const double pt[3], double rad); 689 690 691 // Returns true if the cache is valid. 692 // Also the reference time, point and radius values where the cache 693 // is valid for are returned. 694 bool is_valid_m(double *ref_time, double pt[3], double *rad); 695 bool is_valid_ft(double *ref_time, double pt[3], double *rad); 696 697 // Return the nearest catapult to the given point 698 // pt in wgs84 coordinates. 699 double get_cat_m(double t, const double pt[3], 700 double end[2][3], double vel[2][3]); 701 double get_cat_ft(double t, const double pt[3], 702 double end[2][3], double vel[2][3]); 703 704 705 // Return the orientation and position matrix and the linear and angular 706 // velocity of that local coordinate systems origin for a given time and 707 // body id. The velocities are in the wgs84 frame at the bodys origin. 708 bool get_body_m(double t, simgear::BVHNode::Id id, double bodyToWorld[16], 709 double linearVel[3], double angularVel[3]); 710 711 712 // Return the altitude above ground below the wgs84 point pt 713 // Search for the nearest triangle to pt in downward direction. 714 // Return ground properties. The velocities are in the wgs84 frame at the 715 // contact point. 716 bool get_agl_m(double t, const double pt[3], double max_altoff, 717 double contact[3], double normal[3], double linearVel[3], 718 double angularVel[3], simgear::BVHMaterial const*& material, 719 simgear::BVHNode::Id& id); 720 bool get_agl_ft(double t, const double pt[3], double max_altoff, 721 double contact[3], double normal[3], double linearVel[3], 722 double angularVel[3], simgear::BVHMaterial const*& material, 723 simgear::BVHNode::Id& id); 724 double get_groundlevel_m(double lat, double lon, double alt); 725 double get_groundlevel_m(const SGGeod& geod); 726 727 728 // Return the nearest point in any direction to the point pt with a maximum 729 // distance maxDist. The velocities are in the wgs84 frame at the query 730 // position pt. 731 bool get_nearest_m(double t, const double pt[3], double maxDist, 732 double contact[3], double normal[3], double linearVel[3], 733 double angularVel[3], simgear::BVHMaterial const*& material, 734 simgear::BVHNode::Id& id); 735 bool get_nearest_ft(double t, const double pt[3], double maxDist, 736 double contact[3], double normal[3],double linearVel[3], 737 double angularVel[3], simgear::BVHMaterial const*& material, 738 simgear::BVHNode::Id& id); 739 740 741 // Return 1 if the hook intersects with a wire. 742 // That test is done by checking if the quad spanned by the points pt* 743 // intersects with the line representing the wire. 744 // If the wire is caught, the cache will trace this wires endpoints until 745 // the FDM calls release_wire(). 746 bool caught_wire_m(double t, const double pt[4][3]); 747 bool caught_wire_ft(double t, const double pt[4][3]); 748 749 // Return the location and speed of the wire endpoints. 750 bool get_wire_ends_m(double t, double end[2][3], double vel[2][3]); 751 bool get_wire_ends_ft(double t, double end[2][3], double vel[2][3]); 752 753 // Tell the cache code that it does no longer need to care for 754 // the wire end position. 755 void release_wire(void); 756 757 // Manages the AI wake computations. add_ai_wake(FGAIAircraft * ai)758 void add_ai_wake(FGAIAircraft* ai) { wake_group.AddAI(ai); } reset_wake_group(void)759 void reset_wake_group(void) { wake_group.gc(); } get_wake_group(void)760 const AIWakeGroup& get_wake_group(void) { return wake_group; } 761 }; 762 763 #endif // _FLIGHT_HXX 764