setpoint iterm relax

rtlopez · Aug 10, 2023 · 6b0d8c6 · 6b0d8c6
1 parent f6aa33f
commit 6b0d8c6
Show file tree

Hide file tree

Showing 7 changed files with 58 additions and 16 deletions.
diff --git a/docs/calculations.ods b/docs/calculations.ods
diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h
@@ -554,6 +554,8 @@ class Cli
         Param(PSTR("pid_dterm_weight"), &c.dtermSetpointWeight),
         Param(PSTR("pid_iterm_limit"), &c.itermWindupPointPercent),
         Param(PSTR("pid_iterm_zero"), &c.lowThrottleZeroIterm),
+        Param(PSTR("pid_iterm_relax"), &c.itermRelax),
+        Param(PSTR("pid_iterm_relax_cutoff"), &c.itermRelaxCutoff),
         Param(PSTR("pid_tpa_scale"), &c.tpaScale),
         Param(PSTR("pid_tpa_breakpoint"), &c.tpaBreakpoint),
 

diff --git a/lib/Espfc/src/Control/Pid.h b/lib/Espfc/src/Control/Pid.h
@@ -28,11 +28,8 @@ class Pid
 {
   public:
     Pid():
-      Kp(0.1), Ki(0), Kd(0), iLimit(0), dGamma(0), oLimit(1.f),
-      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),
-      outputSaturated(false)
+      Kp(0.1), Ki(0), Kd(0), iLimit(0.3), oLimit(1.f),
+      pScale(1.f), iScale(1.f), dScale(1.f), fScale(1.f)
       {}
 
     void begin()
@@ -49,12 +46,22 @@ class Pid
       pTerm = ptermFilter.update(pTerm);
 
       // I-term
-      // TODO: https://github.com/betaflight/betaflight/blob/master/src/main/flight/pid.c#L667
+      iTermError = error;
       if(Ki > 0.f && iScale > 0.f)
       {
         if(!outputSaturated)
         {
-          iTerm += Ki * error * dt * iScale;
+          // I-term relax
+          if(itermRelax)
+          {
+            const bool increasing = (iTerm > 0 && iTermError > 0) || (iTerm < 0 && iTermError < 0);
+            const bool incrementOnly = itermRelax == ITERM_RELAX_RP_INC || itermRelax == ITERM_RELAX_RPY_INC;
+            float setpointLpf = itermRelaxFilter.update(setpoint);
+            itermRelaxBase = Math::toDeg(setpoint - setpointLpf);
+            itermRelaxFactor = std::max(0.0f, 1.0f - std::abs(itermRelaxBase) * 0.025f); // (itermRelaxBase / 40)
+            if(!incrementOnly || increasing) iTermError *= itermRelaxFactor;
+          }
+          iTerm += Ki * iScale * iTermError * dt;
           iTerm = Math::clamp(iTerm, -iLimit, iLimit);
         }
       }
@@ -80,7 +87,7 @@ class Pid
       // F-term
       if(Kf > 0.f && fScale > 0.f)
       {
-        fTerm = Kf * fScale * ((setpoint - prevSetpoint) * rate);
+        fTerm = Kf * fScale * (setpoint - prevSetpoint) * rate;
         fTerm = ftermFilter.update(fTerm);
       }
       else
@@ -113,6 +120,8 @@ class Pid
     float fScale;
 
     float error;
+    float iTermError;
+
     float pTerm;
     float iTerm;
     float dTerm;
@@ -123,12 +132,16 @@ class Pid
     Filter dtermNotchFilter;
     Filter ptermFilter;
     Filter ftermFilter;
+    Filter itermRelaxFilter;
 
     float prevMeasure;
     float prevError;
     float prevSetpoint;
 
     bool outputSaturated;
+    int8_t itermRelax;
+    float itermRelaxFactor;
+    float itermRelaxBase;
 };
 
 }

diff --git a/lib/Espfc/src/Controller.h b/lib/Espfc/src/Controller.h
@@ -175,6 +175,14 @@ class Controller
         //_model.state.debug[i] = lrintf(_model.state.innerPid[i].fTerm * 1000);
       }
       _model.state.output[AXIS_THRUST] = _model.state.desiredRate[AXIS_THRUST];
+
+      if(_model.config.debugMode == DEBUG_ITERM_RELAX)
+      {
+        _model.state.debug[0] = lrintf(Math::toDeg(_model.state.innerPid[0].itermRelaxBase));
+        _model.state.debug[1] = lrintf(_model.state.innerPid[0].itermRelaxFactor * 100.0f);
+        _model.state.debug[2] = lrintf(Math::toDeg(_model.state.innerPid[0].iTermError));
+        _model.state.debug[3] = lrintf(_model.state.innerPid[0].iTerm * 1000.0f);
+      }
     }
 
     float getTpaFactor() const

diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h
@@ -502,7 +502,13 @@ class Model
         }
         pid.dtermFilter2.begin(config.dtermFilter2, pidFilterRate);
         pid.ftermFilter.begin(config.input.filterDerivative, pidFilterRate);
-        if(i == AXIS_YAW) pid.ptermFilter.begin(config.yawFilter, pidFilterRate);
+        pid.itermRelaxFilter.begin(FilterConfig(FILTER_PT1, config.itermRelaxCutoff), pidFilterRate);
+        if(i == AXIS_YAW) {
+          pid.itermRelax = config.itermRelax == ITERM_RELAX_RPY || config.itermRelax == ITERM_RELAX_RPY_INC ? config.itermRelax : ITERM_RELAX_OFF;
+          pid.ptermFilter.begin(config.yawFilter, pidFilterRate);
+        } else {
+          pid.itermRelax = config.itermRelax;
+        }
         pid.begin();
       }
 

diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h
@@ -539,6 +539,15 @@ class FailsafeConfig
     uint8_t killSwitch;
 };
 
+enum ItermRelaxType {
+  ITERM_RELAX_OFF,
+  ITERM_RELAX_RP,
+  ITERM_RELAX_RPY,
+  ITERM_RELAX_RP_INC,
+  ITERM_RELAX_RPY_INC,
+  ITERM_RELAX_COUNT,
+};
+
 // persistent data
 class ModelConfig
 {
@@ -594,6 +603,8 @@ class ModelConfig
 
     int16_t dtermSetpointWeight;
     int8_t itermWindupPointPercent;
+    int8_t itermRelax;
+    int8_t itermRelaxCutoff;
 
     int8_t angleLimit;
     int16_t angleRateLimit;
@@ -738,7 +749,7 @@ class ModelConfig
       gyroDynLpfFilter = FilterConfig(FILTER_PT1, 425, 170);
       gyroFilter = FilterConfig(FILTER_PT1, 100);
       gyroFilter2 = FilterConfig(FILTER_PT1, 213);
-      dynamicFilter = DynamicFilterConfig(0, 300, 80, 400); // 8%. q:3.0, 80-400 Hz
+      dynamicFilter = DynamicFilterConfig(4, 300, 80, 400); // 8%. q:3.0, 80-400 Hz
 
       dtermDynLpfFilter = FilterConfig(FILTER_PT1, 145, 60);
       dtermFilter = FilterConfig(FILTER_PT1, 128);
@@ -915,6 +926,8 @@ class ModelConfig
       pid[PID_VEL]   = { .P = 0, .I =  0, .D =  0, .F = 0 };
 
       itermWindupPointPercent = 30;
+      itermRelax = ITERM_RELAX_RP_INC;
+      itermRelaxCutoff = 15;
       dtermSetpointWeight = 30;
 
       angleLimit = 55;  // deg

diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h
@@ -1096,8 +1096,8 @@ class MspProcessor
           r.writeU16(_model.config.dtermSetpointWeight);
           r.writeU8(0); // iterm rotation
           r.writeU8(0); // smart feed forward
-          r.writeU8(0); // iterm relax
-          r.writeU8(0); // iterm ralx type
+          r.writeU8(_model.config.itermRelax); // iterm relax
+          r.writeU8(1); // iterm relax type (setpoint only)
           r.writeU8(0); // abs control gain
           r.writeU8(0); // throttle boost
           r.writeU8(0); // acro trainer max angle
@@ -1114,7 +1114,7 @@ class MspProcessor
           r.writeU8(0); // use_integrated_yaw
           r.writeU8(0); // integrated_yaw_relax
           // 1.42+
-          r.writeU8(0); // iterm_relax_cutoff
+          r.writeU8(_model.config.itermRelaxCutoff); // iterm_relax_cutoff
           // 1.43+
           r.writeU8(_model.config.output.motorLimit); // motor_output_limit
           r.writeU8(0); // auto_profile_cell_count
@@ -1149,8 +1149,8 @@ class MspProcessor
           if (m.remain() >= 14) {
             m.readU8(); //iterm rotation
             m.readU8(); //smart feed forward
-            m.readU8(); //iterm relax
-            m.readU8(); //iterm ralx type
+            _model.config.itermRelax = m.readU8(); //iterm relax
+            m.readU8(); //iterm relax type
             m.readU8(); //abs control gain
             m.readU8(); //throttle boost
             m.readU8(); //acro trainer max angle
@@ -1171,7 +1171,7 @@ class MspProcessor
           }
           // 1.42+
           if (m.remain() >= 1) {
-            m.readU8(); // iterm_relax_cutoff
+            _model.config.itermRelaxCutoff = m.readU8(); // iterm_relax_cutoff
           }
           // 1.43+
           if (m.remain() >= 3) {