Fix for bug 1304 - crash loading XML route

[flightgear.git] / src / FDM / YASim / YASim.cxx

diff --git a/src/FDM/YASim/YASim.cxx b/src/FDM/YASim/YASim.cxx
index 91662b6c4db130a96ecd4b23bc14cd74ddb91c4a..d82d14398c0a2628430360ae2f0266a10a956e15 100644 (file)
--- a/src/FDM/YASim/YASim.cxx
+++ b/src/FDM/YASim/YASim.cxx
@@ -4,6 +4,7 @@
 #endif
 
 #include <cstdlib>
+#include <cstdio>
 
 #include <simgear/debug/logstream.hxx>
 #include <simgear/math/sg_geodesy.hxx>
@@ -31,6 +32,7 @@
 #include "YASim.hxx"
 
 using namespace yasim;
+using std::string;
 
 static const float YASIM_PI = 3.14159265358979323846;
 static const float RAD2DEG = 180/YASIM_PI;
@@ -219,8 +221,14 @@ void YASim::init()
                 minGearZ = pos[2];
         }
         _set_Altitude(runway_altitude - minGearZ*M2FT);
+        // ground start-up: gear down
         fgSetBool("/controls/gear/gear-down", true);
     }
+    else
+    {
+        // airborne start-up: gear up
+        fgSetBool("/controls/gear/gear-down", false);
+    }
 
     // Blank the state, and copy in ours
     State s;
@@ -251,7 +259,11 @@ void YASim::update(double dt)
     // ground.  Calculate a cartesian coordinate for the ground under
     // us, find the (geodetic) up vector normal to the ground, then
     // use that to find the final (radius) term of the plane equation.
-    float v[3] = { get_uBody(), get_vBody(), get_wBody() };
+    float v[3] = {
+      static_cast<float>(get_uBody()),
+      static_cast<float>(get_vBody()),
+      static_cast<float>(get_wBody())
+    };
     float lat = get_Latitude(); float lon = get_Longitude();
     float alt = get_Altitude() * FT2M; double xyz[3];
     sgGeodToCart(lat, lon, alt, xyz);
@@ -385,7 +397,7 @@ void YASim::copyToYASim(bool copyState)
 // _set_Accels_CG_Body_N 
 // _set_Velocities_Local
 // _set_Velocities_Ground
-// _set_Velocities_Wind_Body
+// _set_Velocities_Body
 // _set_Omega_Body
 // _set_Euler_Rates
 // _set_Euler_Angles
@@ -458,6 +470,10 @@ void YASim::copyFromYASim()
 
     // _set_Geocentric_Rates(M2FT*v[0], M2FT*v[1], M2FT*v[2]); // UNUSED
 
+    // ecef speed in body axis
+    Math::vmul33(s->orient, s->v, v);
+    _set_Velocities_Body(v[0]*M2FT, -v[1]*M2FT, -v[2]*M2FT);
+
     // Airflow velocity.
     float wind[3];
     wind[0] = get_V_north_airmass() * FT2M * -1.0;  // Wind in NED
@@ -466,7 +482,6 @@ void YASim::copyFromYASim()
     Math::tmul33(xyz2ned, wind, wind);              // Wind in global
     Math::sub3(s->v, wind, v);                      // V - wind in global
     Math::vmul33(s->orient, v, v);               // to body coordinates
-    _set_Velocities_Wind_Body(v[0]*M2FT, -v[1]*M2FT, -v[2]*M2FT);
     _set_V_rel_wind(Math::mag3(v)*M2FT); // units?
 
     float P = _pressure_inhg->getFloatValue() * INHG2PA;
@@ -475,7 +490,7 @@ void YASim::copyFromYASim()
         *SLUG2KG * M2FT*M2FT*M2FT;
     _set_V_equiv_kts(Atmosphere::calcVEAS(v[0], P, T, D)*MPS2KTS);
     _set_V_calibrated_kts(Atmosphere::calcVCAS(v[0], P, T)*MPS2KTS);
-    _set_Mach_number(Atmosphere::calcMach(v[0], T));
+    _set_Mach_number(Atmosphere::calcMach(Math::mag3(v), T));
 
     // acceleration
     Math::vmul33(xyz2ned, s->acc, v);
@@ -502,7 +517,7 @@ void YASim::copyFromYASim()
     float alpha, beta;
     Glue::calcAlphaBeta(s, wind, &alpha, &beta);
     _set_Alpha(alpha);
-    _set_Beta(beta);
+    _set_Beta(-beta);
 
     float tmp[9];
     Math::trans33(xyz2ned, tmp);