X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2FYASim%2FYASim.cxx;h=70c7459d2fabba8d304638c191ac435c5ee3fa81;hb=d66903e9ad63b91182ccc25d9bb82f18f8dd98b6;hp=a57e5e42cad68aef8ca14451b83261e38b3efce2;hpb=575b3ac98c2aad3fd80375cdfbe49c9180b8788e;p=flightgear.git

diff --git a/src/FDM/YASim/YASim.cxx b/src/FDM/YASim/YASim.cxx
index a57e5e42c..70c7459d2 100644
--- a/src/FDM/YASim/YASim.cxx
+++ b/src/FDM/YASim/YASim.cxx
@@ -1,12 +1,16 @@
+
+#ifdef HAVE_CONFIG_H
+#  include "config.h"
+#endif
+
 #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/placement.hxx>
 
 #include "FGFDM.hpp"
 #include "Atmosphere.hpp"
@@ -16,6 +20,9 @@
 #include "Integrator.hpp"
 #include "Glue.hpp"
 #include "Gear.hpp"
+#include "Hook.hpp"
+#include "Launchbar.hpp"
+#include "FGGround.hpp"
 #include "PropEngine.hpp"
 #include "PistonEngine.hpp"
 
@@ -23,7 +30,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;
@@ -34,42 +43,23 @@ 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/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)
+YASim::YASim(double dt) :
+    _simTime(0)
 {
 //     set_delta_t(dt);
     _fdm = new FGFDM();
 
     _dt = dt;
 
+    _fdm->getAirplane()->getModel()->setGroundCallback( new FGGround(this) );
     _fdm->getAirplane()->getModel()->getIntegrator()->setInterval(_dt);
 }
 
+YASim::~YASim()
+{
+    delete _fdm;
+}
+
 void YASim::report()
 {
     Airplane* a = _fdm->getAirplane();
@@ -111,12 +101,12 @@ void YASim::bind()
 
     char buf[256];
     for(int i=0; i<_fdm->getAirplane()->getModel()->numThrusters(); i++) {
-	sprintf(buf, "/engines/engine[%d]/fuel-flow-gph", i); fgUntie(buf);
-	sprintf(buf, "/engines/engine[%d]/rpm", i);           fgUntie(buf);
-	sprintf(buf, "/engines/engine[%d]/mp-osi", i);        fgUntie(buf);
-	sprintf(buf, "/engines/engine[%d]/egt-degf", i);      fgUntie(buf);
+	sprintf(buf, "/engines/engine[%d]/fuel-flow-gph", i);        fgUntie(buf);
+	sprintf(buf, "/engines/engine[%d]/rpm", i);                  fgUntie(buf);
+	sprintf(buf, "/engines/engine[%d]/mp-osi", i);               fgUntie(buf);
+	sprintf(buf, "/engines/engine[%d]/egt-degf", i);             fgUntie(buf);
+	sprintf(buf, "/engines/engine[%d]/oil-temperature-degf", i); fgUntie(buf);
     }
-
 }
 
 void YASim::init()
@@ -145,8 +135,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);
@@ -164,21 +153,10 @@ void YASim::init()
 	SGPropertyNode * node = fgGetNode("gear/gear", i, true);
         float pos[3];
         g->getPosition(pos);
-	node->setDoubleValue("xoffset-in", pos[0]);
-	node->setDoubleValue("yoffset-in", pos[1]);
-	node->setDoubleValue("zoffset-in", pos[2]);
+	node->setDoubleValue("xoffset-in", pos[0] * M2FT * 12);
+	node->setDoubleValue("yoffset-in", pos[1] * M2FT * 12);
+	node->setDoubleValue("zoffset-in", pos[2] * M2FT * 12);
     }
-//     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.
@@ -197,13 +175,6 @@ void YASim::init()
 	fgSetBool("/controls/gear/gear-down", true);
     }
 
-    // The pilot's eyepoint
-    float pilot[3];
-    a->getPilotPos(pilot);
-//     fgSetFloat("/sim/view/pilot/x-offset-m", -pilot[0]);
-//     fgSetFloat("/sim/view/pilot/y-offset-m", -pilot[1]);
-//     fgSetFloat("/sim/view/pilot/z-offset-m", pilot[2]);
-
     // Blank the state, and copy in ours
     State s;
     m->setState(&s);
@@ -218,29 +189,51 @@ 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;
+    }
+
+    // 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 lat = get_Latitude(); float lon = get_Longitude();
+    float alt = get_Altitude() * FT2M; double xyz[3];
+    sgGeodToCart(lat, lon, alt, xyz);
+    // build the environment cache.
+    float vr = _fdm->getVehicleRadius();
+    vr += 2.0*FT2M*dt*Math::mag3(v);
+    prepare_ground_cache_m( _simTime, _simTime + dt, xyz, vr );
+
+    // Track time increments.
+    FGGround* gr
+      = (FGGround*)_fdm->getAirplane()->getModel()->getGroundCallback();
 
     int i;
     for(i=0; i<iterations; i++) {
-	    copyToYASim(false);
-	    _fdm->iterate(_dt);
-	    copyFromYASim();
-
-	    printDEBUG();
+        gr->setTimeOffset(_simTime + i*_dt);
+        copyToYASim(false);
+        _fdm->iterate(_dt);
+        copyFromYASim();
     }
