X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2FYASim%2FAirplane.cpp;h=09707c7f7bc6fa2fc25f638d643f1a4f2ff65fca;hb=ce91286e19d6d66b316811d04b5b66b8b768827b;hp=1a4e7713aff61c400441520dfdbcf44690d69f92;hpb=fe4e83a10bfa6927a4925215cbce48433a05ec6e;p=flightgear.git

diff --git a/src/FDM/YASim/Airplane.cpp b/src/FDM/YASim/Airplane.cpp
index 1a4e7713a..09707c7f7 100644
--- a/src/FDM/YASim/Airplane.cpp
+++ b/src/FDM/YASim/Airplane.cpp
@@ -5,8 +5,9 @@
 #include "Glue.hpp"
 #include "RigidBody.hpp"
 #include "Surface.hpp"
+#include "Rotorpart.hpp"
+#include "Rotorblade.hpp"
 #include "Thruster.hpp"
-
 #include "Airplane.hpp"
 
 namespace yasim {
@@ -15,6 +16,18 @@ namespace yasim {
 inline float norm(float f) { return f<1 ? 1/f : f; }
 inline float abs(float f) { return f<0 ? -f : f; }
 
+// Solver threshold.  How close to the solution are we trying
+// to get?  Trying too hard can result in oscillations about
+// the correct solution, which is bad.  Stick this in as a
+// compile time constant for now, and consider making it
+// settable per-model.
+const float STHRESH = 1;
+
+// How slowly do we change values in the solver.  Too slow, and
+// the solution converges very slowly.  Too fast, and it can
+// oscillate.
+const float SOLVE_TWEAK = 0.3226;
+
 Airplane::Airplane()
 {
     _emptyWeight = 0;
@@ -61,8 +74,37 @@ void Airplane::iterate(float dt)
     updateGearState();
 
     _model.iterate();
+}
 
-    // FIXME: Consume fuel
+void Airplane::consumeFuel(float dt)
+{
+    // This is a really simple implementation that assumes all engines
+    // draw equally from all tanks in proportion to the amount of fuel
+    // stored there.  Needs to be fixed, but that has to wait for a
+    // decision as to what the property interface will look like.
+    int i, outOfFuel = 0;
+    float fuelFlow = 0, totalFuel = 0.00001; // <-- overflow protection
+    for(i=0; i<_thrusters.size(); i++)
+        fuelFlow += ((ThrustRec*)_thrusters.get(i))->thruster->getFuelFlow();
+    for(i=0; i<_tanks.size(); i++)
+        totalFuel += ((Tank*)_tanks.get(i))->fill;
+    for(i=0; i<_tanks.size(); i++) {
+        Tank* t = (Tank*)_tanks.get(i);
+        t->fill -= dt * fuelFlow * (t->fill/totalFuel);
+        if(t->fill <= 0) {
+            t->fill = 0;
+            outOfFuel = 1;
+        }
+    }
+    if(outOfFuel)
+        for(int i=0; i<_thrusters.size(); i++)
+            ((ThrustRec*)_thrusters.get(i))->thruster->setFuelState(false);
+
+    // Set the tank masses on the RigidBody
+    for(i=0; i<_tanks.size(); i++) {
+        Tank* t = (Tank*)_tanks.get(i);
+        _model.getBody()->setMass(t->handle, t->fill);
+    }
 }
 
 ControlMap* Airplane::getControlMap()
@@ -189,6 +231,11 @@ float Airplane::getFuelDensity(int tank)
     return ((Tank*)_tanks.get(tank))->density;
 }
 
+float Airplane::getTankCapacity(int tank)
+{
+    return ((Tank*)_tanks.get(tank))->cap;
+}
+
 void Airplane::setWeight(float weight)
 {
     _emptyWeight = weight;
@@ -209,6 +256,11 @@ void Airplane::addVStab(Wing* vstab)
     _vstabs.add(vstab);
 }
 
+void Airplane::addRotor(Rotor* rotor)
+{
+    _rotors.add(rotor);
+}
+
 void Airplane::addFuselage(float* front, float* back, float width,
                            float taper, float mid)
 {
@@ -298,6 +350,7 @@ void Airplane::setFuelFraction(float frac)
     int i;
     for(i=0; i<_tanks.size(); i++) {
         Tank* t = (Tank*)_tanks.get(i);
+        t->fill = frac * t->cap;
         _model.getBody()->setMass(t->handle, t->cap * frac);
     }
 }
@@ -396,6 +449,43 @@ float Airplane::compileWing(Wing* w)
     return wgt;
 }
 
