YASim-0.1.3 updates.

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

diff --git a/src/FDM/YASim/YASim.cxx b/src/FDM/YASim/YASim.cxx
index 3774618c7b40f164b29e180fc63c3c92911f2592..6f0ca2bd6665885428c36d667ef65c561519d9a7 100644 (file)
--- a/src/FDM/YASim/YASim.cxx
+++ b/src/FDM/YASim/YASim.cxx
@@ -16,16 +16,20 @@
 #include "Glue.hpp"
 #include "Gear.hpp"
 #include "PropEngine.hpp"
+#include "PistonEngine.hpp"
 
 #include "YASim.hxx"
 
 using namespace yasim;
 
 static const float RAD2DEG = 180/3.14159265358979323846;
+static const float RAD2RPM = 9.54929658551;
 static const float M2FT = 3.2808399;
 static const float FT2M = 0.3048;
 static const float MPS2KTS = 3600.0/1852.0;
 static const float CM2GALS = 264.172037284; // gallons/cubic meter
+static const float KG2LBS = 2.20462262185;
+static const float W2HP = 1.3416e-3;
 
 void YASim::printDEBUG()
 {
@@ -101,10 +105,12 @@ void YASim::init()
     // Superclass hook
     common_init();
 
+    m->setCrashed(false);
+
     // Build a filename and parse it
     SGPath f(globals->get_fg_root());
     f.append("Aircraft-yasim");
-    f.append(fgGetString("/sim/aircraft"));
+    f.append(fgGetString("/sim/aero"));
     f.concat(".xml");
     readXML(f.str(), *_fdm);
 
@@ -114,16 +120,42 @@ void YASim::init()
 
     _fdm->init();
 
-    // Lift the plane up so the gear clear the ground
-    float minGearZ = 1e18;
-    for(int i=0; i<a->numGear(); i++) {
-       Gear* g = a->getGear(i);
-       float pos[3];
-       g->getPosition(pos);
-       if(pos[2] < minGearZ)
-           minGearZ = pos[2];
+    // Create some FG{Eng|Gear}Interface objects
+    int i;
+    for(i=0; i<a->numGear(); i++) {
+        Gear* g = a->getGear(i);
+        FGGearInterface fgg;
+        float pos[3];
+        g->getPosition(pos);
+        fgg.SetX(pos[0]); fgg.SetY(-pos[1]); fgg.SetZ(-pos[2]);
+        add_gear_unit(fgg);
+    }
+    for(i=0; i<m->numThrusters(); i++) {
+        FGEngInterface fge;
+        add_engine(fge);
+
+       // 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);
+    }
+    
+
+    // Are we at ground level?  If so, lift the plane up so the gear
+    // clear the ground
+    if(get_Altitude() - get_Runway_altitude() < 50) {
+       float minGearZ = 1e18;
+       for(i=0; i<a->numGear(); i++) {
+           Gear* g = a->getGear(i);
+           float pos[3];
+           g->getPosition(pos);
+           if(pos[2] < minGearZ)
+               minGearZ = pos[2];
+       }
+       _set_Altitude(get_Runway_altitude() - minGearZ*M2FT);
     }
-    _set_Altitude(get_Altitude() - minGearZ*M2FT);
 
     // The pilot's eyepoint
     float pilot[3];
@@ -142,7 +174,12 @@ void YASim::init()
 
 bool YASim::update(int iterations)
 {
-    for(int i=0; i<iterations; i++) {
+    // If we're crashed, then we don't care
+    if(_fdm->getAirplane()->getModel()->isCrashed())
+       return true;
+
+    int i;
+    for(i=0; i<iterations; i++) {
         // Remember, update only every 4th call
         _updateCount++;
         if(_updateCount >= 4) {
@@ -214,7 +251,8 @@ void YASim::copyToYASim(bool copyState)
     double xyz[3], gplane[3]; float up[3];
     Glue::geod2xyz(lat, lon, ground, xyz);
     Glue::geodUp(xyz, up); // FIXME, needless reverse computation...
-    for(int i=0; i<3; i++) gplane[i] = up[i];
+    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];
     model->setGroundPlane(gplane, rad);
 
@@ -269,7 +307,8 @@ void YASim::copyToYASim(bool copyState)
 // _set_Static_temperature
 void YASim::copyFromYASim()
 {
-    Model* model = _fdm->getAirplane()->getModel();
+    Airplane* airplane = _fdm->getAirplane();
+    Model* model = airplane->getModel();
     State* s = model->getState();
 
     // position
@@ -280,10 +319,7 @@ void YASim::copyFromYASim()
     // UNUSED
     //_set_Geocentric_Position(Glue::geod2geocLat(lat), lon, alt*M2FT);
 
-    // FIXME: there's a normal vector available too, use it.
-    float groundMSL = scenery.get_cur_elev();
-    _set_Runway_altitude(groundMSL*M2FT);
-    _set_Altitude_AGL((alt - groundMSL)*M2FT);
+    _set_Altitude_AGL(model->getAGL() * M2FT);
 
     // useful conversion matrix
     float xyz2ned[9];
@@ -350,4 +386,46 @@ void YASim::copyFromYASim()
 
     Glue::calcEulerRates(s, &roll, &pitch, &hdg);
     _set_Euler_Rates(roll, pitch, hdg);
+
+    // Fill out our engine and gear objects
+    int i;
+    for(i=0; i<get_num_gear(); i++) {
+        FGGearInterface* fgg = get_gear_unit(i);
+        Gear* g = airplane->getGear(i);
+        if(g->getBrake() != 0)
+            fgg->SetBrake(true);
+        if(g->getCompressFraction() != 0)
+            fgg->SetWoW(true);
+       else
+           fgg->SetWoW(false);
+        fgg->SetPosition(g->getExtension());
+    }
+
+    for(i=0; i<get_num_engines(); i++) {
+        FGEngInterface* fge = get_engine(i);
+        Thruster* t = model->getThruster(i);
+
+        fge->set_Running_Flag(true);
+        fge->set_Cranking_Flag(false);
+
+        // Note: assumes all tanks have the same fuel density!
+        fge->set_Fuel_Flow(CM2GALS * t->getFuelFlow()
+                           / airplane->getFuelDensity(0));
+
+        float tmp[3];
+        t->getThrust(tmp);
+        fge->set_prop_thrust(Math::mag3(tmp) * KG2LBS / 9.8);
+
+        PropEngine* pe = t->getPropEngine();
+        if(pe) {
+            fge->set_RPM(pe->getOmega() * RAD2RPM);
+
+            pe->getTorque(tmp);
+            float power = Math::mag3(tmp) * pe->getOmega();
+            float maxPower = pe->getPistonEngine()->getPower();
+
+            fge->set_MaxHP(maxPower * W2HP);
+            fge->set_Percentage_Power(100 * power/maxPower);
+        }
+    }
 }