+
+    // Increment the local sim time
+    _simTime += dt;
+    gr->setTimeOffset(_simTime);
 }
 
 void YASim::copyToYASim(bool copyState)
 {
     // Physical state
-    float lat = get_Latitude();
-    float lon = get_Longitude();
+    double lat = get_Latitude();
+    double lon = get_Longitude();
     float alt = get_Altitude() * FT2M;
     float roll = get_Phi();
     float pitch = get_Theta();
@@ -252,10 +245,6 @@ void YASim::copyToYASim(bool copyState)
     wind[1] = get_V_east_airmass() * FT2M * -1.0;
     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();
-    // cout << "YASIM: ground = " << ground << endl;
-
     float pressure = fgGetFloat("/environment/pressure-inhg") * INHG2PA;
     float temp = fgGetFloat("/environment/temperature-degc") + 273.15;
     float dens = fgGetFloat("/environment/density-slugft3") 
@@ -268,7 +257,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);
@@ -322,19 +311,16 @@ void YASim::copyToYASim(bool copyState)
     Math::tmul33(xyz2ned, wind, wind);
     model->setWind(wind);
 
-    // 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.
-    double xyz[3], gplane[3]; float up[3];
-    Glue::geod2xyz(lat, lon, ground, xyz);
-    Glue::geodUp(xyz, 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];
-    model->setGroundPlane(gplane, rad);
-
     // air
     model->setAir(pressure, temp, dens);
+
+    // Query a ground plane for each gear/hook/launchbar and
+    // write that value into the corresponding class.
+    _fdm->getAirplane()->getModel()->updateGround(&s);
+
+    Launchbar* l = model->getLaunchbar();
+    if (l)
+        l->setLaunchCmd(0.0<fgGetFloat("/controls/gear/catapult-launch-cmd"));
 }
 
 // All the settables:
@@ -390,14 +376,19 @@ 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);
+    double groundlevel_m = get_groundlevel_m(lat, lon, alt);
+    _set_Runway_altitude(groundlevel_m*SG_METER_TO_FEET);
+    _set_Altitude_AGL((alt-groundlevel_m)*SG_METER_TO_FEET);
+
+    // the smallest agl of all gears
+    fgSetFloat("/position/gear-agl-m", model->getAGL());
+    fgSetFloat("/position/gear-agl-ft", model->getAGL()*M2FT);
 
     // UNUSED
     //_set_Geocentric_Position(Glue::geod2geocLat(lat), lon, alt*M2FT);
 
-    _set_Altitude_AGL(model->getAGL() * M2FT);
-
     // useful conversion matrix
     float xyz2ned[9];
     Glue::xyz2nedMat(lat, lon, xyz2ned);
@@ -427,7 +418,7 @@ void YASim::copyFromYASim()
     _set_V_rel_wind(Math::mag3(v)*M2FT); // units?
 
     float P = fgGetDouble("/environment/pressure-inhg") * INHG2PA;
-    float T = fgGetDouble("/environment/temperature-degC") + 273.15;
+    float T = fgGetDouble("/environment/temperature-degc") + 273.15;
     float D = fgGetFloat("/environment/density-slugft3")
         *SLUG2KG * M2FT*M2FT*M2FT;
     _set_V_equiv_kts(Atmosphere::calcVEAS(v[0], P, T, D)*MPS2KTS);
@@ -457,7 +448,7 @@ void YASim::copyFromYASim()
 
     // orientation
     float alpha, beta;
-    Glue::calcAlphaBeta(s, &alpha, &beta);
+    Glue::calcAlphaBeta(s, wind, &alpha, &beta);
     _set_Alpha(alpha);
     _set_Beta(beta);
 
@@ -466,6 +457,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
@@ -483,29 +476,26 @@ void YASim::copyFromYASim()
 	node->setBoolValue("has-brake", g->getBrake() != 0);
 	node->setBoolValue("wow", g->getCompressFraction() != 0);
 	node->setFloatValue("compression-norm", g->getCompressFraction());
+	node->setFloatValue("compression-m", g->getCompressDist());
+        node->setFloatValue("caster-angle-deg", g->getCasterAngle() * RAD2DEG);
+        node->setFloatValue("rollspeed-ms", g->getRollSpeed());
+        node->setBoolValue("ground-is-solid", g->getGroundIsSolid()!=0);
+        node->setFloatValue("ground-friction-factor", g->getGroundFrictionFactor());
     }
 
-    for(i=0; i<model->numThrusters(); i++) {
-        SGPropertyNode * node = fgGetNode("engines/engine", i, true);
-        Thruster* t = model->getThruster(i);
-
-	node->setBoolValue("running", t->isRunning());
-	node->setBoolValue("cranking", t->isCranking());
-
-        float tmp[3];
-        t->getThrust(tmp);
-	node->setDoubleValue("prop-thrust", Math::mag3(tmp) * KG2LBS / 9.8);
-
-        PropEngine* pe = t->getPropEngine();
-        if(pe) {
-	    node->setDoubleValue("rpm", pe->getOmega() * RAD2RPM);
-
-            pe->getTorque(tmp);
-            float power = Math::mag3(tmp) * pe->getOmega();
-            float maxPower = pe->getPistonEngine()->getMaxPower();
+    Hook* h = airplane->getHook();
+    if(h) {
+	SGPropertyNode * node = fgGetNode("gear/tailhook", 0, true);
+	node->setFloatValue("position-norm", h->getCompressFraction());
+    }
 
-	    node->setDoubleValue("max-hp", maxPower * W2HP);
-	    node->setDoubleValue("power-pct", 100 * power/maxPower);
-        }
+    Launchbar* l = airplane->getLaunchbar();
+    if(l) {
+	SGPropertyNode * node = fgGetNode("gear/launchbar", 0, true);
+	node->setFloatValue("position-norm", l->getCompressFraction());
+        node->setFloatValue("holdback-position-norm", l->getHoldbackCompressFraction());
+        node->setStringValue("state", l->getState());
+        node->setBoolValue("strop", l->getStrop());
     }
+
 }