+float Airplane::compileRotor(Rotor* r)
+{
+    // Todo: add rotor to model!!!
+    // Todo: calc and add mass!!!
+    r->compile();
+    _model.addRotor(r);
+
+    float wgt = 0;
+    int i;
+    for(i=0; i<r->numRotorparts(); i++) {
+        Rotorpart* s = (Rotorpart*)r->getRotorpart(i);
+
+        _model.addRotorpart(s);
+        
+        float mass = s->getWeight();
+        mass = mass * Math::sqrt(mass);
+        float pos[3];
+        s->getPosition(pos);
+        _model.getBody()->addMass(mass, pos);
+        wgt += mass;
+    }
+    
+    for(i=0; i<r->numRotorblades(); i++) {
+        Rotorblade* b = (Rotorblade*)r->getRotorblade(i);
+
+        _model.addRotorblade(b);
+        
+        float mass = b->getWeight();
+        mass = mass * Math::sqrt(mass);
+        float pos[3];
+        b->getPosition(pos);
+        _model.getBody()->addMass(mass, pos);
+        wgt += mass;
+    }
+    return wgt;
+}
+
 float Airplane::compileFuselage(Fuselage* f)
 {
     // The front and back are contact points
@@ -529,17 +619,19 @@ void Airplane::compile()
     float aeroWgt = 0;
 
     // The Wing objects
-    aeroWgt += compileWing(_wing);
-    aeroWgt += compileWing(_tail);
+    if (_wing)
+      aeroWgt += compileWing(_wing);
+    if (_tail)
+      aeroWgt += compileWing(_tail);
     int i;
-    for(i=0; i<_vstabs.size(); i++) {
+    for(i=0; i<_vstabs.size(); i++)
         aeroWgt += compileWing((Wing*)_vstabs.get(i)); 
-    }
+    for(i=0; i<_rotors.size(); i++)
+        aeroWgt += compileRotor((Rotor*)_rotors.get(i)); 
     
     // The fuselage(s)
-    for(i=0; i<_fuselages.size(); i++) {
+    for(i=0; i<_fuselages.size(); i++)
         aeroWgt += compileFuselage((Fuselage*)_fuselages.get(i));
-    }
 
     // Count up the absolute weight we have
     float nonAeroWgt = _ballast;
@@ -581,11 +673,14 @@ void Airplane::compile()
 
     // Ground effect
     float gepos[3];
-    float gespan = _wing->getGroundEffect(gepos);
+    float gespan = 0;
+    if(_wing)
+      gespan = _wing->getGroundEffect(gepos);
     _model.setGroundEffect(gepos, gespan, 0.15f);
 
     solveGear();
-    solve();
+    if(_wing && _tail) solve();
+    else solveHelicopter();
 
     // Do this after solveGear, because it creates "gear" objects that
     // we don't want to affect.
@@ -752,10 +847,12 @@ void Airplane::runApproach()
 
 void Airplane::applyDragFactor(float factor)
 {
-    float applied = Math::sqrt(factor);
+    float applied = Math::pow(factor, SOLVE_TWEAK);
     _dragFactor *= applied;
-    _wing->setDragScale(_wing->getDragScale() * applied);
-    _tail->setDragScale(_tail->getDragScale() * applied);
+    if(_wing)
+      _wing->setDragScale(_wing->getDragScale() * applied);
+    if(_tail)
+      _tail->setDragScale(_tail->getDragScale() * applied);
     int i;
     for(i=0; i<_vstabs.size(); i++) {
 	Wing* w = (Wing*)_vstabs.get(i);
@@ -769,10 +866,12 @@ void Airplane::applyDragFactor(float factor)
 
 void Airplane::applyLiftRatio(float factor)
 {
-    float applied = Math::sqrt(factor);
+    float applied = Math::pow(factor, SOLVE_TWEAK);
     _liftRatio *= applied;
-    _wing->setLiftRatio(_wing->getLiftRatio() * applied);
-    _tail->setLiftRatio(_tail->getLiftRatio() * applied);
+    if(_wing)
+      _wing->setLiftRatio(_wing->getLiftRatio() * applied);
+    if(_tail)
+      _tail->setLiftRatio(_tail->getLiftRatio() * applied);
     int i;
     for(i=0; i<_vstabs.size(); i++) {
         Wing* w = (Wing*)_vstabs.get(i);
@@ -801,20 +900,11 @@ void Airplane::solve()
     float tmp[3];
     _solutionIterations = 0;
     _failureMsg = 0;
+
     while(1) {
-#if 0
-        printf("%d %f %f %f %f %f\n", //DEBUG
-               _solutionIterations,
-               1000*_dragFactor,
-               _liftRatio,
-               _cruiseAoA,
-               _tailIncidence,
-               _approachElevator.val);
-#endif
-
-	if(_solutionIterations++ > 10000) {
+        if(_solutionIterations++ > 10000) { 
             _failureMsg = "Solution failed to converge after 10000 iterations";
-	    return;
+            return;
         }
 
 	// Run an iteration at cruise, and extract the needed numbers:
@@ -895,30 +985,30 @@ void Airplane::solve()
 	applyLiftRatio(liftFactor);
 
 	// DON'T do the following until the above are sane
-	if(normFactor(dragFactor) > 1.0001
-	   || normFactor(liftFactor) > 1.0001)
+	if(normFactor(dragFactor) > STHRESH*1.0001
+	   || normFactor(liftFactor) > STHRESH*1.0001)
 	{
 	    continue;
 	}
 
 	// OK, now we can adjust the minor variables:
-	_cruiseAoA += 0.5f*aoaDelta;
-	_tailIncidence += 0.5f*tailDelta;
+	_cruiseAoA += SOLVE_TWEAK*aoaDelta;
+	_tailIncidence += SOLVE_TWEAK*tailDelta;
 	
 	_cruiseAoA = clamp(_cruiseAoA, -0.175f, 0.175f);
 	_tailIncidence = clamp(_tailIncidence, -0.175f, 0.175f);
 
-        if(abs(xforce/_cruiseWeight) < 0.0001 &&
-           abs(alift/_approachWeight) < 0.0001 &&
-           abs(aoaDelta) < .000017 &&
-           abs(tailDelta) < .000017)
+        if(abs(xforce/_cruiseWeight) < STHRESH*0.0001 &&
+           abs(alift/_approachWeight) < STHRESH*0.0001 &&
+           abs(aoaDelta) < STHRESH*.000017 &&
+           abs(tailDelta) < STHRESH*.000017)
         {
             // If this finaly value is OK, then we're all done
-            if(abs(elevDelta) < 0.0001)
+            if(abs(elevDelta) < STHRESH*0.0001)
                 break;
 
             // Otherwise, adjust and do the next iteration
-            _approachElevator.val += 0.8 * elevDelta;
+            _approachElevator.val += SOLVE_TWEAK * elevDelta;
             if(abs(_approachElevator.val) > 1) {
                 _failureMsg = "Insufficient elevator to trim for approach";
                 break;
@@ -940,4 +1030,18 @@ void Airplane::solve()
 	return;
     }
 }
+
+void Airplane::solveHelicopter()
+{
+    _solutionIterations = 0;
+    _failureMsg = 0;
+
+    applyDragFactor(Math::pow(15.7/1000, 1/SOLVE_TWEAK));
+    applyLiftRatio(Math::pow(104, 1/SOLVE_TWEAK));
+    setupState(0,0, &_cruiseState);
+    _model.setState(&_cruiseState);
+    _controls.reset();
+    _model.getBody()->reset();
+}
+
 }; // namespace yasim