Oops. Stray checkin. Revert.

[flightgear.git] / src / FDM / YASim / Airplane.cpp

diff --git a/src/FDM/YASim/Airplane.cpp b/src/FDM/YASim/Airplane.cpp
index 3e5b689f03c3572a0a32f5413dd94db0877483e7..82b10d7d144844c255759b23206c5a6867243f0e 100644 (file)
--- a/src/FDM/YASim/Airplane.cpp
+++ b/src/FDM/YASim/Airplane.cpp
@@ -10,6 +10,10 @@
 #include "Airplane.hpp"
 namespace yasim {
 
+// gadgets
+inline float norm(float f) { return f<1 ? 1/f : f; }
+inline float abs(float f) { return f<0 ? -f : f; }
+
 Airplane::Airplane()
 {
     _emptyWeight = 0;
@@ -76,7 +80,7 @@ void Airplane::getPilotAccel(float* out)
 
     // Gravity
     Glue::geodUp(s->pos, out);
-    Math::mul3(-9.8, out, out);
+    Math::mul3(-9.8f, out, out);
 
     // The regular acceleration
     float tmp[3];
@@ -146,6 +150,13 @@ void Airplane::setCruise(float speed, float altitude)
     _tailIncidence = 0;
 }
 
+void Airplane::setElevatorControl(int control)
+{
+    _approachElevator.control = control;
+    _approachElevator.val = 0;
+    _approachControls.add(&_approachElevator);
+}
+
 void Airplane::addApproachControl(int control, float val)
 {
     Control* c = new Control();
@@ -399,7 +410,7 @@ float Airplane::compileFuselage(Fuselage* f)
     float segWgt = len*wid/segs;
     int j;
     for(j=0; j<segs; j++) {
-        float frac = (j+0.5) / segs;
+        float frac = (j+0.5f) / segs;
 
         float scale = 1;
         if(frac < f->mid)
@@ -473,13 +484,13 @@ void Airplane::compileContactPoints()
 {
     // Figure it will compress by 20cm
     float comp[3];
-    float DIST = 0.2;
+    float DIST = 0.2f;
     comp[0] = 0; comp[1] = 0; comp[2] = DIST;
 
     // Give it a spring constant such that at full compression it will
     // hold up 10 times the planes mass.  That's about right.  Yeah.
     float mass = _model.getBody()->getTotalMass();
-    float spring = (1/DIST) * 9.8 * 10 * mass;
+    float spring = (1/DIST) * 9.8f * 10.0f * mass;
     float damp = 2 * Math::sqrt(spring * mass);
 
     int i;
@@ -495,8 +506,8 @@ void Airplane::compileContactPoints()
         g->setBrake(1);
 
         // I made these up
-        g->setStaticFriction(0.6);
-        g->setDynamicFriction(0.5);
+        g->setStaticFriction(0.6f);
+        g->setDynamicFriction(0.5f);
 
         _model.addGear(g);
     }
@@ -552,8 +563,8 @@ void Airplane::compile()
         t->handle = body->addMass(0, t->pos);
         totalFuel += t->cap;
     }
-    _cruiseWeight = _emptyWeight + totalFuel*0.5;
-    _approachWeight = _emptyWeight + totalFuel*0.2;
+    _cruiseWeight = _emptyWeight + totalFuel*0.5f;
+    _approachWeight = _emptyWeight + totalFuel*0.2f;
 
     body->recalc();
 
@@ -570,7 +581,7 @@ void Airplane::compile()
     // Ground effect
     float gepos[3];
     float gespan = _wing->getGroundEffect(gepos);
-    _model.setGroundEffect(gepos, gespan, .3);
+    _model.setGroundEffect(gepos, gespan, 0.3f);
 
     solveGear();
     solve();
@@ -597,7 +608,7 @@ void Airplane::solveGear()
         Gear* g = gr->gear;
         g->getPosition(pos);
        Math::sub3(cg, pos, pos);
-        gr->wgt = 1/(0.5+Math::sqrt(pos[0]*pos[0] + pos[1]*pos[1]));
+        gr->wgt = 1.0f/(0.5f+Math::sqrt(pos[0]*pos[0] + pos[1]*pos[1]));
         total += gr->wgt;
     }
 
@@ -608,12 +619,12 @@ void Airplane::solveGear()
     // The force at max compression should be sufficient to stop a
     // plane moving downwards at 3x the approach descent rate.  Assume
     // a 3 degree approach.
-    float descentRate = 3*_approachSpeed/19.1;
+    float descentRate = 3.0f*_approachSpeed/19.1f;
 
     // Spread the kinetic energy according to the gear weights.  This
     // will results in an equal compression fraction (not distance) of
     // each gear.
-    float energy = 0.5*_approachWeight*descentRate*descentRate;
+    float energy = 0.5f*_approachWeight*descentRate*descentRate;
 
     for(i=0; i<_gears.size(); i++) {
         GearRec* gr = (GearRec*)_gears.get(i);
@@ -631,8 +642,8 @@ void Airplane::solveGear()
         gr->gear->setDamping(2*Math::sqrt(k*_approachWeight*gr->wgt));
 
         // These are pretty generic
-        gr->gear->setStaticFriction(0.8);
-        gr->gear->setDynamicFriction(0.7);
+        gr->gear->setStaticFriction(0.8f);
+        gr->gear->setDynamicFriction(0.7f);
     }
 }
 
