From f6aa33fe52c3e70671ca1c7461be2b7b0f007aea Mon Sep 17 00:00:00 2001
From: rtlopez <rtlopezdev@gmail.com>
Date: Thu, 10 Aug 2023 22:38:21 +0200
Subject: [PATCH] output saturation

---
 lib/Espfc/src/Control/Pid.h  | 7 ++++---
 lib/Espfc/src/Controller.h   | 5 -----
 lib/Espfc/src/Model.h        | 7 ++++---
 lib/Espfc/src/ModelState.h   | 1 +
 lib/Espfc/src/Output/Mixer.h | 9 +++++----
 5 files changed, 14 insertions(+), 15 deletions(-)

diff --git a/lib/Espfc/src/Control/Pid.h b/lib/Espfc/src/Control/Pid.h
index 158459e0..892ac8d2 100644
--- a/lib/Espfc/src/Control/Pid.h
+++ b/lib/Espfc/src/Control/Pid.h
@@ -32,7 +32,7 @@ class Pid
       pScale(1.f), iScale(1.f), dScale(1.f), fScale(1.f),
       pTerm(0.f), iTerm(0.f), dTerm(0.f), fTerm(0.f),
       prevMeasure(0.f), prevError(0.f), prevSetpoint(0.f),
-      outputStaurated(false)
+      outputSaturated(false)
       {}
 
     void begin()
@@ -49,9 +49,10 @@ class Pid
       pTerm = ptermFilter.update(pTerm);
 
       // I-term
+      // TODO: https://github.com/betaflight/betaflight/blob/master/src/main/flight/pid.c#L667
       if(Ki > 0.f && iScale > 0.f)
       {
-        if(!outputStaurated)
+        if(!outputSaturated)
         {
           iTerm += Ki * error * dt * iScale;
           iTerm = Math::clamp(iTerm, -iLimit, iLimit);
@@ -127,7 +128,7 @@ class Pid
     float prevError;
     float prevSetpoint;
 
-    bool outputStaurated;
+    bool outputSaturated;
 };
 
 }
diff --git a/lib/Espfc/src/Controller.h b/lib/Espfc/src/Controller.h
index a1bd9f35..956fd145 100644
--- a/lib/Espfc/src/Controller.h
+++ b/lib/Espfc/src/Controller.h
@@ -205,11 +205,6 @@ class Controller
     }
 
   private:
-    float power3(float x)
-    {
-      return x * x * x;
-    }
-
     Model& _model;
     Rates _rates;
     Filter _speedFilter;
diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h
index 3b2c1954..373ed116 100644
--- a/lib/Espfc/src/Model.h
+++ b/lib/Espfc/src/Model.h
@@ -186,12 +186,13 @@ class Model
       return state.armingDisabledFlags & flag;
     }
 
-    bool setOputputSaturated(bool val)
+    void setOutputSaturated(bool val)
     {
+      state.outputSaturated = val;
       for(size_t i = 0; i < 3; i++)
       {
-        state.innerPid[i].outputStaurated = val;
-        state.outerPid[i].outputStaurated = val;
+        state.innerPid[i].outputSaturated = val;
+        state.outerPid[i].outputSaturated = val;
       }
     }
 
diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h
index b5216df3..5e3ab3e1 100644
--- a/lib/Espfc/src/ModelState.h
+++ b/lib/Espfc/src/ModelState.h
@@ -214,6 +214,7 @@ struct ModelState
   float output[OUTPUT_CHANNELS];
   int16_t outputUs[OUTPUT_CHANNELS];
   int16_t outputDisarmed[OUTPUT_CHANNELS];
+  bool outputSaturated;
 
   // other state
   Kalman kalman[AXES];
diff --git a/lib/Espfc/src/Output/Mixer.h b/lib/Espfc/src/Output/Mixer.h
index e87937dc..8e1b473c 100644
--- a/lib/Espfc/src/Output/Mixer.h
+++ b/lib/Espfc/src/Output/Mixer.h
@@ -182,10 +182,14 @@ class Mixer
         entry++;
       }
 
+      bool saturated = false;
       for(size_t i = 0; i < mixer.count; i++)
       {
-        outputs[i] = limitOutput(outputs[i], _model.config.output.channel[i], _model.config.output.motorLimit);
+        const OutputChannelConfig& occ = _model.config.output.channel[i];
+        if(!occ.servo && outputs[i] >= 0.98f) saturated = true;
+        outputs[i] = limitOutput(outputs[i], occ, _model.config.output.motorLimit);
       }
+      _model.setOutputSaturated(saturated);
     }
 
     float limitThrust(float thrust, ThrottleLimitType type, int8_t limit)
@@ -227,7 +231,6 @@ class Mixer
       Stats::Measure mixerMeasure(_model.state.stats, COUNTER_MIXER_WRITE);
 
       bool stop = _stop();
-      bool saturated = false;
       for(size_t i = 0; i < OUTPUT_CHANNELS; i++)
       {
         const OutputChannelConfig& och = _model.config.output.channel[i];
@@ -244,13 +247,11 @@ class Mixer
           }
           else
           {
-            if(out[i] >= 0.98f) saturated = true;
             float v = Math::clamp(out[i], -1.f, 1.f);
             _model.state.outputUs[i] = lrintf(Math::map(v, -1.f, 1.f, _model.state.minThrottle, _model.state.maxThrottle));
           }
         }
       }
-      _model.setOputputSaturated(saturated);
       applyOutput();
     }