Fix bug 141, by ensuring certain subsystems are assigned to the 'post FDM' group...

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

diff --git a/src/FDM/YASim/Airplane.cpp b/src/FDM/YASim/Airplane.cpp
index 067e092b5c0a328baaad2841170fd6985395cba1..483da426911dcdcd73addc1da6bb6991d6068551 100644 (file)
--- a/src/FDM/YASim/Airplane.cpp
+++ b/src/FDM/YASim/Airplane.cpp
@@ -7,6 +7,7 @@
 #include "Surface.hpp"
 #include "Rotorpart.hpp"
 #include "Thruster.hpp"
+#include "Hitch.hpp"
 #include "Airplane.hpp"
 
 namespace yasim {
@@ -38,16 +39,20 @@ Airplane::Airplane()
     _cruiseT = 0;
     _cruiseSpeed = 0;
     _cruiseWeight = 0;
+    _cruiseGlideAngle = 0;
     _approachP = 0;
     _approachT = 0;
     _approachSpeed = 0;
     _approachAoA = 0;
     _approachWeight = 0;
+    _approachGlideAngle = 0;
 
     _dragFactor = 1;
     _liftRatio = 1;
     _cruiseAoA = 0;
     _tailIncidence = 0;
+
+    _failureMsg = 0;
 }
 
 Airplane::~Airplane()
@@ -182,16 +187,17 @@ void Airplane::updateGearState()
     }
 }
 
-void Airplane::setApproach(float speed, float altitude, float aoa, float fuel)
+void Airplane::setApproach(float speed, float altitude, float aoa, float fuel, float gla)
 {
     _approachSpeed = speed;
     _approachP = Atmosphere::getStdPressure(altitude);
     _approachT = Atmosphere::getStdTemperature(altitude);
     _approachAoA = aoa;
     _approachFuel = fuel;
+    _approachGlideAngle = gla;
 }
  
-void Airplane::setCruise(float speed, float altitude, float fuel)
+void Airplane::setCruise(float speed, float altitude, float fuel, float gla)
 {
     _cruiseSpeed = speed;
     _cruiseP = Atmosphere::getStdPressure(altitude);
@@ -199,6 +205,7 @@ void Airplane::setCruise(float speed, float altitude, float fuel)
     _cruiseAoA = 0;
     _tailIncidence = 0;
     _cruiseFuel = fuel;
+    _cruiseGlideAngle = gla;
 }
 
 void Airplane::setElevatorControl(int control)
@@ -323,6 +330,11 @@ void Airplane::addHook(Hook* hook)
     _model.addHook(hook);
 }
 
+void Airplane::addHitch(Hitch* hitch)
+{
+    _model.addHitch(hitch);
+}
+
 void Airplane::addLaunchbar(Launchbar* launchbar)
 {
     _model.addLaunchbar(launchbar);
@@ -407,7 +419,7 @@ float Airplane::getTailIncidence()
     return _tailIncidence;
 }
 
-char* Airplane::getFailureMsg()
+const char* Airplane::getFailureMsg()
 {
     return _failureMsg;
 }
@@ -417,7 +429,7 @@ int Airplane::getSolutionIterations()
     return _solutionIterations;
 }
 
-void Airplane::setupState(float aoa, float speed, State* s)
+void Airplane::setupState(float aoa, float speed, float gla, State* s)
 {
     float cosAoA = Math::cos(aoa);
     float sinAoA = Math::sin(aoa);
@@ -425,7 +437,7 @@ void Airplane::setupState(float aoa, float speed, State* s)
     s->orient[3] =       0; s->orient[4] = 1; s->orient[5] =      0;
     s->orient[6] = -sinAoA; s->orient[7] = 0; s->orient[8] = cosAoA;
 
-    s->v[0] = speed; s->v[1] = 0; s->v[2] = 0;
+    s->v[0] = speed*Math::cos(gla); s->v[1] = -speed*Math::sin(gla); s->v[2] = 0;
 
     int i;
     for(i=0; i<3; i++)
@@ -482,9 +494,9 @@ float Airplane::compileWing(Wing* w)
     return wgt;
 }
 
-float Airplane::compileRotorgear()
+void Airplane::compileRotorgear()
 {
-    return getRotorgear()->compile(_model.getBody());
+    getRotorgear()->compile();
 }
 
 float Airplane::compileFuselage(Fuselage* f)
@@ -497,6 +509,10 @@ float Airplane::compileFuselage(Fuselage* f)
     float fwd[3];
     Math::sub3(f->front, f->back, fwd);
     float len = Math::mag3(fwd);
