From a4b0be3d8a7ba09eb32313ffbdc189824a85c36c Mon Sep 17 00:00:00 2001 From: Torsten Dreyer Date: Tue, 19 Aug 2014 00:01:08 +0200 Subject: [PATCH] Expose the vertical flight path as a property write the vertical flight path to /orientation/path-deg zero means level flight, positive angles climb Also use SGD_DEGREES_TO_RADIANS (and vice versa) for double computations instead of the float constants --- src/FDM/flight.cxx | 8 +++++--- src/FDM/flight.hxx | 24 +++++++++++++++++------- 2 files changed, 22 insertions(+), 10 deletions(-) diff --git a/src/FDM/flight.cxx b/src/FDM/flight.cxx index 74bc1b5d9..91a2b1334 100644 --- a/src/FDM/flight.cxx +++ b/src/FDM/flight.cxx @@ -297,6 +297,8 @@ FGInterface::bind () fgSetArchivable("/orientation/heading-deg"); _tiedProperties.Tie("/orientation/track-deg", this, &FGInterface::get_Track); // read-only + _tiedProperties.Tie("/orientation/path-deg", this, + &FGInterface::get_Path); // read-only // Body-axis "euler rates" (rotation speed, but in a funny // representation). @@ -481,7 +483,7 @@ bool FGInterface::writeState(SGIOChannel* io) void FGInterface::_updatePositionM(const SGVec3d& cartPos) { - TrackComputer tracker( _state.track, _state.geodetic_position_v ); + TrackComputer tracker( _state.track, _state.path, _state.geodetic_position_v ); _state.cartesian_position_v = cartPos; _state.geodetic_position_v = SGGeod::fromCart(_state.cartesian_position_v); _state.geocentric_position_v = SGGeoc::fromCart(_state.cartesian_position_v); @@ -492,7 +494,7 @@ void FGInterface::_updatePositionM(const SGVec3d& cartPos) void FGInterface::_updatePosition(const SGGeod& geod) { - TrackComputer tracker( _state.track, _state.geodetic_position_v ); + TrackComputer tracker( _state.track, _state.path, _state.geodetic_position_v ); _state.geodetic_position_v = geod; _state.cartesian_position_v = SGVec3d::fromGeod(_state.geodetic_position_v); _state.geocentric_position_v = SGGeoc::fromCart(_state.cartesian_position_v); @@ -504,7 +506,7 @@ void FGInterface::_updatePosition(const SGGeod& geod) void FGInterface::_updatePosition(const SGGeoc& geoc) { - TrackComputer tracker( _state.track, _state.geodetic_position_v ); + TrackComputer tracker( _state.track, _state.path, _state.geodetic_position_v ); _state.geocentric_position_v = geoc; _state.cartesian_position_v = SGVec3d::fromGeoc(_state.geocentric_position_v); _state.geodetic_position_v = SGGeod::fromCart(_state.cartesian_position_v); diff --git a/src/FDM/flight.hxx b/src/FDM/flight.hxx index fdb729aaa..2d291b3cf 100644 --- a/src/FDM/flight.hxx +++ b/src/FDM/flight.hxx @@ -99,18 +99,26 @@ class SGIOChannel; */ class TrackComputer { public: - inline TrackComputer( double & track, const SGGeod & position ) : + inline TrackComputer( double & track, double & path, const SGGeod & position ) : _track( track ), + _path( path ), _position( position ), _prevPosition( position ) { } inline ~TrackComputer() { if( _prevPosition == _position ) return; - _track = SGGeodesy::courseDeg( _prevPosition, _position ); +// _track = SGGeodesy::courseDeg( _prevPosition, _position ); + double d = .0; + double distance = .0; + if( SGGeodesy::inverse( _prevPosition, _position, _track, d, distance ) ) { + d = _position.getElevationM() - _prevPosition.getElevationM(); + _path = atan2( d, distance ) * SGD_RADIANS_TO_DEGREES; + } } private: double & _track; + double & _path; const SGGeod & _position; const SGGeod _prevPosition; }; @@ -194,6 +202,7 @@ private: double climb_rate; // in feet per second double altitude_agl; double track; + double path; }; FlightState _state; @@ -306,17 +315,17 @@ public: void set_Phi_dot_degps(double x) { - _state.euler_rates_v[0] = x * SG_DEGREES_TO_RADIANS; + _state.euler_rates_v[0] = x * SGD_DEGREES_TO_RADIANS; } void set_Theta_dot_degps(double x) { - _state.euler_rates_v[1] = x * SG_DEGREES_TO_RADIANS; + _state.euler_rates_v[1] = x * SGD_DEGREES_TO_RADIANS; } void set_Psi_dot_degps(double x) { - _state.euler_rates_v[2] = x * SG_DEGREES_TO_RADIANS; + _state.euler_rates_v[2] = x * SGD_DEGREES_TO_RADIANS; } inline void _set_Geocentric_Rates( double lat, double lon, double rad ) { @@ -349,7 +358,7 @@ public: _set_Geodetic_Position( lat, lon, _state.geodetic_position_v.getElevationFt()); } inline void _set_Geodetic_Position( double lat, double lon, double alt ) { - TrackComputer tracker( _state.track, _state.geodetic_position_v ); + TrackComputer tracker( _state.track, _state.path, _state.geodetic_position_v ); _state.geodetic_position_v.setLatitudeRad(lat); _state.geodetic_position_v.setLongitudeRad(lon); _state.geodetic_position_v.setElevationFt(alt); @@ -364,7 +373,7 @@ public: inline void _set_Alpha( double a ) { _state.alpha = a; } inline void _set_Beta( double b ) { _state.beta = b; } - inline void set_Alpha_deg( double a ) { _state.alpha = a * SG_DEGREES_TO_RADIANS; } + inline void set_Alpha_deg( double a ) { _state.alpha = a * SGD_DEGREES_TO_RADIANS; } inline void _set_Gamma_vert_rad( double gv ) { _state.gamma_vert_rad = gv; } inline void _set_Density( double d ) { _state.density = d; } @@ -617,6 +626,7 @@ public: } inline double get_Altitude_AGL(void) const { return _state.altitude_agl; } inline double get_Track(void) const { return _state.track; } + inline double get_Path(void) const { return _state.path; } inline double get_Latitude_deg () const { return _state.geodetic_position_v.getLatitudeDeg();