X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2FYASim%2FYASim.cxx;h=88937ed2db0834066d5853ab5cce05c79844ec8b;hb=d0af0c5f58b2d7c00c3ad9d02363f95fd63a1c7b;hp=f4c87bbbeb8b4f14517f43e58e382fd4e5f0acfb;hpb=c07f22ea160581b774e71f00100cb88324f7117d;p=flightgear.git

diff --git a/src/FDM/YASim/YASim.cxx b/src/FDM/YASim/YASim.cxx
index f4c87bbbe..88937ed2d 100644
--- a/src/FDM/YASim/YASim.cxx
+++ b/src/FDM/YASim/YASim.cxx
@@ -1,11 +1,13 @@
-#include <simgear/misc/sg_path.hxx>
 #include <simgear/debug/logstream.hxx>
+#include <simgear/math/sg_geodesy.hxx>
+#include <simgear/misc/sg_path.hxx>
+#include <simgear/scene/model/location.hxx>
+#include <simgear/scene/model/placement.hxx>
 #include <simgear/xml/easyxml.hxx>
+
 #include <Main/globals.hxx>
 #include <Main/fg_props.hxx>
 #include <Model/acmodel.hxx>
-#include <Model/model.hxx>
-#include <Main/location.hxx>
 
 #include "FGFDM.hpp"
 #include "Atmosphere.hpp"
@@ -22,7 +24,9 @@
 
 using namespace yasim;
 
-static const float RAD2DEG = 180/3.14159265358979323846;
+static const float YASIM_PI = 3.14159265358979323846;
+static const float RAD2DEG = 180/YASIM_PI;
+static const float PI2 = YASIM_PI*2;
 static const float RAD2RPM = 9.54929658551;
 static const float M2FT = 3.2808399;
 static const float FT2M = 0.3048;
@@ -33,32 +37,6 @@ static const float W2HP = 1.3416e-3;
 static const float INHG2PA = 3386.389;
 static const float SLUG2KG = 14.59390;
 
-void YASim::printDEBUG()
-{
-    static int debugCount = 0;
-    
-    debugCount++;
-    if(debugCount >= 3) {
-	debugCount = 0;
-
-//  	printf("N1 %5.1f N2 %5.1f FF %7.1f EPR %4.2f EGT %6.1f\n",
-//  	       fgGetFloat("/engines/engine[0]/n1"),
-//  	       fgGetFloat("/engines/engine[0]/n2"),
-//  	       fgGetFloat("/engines/engine[0]/fuel-flow-gph"),
-//  	       fgGetFloat("/engines/engine[0]/epr"),
-//  	       fgGetFloat("/engines/engine[0]/egt"));
-
-// 	printf("gear: %f\n", fgGetFloat("/controls/gear-down"));
-
-//    	printf("alpha %5.1f beta %5.1f\n", get_Alpha()*57.3, get_Beta()*57.3);
-
-// 	printf("pilot: %f %f %f\n",
-// 	       fgGetDouble("/sim/view/pilot/x-offset-m"),
-// 	       fgGetDouble("/sim/view/pilot/y-offset-m"),
-// 	       fgGetDouble("/sim/view/pilot/z-offset-m"));
-    }
-}
-
 YASim::YASim(double dt)
 {
 //     set_delta_t(dt);
@@ -115,7 +93,6 @@ void YASim::bind()
 	sprintf(buf, "/engines/engine[%d]/mp-osi", i);        fgUntie(buf);
 	sprintf(buf, "/engines/engine[%d]/egt-degf", i);      fgUntie(buf);
     }
-
 }
 
 void YASim::init()
@@ -144,8 +121,7 @@ void YASim::init()
     }
 
     // Build a filename and parse it
-    SGPath f(globals->get_fg_root());
-    f.append("Aircraft-yasim");
+    SGPath f(fgGetString("/sim/aircraft-dir"));
     f.append(fgGetString("/sim/aero"));
     f.concat(".xml");
     readXML(f.str(), *_fdm);
@@ -167,23 +143,23 @@ void YASim::init()
 	node->setDoubleValue("yoffset-in", pos[1]);
 	node->setDoubleValue("zoffset-in", pos[2]);
     }
-    for(i=0; i<m->numThrusters(); i++) {
-	// Sanify the initial input conditions
-	char buf[64];
- 	sprintf(buf, "/controls/throttle[%d]", i);        fgSetFloat(buf, 0);
-	sprintf(buf, "/controls/mixture[%d]", i);         fgSetFloat(buf, 1);
-	sprintf(buf, "/controls/propeller-pitch[%d]", i); fgSetFloat(buf, 1);
-	sprintf(buf, "/controls/afterburner[%d]", i);     fgSetFloat(buf, 0);
-    }
-
-    fgSetFloat("/controls/slats", 0);
-    fgSetFloat("/controls/spoilers", 0);
+//     for(i=0; i<m->numThrusters(); i++) {
+// 	// Sanify the initial input conditions
+// 	char buf[64];
+//  	sprintf(buf, "/controls/engines/engine[%d]/throttle", i);        fgSetFloat(buf, 0);
+// 	sprintf(buf, "/controls/engines/engine[%d]/mixture", i);         fgSetFloat(buf, 1);
+// 	sprintf(buf, "/controls/engines/engine[%d]/propeller-pitch", i); fgSetFloat(buf, 1);
+// 	sprintf(buf, "/controls/engines/engine[%d]/augmentation", i);     fgSetFloat(buf, 0);
+//     }
+
+//     fgSetFloat("/controls/flight/slats", 0);
+//     fgSetFloat("/controls/flight/spoilers", 0);
 
     // Are we at ground level?  If so, lift the plane up so the gear
     // clear the ground.
     double runway_altitude = get_Runway_altitude();