+    if (len == 0) {
+        _failureMsg = "Zero length fuselage";
+       return 0;
+    }
     float wid = f->width;
     int segs = (int)Math::ceil(len/wid);
     float segWgt = len*wid/segs;
@@ -602,6 +618,7 @@ void Airplane::compileContactPoints()
         // I made these up
         g->setStaticFriction(0.6f);
         g->setDynamicFriction(0.5f);
+        g->setContactPoint(1);
 
         _model.addGear(g);
     }
@@ -626,8 +643,6 @@ void Airplane::compile()
     for(i=0; i<_vstabs.size(); i++)
         aeroWgt += compileWing((Wing*)_vstabs.get(i)); 
 
-    // The rotor(s)
-    aeroWgt += compileRotorgear(); 
 
     // The fuselage(s)
     for(i=0; i<_fuselages.size(); i++)
@@ -679,9 +694,18 @@ void Airplane::compile()
         _model.setGroundEffect(gepos, gespan, 0.15f);
     }
 
+    // solve function below resets failure message
+    // so check if we have any problems and abort here
+    if (_failureMsg) return;
+
     solveGear();
     if(_wing && _tail) solve();
-    else solveHelicopter();
+    else
+    {
+       // The rotor(s) mass:
+       compileRotorgear(); 
+       solveHelicopter();
+    }
 
     // Do this after solveGear, because it creates "gear" objects that
     // we don't want to affect.
@@ -706,7 +730,8 @@ void Airplane::solveGear()
         g->getPosition(pos);
        Math::sub3(cg, pos, pos);
         gr->wgt = 1.0f/(0.5f+Math::sqrt(pos[0]*pos[0] + pos[1]*pos[1]));
-        total += gr->wgt;
+        if (!g->getIgnoreWhileSolving())
+            total += gr->wgt;
     }
 
     // Renormalize so they sum to 1
@@ -728,7 +753,7 @@ void Airplane::solveGear()
         float e = energy * gr->wgt;
         float comp[3];
         gr->gear->getCompression(comp);
-        float len = Math::mag3(comp);
+        float len = Math::mag3(comp)*(1+2*gr->gear->getInitialLoad());
 
         // Energy in a spring: e = 0.5 * k * len^2
         float k = 2 * e / (len*len);
@@ -738,10 +763,6 @@ void Airplane::solveGear()
         // Critically damped (too damped, too!)
         gr->gear->setDamping(2*Math::sqrt(k*_approachWeight*gr->wgt)
                              * gr->gear->getDamping());
-
-        // These are pretty generic
-        gr->gear->setStaticFriction(0.8f);
-        gr->gear->setDynamicFriction(0.7f);
     }
 }
 
@@ -774,7 +795,7 @@ void Airplane::setupWeights(bool isApproach)
 
 void Airplane::runCruise()
 {
-    setupState(_cruiseAoA, _cruiseSpeed, &_cruiseState);
+    setupState(_cruiseAoA, _cruiseSpeed,_approachGlideAngle, &_cruiseState);
     _model.setState(&_cruiseState);
     _model.setAir(_cruiseP, _cruiseT,
                   Atmosphere::calcStdDensity(_cruiseP, _cruiseT));
@@ -817,7 +838,7 @@ void Airplane::runCruise()
 
 void Airplane::runApproach()
 {
-    setupState(_approachAoA, _approachSpeed, &_approachState);
+    setupState(_approachAoA, _approachSpeed,_approachGlideAngle, &_approachState);
     _model.setState(&_approachState);
     _model.setAir(_approachP, _approachT,
                   Atmosphere::calcStdDensity(_approachP, _approachT));
@@ -925,7 +946,7 @@ void Airplane::solve()
        runCruise();
 
        _model.getThrust(tmp);
-       float thrust = tmp[0];
+        float thrust = tmp[0] + _cruiseWeight * Math::sin(_cruiseGlideAngle) * 9.81;
 
        _model.getBody()->getAccel(tmp);
         Math::tmul33(_cruiseState.orient, tmp, tmp);
@@ -1064,7 +1085,7 @@ void Airplane::solveHelicopter()
         applyDragFactor(Math::pow(15.7/1000, 1/SOLVE_TWEAK));
         applyLiftRatio(Math::pow(104, 1/SOLVE_TWEAK));
     }
-    setupState(0,0, &_cruiseState);
+    setupState(0,0,0, &_cruiseState);
     _model.setState(&_cruiseState);
     setupWeights(true);
     _controls.reset();