Merge /u/jsb1685/flightgear/ branch yasim into next

https://sourceforge.net/p/flightgear/flightgear/merge-requests/124/

src/FDM/YASim/Airplane.cpp

@@ -22,18 +22,6 @@ static const char* DEF_PROP_ELEVATOR_TRIM = "/controls/flight/elevator-trim";
 // gadgets
 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()
 {
     _approachConfig.isApproach = true;
@@ -522,7 +510,7 @@ void Airplane::compileContactPoints()
     }
 }
 
-void Airplane::compile()
+void Airplane::compile(bool verbose)
 {
     RigidBody* body = _model.getBody();
     int firstMass = body->numMasses();
@@ -650,12 +638,12 @@ void Airplane::compile()
     solveGear();
     calculateCGHardLimits();
     
-    if(_wing && _tail) solveAirplane();
+    if(_wing && _tail) solveAirplane(verbose);
     else
     {
        // The rotor(s) mass:
        compileRotorgear(); 
-       solveHelicopter();
+       solveHelicopter(verbose);
     }
 
     // Do this after solveGear, because it creates "gear" objects that
@@ -810,7 +798,7 @@ void Airplane::runConfig(Config &cfg)
 /// Used only in solveAirplane() and solveHelicopter(), not at runtime
 void Airplane::applyDragFactor(float factor)
 {
-    float applied = Math::pow(factor, SOLVE_TWEAK);
+    float applied = Math::pow(factor, _solverDelta);
     _dragFactor *= applied;
     if(_wing)
       _wing->multiplyDragCoefficient(applied);
@@ -856,7 +844,7 @@ void Airplane::applyDragFactor(float factor)
 /// change lift coefficient cz in surfaces
 void Airplane::applyLiftRatio(float factor)
 {
-    float applied = Math::pow(factor, SOLVE_TWEAK);
+    float applied = Math::pow(factor, _solverDelta);
     _liftRatio *= applied;
     if(_wing)
       _wing->multiplyLiftRatio(applied);
@@ -895,7 +883,7 @@ float Airplane::_getLift(Config &cfg)
     return cfg.weight * tmp[2];
 }
 
-void Airplane::solveAirplane()
+void Airplane::solveAirplane(bool verbose)
 {
     static const float ARCMIN = 0.0002909f;
 
@@ -913,14 +901,18 @@ void Airplane::solveAirplane()
         _tailIncidence = new ControlSetting;
         _tailIncidenceCopy = new ControlSetting;
     }
+    if (verbose) {
+        fprintf(stdout,"i\tdAoa\tdTail\tcl0\tcp1\n");
+    }
 
     while(1) {
-        if(_solutionIterations++ > SOLVER_MAX_ITERATIONS) { 
+        if(_solutionIterations++ > _solverMaxIterations) { 
             _failureMsg = "Solution failed to converge!";
             return;
         }
         // Run an iteration at cruise, and extract the needed numbers:
         runConfig(_cruiseConfig);
+        
         _model.getThrust(tmp);
         float thrust = tmp[0] + _cruiseConfig.weight * Math::sin(_cruiseConfig.glideAngle) * 9.81;
 
@@ -985,8 +977,8 @@ void Airplane::solveAirplane()
         applyLiftRatio(liftFactor);
 
         // DON'T do the following until the above are sane
-        if(normFactor(dragFactor) > STHRESH*1.0001
+        if(normFactor(dragFactor) > _solverThreshold*1.0001
-        || normFactor(liftFactor) > STHRESH*1.0001)
+        || normFactor(liftFactor) > _solverThreshold*1.0001)
         {
             continue;
         }
@@ -994,24 +986,31 @@ void Airplane::solveAirplane()
         // OK, now we can adjust the minor variables:
         float aoaDelta = -clift0 * (ARCMIN/(clift1-clift0));
         float tailDelta = -cpitch0 * (ARCMIN/(cpitch1-cpitch0));
-        _cruiseConfig.aoa += SOLVE_TWEAK*aoaDelta;
+        
-        _tailIncidence->val += SOLVE_TWEAK*tailDelta;
+        if (verbose) {
+            fprintf(stdout,"%4d\t%f\t%f\t%f\t%f\n", _solutionIterations, aoaDelta, tailDelta, clift0, cpitch1);
+        }
+        _cruiseConfig.aoa += _solverDelta*aoaDelta;
+        _tailIncidence->val += _solverDelta*tailDelta;
         
         _cruiseConfig.aoa = Math::clamp(_cruiseConfig.aoa, -0.175f, 0.175f);
         _tailIncidence->val = Math::clamp(_tailIncidence->val, -0.175f, 0.175f);
 
-        if(abs(xforce/_cruiseConfig.weight) < STHRESH*0.0001 &&
+        if(abs(xforce/_cruiseConfig.weight) < _solverThreshold*0.0001 &&
-          abs(alift/_approachConfig.weight) < STHRESH*0.0001 &&
+          abs(alift/_approachConfig.weight) < _solverThreshold*0.0001 &&
-          abs(aoaDelta) < STHRESH*.000017 &&
+          abs(aoaDelta) < _solverThreshold*.000017 &&
-          abs(tailDelta) < STHRESH*.000017)
+          abs(tailDelta) < _solverThreshold*.000017)
         {
             float elevDelta = -apitch0 * (ELEVDIDDLE/(apitch1-apitch0));
+            if (verbose) {
+                fprintf(stdout,"%4d dElev %f, ap0 %f,ap1 %f \n", _solutionIterations, elevDelta, apitch0, apitch1);
+            }
             // If this finaly value is OK, then we're all done
-            if(abs(elevDelta) < STHRESH*0.0001)
+            if(abs(elevDelta) < _solverThreshold*0.0001)
                 break;
 
             // Otherwise, adjust and do the next iteration
-            _approachElevator->val += SOLVE_TWEAK * elevDelta;
+            _approachElevator->val += _solverDelta * elevDelta;
             if(abs(_approachElevator->val) > 1) {
                 _failureMsg = "Insufficient elevator to trim for approach.";
                 break;
@@ -1040,7 +1039,7 @@ void Airplane::solveAirplane()
     }
 }
 
-void Airplane::solveHelicopter()
+void Airplane::solveHelicopter(bool verbose)
 {
     _solutionIterations = 0;
     _failureMsg = 0;
@@ -1048,16 +1047,16 @@ void Airplane::solveHelicopter()
     {
         Rotorgear* rg = getRotorgear();
         applyDragFactor(Math::pow(rg->getYasimDragFactor()/1000,
-            1/SOLVE_TWEAK));
+            1/_solverDelta));
         applyLiftRatio(Math::pow(rg->getYasimLiftFactor(),
-            1/SOLVE_TWEAK));
+            1/_solverDelta));
     }
     else
     //huh, no wing and no rotor? (_rotorgear is constructed, 
     //if a rotor is defined
     {
-        applyDragFactor(Math::pow(15.7/1000, 1/SOLVE_TWEAK));
+        applyDragFactor(Math::pow(15.7/1000, 1/_solverDelta));
-        applyLiftRatio(Math::pow(104, 1/SOLVE_TWEAK));
+        applyLiftRatio(Math::pow(104, 1/_solverDelta));
     }
     _cruiseConfig.state.setupState(0,0,0);
     _model.setState(&_cruiseConfig.state);
@@ -1126,4 +1125,20 @@ float Airplane::getMaxThrust()
     return sum[0];
 }
 