@@ -712,7 +723,7 @@ void Airplane::runApproach()
     Math::vmul33(_approachState.orient, wind, wind);
     
     // Approach is by convention at 20% tank capacity
-    setFuelFraction(0.2);
+    setFuelFraction(0.2f);
 
     // Run the thrusters until they get to a stable setting.  FIXME:
     // this is lots of wasted work.
@@ -777,7 +788,7 @@ float Airplane::normFactor(float f)
 
 void Airplane::solve()
 {
-    static const float ARCMIN = 0.0002909;
+    static const float ARCMIN = 0.0002909f;
 
     float tmp[3];
     _solutionIterations = 0;
@@ -804,6 +815,9 @@ void Airplane::solve()
        // Run an approach iteration, and do likewise
        runApproach();
 
+       _model.getBody()->getAngularAccel(tmp);
+       float apitch0 = tmp[1];
+
        _model.getBody()->getAccel(tmp);
        float alift = _approachWeight * tmp[2];
 
@@ -824,7 +838,7 @@ void Airplane::solve()
        float pitch1 = tmp[1];
 
        // Now calculate:
-       float awgt = 9.8 * _approachWeight;
+       float awgt = 9.8f * _approachWeight;
 
        float dragFactor = thrust / (thrust-xforce);
        float liftFactor = awgt / (awgt+alift);
@@ -840,7 +854,23 @@ void Airplane::solve()
             return;
         }
 
-       // And apply:
+        // And the elevator control in the approach.  This works just
+        // like the tail incidence computation (it's solving for the
+        // same thing -- pitching moment -- by diddling a different
+        // variable).
+        const float ELEVDIDDLE = 0.0001f;
+        _approachElevator.val += ELEVDIDDLE;
+        runApproach();
+        _approachElevator.val -= ELEVDIDDLE;
+
+       _model.getBody()->getAngularAccel(tmp);
+       float apitch1 = tmp[1];
+        float elevDelta = -apitch0 * (ELEVDIDDLE/(apitch1-apitch0));
+
+        // Now apply the values we just computed.  Note that the
+        // "minor" variables are deferred until we get the lift/drag
+        // numbers in the right ballpark.
+
        applyDragFactor(dragFactor);
        applyLiftRatio(liftFactor);
 
@@ -851,15 +881,20 @@ void Airplane::solve()
            continue;
        }
 
-       // OK, now we can adjust the minor variables
-       _cruiseAoA += 0.5*aoaDelta;
-       _tailIncidence += 0.5*tailDelta;
+       // OK, now we can adjust the minor variables:
+       _cruiseAoA += 0.5f*aoaDelta;
+       _tailIncidence += 0.5f*tailDelta;
+        _approachElevator.val += 0.5f*elevDelta;
        
-       _cruiseAoA = clamp(_cruiseAoA, -.174, .174);
-       _tailIncidence = clamp(_tailIncidence, -.174, .174);
-
-        if(dragFactor < 1.00001 && liftFactor < 1.00001 &&
-           aoaDelta < .000017   && tailDelta < .000017)
+       _cruiseAoA = clamp(_cruiseAoA, -0.174f, 0.174f);
+       _tailIncidence = clamp(_tailIncidence, -0.174f, 0.174f);
+        _approachElevator.val = clamp(_approachElevator.val, -1.f, 1.f);
+
+        if(norm(dragFactor) < 1.00001 &&
+           norm(liftFactor) < 1.00001 &&
+           abs(aoaDelta) < .000017 &&
+           abs(tailDelta) < .000017 &&
+           abs(elevDelta) < 0.00001)
         {
             break;
         }