FGPUIDialog: fix reading from already free'd memory.

[flightgear.git] / src / FDM / SP / ACMS.cxx

diff --git a/src/FDM/SP/ACMS.cxx b/src/FDM/SP/ACMS.cxx
index c5b7eefdd23e501b3cfb756df4f7aeff6210f5bd..6fd219bc932ec3bf1b67e63925d35e9af47b1d96 100644 (file)
--- a/src/FDM/SP/ACMS.cxx
+++ b/src/FDM/SP/ACMS.cxx
@@ -16,9 +16,14 @@
 //
 // You should have received a copy of the GNU General Public License
 // along with this program; if not, write to the Free Software
-// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 //
 
+#ifdef HAVE_CONFIG_H
+#  include <config.h>
+#endif
+
+#include <simgear/math/SGMath.hxx>
 #include <simgear/math/sg_geodesy.hxx>
 #include <Main/fg_props.hxx>
 
@@ -74,9 +79,7 @@ void FGACMS::update( double dt ) {
     double acc_down = _acc_down->getDoubleValue();
     _set_Accels_Local( acc_lon, acc_lat, acc_down );
 
-    sgVec3 accel_ned;
-    sgSetVec3(accel_ned, acc_lon, acc_lat, acc_down);
-    double accel = sgLengthVec3 (accel_ned) * SG_FEET_TO_METER;
+    double accel = norm(SGVec3d(acc_lon, acc_lat, acc_down)) * SG_FEET_TO_METER;
 
     double velocity = (_speed->getDoubleValue() * SG_KT_TO_MPS) + accel * dt;
     double dist = cos (pitch) * velocity * dt;
@@ -85,16 +88,14 @@ void FGACMS::update( double dt ) {
     _set_V_calibrated_kts( kts );
     _set_V_ground_speed( kts );
 
+    SGGeod pos = getPosition();
     // update (lon/lat) position
-    double lat2, lon2, az2;
-    geo_direct_wgs_84 ( get_Altitude(),
-                        get_Latitude() * SGD_RADIANS_TO_DEGREES,
-                        get_Longitude() * SGD_RADIANS_TO_DEGREES,
-                        heading * SGD_RADIANS_TO_DEGREES,
-                        dist, &lat2, &lon2, &az2 );
+    SGGeod pos2;
+    double az2;
+    geo_direct_wgs_84 ( pos, heading * SGD_RADIANS_TO_DEGREES,
+                        dist, pos2, &az2 );
 
-    _set_Longitude( lon2 * SGD_DEGREES_TO_RADIANS );
-    _set_Latitude( lat2 * SGD_DEGREES_TO_RADIANS );
+    _set_Geodetic_Position(  pos2.getLatitudeRad(), pos2.getLongitudeRad(), pos.getElevationFt() );
 
     double sl_radius, lat_geoc;
     sgGeodToGeoc( get_Latitude(), get_Altitude(), &sl_radius, &lat_geoc );
@@ -103,6 +104,7 @@ void FGACMS::update( double dt ) {
     _set_Euler_Rates(0,0,0);
 
     _set_Geocentric_Position( lat_geoc, get_Longitude(), sl_radius);
+    _update_ground_elev_at_pos();
     _set_Sea_level_radius( sl_radius * SG_METER_TO_FEET);
 
 }