*/
-#include <math.h>
-
-#include <list>
-#include <vector>
-#include <string>
+#include <cmath>
#include <simgear/compiler.h>
#include <simgear/constants.h>
#include <simgear/props/tiedpropertylist.hxx>
#include <FDM/groundcache.hxx>
-using std::list;
-using std::vector;
-using std::string;
-
namespace simgear {
class BVHMaterial;
}
*/
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;
};
SGVec3d v_local_v;
SGVec3d v_local_rel_ground_v; // V rel w.r.t. earth surface
SGVec3d v_local_airmass_v; // velocity of airmass (steady winds)
- SGVec3d v_wind_body_v; // Wind-relative velocities in body axis
+ SGVec3d v_body_v; // ECEF velocities in body axis
SGVec3d omega_body_v; // Angular B rates
SGVec3d euler_rates_v;
double climb_rate; // in feet per second
double altitude_agl;
double track;
+ double path;
};
FlightState _state;
_state.v_local_airmass_v[1] = east;
_state.v_local_airmass_v[2] = down;
}
- inline void _set_Velocities_Wind_Body( double u, double v, double w) {
- _state.v_wind_body_v[0] = u;
- _state.v_wind_body_v[1] = v;
- _state.v_wind_body_v[2] = w;
+ inline void _set_Velocities_Body( double u, double v, double w) {
+ _state.v_body_v[0] = u;
+ _state.v_body_v[1] = v;
+ _state.v_body_v[2] = w;
}
inline void _set_V_rel_wind(double vt) { _state.v_rel_wind = vt; }
inline void _set_V_ground_speed( double v) { _state.v_ground_speed = v; }
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 ) {
_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);
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; }
inline void set_V_down (double down) {
set_Velocities_Local(_state.v_local_v[0], _state.v_local_v[1], down);
}
- virtual void set_Velocities_Wind_Body( double u, double v, double w);
+ virtual void set_Velocities_Body( double u, double v, double w);
virtual void set_uBody (double uBody) {
- set_Velocities_Wind_Body(uBody, _state.v_wind_body_v[1], _state.v_wind_body_v[2]);
+ set_Velocities_Body(uBody, _state.v_body_v[1], _state.v_body_v[2]);
}
virtual void set_vBody (double vBody) {
- set_Velocities_Wind_Body(_state.v_wind_body_v[0], vBody, _state.v_wind_body_v[2]);
+ set_Velocities_Body(_state.v_body_v[0], vBody, _state.v_body_v[2]);
}
virtual void set_wBody (double wBody) {
- set_Velocities_Wind_Body(_state.v_wind_body_v[0], _state.v_wind_body_v[1], wBody);
+ set_Velocities_Body(_state.v_body_v[0], _state.v_body_v[1], wBody);
}
// Euler angles
inline double get_V_north() const { return _state.v_local_v[0]; }
inline double get_V_east() const { return _state.v_local_v[1]; }
inline double get_V_down() const { return _state.v_local_v[2]; }
- inline double get_uBody () const { return _state.v_wind_body_v[0]; }
- inline double get_vBody () const { return _state.v_wind_body_v[1]; }
- inline double get_wBody () const { return _state.v_wind_body_v[2]; }
+ inline double get_uBody () const { return _state.v_body_v[0]; }
+ inline double get_vBody () const { return _state.v_body_v[1]; }
+ inline double get_wBody () const { return _state.v_body_v[2]; }
// Please dont comment these out. fdm=ada uses these (see
// cockpit.cxx) --->
inline double get_V_east_airmass() const { return _state.v_local_airmass_v[1]; }
inline double get_V_down_airmass() const { return _state.v_local_airmass_v[2]; }
- inline double get_U_body() const { return _state.v_wind_body_v[0]; }
- inline double get_V_body() const { return _state.v_wind_body_v[1]; }
- inline double get_W_body() const { return _state.v_wind_body_v[2]; }
+ inline double get_U_body() const { return _state.v_body_v[0]; }
+ inline double get_V_body() const { return _state.v_body_v[1]; }
+ inline double get_W_body() const { return _state.v_body_v[2]; }
inline double get_V_rel_wind() const { return _state.v_rel_wind; }
}
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();