+float Airplane::getTailIncidence() const 
+{
+    if (_tailIncidence != nullptr) { 
+        return _tailIncidence->val;        
+    }
+    else return 0;
+}
+
+float Airplane::getApproachElevator() const 
+{
+    if (_approachElevator != nullptr) { 
+        return _approachElevator->val;        
+    }
+    else return 0;
+}
+
 }; // namespace yasim

src/FDM/YASim/Airplane.hpp

@@ -92,7 +92,7 @@ public:
     float getFuelDensity(int tank) const { return ((Tank*)_tanks.get(tank))->density; }
     float getTankCapacity(int tank) const { return ((Tank*)_tanks.get(tank))->cap; }
 
-    void compile(); // generate point masses & such, then solve
+    void compile(bool verbose = false); // generate point masses & such, then solve
     void initEngines();
     void stabilizeThrust();
 
@@ -101,8 +101,8 @@ public:
     float getDragCoefficient() const { return _dragFactor; }
     float getLiftRatio() const { return _liftRatio; }
     float getCruiseAoA() const { return _cruiseConfig.aoa; }
-    float getTailIncidence() const { return _tailIncidence->val; }
+    float getTailIncidence()const;
-    float getApproachElevator() const { return _approachElevator->val; }
+    float getApproachElevator() const;
     const char* getFailureMsg() const { return _failureMsg; }
 
     // next two are used only in yasim CLI tool
