From d536c30faac7c559dc2d4b579613543e707ec5b2 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Jos=C3=A9=20=C3=81ngel?= <gelanchez@gmail.com>
Date: Thu, 21 Dec 2023 17:00:11 +0000
Subject: [PATCH] PID avoid derivative kick

---
 src/utils/interface/pid.h |   2 +-
 src/utils/src/pid.c       | 118 ++++++++++++++++++--------------------
 2 files changed, 58 insertions(+), 62 deletions(-)

diff --git a/src/utils/interface/pid.h b/src/utils/interface/pid.h
index 50248f0aec..cbdfb828d4 100644
--- a/src/utils/interface/pid.h
+++ b/src/utils/interface/pid.h
@@ -38,7 +38,7 @@ typedef struct
 {
   float desired;      //< set point
   float error;        //< error
-  float prevError;    //< previous error
+  float prevMeasured; //< previous measurement
   float integ;        //< integral
   float deriv;        //< derivative
   float kp;           //< proportional gain
diff --git a/src/utils/src/pid.c b/src/utils/src/pid.c
index 67b00ba9a0..7152e9fad8 100644
--- a/src/utils/src/pid.c
+++ b/src/utils/src/pid.c
@@ -36,7 +36,7 @@ void pidInit(PidObject* pid, const float desired, const float kp,
              bool enableDFilter)
 {
   pid->error         = 0;
-  pid->prevError     = 0;
+  pid->prevMeasured  = 0;
   pid->integ         = 0;
   pid->deriv         = 0;
   pid->desired       = desired;
@@ -56,86 +56,82 @@ void pidInit(PidObject* pid, const float desired, const float kp,
 
 float pidUpdate(PidObject* pid, const float measured, const bool updateError)
 {
-    float output = 0.0f;
+  float output = 0.0f;
 
-    if (updateError)
-    {
-        pid->error = pid->desired - measured;
-    }
-
-    pid->outP = pid->kp * pid->error;
-    output += pid->outP;
+  if (updateError)
+  {
+      pid->error = pid->desired - measured;
+  }
 
-    float deriv = (pid->error - pid->prevError) / pid->dt;
-    
-    #if CONFIG_CONTROLLER_PID_FILTER_ALL
+  pid->outP = pid->kp * pid->error;
+  output += pid->outP;
+
+  float deriv = -(measured - pid->prevMeasured) / pid->dt;
+  
+  #if CONFIG_CONTROLLER_PID_FILTER_ALL
+    pid->deriv = deriv;
+  #else
+    if (pid->enableDFilter){
+      pid->deriv = lpf2pApply(&pid->dFilter, deriv);
+    } else {
       pid->deriv = deriv;
-    #else
-      if (pid->enableDFilter){
-        pid->deriv = lpf2pApply(&pid->dFilter, deriv);
-      } else {
-        pid->deriv = deriv;
-      }
-    #endif
-    if (isnan(pid->deriv)) {
-      pid->deriv = 0;
     }
-    pid->outD = pid->kd * pid->deriv;
-    output += pid->outD;
+  #endif
+  if (isnan(pid->deriv)) {
+    pid->deriv = 0;
+  }
+  pid->outD = pid->kd * pid->deriv;
+  output += pid->outD;
 
-    pid->integ += pid->error * pid->dt;
+  pid->integ += pid->error * pid->dt;
 
-    // Constrain the integral (unless the iLimit is zero)
-    if(pid->iLimit != 0)
-    {
-    	pid->integ = constrain(pid->integ, -pid->iLimit, pid->iLimit);
-    }
+  // Constrain the integral (unless the iLimit is zero)
+  if(pid->iLimit != 0)
+  {
+    pid->integ = constrain(pid->integ, -pid->iLimit, pid->iLimit);
+  }
+
+  pid->outI = pid->ki * pid->integ;
+  output += pid->outI;
 
-    pid->outI = pid->ki * pid->integ;
-    output += pid->outI;
-
-    pid->outFF = pid->kff * pid->desired;
-    output += pid->outFF;
-    
-    #if CONFIG_CONTROLLER_PID_FILTER_ALL
-      //filter complete output instead of only D component to compensate for increased noise from increased barometer influence
-      if (pid->enableDFilter)
-      {
-        output = lpf2pApply(&pid->dFilter, output);
-      }
-      else {
-        output = output;
-      }
-      if (isnan(output)) {
-        output = 0;
-      }
-     #endif
-      
-    
-
-    // Constrain the total PID output (unless the outputLimit is zero)
-    if(pid->outputLimit != 0)
+  pid->outFF = pid->kff * pid->desired;
+  output += pid->outFF;
+  
+  #if CONFIG_CONTROLLER_PID_FILTER_ALL
+    //filter complete output instead of only D component to compensate for increased noise from increased barometer influence
+    if (pid->enableDFilter)
     {
-      output = constrain(output, -pid->outputLimit, pid->outputLimit);
+      output = lpf2pApply(&pid->dFilter, output);
+    }
+    else {
+      output = output;
+    }
+    if (isnan(output)) {
+      output = 0;
     }
+    #endif
 
+  // Constrain the total PID output (unless the outputLimit is zero)
+  if(pid->outputLimit != 0)
+  {
+    output = constrain(output, -pid->outputLimit, pid->outputLimit);
+  }
 
-    pid->prevError = pid->error;
+  pid->prevMeasured = measured;
 
-    return output;
+  return output;
 }
 
 void pidSetIntegralLimit(PidObject* pid, const float limit) {
     pid->iLimit = limit;
 }
 
-
 void pidReset(PidObject* pid)
 {
-  pid->error     = 0;
-  pid->prevError = 0;
-  pid->integ     = 0;
-  pid->deriv     = 0;
+  pid->error        = 0;
+  pid->prevMeasured = 0;
+  pid->integ        = 0;
+  pid->deriv        = 0;
 }
 
 void pidSetError(PidObject* pid, const float error)