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 d92d4f9623d5a6bd9a9c527b1beeee42bc2d0899..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;
@@ -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();
@@ -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];
 
@@ -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
+       // OK, now we can adjust the minor variables:
        _cruiseAoA += 0.5f*aoaDelta;
        _tailIncidence += 0.5f*tailDelta;
+        _approachElevator.val += 0.5f*elevDelta;
        
        _cruiseAoA = clamp(_cruiseAoA, -0.174f, 0.174f);
        _tailIncidence = clamp(_tailIncidence, -0.174f, 0.174f);
+        _approachElevator.val = clamp(_approachElevator.val, -1.f, 1.f);
 
-        if(dragFactor < 1.00001 && liftFactor < 1.00001 &&
-           aoaDelta < .000017   && tailDelta < .000017)
+        if(norm(dragFactor) < 1.00001 &&
+           norm(liftFactor) < 1.00001 &&
+           abs(aoaDelta) < .000017 &&
+           abs(tailDelta) < .000017 &&
+           abs(elevDelta) < 0.00001)
         {
             break;
         }