@@ -130,6 +130,9 @@ public:
     float getMaxThrust();
     float getThrust2WeightEmpty() { return getMaxThrust()/(_emptyWeight * KG2N); };
     float getThrust2WeightMTOW() { return getMaxThrust()/(_mtow*KG2N); };
+    void  setSolverTweak(float f) { _solverDelta = f; };
+    void  setSolverThreshold(float threshold) { _solverThreshold = threshold; };
+    void  setSolverMaxIterations(int i) { _solverMaxIterations = i; };
     
 private:
     struct Tank { 
@@ -191,8 +194,8 @@ private:
     void solveGear();
     float _getPitch(Config &cfg);
     float _getLift(Config &cfg);
-    void solveAirplane();
+    void solveAirplane(bool verbose = false);
-    void solveHelicopter();
+    void solveHelicopter(bool verbose = false);
     float compileWing(Wing* w);
     void compileRotorgear();
     float compileFuselage(Fuselage* f);
@@ -215,6 +218,11 @@ private:
     /// set property name controling tail trim (incidence)
     void setHstabTrimControl(const char* propName);
     
+    float _solverDelta {0.3226};
+    // How close to the solution are we trying get?  
+    // Trying too hard can result in oscillations (no convergence). 
+    float _solverThreshold {1};
+    int   _solverMaxIterations {4000};
     Model _model;
     ControlMap _controlMap;
 

src/FDM/YASim/Version.cpp

@@ -4,29 +4,40 @@
 
 #include "Version.hpp"
 #include <simgear/debug/logstream.hxx>
-#include <string>
 #include <iostream>
 
 namespace yasim {
+    
+static const std::vector<std::string> VersionStrings = {
+    "YASIM_VERSION_ORIGINAL",
+    "YASIM_VERSION_32",
+    "2017.2",
+    "2018.1",
+    "YASIM_VERSION_CURRENT",
+};
+    
+Version::YASIM_VERSION Version::getByName(const std::string& name)
+{
+    auto it = std::find(VersionStrings.begin(), VersionStrings.end(), name);
+    if (it == VersionStrings.end()) {
+        SG_LOG(SG_FLIGHT,SG_ALERT,"Unknown yasim version '" << name << "' ignored, using YASIM_VERSION_ORIGINAL");        
+        return YASIM_VERSION_ORIGINAL;
+    }
+    YASIM_VERSION v = static_cast<YASIM_VERSION>(std::distance(VersionStrings.begin(), it));
+    if (v > YASIM_VERSION_CURRENT) return YASIM_VERSION_CURRENT;
+    else return v;
+}
+
+std::string Version::getName(YASIM_VERSION v)
+{
+    return VersionStrings.at(static_cast<int>(v));
+}
+
 void Version::setVersion( const char * version )
 {
-  const std::string v(version);
+    const std::string v(version);
-  
+    _version = getByName(v);
-  if( v ==  "YASIM_VERSION_ORIGINAL" ) {
+    SG_LOG(SG_FLIGHT,SG_ALERT, "This aircraft uses yasim version '" << v << "' (" << _version << ")\n");
-    _version = YASIM_VERSION_ORIGINAL;
-  } else if( v == "YASIM_VERSION_32" ) {
-    _version = YASIM_VERSION_32;
-  } else if( v == "2017.2" ) {
-    _version = YASIM_VERSION_2017_2;
-  } else if( v == "2018.1" ) {
-    _version = YASIM_VERSION_2018_1;
-  } else if( v == "YASIM_VERSION_CURRENT" ) {
-    _version = YASIM_VERSION_CURRENT;
-  } else {
-    SG_LOG(SG_FLIGHT,SG_ALERT,"unknown yasim version '" << version << "' ignored, using YASIM_VERSION_ORIGINAL");
-    return;
-  }
-  std::cout << "This aircraft uses yasim version '" << v << "'\n";
 }
 
 } // namespace yasim

src/FDM/YASim/Version.hpp

@@ -1,6 +1,8 @@
 #ifndef _VERSION_HPP
 #define _VERSION_HPP
 
+#include "yasim-common.hpp"
+
 namespace yasim {
 
 class Version {
@@ -8,19 +10,20 @@ public:
   Version() : _version(YASIM_VERSION_ORIGINAL) {}
   virtual ~Version() {}
 
-  typedef enum {
+  enum YASIM_VERSION {
     YASIM_VERSION_ORIGINAL = 0,
     YASIM_VERSION_32,
     YASIM_VERSION_2017_2,
     YASIM_VERSION_2018_1,
     YASIM_VERSION_CURRENT = YASIM_VERSION_2018_1
-  } YASIM_VERSION;
+  } ;
 
   void setVersion( const char * version );
   int getVersion() const { return _version; }
   bool isVersion( YASIM_VERSION version ) const;
   bool isVersionOrNewer( YASIM_VERSION version ) const;
-
+  static YASIM_VERSION getByName(const std::string& name);
+  static std::string getName(YASIM_VERSION v);
 private:
   YASIM_VERSION _version;
 };

src/FDM/YASim/yasim-common.hpp

@@ -8,7 +8,6 @@
  common file for YASim wide constants and static helper functions
  */
 namespace yasim {
-    static const int SOLVER_MAX_ITERATIONS = 4000;
     static const float YASIM_PI = 3.14159265358979323846f;
     static const float PI2 = YASIM_PI*2;
     static const float RAD2DEG = 180/YASIM_PI;

src/FDM/YASim/yasim-test.cpp

@@ -262,11 +262,21 @@ int main(int argc, char** argv)
   catch (const sg_exception &e) {
     printf("XML parse error: %s (%s)\n", e.getFormattedMessage().c_str(), e.getOrigin());
   }
-
   // ... and run
-  a->compile();
+  bool verbose {false};
-  if(a->getFailureMsg())
+  if (argc > 2 && strcmp(argv[2], "-v") == 0) {
+    verbose=true;
+  }
+  if ((argc == 4) && (strcmp(argv[2], "--tweak") == 0)) {
+    float tweak = std::atof(argv[3]);
+    a->setSolverTweak(tweak);
+    a->setSolverMaxIterations(2000);
+    verbose=true;
+  }
+  a->compile(verbose);
+  if(a->getFailureMsg()) {
       printf("SOLUTION FAILURE: %s\n", a->getFailureMsg());
+  }
   if(!a->getFailureMsg() && argc > 2 ) {
     bool test = (strcmp(argv[2], "-test") == 0);
     if((strcmp(argv[2], "-g") == 0) || test)