Add support for a turbo prop condition lever.

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

diff --git a/src/FDM/YASim/Surface.cpp b/src/FDM/YASim/Surface.cpp
index f945050783140612811524ceee450b882d3c1bf2..77349b7683d562432e4d95c7e74cdb037898d782 100644 (file)
--- a/src/FDM/YASim/Surface.cpp
+++ b/src/FDM/YASim/Surface.cpp
@@ -20,12 +20,14 @@ Surface::Surface()
     
     _chord = 0;
     _incidence = 0;
+    _twist = 0;
     _slatPos = _spoilerPos = _flapPos = 0;
     _slatDrag = _spoilerDrag = _flapDrag = 1;
 
     _flapLift = 0;
     _slatAlpha = 0;
     _spoilerLift = 1;
+    _inducedDrag = 1;
 }
 
 void Surface::setPosition(float* p)
@@ -102,6 +104,11 @@ void Surface::setIncidence(float angle)
     _incidence = angle;
 }
 
+void Surface::setTwist(float angle)
+{
+    _twist = angle;
+}
+
 void Surface::setSlatParams(float stallDelta, float dragPenalty)
 {
     _slatAlpha = stallDelta;
@@ -146,7 +153,7 @@ void Surface::calcForce(float* v, float rho, float* out, float* torque)
     // Handle the blowup condition.  Zero velocity means zero force by
     // definition.
     if(vel == 0) {
-       int i;
+        int i;
        for(i=0; i<3; i++) out[i] = torque[i] = 0;
        return;
     }
@@ -159,8 +166,14 @@ void Surface::calcForce(float* v, float rho, float* out, float* torque)
     // "Rotate" by the incidence angle.  Assume small angles, so we
     // need to diddle only the Z component, X is relatively unchanged
     // by small rotations.
-    out[2] += _incidence * out[0]; // z' = z + incidence * x
+    float incidence = _incidence + _twist;
+    out[2] += incidence * out[0]; // z' = z + incidence * x
 
+    // Hold onto the local wind vector so we can multiply the induced
+    // drag at the end.
+    float lwind[3];
+    Math::set3(out, lwind);
+    
     // Diddle the Z force according to our configuration
     float stallMul = stallFunc(out);
     stallMul *= 1 + _spoilerPos * (_spoilerLift - 1);
@@ -188,9 +201,13 @@ void Surface::calcForce(float* v, float rho, float* out, float* torque)
     // Add in any specific Y (side force) coefficient.
     out[1] *= _cy;
 
+    // Diddle the induced drag
+    Math::mul3(-1*_inducedDrag*out[2]*lwind[2], lwind, lwind);
+    Math::add3(lwind, out, out);
+
     // Reverse the incidence rotation to get back to surface
     // coordinates.
-    out[2] -= _incidence * out[0];
+    out[2] -= incidence * out[0];
 
     // Convert back to external coordinates
     Math::tmul33(_orient, out, out);