diff --git a/src/FDM/SP/ACMS.cxx b/src/FDM/SP/ACMS.cxx index ad36f28cf..6fd219bc9 100644 --- a/src/FDM/SP/ACMS.cxx +++ b/src/FDM/SP/ACMS.cxx @@ -88,15 +88,14 @@ void FGACMS::update( double dt ) { _set_V_calibrated_kts( kts ); _set_V_ground_speed( kts ); - SGGeod pos = SGGeod::fromDegM(get_Longitude(), get_Latitude(), get_Altitude()); + SGGeod pos = getPosition(); // update (lon/lat) position SGGeod pos2; double az2; geo_direct_wgs_84 ( pos, heading * SGD_RADIANS_TO_DEGREES, dist, pos2, &az2 ); - _set_Longitude( pos2.getLongitudeRad() ); - _set_Latitude( pos2.getLatitudeRad() ); + _set_Geodetic_Position( pos2.getLatitudeRad(), pos2.getLongitudeRad(), pos.getElevationFt() ); double sl_radius, lat_geoc; sgGeodToGeoc( get_Latitude(), get_Altitude(), &sl_radius, &lat_geoc ); diff --git a/src/FDM/SP/MagicCarpet.cxx b/src/FDM/SP/MagicCarpet.cxx index 99c937433..44bffd0ef 100644 --- a/src/FDM/SP/MagicCarpet.cxx +++ b/src/FDM/SP/MagicCarpet.cxx @@ -93,8 +93,7 @@ void FGMagicCarpet::update( double dt ) { get_Psi() * SGD_RADIANS_TO_DEGREES, dist, &lat2, &lon2, &az2 ); - _set_Longitude( lon2 * SGD_DEGREES_TO_RADIANS ); - _set_Latitude( lat2 * SGD_DEGREES_TO_RADIANS ); + _set_Geodetic_Position( lat2 * SGD_DEGREES_TO_RADIANS, lon2 * SGD_DEGREES_TO_RADIANS ); } // cout << "lon error = " << fabs(end.x()*SGD_RADIANS_TO_DEGREES - lon2) diff --git a/src/FDM/UFO.cxx b/src/FDM/UFO.cxx index e8a37fc7b..93928f67d 100644 --- a/src/FDM/UFO.cxx +++ b/src/FDM/UFO.cxx @@ -162,8 +162,7 @@ void FGUFO::update( double dt ) { get_Psi() * SGD_RADIANS_TO_DEGREES, dist, &lat2, &lon2, &az2 ); - _set_Longitude( lon2 * SGD_DEGREES_TO_RADIANS ); - _set_Latitude( lat2 * SGD_DEGREES_TO_RADIANS ); + _set_Geodetic_Position( lat2 * SGD_DEGREES_TO_RADIANS, lon2 * SGD_DEGREES_TO_RADIANS ); } // cout << "lon error = " << fabs(end.x()*SGD_RADIANS_TO_DEGREES - lon2) diff --git a/src/FDM/flight.cxx b/src/FDM/flight.cxx index bc5b3059a..10ce9948a 100644 --- a/src/FDM/flight.cxx +++ b/src/FDM/flight.cxx @@ -120,6 +120,7 @@ FGInterface::_setup () runway_altitude=0; climb_rate=0; altitude_agl=0; + track=0; } void @@ -278,6 +279,8 @@ FGInterface::bind () &FGInterface::get_Psi_deg, &FGInterface::set_Psi_deg); fgSetArchivable("/orientation/heading-deg"); + fgTie("/orientation/track-deg", this, + &FGInterface::get_Track); // Body-axis "euler rates" (rotation speed, but in a funny // representation). @@ -404,6 +407,7 @@ FGInterface::unbind () fgUntie("/orientation/roll-deg"); fgUntie("/orientation/pitch-deg"); fgUntie("/orientation/heading-deg"); + fgUntie("/orientation/track-deg"); fgUntie("/orientation/roll-rate-degps"); fgUntie("/orientation/pitch-rate-degps"); fgUntie("/orientation/yaw-rate-degps"); @@ -442,6 +446,7 @@ FGInterface::update (double dt) void FGInterface::_updatePositionM(const SGVec3d& cartPos) { + TrackComputer tracker( track, geodetic_position_v ); cartesian_position_v = cartPos; geodetic_position_v = SGGeod::fromCart(cartesian_position_v); geocentric_position_v = SGGeoc::fromCart(cartesian_position_v); @@ -452,6 +457,7 @@ void FGInterface::_updatePositionM(const SGVec3d& cartPos) void FGInterface::_updatePosition(const SGGeod& geod) { + TrackComputer tracker( track, geodetic_position_v ); geodetic_position_v = geod; cartesian_position_v = SGVec3d::fromGeod(geodetic_position_v); geocentric_position_v = SGGeoc::fromCart(cartesian_position_v); @@ -463,6 +469,7 @@ void FGInterface::_updatePosition(const SGGeod& geod) void FGInterface::_updatePosition(const SGGeoc& geoc) { + TrackComputer tracker( track, geodetic_position_v ); geocentric_position_v = geoc; cartesian_position_v = SGVec3d::fromGeoc(geocentric_position_v); geodetic_position_v = SGGeod::fromCart(cartesian_position_v); diff --git a/src/FDM/flight.hxx b/src/FDM/flight.hxx index ba9551456..222df3b54 100644 --- a/src/FDM/flight.hxx +++ b/src/FDM/flight.hxx @@ -88,6 +88,34 @@ using std::list; using std::vector; using std::string; +/** + * A little helper class to update the track if + * the position has changed. In the constructor, + * create a copy of the current position and store + * references to the position object and the track + * variable to update. + * The destructor, called at TrackComputer's end of + * life/visibility, computes the track if the + * position has changed. + */ +class TrackComputer { +public: + inline TrackComputer( double & track, const SGGeod & position ) : + _track( track ), + _position( position ), + _prevPosition( position ) { + } + + inline ~TrackComputer() { + if( _prevPosition == _position ) return; + _track = SGGeodesy::courseDeg( _prevPosition, _position ); + } +private: + double & _track; + const SGGeod & _position; + const SGGeod _prevPosition; +}; + // This is based heavily on LaRCsim/ls_generic.h class FGInterface : public SGSubsystem { @@ -157,6 +185,7 @@ private: double runway_altitude; double climb_rate; // in feet per second double altitude_agl; + double track; double daux[16]; // auxilliary doubles float faux[16]; // auxilliary floats @@ -265,19 +294,27 @@ public: geocentric_position_v.setLongitudeRad(lon); geocentric_position_v.setRadiusFt(rad); } +/* Don't call _set_L[at|ong]itude() directly, use _set_Geodetic_Position() instead. + These methods can't update the track. + * inline void _set_Latitude(double lat) { geodetic_position_v.setLatitudeRad(lat); } inline void _set_Longitude(double lon) { geodetic_position_v.setLongitudeRad(lon); } +*/ inline void _set_Altitude(double altitude) { geodetic_position_v.setElevationFt(altitude); } inline void _set_Altitude_AGL(double agl) { altitude_agl = agl; } + inline void _set_Geodetic_Position( double lat, double lon ) { + _set_Geodetic_Position( lat, lon, geodetic_position_v.getElevationFt()); + } inline void _set_Geodetic_Position( double lat, double lon, double alt ) { + TrackComputer tracker( track, geodetic_position_v ); geodetic_position_v.setLatitudeRad(lat); geodetic_position_v.setLongitudeRad(lon); geodetic_position_v.setElevationFt(alt); @@ -541,6 +578,7 @@ public: return geodetic_position_v.getElevationFt(); } inline double get_Altitude_AGL(void) const { return altitude_agl; } + inline double get_Track(void) const { return track; } inline double get_Latitude_deg () const { return geodetic_position_v.getLatitudeDeg(); diff --git a/src/Main/fg_props.cxx b/src/Main/fg_props.cxx index 31f4ae0da..a5726b79b 100644 --- a/src/Main/fg_props.cxx +++ b/src/Main/fg_props.cxx @@ -355,6 +355,18 @@ getHeadingMag () return magheading; } +/** + * Return the current track in degrees. + */ +static double +getTrackMag () +{ + double magtrack; + magtrack = current_aircraft.fdm_state->get_Track() - getMagVar(); + if (magtrack < 0) magtrack += 360; + return magtrack; +} + static long getWarp () { @@ -514,6 +526,7 @@ FGProperties::bind () // Orientation fgTie("/orientation/heading-magnetic-deg", getHeadingMag); + fgTie("/orientation/track-magnetic-deg", getTrackMag); fgTie("/environment/magnetic-variation-deg", getMagVar); fgTie("/environment/magnetic-dip-deg", getMagDip); @@ -543,6 +556,7 @@ FGProperties::unbind () // Orientation fgUntie("/orientation/heading-magnetic-deg"); + fgUntie("/orientation/track-magnetic-deg"); // Environment fgUntie("/environment/magnetic-variation-deg");