From 25e34cc49c887396cd92a3e7b9d4717b57bc0d1b Mon Sep 17 00:00:00 2001
From: Henning Stahlke <github@henningstahlke.de>
Date: Wed, 21 Feb 2018 01:41:13 +0100
Subject: [PATCH] YASim solver convergence workaround

---
 src/FDM/YASim/Airplane.cpp | 26 ++++++++++++++++++++++++--
 src/FDM/YASim/Airplane.hpp |  3 +++
 src/FDM/YASim/FGFDM.cpp    |  2 +-
 3 files changed, 28 insertions(+), 3 deletions(-)

diff --git a/src/FDM/YASim/Airplane.cpp b/src/FDM/YASim/Airplane.cpp
index 1b9bc64e5..51a92b56c 100644
--- a/src/FDM/YASim/Airplane.cpp
+++ b/src/FDM/YASim/Airplane.cpp
@@ -877,6 +877,21 @@ float Airplane::_getDragForce(Config &cfg)
     return cfg.weight * tmp[0];
 }
 
+// This is a heuristic approach to "force" solver converge due to numeric
+// problems. To be replaced by a better solution later.
+float Airplane::_checkConvergence(float prev, float current)
+{
+    static int damping {0};
+    //different sign and almost same value -> oscilation; 
+    if ((prev*current) < 0 && (abs(current + prev) < 0.01f)) {
+        if (!damping) fprintf(stderr,"YASim warning: possible convergence problem.\n");
+        damping++;
+        if (current < 1) current *= abs(current); // quadratic
+        else current = sqrt(current);
+    }
+    return current;
+}
+
 void Airplane::solveAirplane(bool verbose)
 {
     static const float ARCMIN = 0.0002909f;
@@ -899,6 +914,7 @@ void Airplane::solveAirplane(bool verbose)
         fprintf(stdout,"i\tdAoa\tdTail\tcl0\tcp1\n");
     }
 
+    float prevTailDelta {0};
     while(1) {
         if(_solutionIterations++ > _solverMaxIterations) { 
             _failureMsg = "Solution failed to converge!";
@@ -920,9 +936,10 @@ void Airplane::solveAirplane(bool verbose)
         float alift = _getLiftForce(_config[APPROACH]);
 
         // Modify the cruise AoA a bit to get a derivative
+        float savedAoa = _config[CRUISE].aoa;
         _config[CRUISE].aoa += ARCMIN;
         runConfig(_config[CRUISE]);
-        _config[CRUISE].aoa -= ARCMIN;
+        _config[CRUISE].aoa = savedAoa;
             
         float clift1 = _getLiftForce(_config[CRUISE]);
 
@@ -978,7 +995,12 @@ void Airplane::solveAirplane(bool verbose)
         // OK, now we can adjust the minor variables:
         float aoaDelta = -clift0 * (ARCMIN/(clift1-clift0));
         float tailDelta = -cpitch0 * (ARCMIN/(cpitch1-cpitch0));
-        
+        // following is a hack against oszilation variables,
+        // needs more research to get a fully understood - it works 
+        if (_solverMode > 0) {
+            tailDelta = _checkConvergence(prevTailDelta, tailDelta);
+            prevTailDelta = tailDelta;
+        }
         if (verbose) {
             fprintf(stdout,"%4d\t%f\t%f\t%f\t%f\n", _solutionIterations, aoaDelta, tailDelta, clift0, cpitch1);
         }
diff --git a/src/FDM/YASim/Airplane.hpp b/src/FDM/YASim/Airplane.hpp
index cade0f390..1d4b0faa8 100644
--- a/src/FDM/YASim/Airplane.hpp
+++ b/src/FDM/YASim/Airplane.hpp
@@ -137,6 +137,7 @@ public:
     void  setSolverTweak(float f) { _solverDelta = f; };
     void  setSolverThreshold(float threshold) { _solverThreshold = threshold; };
     void  setSolverMaxIterations(int i) { _solverMaxIterations = i; };
+    void  setSolverMode(int i) { _solverMode = i; };
     
 private:
     struct Tank { 
@@ -198,6 +199,7 @@ private:
     float _getPitch(Config &cfg);
     float _getLiftForce(Config &cfg);
     float _getDragForce(Config &cfg);
+    float _checkConvergence(float prev, float current);
     void solveAirplane(bool verbose = false);
     void solveHelicopter(bool verbose = false);
     float compileWing(Wing* w);
@@ -222,6 +224,7 @@ private:
     /// set property name controling tail trim (incidence)
     void setHstabTrimControl(const char* propName);
     
+    int   _solverMode {1};
     float _solverDelta {0.3226f};
     // How close to the solution are we trying get?  
     // Trying too hard can result in oscillations (no convergence). 
diff --git a/src/FDM/YASim/FGFDM.cpp b/src/FDM/YASim/FGFDM.cpp
index 7fa94a85c..13a40b5dc 100644
--- a/src/FDM/YASim/FGFDM.cpp
+++ b/src/FDM/YASim/FGFDM.cpp
@@ -283,7 +283,7 @@ void FGFDM::parseAirplane(const XMLAttributes* a)
     
     if (a->hasAttribute("mtow-lbs")) { _airplane.setMTOW(attrf(a, "mtow-lbs") * LBS2KG); }
     else if (a->hasAttribute("mtow-kg")) { _airplane.setMTOW(attrf(a, "mtow-kg")); }
-    
+    if (a->hasAttribute("solver-mode")) { _airplane.setSolverMode(attri(a, "solver-mode")); }    
 }
 
 void FGFDM::parseApproachCruise(const XMLAttributes* a, const char* name)