diff --git a/src/FDM/YASim/Atmosphere.cpp b/src/FDM/YASim/Atmosphere.cpp index 7e532dd65..9f519d510 100644 --- a/src/FDM/YASim/Atmosphere.cpp +++ b/src/FDM/YASim/Atmosphere.cpp @@ -31,6 +31,10 @@ float Atmosphere::data[][4] = {{ 0, 288.20, 101325, 1.22500 }, { 18000, 216.66, 7565, 0.12165 }, { 18900, 216.66, 6570, 0.10564 }}; +// Universal gas constant for air, in SI units. P = R * rho * T. +// P in pascals (N/m^2), rho is kg/m^3, T in kelvin. +const float R = 287.1; + float Atmosphere::getStdTemperature(float alt) { return getRecord(alt, 1); @@ -48,7 +52,9 @@ float Atmosphere::getStdDensity(float alt) float Atmosphere::calcVEAS(float spd, float pressure, float temp) { - return 0; //FIXME + static float rho0 = getStdDensity(0); + float densityRatio = calcDensity(pressure, temp) / rho0; + return spd * Math::sqrt(densityRatio); } float Atmosphere::calcVCAS(float spd, float pressure, float temp) @@ -84,13 +90,12 @@ float Atmosphere::calcVCAS(float spd, float pressure, float temp) float Atmosphere::calcDensity(float pressure, float temp) { - // P = rho*R*T, R == 287 kPa*m^3 per kg*kelvin for air - return pressure / (287 * temp); + return pressure / (R * temp); } float Atmosphere::calcMach(float spd, float temp) { - return spd / Math::sqrt(1.4 * 287 * temp); + return spd / Math::sqrt(1.4 * R * temp); } float Atmosphere::getRecord(float alt, int recNum) diff --git a/src/FDM/YASim/FGFDM.cpp b/src/FDM/YASim/FGFDM.cpp index 96e431f65..6c7903409 100644 --- a/src/FDM/YASim/FGFDM.cpp +++ b/src/FDM/YASim/FGFDM.cpp @@ -77,23 +77,10 @@ Airplane* FGFDM::getAirplane() void FGFDM::init() { - // We don't want to use these ties (we set the values ourselves, - // and this works only for the first piston engine anyway). - fgUntie("/engines/engine[0]/rpm"); - fgUntie("/engines/engine[0]/egt-degf"); - fgUntie("/engines/engine[0]/cht-degf"); - fgUntie("/engines/engine[0]/oil-temperature-degf"); - fgUntie("/engines/engine[0]/mp-osi"); - fgUntie("/engines/engine[0]/fuel-flow-gph"); - fgUntie("/engines/engine[0]/running"); - fgUntie("/engines/engine[0]/cranking"); + // We don't want to use these ties (we set the values ourselves) fgUntie("/consumables/fuel/tank[0]/level-gal_us"); fgUntie("/consumables/fuel/tank[1]/level-gal_us"); - // Set these to sane values. We don't support engine start yet. - fgSetBool("/engines/engine[0]/running", true); - fgSetBool("/engines/engine[0]/cranking", false); - // Allows the user to start with something other than full fuel _airplane.setFuelFraction(fgGetFloat("/yasim/fuel-fraction", 1)); diff --git a/src/FDM/YASim/Gear.cpp b/src/FDM/YASim/Gear.cpp index 6c01885f0..cc27877fd 100644 --- a/src/FDM/YASim/Gear.cpp +++ b/src/FDM/YASim/Gear.cpp @@ -143,8 +143,11 @@ void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground) // First off, make sure that the gear "tip" is below the ground. // If it's not, there's no force. float a = ground[3] - Math::dot3(_pos, ground); - if(a > 0) + if(a > 0) { + _wow = 0; + _frac = 0; return; + } // Now a is the distance from the tip to ground, so make b the // distance from the base to ground. We can get the fraction @@ -169,11 +172,10 @@ void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground) body->pointVelocity(_contact, rot, cv); Math::add3(cv, v, cv); - // Finally, we can start adding up the forces. First the - // spring compression (note the clamping of _frac to 1): + // Finally, we can start adding up the forces. First the spring + // compression. (note the clamping of _frac to 1): _frac = (_frac > 1) ? 1 : _frac; float fmag = _frac*clen*_spring; - Math::mul3(fmag, cmpr, _force); // Then the damping. Use the only the velocity into the ground // (projection along "ground") projected along the compression @@ -183,10 +185,11 @@ void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground) float damp = _damp * dv; if(damp > fmag) damp = fmag; // can't pull the plane down! if(damp < -fmag) damp = -fmag; // sanity - Math::mul3(-damp, cmpr, tmp); - Math::add3(_force, tmp, _force); - _wow = fmag - damp; + // The actual force applied is only the component perpendicular to + // the ground. Side forces come from velocity only. + _wow = (fmag - damp) * -Math::dot3(cmpr, ground); + Math::mul3(-_wow, ground, _force); // Wheels are funky. Split the velocity along the ground plane // into rolling and skidding components. Assuming small angles, diff --git a/src/FDM/YASim/Math.cpp b/src/FDM/YASim/Math.cpp index 000866373..b5d2c4e65 100644 --- a/src/FDM/YASim/Math.cpp +++ b/src/FDM/YASim/Math.cpp @@ -20,11 +20,6 @@ float Math::sqrt(float f) return ::sqrt(f); } -float Math::pow(float base, float exp) -{ - return ::pow(base, exp); -} - float Math::ceil(float f) { return ::ceil(f); @@ -60,7 +55,7 @@ double Math::sqrt(double f) return ::sqrt(f); } -double Math::pow(double base, double exp) +float Math::pow(double base, double exp) { return ::pow(base, exp); } diff --git a/src/FDM/YASim/Math.hpp b/src/FDM/YASim/Math.hpp index ef97f17f2..071f35cfc 100644 --- a/src/FDM/YASim/Math.hpp +++ b/src/FDM/YASim/Math.hpp @@ -12,17 +12,18 @@ public: // Simple wrappers around library routines static float abs(float f); static float sqrt(float f); - static float pow(float base, float exp); static float ceil(float f); static float sin(float f); static float cos(float f); static float tan(float f); static float atan2(float y, float x); + // Takes two args and runs afoul of the Koenig rules. + static float pow(double base, double exp); + // double variants of the above static double abs(double f); static double sqrt(double f); - static double pow(double base, double exp); static double ceil(double f); static double sin(double f); static double cos(double f); diff --git a/src/FDM/YASim/Model.cpp b/src/FDM/YASim/Model.cpp index ae7d6a655..0edc17bf7 100644 --- a/src/FDM/YASim/Model.cpp +++ b/src/FDM/YASim/Model.cpp @@ -47,6 +47,9 @@ Model::Model() // Default value of 30 Hz _integrator.setInterval(1.0/30.0); + + _agl = 0; + _crashed = false; } Model::~Model() @@ -99,6 +102,21 @@ void Model::iterate() _integrator.calcNewInterval(); } +bool Model::isCrashed() +{ + return _crashed; +} + +void Model::setCrashed(bool crashed) +{ + _crashed = crashed; +} + +float Model::getAGL() +{ + return _agl; +} + State* Model::getState() { return _s; @@ -269,20 +287,27 @@ void Model::newState(State* s) //printState(s); // Some simple collision detection + float min = 1e8; float ground[4], pos[3], cmpr[3]; ground[3] = localGround(s, ground); int i; for(i=0; i<_gears.size(); i++) { Gear* g = (Gear*)_gears.get(i); + + // Get the point of ground contact g->getPosition(pos); g->getCompression(cmpr); + Math::mul3(g->getCompressFraction(), cmpr, cmpr); Math::add3(cmpr, pos, pos); float dist = ground[3] - Math::dot3(pos, ground); - if(dist < 0) { - printf("CRASH: gear %d\n", i); - *(int*)0=0; - } + + // Find the lowest one + if(dist < min) + min = dist; } + _agl = min; + if(_agl < -1) // Allow for some integration slop + _crashed = true; } // Returns a unit "down" vector for the ground in out, and the diff --git a/src/FDM/YASim/Model.hpp b/src/FDM/YASim/Model.hpp index 42f9d2a72..b6ccdb3ad 100644 --- a/src/FDM/YASim/Model.hpp +++ b/src/FDM/YASim/Model.hpp @@ -24,6 +24,9 @@ public: State* getState(); void setState(State* s); + bool isCrashed(); + void setCrashed(bool crashed); + float getAGL(); void iterate(); @@ -81,6 +84,8 @@ private: float _torque[3]; State* _s; + bool _crashed; + float _agl; }; }; // namespace yasim diff --git a/src/FDM/YASim/PropEngine.cpp b/src/FDM/YASim/PropEngine.cpp index 838f4825e..2c028bbc8 100644 --- a/src/FDM/YASim/PropEngine.cpp +++ b/src/FDM/YASim/PropEngine.cpp @@ -126,9 +126,6 @@ void PropEngine::integrate(float dt) // needs to go away when the startup code gets written. if(_omega < 52.3) _omega = 52.3; - // FIXME: Integrate the propeller governor here, when that gets - // implemented... - // Store the total angular momentum into _gyro Math::mul3(_omega*_moment, _dir, _gyro); @@ -140,15 +137,25 @@ void PropEngine::integrate(float dt) float tau = _moment < 0 ? engTorque : -engTorque; Math::mul3(tau, _dir, _torque); - // Play with the variable propeller, but only if the propeller is - // vaguely stable alread (accelerating less than 100 rpm/s) - if(_variable && Math::abs(rotacc) < 20) { - float target = _minOmega + _advance*(_maxOmega-_minOmega); - float mod = 1.04; - if(target > _omega) mod = 1/mod; - float diff = Math::abs(target - _omega); - if(diff < 1) mod = 1 + (mod-1)*diff; - if(thrust < 0) mod = 1; + // Iterate the propeller governor, if we have one. Since engine + // torque is basically constant with RPM, we want to make the + // propeller torque at the target RPM equal to the engine by + // varying the pitch. Assume the the torque goes as the square of + // the RPM (roughly correct) and compute a "target" torque for the + // _current_ RPM. Seek to that. This is sort of a continuous + // Newton-Raphson, basically. + if(_variable) { + float targetOmega = _minOmega + _advance*(_maxOmega-_minOmega); + float ratio2 = (_omega*_omega)/(targetOmega*targetOmega); + float targetTorque = engTorque * ratio2; + + float mod = propTorque < targetTorque ? 1.04 : (1/1.04); + + // Convert to an acceleration here, so that big propellers + // don't seek faster than small ones. + float diff = Math::abs(propTorque - targetTorque) / _moment; + if(diff < 10) mod = 1 + (mod-1)*(0.1*diff); + _prop->modPitch(mod); } } diff --git a/src/FDM/YASim/YASim.cxx b/src/FDM/YASim/YASim.cxx index da2a2d0bd..6f0ca2bd6 100644 --- a/src/FDM/YASim/YASim.cxx +++ b/src/FDM/YASim/YASim.cxx @@ -105,6 +105,8 @@ 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"); @@ -141,16 +143,19 @@ void YASim::init() } - // Lift the plane up so the gear clear the ground - float minGearZ = 1e18; - for(i=0; inumGear(); i++) { - Gear* g = a->getGear(i); - float pos[3]; - g->getPosition(pos); - if(pos[2] < minGearZ) - minGearZ = pos[2]; + // 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; inumGear(); 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]; @@ -169,6 +174,10 @@ void YASim::init() bool YASim::update(int iterations) { + // If we're crashed, then we don't care + if(_fdm->getAirplane()->getModel()->isCrashed()) + return true; + int i; for(i=0; igetAGL() * M2FT); // useful conversion matrix float xyz2ned[9]; @@ -390,6 +396,8 @@ void YASim::copyFromYASim() fgg->SetBrake(true); if(g->getCompressFraction() != 0) fgg->SetWoW(true); + else + fgg->SetWoW(false); fgg->SetPosition(g->getExtension()); } @@ -398,6 +406,7 @@ void YASim::copyFromYASim() 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()