-    fgSetBool("/controls/gear-down", false);
     if(get_Altitude() - runway_altitude < 50) {
+        fgSetBool("/controls/gear/gear-down", false);
 	float minGearZ = 1e18;
 	for(i=0; i<a->numGear(); i++) {
 	    Gear* g = a->getGear(i);
@@ -193,7 +169,7 @@ void YASim::init()
 		minGearZ = pos[2];
 	}
 	_set_Altitude(runway_altitude - minGearZ*M2FT);
-	fgSetBool("/controls/gear-down", true);
+	fgSetBool("/controls/gear/gear-down", true);
     }
 
     // The pilot's eyepoint
@@ -217,21 +193,22 @@ void YASim::init()
 void YASim::update(double dt)
 {
     if (is_suspended())
-      return;
+        return;
 
     int iterations = _calc_multiloop(dt);
 
     // If we're crashed, then we don't care
-    if(_fdm->getAirplane()->getModel()->isCrashed())
-      return;
+    if(_fdm->getAirplane()->getModel()->isCrashed()) {
+        if(!fgGetBool("/sim/crashed"))
+            fgSetBool("/sim/crashed", true);
+        return;
+    }
 
     int i;
     for(i=0; i<iterations; i++) {
-	    copyToYASim(false);
-	    _fdm->iterate(_dt);
-	    copyFromYASim();
-
-	    printDEBUG();
+        copyToYASim(false);
+        _fdm->iterate(_dt);
+        copyFromYASim();
     }
 }
 
@@ -252,7 +229,7 @@ void YASim::copyToYASim(bool copyState)
     wind[2] = get_V_down_airmass() * FT2M * -1.0;
 
     // Get ground elevation from the FGinterface's FGlocation data
-    double ground = getACModel()->get3DModel()->getFGLocation()->get_cur_elev_m();
+    double ground = getACModel()->get3DModel()->getSGLocation()->get_cur_elev_m();
     // cout << "YASIM: ground = " << ground << endl;
 
     float pressure = fgGetFloat("/environment/pressure-inhg") * INHG2PA;
@@ -267,7 +244,7 @@ void YASim::copyToYASim(bool copyState)
     Glue::xyz2nedMat(lat, lon, xyz2ned);
 
     // position
-    Glue::geod2xyz(lat, lon, alt, s.pos);
+    sgGeodToCart(lat, lon, alt, s.pos);
 
     // orientation
     Glue::euler2orient(roll, pitch, hdg, s.orient);
@@ -276,6 +253,7 @@ void YASim::copyToYASim(bool copyState)
     // Velocity
     string speed_set = fgGetString("/sim/presets/speed-set", "UVW");
     float v[3];
+    bool needCopy = false;
     switch (_speed_set) {
     case NED:
         v[0] = get_V_north() * FT2M * -1.0;
@@ -289,17 +267,19 @@ void YASim::copyToYASim(bool copyState)
         Math::tmul33(s.orient, v, v);
         break;
     case KNOTS:
-        // FIXME: right now this does true instead of calibrated airspeed
-        v[0] = get_V_calibrated_kts()/MPS2KTS;
+        v[0] = Atmosphere::spdFromVCAS(get_V_calibrated_kts()/MPS2KTS,
+                                       pressure, temp);
         v[1] = 0;
         v[2] = 0;
         Math::tmul33(s.orient, v, v);
+        needCopy = true;
         break;
     case MACH:
-        // FIXME: not even trying to implement this yet
-        v[0] = 0;
+        v[0] = Atmosphere::spdFromMach(get_Mach_number(), temp);
         v[1] = 0;
         v[2] = 0;
+        Math::tmul33(s.orient, v, v);
+        needCopy = true;
         break;
     default:
         v[0] = 0;
@@ -307,10 +287,11 @@ void YASim::copyToYASim(bool copyState)
         v[2] = 0;
         break;
     }
-    _speed_set = UVW;           // change to this after initial setting
+    if (!copyState)
+        _speed_set = UVW;       // change to this after initial setting
     Math::set3(v, s.v);
 
-    if(copyState)
+    if(copyState || needCopy)
 	model->setState(&s);
 
     // wind
@@ -321,8 +302,8 @@ void YASim::copyToYASim(bool copyState)
     // us, find the (geodetic) up vector normal to the ground, then
     // use that to find the final (radius) term of the plane equation.
     double xyz[3], gplane[3]; float up[3];
-    Glue::geod2xyz(lat, lon, ground, xyz);
-    Glue::geodUp(xyz, up); // FIXME, needless reverse computation...
+    sgGeodToCart(lat, lon, ground, xyz);
+    Glue::geodUp(lat, lon, up); // FIXME, needless reverse computation...
     int i;
     for(i=0; i<3; i++) gplane[i] = up[i];
     double rad = gplane[0]*xyz[0] + gplane[1]*xyz[1] + gplane[2]*xyz[2];
@@ -385,7 +366,7 @@ void YASim::copyFromYASim()
 
     // position
     double lat, lon, alt;
-    Glue::xyz2geod(s->pos, &lat, &lon, &alt);
+    sgCartToGeod(s->pos, &lat, &lon, &alt);
     _set_Geodetic_Position(lat, lon, alt*M2FT);
 
     // UNUSED
@@ -461,6 +442,8 @@ void YASim::copyFromYASim()
     Math::mmul33(s->orient, tmp, tmp);
     float roll, pitch, hdg;
     Glue::orient2euler(tmp, &roll, &pitch, &hdg);
+    // make heading positive value
+    if(hdg < 0.0) hdg += PI2;
     _set_Euler_Angles(roll, pitch, hdg);
 
     // rotation