Updated to YASim-0.1.1

src/FDM/YASim/Airplane.cpp

@@ -10,8 +10,6 @@
 #include "Airplane.hpp"
 namespace yasim {
 
-// FIXME: hook gear extension into the force calculation somehow...
-
 Airplane::Airplane()
 {
     _emptyWeight = 0;
@@ -19,10 +17,12 @@ Airplane::Airplane()
     _wing = 0;
     _tail = 0;
     _ballast = 0;
-    _cruiseRho = 0;
+    _cruiseP = 0;
+    _cruiseT = 0;
     _cruiseSpeed = 0;
     _cruiseWeight = 0;
-    _approachRho = 0;
+    _approachP = 0;
+    _approachT = 0;
     _approachSpeed = 0;
     _approachAoA = 0;
     _approachWeight = 0;
@@ -51,8 +51,7 @@ void Airplane::iterate(float dt)
 {
     _model.iterate();
 
-    // Consume fuel
+    // FIXME: Consume fuel
-    // FIXME
 }
 
 ControlMap* Airplane::getControlMap()
@@ -139,14 +138,16 @@ void Airplane::setApproach(float speed, float altitude)
 void Airplane::setApproach(float speed, float altitude, float aoa)
 {
     _approachSpeed = speed;
-    _approachRho = Atmosphere::getStdDensity(altitude);
+    _approachP = Atmosphere::getStdPressure(altitude);
+    _approachT = Atmosphere::getStdTemperature(altitude);
     _approachAoA = aoa;
 }
  
 void Airplane::setCruise(float speed, float altitude)
 {
     _cruiseSpeed = speed;
-    _cruiseRho = Atmosphere::getStdDensity(altitude);
+    _cruiseP = Atmosphere::getStdPressure(altitude);
+    _cruiseT = Atmosphere::getStdTemperature(altitude);
     _cruiseAoA = 0;
     _tailIncidence = 0;
 }
@@ -254,6 +255,7 @@ int Airplane::addWeight(float* pos, float size)
     wr->handle = _model.getBody()->addMass(0, pos);
 
     wr->surf = new Surface();
+    wr->surf->setPosition(pos);
     wr->surf->setTotalDrag(size*size);
     _model.addSurface(wr->surf);
     _surfs.add(wr->surf);
@@ -364,7 +366,7 @@ float Airplane::compileFuselage(Fuselage* f)
     float len = Math::mag3(fwd);
     float wid = f->width;
     int segs = (int)Math::ceil(len/wid);
-    float segWgt = wid*wid/segs;
+    float segWgt = len*wid/segs;
     for(int j=0; j<segs; j++) {
         float frac = (j+0.5) / segs;
         float pos[3];
@@ -406,10 +408,11 @@ void Airplane::compileGear(GearRec* gr)
 
     // Put the surface at the half-way point on the gear strut, give
     // it a drag coefficient equal to a square of the same dimension
-    // (gear are really draggy) and make it symmetric.
+    // (gear are really draggy) and make it symmetric.  Assume that
+    // the "length" of the gear is 3x the compression distance
     float pos[3], cmp[3];
     g->getCompression(cmp);
-    float length = Math::mag3(cmp);
+    float length = 3 * Math::mag3(cmp);
     g->getPosition(pos);
     Math::mul3(0.5, cmp, cmp);
     Math::add3(pos, cmp, pos);
@@ -486,6 +489,11 @@ void Airplane::compile()
         tr->handle = _model.addThruster(tr->thruster);
     }
 
+    // Ground effect
+    float gepos[3];
+    float gespan = _wing->getGroundEffect(gepos);
+    _model.setGroundEffect(gepos, gespan, .3);
+
     solveGear();
     solve();
 
@@ -550,31 +558,15 @@ void Airplane::solveGear()
 
 void Airplane::stabilizeThrust()
 {
-    float thrust[3], tmp[3];
+    for(int i=0; i<_thrusters.size(); i++)
-    float last = 0;
+	_model.getThruster(i)->stabilize();
-    while(1) {
-        thrust[0] = thrust[1] = thrust[2] = 0;
-        for(int i=0; i<_thrusters.size(); i++) {
-            Thruster* t = _model.getThruster(i);
-            t->integrate(0.033);
-            t->getThrust(tmp);
-            Math::add3(thrust, tmp, thrust);
-        }
-
-        float t = Math::mag3(thrust);
-        float ratio = (t+0.1)/(last+0.1);
- 	if(ratio < 1.00001 && ratio > 0.99999)
- 	    break;
-
-        last = t;
-    }
 }
 
 void Airplane::runCruise()
 {
     setupState(_cruiseAoA, _cruiseSpeed, &_cruiseState);
     _model.setState(&_cruiseState);
-    _model.setAirDensity(_cruiseRho);
+    _model.setAir(_cruiseP, _cruiseT);
 
     // The control configuration
     _controls.reset();
@@ -600,7 +592,7 @@ void Airplane::runCruise()
     for(int i=0; i<_thrusters.size(); i++) {
 	Thruster* t = ((ThrustRec*)_thrusters.get(i))->thruster;
 	t->setWind(wind);
-	t->setDensity(_cruiseRho);
+	t->setAir(_cruiseP, _cruiseT);
     }
     stabilizeThrust();
 
@@ -614,7 +606,7 @@ void Airplane::runApproach()
 {
     setupState(_approachAoA, _approachSpeed, &_approachState);
     _model.setState(&_approachState);
-    _model.setAirDensity(_approachRho);
+    _model.setAir(_approachP, _approachT);
 
     // The control configuration
     _controls.reset();
@@ -640,7 +632,7 @@ void Airplane::runApproach()
     for(int i=0; i<_thrusters.size(); i++) {
 	Thruster* t = ((ThrustRec*)_thrusters.get(i))->thruster;
 	t->setWind(wind);
-	t->setDensity(_approachRho);
+	t->setAir(_approachP, _approachT);
     }
     stabilizeThrust();
 

src/FDM/YASim/Airplane.hpp

@@ -112,13 +112,15 @@ private:
 
     Vector _cruiseControls;
     State _cruiseState;
-    float _cruiseRho;
+    float _cruiseP;
+    float _cruiseT;
     float _cruiseSpeed;
     float _cruiseWeight;
 
     Vector _approachControls;
     State _approachState;
-    float _approachRho;
+    float _approachP;
+    float _approachT;
     float _approachSpeed;
     float _approachAoA;
     float _approachWeight;

src/FDM/YASim/Atmosphere.cpp

@@ -7,7 +7,7 @@ namespace yasim {
 // McCormick lists 299.16/101325/1.22500, but those don't agree with
 // R=287.  I chose to correct the temperature to 288.20, since 79F is
 // pretty hot for a "standard" atmosphere.
-//                             meters   kelvin     kPa   kg/m^3
+//                             meters   kelvin      Pa   kg/m^3
 float Atmosphere::data[][4] = {{ 0,     288.20, 101325, 1.22500 },
 			       { 900,   282.31,  90971, 1.12260 },
 			       { 1800,  276.46,  81494, 1.02690 },

src/FDM/YASim/ControlMap.cpp

@@ -1,5 +1,6 @@
 #include "Jet.hpp"
 #include "Thruster.hpp"
+#include "PropEngine.hpp"
 #include "Gear.hpp"
 #include "Wing.hpp"
 #include "Math.hpp"
@@ -114,7 +115,7 @@ void ControlMap::applyControls()
 	switch(o->type) {
 	case THROTTLE: ((Thruster*)obj)->setThrottle(lval);    break;
 	case MIXTURE:  ((Thruster*)obj)->setMixture(lval);     break;
-	case PROP:     ((Thruster*)obj)->setPropAdvance(lval); break;
+	case ADVANCE:  ((PropEngine*)obj)->setAdvance(lval);   break;
 	case REHEAT:   ((Jet*)obj)->setReheat(lval);           break;
 	case BRAKE:    ((Gear*)obj)->setBrake(lval);           break;
 	case STEER:    ((Gear*)obj)->setRotation(lval);        break;

src/FDM/YASim/ControlMap.hpp

@@ -9,7 +9,7 @@ class ControlMap {
 public:
     ~ControlMap();
 
-    enum OutputType { THROTTLE, MIXTURE, REHEAT, PROP,
+    enum OutputType { THROTTLE, MIXTURE, ADVANCE, REHEAT, PROP,
 		      BRAKE, STEER, EXTEND,
 		      INCIDENCE, FLAP0, FLAP1, SLAT, SPOILER };
 

src/FDM/YASim/FGFDM.cpp

@@ -22,6 +22,7 @@ static const float LBS2N = 4.44822;
 static const float LBS2KG = 0.45359237;
 static const float CM2GALS = 264.172037284;
 static const float HP2W = 745.700;
+static const float INHG2PA = 3386.389;
 
 // Stubs, so that this can be compiled without the FlightGear
 // binary.  What's the best way to handle this?
@@ -141,6 +142,7 @@ void FGFDM::startElement(const char* name, const XMLAttributes &atts)
 	v[2] = attrf(a, "z");
 	float mass = attrf(a, "mass") * LBS2KG;
 	j->setDryThrust(attrf(a, "thrust") * LBS2N);
+	j->setReheatThrust(attrf(a, "afterburner", 0) * LBS2N);
 	j->setPosition(v);
 	_airplane.addThruster(j, mass, v);
 	sprintf(buf, "/engines/engine[%d]", _nextEngine++);
@@ -333,21 +335,36 @@ void FGFDM::parsePropeller(XMLAttributes* a)
     float radius = attrf(a, "radius");
     float speed = attrf(a, "cruise-speed") * KTS2MPS;
     float omega = attrf(a, "cruise-rpm") * RPM2RAD;
-    float rho = Atmosphere::getStdDensity(attrf(a, "alt") * FT2M);
+    float power = attrf(a, "cruise-power") * HP2W;
-    float power = attrf(a, "takeoff-power") * HP2W;
+    float rho = Atmosphere::getStdDensity(attrf(a, "cruise-alt") * FT2M);
-    float omega0 = attrf(a, "takeoff-rpm") * RPM2RAD;
 
-    // FIXME: this is a hack.  Find a better way to ask the engine how
+    // Hack, fix this pronto:
-    // much power it can produce under cruise conditions.
+    float engP = attrf(a, "eng-power") * HP2W;
-    float cruisePower = (power * (rho/Atmosphere::getStdDensity(0))
+    float engS = attrf(a, "eng-rpm") * RPM2RAD;
-			 * (omega/omega0));
 
-    Propeller* prop = new Propeller(radius, speed, omega, rho, cruisePower,
+    Propeller* prop = new Propeller(radius, speed, omega, rho, power);
-                                    omega0, power);
+    PistonEngine* eng = new PistonEngine(engP, engS);
-    PistonEngine* eng = new PistonEngine(power, omega0);
     PropEngine* thruster = new PropEngine(prop, eng, moment);
     _airplane.addThruster(thruster, mass, cg);
 
+    if(a->hasAttribute("turbo-mul")) {
+        float mul = attrf(a, "turbo-mul");
+        float mp = attrf(a, "wastegate-mp", 1e6) * INHG2PA;
+        eng->setTurboParams(mul, mp);
+    }
+
+    if(a->hasAttribute("takeoff-power")) {
+	float power0 = attrf(a, "takeoff-power") * HP2W;
+	float omega0 = attrf(a, "takeoff-rpm") * RPM2RAD;
+	prop->setTakeoff(omega0, power0);
+    }
+
+    if(a->hasAttribute("max-rpm")) {
+	float max = attrf(a, "max-rpm") * RPM2RAD;
+	float min = attrf(a, "min-rpm") * RPM2RAD;
+	thruster->setVariableProp(min, max);
+    }
+
     char buf[64];
     sprintf(buf, "/engines/engine[%d]", _nextEngine++);
     EngRec* er = new EngRec();
@@ -381,6 +398,7 @@ int FGFDM::parseOutput(const char* name)
 {
     if(eq(name, "THROTTLE"))  return ControlMap::THROTTLE;
     if(eq(name, "MIXTURE"))   return ControlMap::MIXTURE;
+    if(eq(name, "ADVANCE"))   return ControlMap::ADVANCE;
     if(eq(name, "REHEAT"))    return ControlMap::REHEAT;
     if(eq(name, "PROP"))      return ControlMap::PROP;
     if(eq(name, "BRAKE"))     return ControlMap::BRAKE;

src/FDM/YASim/Gear.cpp

@@ -49,7 +49,7 @@ void Gear::setDynamicFriction(float dfric)
 
 void Gear::setBrake(float brake)
 {
-    _brake = brake;
+    _brake = Math::clamp(brake, 0, 1);
 }
 
 void Gear::setRotation(float rotation)
@@ -59,7 +59,7 @@ void Gear::setRotation(float rotation)
 
 void Gear::setExtension(float extension)
 {
-    _extension = extension;
+    _extension = Math::clamp(extension, 0, 1);
 }
 
 void Gear::getPosition(float* out)

src/FDM/YASim/Jet.cpp

@@ -7,15 +7,13 @@ Jet::Jet()
 {
     _rho0 = Atmosphere::getStdDensity(0);
     _thrust = 0;
+    _abThrust = 0;
     _reheat = 0;
 }
 
-Thruster* Jet::clone()
+void Jet::stabilize()
 {
-    Jet* j = new Jet();
+    return; // no-op for now
-    j->_thrust = _thrust;
-    j->_rho0 = _rho0;
-    return j;
 }
 
 void Jet::setDryThrust(float thrust)
@@ -23,14 +21,21 @@ void Jet::setDryThrust(float thrust)
     _thrust = thrust;
 }
 
+void Jet::setReheatThrust(float thrust)
+{
+    _abThrust = thrust;
+}
+
 void Jet::setReheat(float reheat)
 {
-    _reheat = reheat;
+    _reheat = Math::clamp(reheat, 0, 1);
 }
 
 void Jet::getThrust(float* out)
 {
-    float t = _thrust * _throttle * (_rho / _rho0);
+    float t = _thrust * _throttle;
+    t += (_abThrust - _thrust) * _reheat;
+    t *= _rho / _rho0;
     Math::mul3(t, _dir, out);
 }
 

src/FDM/YASim/Jet.hpp

@@ -11,9 +11,10 @@ class Jet : public Thruster {
 public:
     Jet();
 
-    virtual Thruster* clone();
+    virtual Jet* getJet() { return this; }
 
     void setDryThrust(float thrust);
+    void setReheatThrust(float thrust);
     void setReheat(float reheat);
 
     virtual void getThrust(float* out);
@@ -21,9 +22,11 @@ public:
     virtual void getGyro(float* out);
     virtual float getFuelFlow();
     virtual void integrate(float dt);
+    virtual void stabilize();
 
 private:
     float _thrust;
+    float _abThrust;
     float _rho0;
     float _reheat;
 };

src/FDM/YASim/Math.cpp

@@ -3,6 +3,13 @@
 #include "Math.hpp"
 namespace yasim {
 
+float Math::clamp(float val, float min, float max)
+{
+    if(val < min) return min;
+    if(val > max) return max;
+    return val;
+}
+
 float Math::abs(float f)
 {
     return ::fabs(f);

src/FDM/YASim/Math.hpp

@@ -6,6 +6,9 @@ namespace yasim {
 class Math
 {
 public:
+    // Dumb utilities
+    static float clamp(float val, float min, float max);
+
     // Simple wrappers around library routines
     static float abs(float f);
     static float sqrt(float f);

src/FDM/YASim/Model.cpp

@@ -77,7 +77,7 @@ void Model::initIteration()
         localWind(pos, _s, v);
 
 	t->setWind(v);
-	t->setDensity(_rho);
+	t->setAir(_P, _T);
 	t->integrate(_integrator.getInterval());
 
 	t->getTorque(v);
@@ -126,6 +126,11 @@ int Model::addThruster(Thruster* t)
     return _thrusters.add(t);
 }
 
+int Model::numThrusters()
+{
+    return _thrusters.size();
+}
+
 Thruster* Model::getThruster(int handle)
 {
     return (Thruster*)_thrusters.get(handle);
@@ -146,6 +151,13 @@ int Model::addGear(Gear* gear)
     return _gears.add(gear);
 }
 
+void Model::setGroundEffect(float* pos, float span, float mul)
+{
+    Math::set3(pos, _wingCenter);
+    _groundEffectSpan = span;
+    _groundEffect = mul;
+}
+
 // The first three elements are a unit vector pointing from the global
 // origin to the plane, the final element is the distance from the
 // origin (the radius of the earth, in most implementations).  So
@@ -156,13 +168,10 @@ void Model::setGroundPlane(double* planeNormal, double fromOrigin)
     _ground[3] = fromOrigin;
 }
 
-void Model::setAirDensity(float rho)
-{
-    _rho = rho;
-}
-
 void Model::setAir(float pressure, float temp)
 {
+    _P = pressure;
+    _T = temp;
     _rho = Atmosphere::calcDensity(pressure, temp);
 }
 
@@ -197,6 +206,8 @@ void Model::calcForces(State* s)
 
     // Do each surface, remembering that the local velocity at each
     // point is different due to rotation.
+    float faero[3];
+    faero[0] = faero[1] = faero[2] = 0;
     for(int i=0; i<_surfaces.size(); i++) {
 	Surface* sf = (Surface*)_surfaces.get(i);
 
@@ -207,6 +218,7 @@ void Model::calcForces(State* s)
 
 	float force[3], torque[3];
 	sf->calcForce(vs, _rho, force, torque);
+	Math::add3(faero, force, faero);
 	_body.addForce(pos, force);
 	_body.addTorque(torque);
     }
@@ -218,6 +230,17 @@ void Model::calcForces(State* s)
     float ground[4];
     ground[3] = localGround(s, ground);
 
+    // Account for ground effect by multiplying the vertical force
+    // component by an amount linear with the fraction of the wingspan
+    // above the ground.
+    float dist = ground[4] - Math::dot3(ground, _wingCenter);
+    if(dist > 0 && dist < _groundEffectSpan) {
+	float fz = Math::dot3(faero, ground);
+	Math::mul3(fz * _groundEffect * dist/_groundEffectSpan,
+		   ground, faero);
+	_body.addForce(faero);
+    }
+    
     // Convert the velocity and rotation vectors to local coordinates
     float lrot[3], lv[3];
     Math::vmul33(s->orient, s->rot, lrot);

src/FDM/YASim/Model.hpp

@@ -35,6 +35,7 @@ public:
     Gear* getGear(int handle);
 
     // Semi-private methods for use by the Airplane solver.
+    int numThrusters();
     Thruster* getThruster(int handle);
     void setThruster(int handle, Thruster* t);
     void initIteration();
@@ -44,8 +45,8 @@ public:
     // Per-iteration settables
     //
     void setGroundPlane(double* planeNormal, double fromOrigin);
+    void setGroundEffect(float* pos, float span, float mul);
     void setWind(float* wind);
-    void setAirDensity(float rho);
     void setAir(float pressure, float temp);
 
     // BodyEnvironment callbacks
@@ -65,7 +66,13 @@ private:
     Vector _surfaces;
     Vector _gears;
 
+    float _groundEffectSpan;
+    float _groundEffect;
+    float _wingCenter[3];
+
     double _ground[4];
+    float _P;
+    float _T;
     float _rho;
     float _wind[3];
 

src/FDM/YASim/PistonEngine.cpp

@@ -1,3 +1,5 @@
+#include "Atmosphere.hpp"
+#include "Math.hpp"
 #include "PistonEngine.hpp"
 namespace yasim {
 
@@ -11,13 +13,34 @@ PistonEngine::PistonEngine(float power, float speed)
     _omega0 = speed;
 
     // We must be at sea level under standard conditions
-    _rho0 = 1.225; // kg/m^3
+    _rho0 = Atmosphere::getStdDensity(0);
 
     // Further presume that takeoff is (duh) full throttle and
     // peak-power, that means that by our efficiency function, we are
     // at 11/8 of "ideal" fuel flow.
     float realFlow = _f0 * (11.0/8.0);
     _mixCoeff = realFlow * 1.1 / _omega0;
+
+    _turbo = 1;
+    _maxMP = 1e6; // No waste gate on non-turbo engines.
+}
+
+void PistonEngine::setTurboParams(float turbo, float maxMP)
+{
+    _turbo = turbo;
+    _maxMP = maxMP;
+
+    // This changes the "sea level" manifold air density
+    float P0 = Atmosphere::getStdPressure(0);
+    float P = P0 * _turbo;
+    if(P > _maxMP) P = _maxMP;
+    float T = Atmosphere::getStdTemperature(0) * Math::pow(P/P0, 2./7.);
+    _rho0 = P / (287.1 * T);
+}
+
+float PistonEngine::getPower()
+{
+    return _P0;
 }
 
 void PistonEngine::setThrottle(float t)
@@ -30,13 +53,21 @@ void PistonEngine::setMixture(float m)
     _mixture = m;
 }
 
-void PistonEngine::calc(float density, float speed,
+void PistonEngine::calc(float P, float T, float speed,
 			float* torqueOut, float* fuelFlowOut)
 {
     // The actual fuel flow
     float fuel = _mixture * _mixCoeff * speed;
 
     // manifold air density
+    if(_turbo != 1) {
+        float P1 = P * _turbo;
+        if(P1 > _maxMP) P1 = _maxMP;
+        T *= Math::pow(P1/P, 2./7.);
+        P = P1;
+    }
+    float density = P / (287.1 * T);
+    
     float rho = density * _throttle;
 
     // How much fuel could be burned with ideal (i.e. uncorrected!)

src/FDM/YASim/PistonEngine.hpp

@@ -7,16 +7,19 @@ class PistonEngine {
 public:
     // Initializes an engine from known "takeoff" parameters.
     PistonEngine(float power, float spd);
+    void setTurboParams(float mul, float maxMP);
 
     void setThrottle(float throttle);
     void setMixture(float mixture);
 
+    float getPower();
+
     // Calculates power output and fuel flow, based on a given
     // throttle setting (0-1 corresponding to the fraction of
     // "available" manifold pressure), mixture (fuel flux per rpm,
     // 0-1, where 1 is "max rich", or a little bit more than needed
     // for rated power at sea level)
-    void calc(float density, float speed,
+    void calc(float pressure, float temp, float speed,
 	      float* powerOut, float* fuelFlowOut);
 
 private:
@@ -29,6 +32,9 @@ private:
     // Runtime settables:
     float _throttle;
     float _mixture;
+
+    float _turbo;
+    float _maxMP;
 };
 
 }; // namespace yasim

src/FDM/YASim/PropEngine.cpp

@@ -10,6 +10,8 @@ PropEngine::PropEngine(Propeller* prop, PistonEngine* eng, float moment)
     _omega = 52.3;
     _dir[0] = 1; _dir[1] = 0; _dir[2] = 0;
 
+    _variable = false;
+
     _prop = prop;
     _eng = eng;
     _moment = moment;
@@ -21,21 +23,23 @@ PropEngine::~PropEngine()
     delete _eng;
 }
 
+void PropEngine::setAdvance(float advance)
+{
+    _advance = Math::clamp(advance, 0, 1);
+}
+
+void PropEngine::setVariableProp(float min, float max)
+{
+    _variable = true;
+    _minOmega = min;
+    _maxOmega = max;
+}
+
 float PropEngine::getOmega()
 {
     return _omega;
 }
 
-Thruster* PropEngine::clone()
-{
-    // FIXME: bloody mess
-    PropEngine* p = new PropEngine(_prop, _eng, _moment);
-    cloneInto(p);
-    p->_prop = new Propeller(*_prop);
-    p->_eng = new PistonEngine(*_eng);
-    return p;
-}
-
 void PropEngine::getThrust(float* out)
 {
     for(int i=0; i<3; i++) out[i] = _thrust[i];    
@@ -56,6 +60,44 @@ float PropEngine::getFuelFlow()
     return _fuelFlow;
 }
 
+void PropEngine::stabilize()
+{
+    float speed = -Math::dot3(_wind, _dir);
+    _eng->setThrottle(_throttle);
+    _eng->setMixture(_mixture);
+
+    if(_variable) {
+	_omega = _minOmega + _advance * (_maxOmega - _minOmega);
+	_prop->modPitch(1e6); // Start at maximum pitch and move down
+    } else {
+	_omega = 52;
+    }
+
+    bool goingUp = false;
+    float step = 10;
+    while(true) {
+	float etau, ptau, dummy;
+	_prop->calc(_rho, speed, _omega, &dummy, &ptau);
+	_eng->calc(_P, _T, _omega, &etau, &dummy);
+	float tdiff = etau - ptau;
+
+	if(Math::abs(tdiff/_moment) < 0.1)
+	    break;
+
+	if(tdiff > 0) {
+	    if(!goingUp) step *= 0.5;
+	    goingUp = true;
+ 	    if(!_variable)  _omega += step;
+	    else            _prop->modPitch(1+(step*0.005));
+	} else {
+	    if(goingUp) step *= 0.5;
+	    goingUp = false;
+ 	    if(!_variable)  _omega -= step;
+	    else            _prop->modPitch(1-(step*0.005));
+	}
+    }
+}
+
 void PropEngine::integrate(float dt)
 {
     float speed = -Math::dot3(_wind, _dir);
@@ -67,14 +109,19 @@ void PropEngine::integrate(float dt)
     
     _prop->calc(_rho, speed, _omega,
 		&thrust, &propTorque);
-    _eng->calc(_rho, _omega, &engTorque, &_fuelFlow);
+    _eng->calc(_P, _T, _omega, &engTorque, &_fuelFlow);
 
     // Turn the thrust into a vector and save it
     Math::mul3(thrust, _dir, _thrust);
 
     // Euler-integrate the RPM.  This doesn't need the full-on
     // Runge-Kutta stuff.
-    _omega += dt*(engTorque-propTorque)/Math::abs(_moment);
+    float rotacc = (engTorque-propTorque)/Math::abs(_moment);
+    _omega += dt * rotacc;
+
+    // Clamp to a 500 rpm idle.  This should probably be settable, and
+    // 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...
@@ -82,13 +129,25 @@ void PropEngine::integrate(float dt)
     // Store the total angular momentum into _gyro
     Math::mul3(_omega*_moment, _dir, _gyro);
 
-    // Accumulate the engine torque, it acta on the body as a whole.
+    // Accumulate the engine torque, it acts on the body as a whole.
     // (Note: engine torque, not propeller torque.  They can be
     // different, but the difference goes to accelerating the
     // rotation.  It is the engine torque that is felt at the shaft
     // and works on the body.)
     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;
+	_prop->modPitch(mod);
+    }
 }
 
 }; // namespace yasim

src/FDM/YASim/PropEngine.hpp

@@ -13,7 +13,12 @@ public:
     PropEngine(Propeller* prop, PistonEngine* eng, float moment);
     virtual ~PropEngine();
 
-    virtual Thruster* clone();
+    void setAdvance(float advance);
+    void setVariableProp(float min, float max);
+
+    virtual PropEngine* getPropEngine() { return this; }
+    virtual PistonEngine* getPistonEngine() { return _eng; }
+    virtual Propeller* getPropeller() { return _prop; }
 
     // Dynamic output
     virtual void getThrust(float* out);
@@ -23,6 +28,7 @@ public:
 
     // Runtime instructions
     virtual void integrate(float dt);
+    virtual void stabilize();
 
     float getOmega();
     
@@ -31,6 +37,11 @@ private:
     Propeller* _prop;
     PistonEngine* _eng;
 
+    bool _variable;
+    float _advance; // control input, 0-1
+    float _maxOmega;
+    float _minOmega;
+
     float _omega; // RPM, in radians/sec
     float _thrust[3];
     float _torque[3];

src/FDM/YASim/Propeller.cpp

@@ -1,36 +1,48 @@
+#include <stdio.h>
+
 #include "Atmosphere.hpp"
 #include "Math.hpp"
 #include "Propeller.hpp"
 namespace yasim {
 
 Propeller::Propeller(float radius, float v, float omega,
-		     float rho, float power, float omega0,
+		     float rho, float power)
-                     float power0)
 {
     // Initialize numeric constants:
-    _lambdaPeak = Math::pow(9.0, -1.0/8.0);
+    _lambdaPeak = Math::pow(5.0, -1.0/4.0);
-    _beta = 1.0/(Math::pow(9.0, -1.0/8.0) - Math::pow(9.0, -9.0/8.0));
+    _beta = 1.0/(Math::pow(5.0, -1.0/4.0) - Math::pow(5.0, -5.0/4.0));
 
     _r = radius;
     _etaC = 0.85; // make this settable?
 
     _J0 = v/(omega*_lambdaPeak);
+    _baseJ0 = _J0;
 
     float V2 = v*v + (_r*omega)*(_r*omega);
     _F0 = 2*_etaC*power/(rho*v*V2);
 
-    float stallAngle = 0.25;
+    _matchTakeoff = false;
-    _lambdaS = _r*(_J0/_r - stallAngle) / _J0;
-
-    // Now compute a correction for zero forward speed to make the
-    // takeoff performance correct.
-    float torque0 = power0/omega0; 
-    float thrust, torque;
-    _takeoffCoef = 1;
-    calc(Atmosphere::getStdDensity(0), 0, omega0, &thrust, &torque);
-    _takeoffCoef = torque/torque0;
 }
 
+void Propeller::setTakeoff(float omega0, float power0)
+{
+    // Takeoff thrust coefficient at lambda==0
+    _matchTakeoff = true;
+    float V2 = _r*omega0 * _r*omega0;
+    float gamma = _etaC * _beta / _J0;
+    float torque = power0 / omega0;
+    float density = Atmosphere::getStdDensity(0);
+    _tc0 = (torque * gamma) / (0.5 * density * V2 * _F0);
+}
+    
+void Propeller::modPitch(float mod)
+{
+    _J0 *= mod;
+    if(_J0 < 0.25*_baseJ0) _J0 = 0.25*_baseJ0;
+    if(_J0 > 4*_baseJ0)    _J0 = 4*_baseJ0;
+}
+
+
 void Propeller::calc(float density, float v, float omega,
 		     float* thrustOut, float* torqueOut)
 {
@@ -47,7 +59,7 @@ void Propeller::calc(float density, float v, float omega,
     float torque = 0;
     if(lambda > 1) {
 	lambda = 1.0/lambda;
-	torque = (density*V2*_F0*_J0)/(8*_etaC*_beta*(1-_lambdaPeak));
+	torque = (density*V2*_F0*_J0)/(4*_etaC*_beta*(1-_lambdaPeak));
     }
 
     // There's an undefined point at 1.  Just offset by a tiny bit to
@@ -56,24 +68,18 @@ void Propeller::calc(float density, float v, float omega,
     // point number!)
     if(lambda == 1) lambda = 0.9999;
 
-    // Compute thrust, remembering to clamp lambda to the stall point
+    // Calculate lambda^4
-    float lambda2 = lambda < _lambdaS ? _lambdaS : lambda;
+    float l4 = lambda*lambda; l4 = l4*l4;
-    float thrust = (0.5*density*V2*_F0/(1-_lambdaPeak))*(1-lambda2);
-
-    // Calculate lambda^8
-    float l8 = lambda*lambda; l8 = l8*l8; l8 = l8*l8;
 
     // thrust/torque ratio
-    float gamma = (_etaC*_beta/_J0)*(1-l8);
+    float gamma = (_etaC*_beta/_J0)*(1-l4);
 
-    // Correct slow speeds to get the takeoff parameters correct
+    // Compute a thrust, clamp to takeoff thrust to prevend huge
-    if(lambda < _lambdaPeak) {
+    // numbers at slow speeds.
-        // This will interpolate takeoffCoef along a descending from 1
+    float tc = (1 - lambda) / (1 - _lambdaPeak);
-        // at lambda==0 to 0 at the peak, fairing smoothly into the
+    if(_matchTakeoff && tc > _tc0) tc = _tc0;
-        // flat peak.
+
-        float frac = (lambda - _lambdaPeak)/_lambdaPeak;
+    float thrust = 0.5 * density * V2 * _F0 * tc;
-        gamma *= 1 + (_takeoffCoef - 1)*frac*frac;
-    }
 
     if(torque > 0) {
 	torque -= thrust/gamma;

src/FDM/YASim/Propeller.hpp

@@ -14,8 +14,11 @@ public:
     // numbers for RPM and power (with air speed and density being
     // zero and sea level).  RPM values are in radians per second, of
     // course.
-    Propeller(float radius, float v, float omega, float rho, float power,
+    Propeller(float radius, float v, float omega, float rho, float power);
-              float omega0, float power0);
+
+    void setTakeoff(float omega0, float power0);
+
+    void modPitch(float mod);
 
     void calc(float density, float v, float omega,
 	      float* thrustOut, float* torqueOut);
@@ -23,12 +26,13 @@ public:
 private:
     float _r;           // characteristic radius
     float _J0;          // zero-thrust advance ratio
-    float _lambdaS;     // "propeller stall" normalized advance ratio
+    float _baseJ0;      //  ... uncorrected for prop advance
     float _F0;          // thrust coefficient
     float _etaC;        // Peak efficiency
     float _lambdaPeak;  // constant, ~0.759835;
     float _beta;        // constant, ~1.48058;
-    float _takeoffCoef; // correction to get the zero-speed torque right
+    float _tc0;         // thrust "coefficient" at takeoff
+    bool  _matchTakeoff; // Does _tc0 mean anything?
 };
 
 }; // namespace yasim

src/FDM/YASim/Surface.cpp

@@ -236,8 +236,16 @@ float Surface::controlDrag()
     d *= 1 + _spoilerPos * (_spoilerDrag - 1);
     d *= 1 + _slatPos * (_slatDrag - 1);
 
-    // FIXME: flaps should REDUCE drag when the reduce lift!
+    // Negative flap deflections don't affect drag until their lift
-    d *= 1 + Math::abs(_flapPos) * (_flapDrag - 1);
+    // multiplier exceeds the "camber" (cz0) of the surface.
+    float fp = _flapPos;
+    if(fp < 0) {
+        fp = -fp;
+        fp -= _cz0/(_flapLift-1);
+        if(fp < 0) fp = 0;
+    }
+
+    d *= 1 + fp * (_flapDrag - 1);
 
     return d;
 }

src/FDM/YASim/Thruster.cpp

@@ -8,8 +8,7 @@ Thruster::Thruster()
     for(int i=0; i<3; i++) _pos[i] = _wind[i] = 0;
     _throttle = 0;
     _mixture = 0;
-    _propAdvance = 0;
+    _P = _T = _rho = 0;
-    _rho = 0;
 }
 
 Thruster::~Thruster()
@@ -38,17 +37,12 @@ void Thruster::setDirection(float* dir)
 
 void Thruster::setThrottle(float throttle)
 {
-    _throttle = throttle;
+    _throttle = Math::clamp(throttle, 0, 1);
 }
 
 void Thruster::setMixture(float mixture)
 {
-    _mixture = mixture;
+    _mixture = Math::clamp(mixture, 0, 1);
-}
-
-void Thruster::setPropAdvance(float propAdvance)
-{
-    _propAdvance = propAdvance;
 }
 
 void Thruster::setWind(float* wind)
@@ -56,20 +50,11 @@ void Thruster::setWind(float* wind)
     for(int i=0; i<3; i++) _wind[i] = wind[i];
 }
 
-void Thruster::setDensity(float rho)
+void Thruster::setAir(float pressure, float temp)
 {
-    _rho = rho;
+    _P = pressure;
-}
+    _T = temp;
-
+    _rho = _P / (287.1 * _T);
-void Thruster::cloneInto(Thruster* out)
-{
-    for(int i=0; i<3; i++) {
-	out->_pos[i] = _pos[i];
-	out->_dir[i] = _dir[i];
-    }
-    out->_throttle = _throttle;
-    out->_mixture  = _mixture;
-    out->_propAdvance = _propAdvance;
 }
 
 }; // namespace yasim

src/FDM/YASim/Thruster.hpp

@@ -3,23 +3,33 @@
 
 namespace yasim {
 
+class Jet;
+class PropEngine;
+class Propeller;
+class PistonEngine;
+
 class Thruster {
 public:
     Thruster();
     virtual ~Thruster();
 
+    // Typing info, these are the possible sub-type (or sub-parts)
+    // that a thruster might have.  Any might return null.  A little
+    // clumsy, but much simpler than an RTTI-based implementation.
+    virtual Jet* getJet() { return 0; }
+    virtual PropEngine* getPropEngine() { return 0; }
+    virtual Propeller* getPropeller() { return 0; }
+    virtual PistonEngine* getPistonEngine() { return 0; }
+    
     // Static data
     void getPosition(float* out);
     void setPosition(float* pos);
     void getDirection(float* out);
     void setDirection(float* dir);
 
-    virtual Thruster* clone()=0;
-
     // Controls
     void setThrottle(float throttle);
     void setMixture(float mixture);
-    void setPropAdvance(float advance);
 
     // Dynamic output
     virtual void getThrust(float* out)=0;
@@ -29,19 +39,19 @@ public:
 
     // Runtime instructions
     void setWind(float* wind);
-    void setDensity(float rho);
+    void setAir(float pressure, float temp);
     virtual void integrate(float dt)=0;
+    virtual void stabilize()=0;
 
 protected:
-    void cloneInto(Thruster* out);
-
     float _pos[3];
     float _dir[3];
     float _throttle;
     float _mixture;
-    float _propAdvance;
 
     float _wind[3];
+    float _P;
+    float _T;
     float _rho;
 };
 

src/FDM/YASim/Wing.cpp

@@ -157,6 +157,8 @@ void Wing::setSpoiler(float start, float end, float lift, float drag)
 
 void Wing::setFlap0(float lval, float rval)
 {
+    lval = Math::clamp(lval, -1, 1);
+    rval = Math::clamp(rval, -1, 1);
     for(int i=0; i<_flap0Surfs.size(); i++) {
 	((Surface*)_flap0Surfs.get(i))->setFlap(lval);
 	if(_mirror) ((Surface*)_flap0Surfs.get(++i))->setFlap(rval);
@@ -165,6 +167,8 @@ void Wing::setFlap0(float lval, float rval)
 
 void Wing::setFlap1(float lval, float rval)
 {
+    lval = Math::clamp(lval, -1, 1);
+    rval = Math::clamp(rval, -1, 1);
     for(int i=0; i<_flap1Surfs.size(); i++) {
 	((Surface*)_flap1Surfs.get(i))->setFlap(lval);
 	if(_mirror) ((Surface*)_flap1Surfs.get(++i))->setFlap(rval);
@@ -173,6 +177,8 @@ void Wing::setFlap1(float lval, float rval)
 
 void Wing::setSpoiler(float lval, float rval)
 {
+    lval = Math::clamp(lval, 0, 1);
+    rval = Math::clamp(rval, 0, 1);
     for(int i=0; i<_spoilerSurfs.size(); i++) {
 	((Surface*)_spoilerSurfs.get(i))->setSpoiler(lval);
 	if(_mirror) ((Surface*)_spoilerSurfs.get(++i))->setSpoiler(rval);
@@ -181,10 +187,19 @@ void Wing::setSpoiler(float lval, float rval)
 
 void Wing::setSlat(float val)
 {
+    val = Math::clamp(val, 0, 1);
     for(int i=0; i<_slatSurfs.size(); i++)
 	((Surface*)_slatSurfs.get(i))->setSlat(val);
 }
 
+float Wing::getGroundEffect(float* posOut)
+{
+    for(int i=0; i<3; i++) posOut[i] = _base[i];
+    float span = _length * Math::cos(_sweep) * Math::cos(_dihedral);
+    span = 2*(span + Math::abs(_base[2]));
+    return span;
+}
+
 void Wing::compile()
 {
     // Have we already been compiled?

src/FDM/YASim/Wing.hpp

@@ -44,6 +44,9 @@ public:
     // Compile the thing into a bunch of Surface objects
     void compile();
 
+    // Ground effect information
+    float getGroundEffect(float* posOut);
+    
     // Query the list of Surface objects
     int numSurfaces();
     Surface* getSurface(int n);