X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2FYASim%2FYASim.cxx;h=70c7459d2fabba8d304638c191ac435c5ee3fa81;hb=d66903e9ad63b91182ccc25d9bb82f18f8dd98b6;hp=a4cacde98a651f33d8b7e37e0ee4f458f904f0f2;hpb=f80b8660bf5ffdd3878ee7edf490ec61e7043fbc;p=flightgear.git diff --git a/src/FDM/YASim/YASim.cxx b/src/FDM/YASim/YASim.cxx index a4cacde98..70c7459d2 100644 --- a/src/FDM/YASim/YASim.cxx +++ b/src/FDM/YASim/YASim.cxx @@ -1,9 +1,16 @@ -#include + +#ifdef HAVE_CONFIG_H +# include "config.h" +#endif + #include +#include +#include +#include #include + #include
#include
-#include #include "FGFDM.hpp" #include "Atmosphere.hpp" @@ -13,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" @@ -20,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; @@ -31,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-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(); @@ -108,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() @@ -126,9 +119,23 @@ void YASim::init() m->setCrashed(false); + // Figure out the initial speed type + string speed_set = fgGetString("/sim/presets/speed-set", "UVW"); + if (speed_set == "NED") + _speed_set = NED; + else if (speed_set == "UVW") + _speed_set = UVW; + else if (speed_set == "knots") + _speed_set = KNOTS; + else if (speed_set == "mach") + _speed_set = MACH; + else { + _speed_set = UVW; + SG_LOG(SG_FLIGHT, SG_ALERT, "Unknown speed type " << speed_set); + } + // 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); @@ -146,27 +153,16 @@ 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]); - } - for(i=0; inumThrusters(); 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); + node->setDoubleValue("xoffset-in", pos[0] * M2FT * 12); + node->setDoubleValue("yoffset-in", pos[1] * M2FT * 12); + node->setDoubleValue("zoffset-in", pos[2] * M2FT * 12); } - fgSetFloat("/controls/slats", 0); - fgSetFloat("/controls/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; inumGear(); i++) { Gear* g = a->getGear(i); @@ -176,16 +172,9 @@ 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 - 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); @@ -200,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; iiterate(_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(); @@ -234,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") @@ -250,35 +257,70 @@ 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); Math::mmul33(s.orient, xyz2ned, s.orient); - // Copy in the existing velocity for now... - Math::set3(model->getState()->v, s.v); + // 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; + v[1] = get_V_east() * FT2M * -1.0; + v[2] = get_V_down() * FT2M * -1.0; + break; + case UVW: + v[0] = get_uBody() * FT2M; + v[1] = get_vBody() * FT2M; + v[2] = get_wBody() * FT2M; + Math::tmul33(s.orient, v, v); + break; + case KNOTS: + 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: + 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; + v[1] = 0; + v[2] = 0; + break; + } + 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 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.0pos, &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); @@ -366,12 +413,12 @@ void YASim::copyFromYASim() wind[2] = get_V_down_airmass() * FT2M * -1.0; Math::tmul33(xyz2ned, wind, wind); // Wind in global Math::sub3(s->v, wind, v); // V - wind in global - Math::vmul33(s->orient, s->v, v); // to body coordinates + 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 = 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); @@ -401,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); @@ -410,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 @@ -427,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; inumThrusters(); 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()); } + }