Pid update

src/modules/interface/pid.h

@@ -104,10 +104,9 @@
 
 typedef struct
 {
-  float desired;     //< set point
+  float desired;      //< set point
   float error;        //< error
   float prevError;    //< previous error
-  float errorMax;    //< maximum error
   float integ;        //< integral
   float deriv;        //< derivative
   float kp;           //< proportional gain
@@ -117,8 +116,7 @@ typedef struct
   float outI;         //< integral output (debugging)
   float outD;         //< derivative output (debugging)
   float iLimit;       //< integral limit
-  float iLimitLow;    //< integral limit
-  bool  iCapped;      //< true to stop integration
+  float outputLimit;  //< output limit
   float dt;           //< delta-time dt
   lpf2pData dFilter;  //< filter for D term
   bool enableDFilter; //< filter for D term enable flag

src/modules/src/attitude_pid_controller.c

@@ -30,6 +30,7 @@
 #include "attitude_controller.h"
 #include "pid.h"
 #include "param.h"
+#include "log.h"
 
 #define ATTITUDE_LPF_CUTOFF_FREQ      15.0f
 #define ATTITUDE_LPF_ENABLE false
@@ -55,9 +56,13 @@ PidObject pidRoll;
 PidObject pidPitch;
 PidObject pidYaw;
 
-int16_t rollOutput;
-int16_t pitchOutput;
-int16_t yawOutput;
+static int16_t rollOutput;
+static int16_t pitchOutput;
+static int16_t yawOutput;
+
+static float rpRateLimit  = 150.0f;
+static float yawRateLimit = 150.0f;
+static float rpyRateLimitOverhead = 1.1f;
 
 static bool isInit;
 
@@ -89,6 +94,10 @@ void attitudeControllerInit(const float updateDt)
   pidSetIntegralLimit(&pidPitch, PID_PITCH_INTEGRATION_LIMIT);
   pidSetIntegralLimit(&pidYaw,   PID_YAW_INTEGRATION_LIMIT);
 
+  pidRollRate.outputLimit  = INT16_MAX;
+  pidPitchRate.outputLimit = INT16_MAX;
+  pidYawRate.outputLimit   = INT16_MAX;
+
   isInit = true;
 }
 
@@ -116,6 +125,10 @@ void attitudeControllerCorrectAttitudePID(
        float eulerRollDesired, float eulerPitchDesired, float eulerYawDesired,
        float* rollRateDesired, float* pitchRateDesired, float* yawRateDesired)
 {
+  pidRoll.outputLimit  = rpRateLimit  * rpyRateLimitOverhead;
+  pidPitch.outputLimit = rpRateLimit  * rpyRateLimitOverhead;
+  pidYaw.outputLimit   = yawRateLimit * rpyRateLimitOverhead;
+
   pidSetDesired(&pidRoll, eulerRollDesired);
   *rollRateDesired = pidUpdate(&pidRoll, eulerRollActual, true);
 
@@ -151,6 +164,30 @@ void attitudeControllerGetActuatorOutput(int16_t* roll, int16_t* pitch, int16_t*
   *yaw = yawOutput;
 }
 
+LOG_GROUP_START(pid_attitude)
+LOG_ADD(LOG_FLOAT, roll_outP, &pidRoll.outP)
+LOG_ADD(LOG_FLOAT, roll_outI, &pidRoll.outI)
+LOG_ADD(LOG_FLOAT, roll_outD, &pidRoll.outD)
+LOG_ADD(LOG_FLOAT, pitch_outP, &pidPitch.outP)
+LOG_ADD(LOG_FLOAT, pitch_outI, &pidPitch.outI)
+LOG_ADD(LOG_FLOAT, pitch_outD, &pidPitch.outD)
+LOG_ADD(LOG_FLOAT, yaw_outP, &pidYaw.outP)
+LOG_ADD(LOG_FLOAT, yaw_outI, &pidYaw.outI)
+LOG_ADD(LOG_FLOAT, yaw_outD, &pidYaw.outD)
+LOG_GROUP_STOP(pid_attitude)
+
+LOG_GROUP_START(pid_rate)
+LOG_ADD(LOG_FLOAT, roll_outP, &pidRollRate.outP)
+LOG_ADD(LOG_FLOAT, roll_outI, &pidRollRate.outI)
+LOG_ADD(LOG_FLOAT, roll_outD, &pidRollRate.outD)
+LOG_ADD(LOG_FLOAT, pitch_outP, &pidPitchRate.outP)
+LOG_ADD(LOG_FLOAT, pitch_outI, &pidPitchRate.outI)
+LOG_ADD(LOG_FLOAT, pitch_outD, &pidPitchRate.outD)
+LOG_ADD(LOG_FLOAT, yaw_outP, &pidYawRate.outP)
+LOG_ADD(LOG_FLOAT, yaw_outI, &pidYawRate.outI)
+LOG_ADD(LOG_FLOAT, yaw_outD, &pidYawRate.outD)
+LOG_GROUP_STOP(pid_rate)
+
 PARAM_GROUP_START(pid_attitude)
 PARAM_ADD(PARAM_FLOAT, roll_kp, &pidRoll.kp)
 PARAM_ADD(PARAM_FLOAT, roll_ki, &pidRoll.ki)
@@ -164,6 +201,8 @@ PARAM_ADD(PARAM_FLOAT, yaw_kd, &pidYaw.kd)
 PARAM_GROUP_STOP(pid_attitude)
 
 PARAM_GROUP_START(pid_rate)
+PARAM_ADD(PARAM_FLOAT, rpRateLimit,  &rpRateLimit)
+PARAM_ADD(PARAM_FLOAT, yawRateLimit, &yawRateLimit)
 PARAM_ADD(PARAM_FLOAT, roll_kp, &pidRollRate.kp)
 PARAM_ADD(PARAM_FLOAT, roll_ki, &pidRollRate.ki)
 PARAM_ADD(PARAM_FLOAT, roll_kd, &pidRollRate.kd)

src/modules/src/pid.c

@@ -34,19 +34,17 @@ void pidInit(PidObject* pid, const float desired, const float kp,
              const float samplingRate, const float cutoffFreq,
              bool enableDFilter)
 {
-  pid->error     = 0;
-  pid->errorMax  = 0.0f;
-  pid->prevError = 0;
-  pid->integ     = 0;
-  pid->deriv     = 0;
-  pid->desired = desired;
-  pid->kp = kp;
-  pid->ki = ki;
-  pid->kd = kd;
-  pid->iLimit    = DEFAULT_PID_INTEGRATION_LIMIT;
-  pid->iLimitLow = -DEFAULT_PID_INTEGRATION_LIMIT;
-  pid->iCapped   = false;
-  pid->dt        = dt;
+  pid->error         = 0;
+  pid->prevError     = 0;
+  pid->integ         = 0;
+  pid->deriv         = 0;
+  pid->desired       = desired;
+  pid->kp            = kp;
+  pid->ki            = ki;
+  pid->kd            = kd;
+  pid->iLimit        = DEFAULT_PID_INTEGRATION_LIMIT;
+  pid->outputLimit   = DEFAULT_PID_INTEGRATION_LIMIT;
+  pid->dt            = dt;
   pid->enableDFilter = enableDFilter;
   if (pid->enableDFilter)
   {
@@ -56,28 +54,15 @@ void pidInit(PidObject* pid, const float desired, const float kp,
 
 float pidUpdate(PidObject* pid, const float measured, const bool updateError)
 {
-    float output;
+    float output = 0.0f;
 
     if (updateError)
     {
         pid->error = pid->desired - measured;
-        if (pid->errorMax > FLT_MIN) {
-          float errorMaxScaled = pid->errorMax / pid->kp;
-          pid->error = constrain(pid->error, -errorMaxScaled, errorMaxScaled);
-        }
     }
 
-    if (pid->iCapped == false) {
-      pid->integ += pid->error * pid->dt;
-      if (pid->integ > pid->iLimit)
-      {
-          pid->integ = pid->iLimit;
-      }
-      else if (pid->integ < pid->iLimitLow)
-      {
-          pid->integ = pid->iLimitLow;
-      }
-    }
+    pid->outP = pid->kp * pid->error;
+    output += pid->outP;
 
     float deriv = (pid->error - pid->prevError) / pid->dt;
     if (pid->enableDFilter)
@@ -86,12 +71,22 @@ float pidUpdate(PidObject* pid, const float measured, const bool updateError)
     } else {
       pid->deriv = deriv;
     }
+    if (isnan(pid->deriv)) {
+      pid->deriv = 0;
+    }
+    pid->outD = pid->kd * pid->deriv;
+    output += pid->outD;
+
+    float i = pid->integ + pid->error * pid->dt;
+    // Check if integral is saturated
+    if (abs(i) <= pid->iLimit || abs(pid->ki * i + output) <= pid->outputLimit) {
+      pid->integ = i;
+    }
 
-    pid->outP = pid->kp * pid->error;
     pid->outI = pid->ki * pid->integ;
-    pid->outD = pid->kd * pid->deriv;
+    output += pid->outI;
 
-    output = pid->outP + pid->outI + pid->outD;
+    output = constrain(output, -pid->outputLimit, pid->outputLimit);
 
     pid->prevError = pid->error;
 
@@ -103,10 +98,6 @@ void pidSetIntegralLimit(PidObject* pid, const float limit) {
 }
 
 
-void pidSetIntegralLimitLow(PidObject* pid, const float limitLow) {
-    pid->iLimitLow = limitLow;
-}
-
 void pidReset(PidObject* pid)
 {
   pid->error     = 0;

src/modules/src/position_controller_pid.c

@@ -65,10 +65,12 @@ struct this_s {
 
 // Maximum roll/pitch angle permited
 static float rpLimit  = 20;
+static float rpLimitOverhead = 1.10f;
 // Velocity maximums
 static float xyVelMax = 1.0f;
 static float zVelMax  = 1.0f;
 static float velMaxOverhead = 1.10f;
+static const float thrustScale = 1000.0f;
 
 #define DT (float)(1.0f/POSITION_RATE)
 #define POSITION_LPF_CUTOFF_FREQ 20.0f
@@ -96,8 +98,8 @@ static struct this_s this = {
 
   .pidVZ = {
     .init = {
-      .kp = 25,
-      .ki = 15,
+      .kp = 15,
+      .ki = 20,
       .kd = 0,
     },
     .pid.dt = DT,
@@ -125,7 +127,7 @@ static struct this_s this = {
     .init = {
       .kp = 2.0f,
       .ki = 0,
-      .kd = 0.01f,
+      .kd = 0,
     },
     .pid.dt = DT,
   },
@@ -150,10 +152,6 @@ void positionControllerInit()
       this.pidVY.pid.dt, POSITION_RATE, POSITION_LPF_CUTOFF_FREQ, POSITION_LPF_ENABLE);
   pidInit(&this.pidVZ.pid, this.pidVZ.setpoint, this.pidVZ.init.kp, this.pidVZ.init.ki, this.pidVZ.init.kd,
       this.pidVZ.pid.dt, POSITION_RATE, POSITION_LPF_CUTOFF_FREQ, POSITION_LPF_ENABLE);
-
-  this.pidX.pid.errorMax = xyVelMax * velMaxOverhead;
-  this.pidY.pid.errorMax = xyVelMax * velMaxOverhead;
-  this.pidZ.pid.errorMax = zVelMax  * velMaxOverhead;
 }
 
 static float runPid(float input, struct pidAxis_s *axis, float setpoint, float dt) {
@@ -166,6 +164,12 @@ static float runPid(float input, struct pidAxis_s *axis, float setpoint, float d
 void positionController(float* thrust, attitude_t *attitude, const state_t *state,
                                                              const setpoint_t *setpoint)
 {
+  this.pidX.pid.outputLimit = xyVelMax * velMaxOverhead;
+  this.pidY.pid.outputLimit = xyVelMax * velMaxOverhead;
+  // The ROS landing detector will prematurely trip if
+  // this value is below 0.5
+  this.pidZ.pid.outputLimit = max(zVelMax, 0.5)  * velMaxOverhead;
+
   // X, Y
   pidSetDesired(&this.pidVX.pid, runPid(state->position.x, &this.pidX, setpoint->position.x, DT));
   pidSetDesired(&this.pidVY.pid, runPid(state->position.y, &this.pidY, setpoint->position.y, DT));
@@ -177,6 +181,12 @@ void positionController(float* thrust, attitude_t *attitude, const state_t *stat
 void velocityController(float* thrust, attitude_t *attitude, const state_t *state,
                                                              const setpoint_t *setpoint)
 {
+  this.pidVX.pid.outputLimit = rpLimit * rpLimitOverhead;
+  this.pidVY.pid.outputLimit = rpLimit * rpLimitOverhead;
+  // Set the output limit to the maximum thrust range
+  this.pidVZ.pid.outputLimit = (UINT16_MAX / 2 / thrustScale);
+  //this.pidVZ.pid.outputLimit = (this.thrustBase - this.thrustMin) / thrustScale;
+
   // Roll and Pitch
   float rollRaw  = pidUpdate(&this.pidVX.pid, state->velocity.x, true);
   float pitchRaw = pidUpdate(&this.pidVY.pid, state->velocity.y, true);
@@ -185,28 +195,16 @@ void velocityController(float* thrust, attitude_t *attitude, const state_t *stat
   attitude->pitch = -(rollRaw  * cosf(yawRad)) - (pitchRaw * sinf(yawRad));
   attitude->roll  = -(pitchRaw * cosf(yawRad)) + (rollRaw  * sinf(yawRad));
 
-  // Check for roll/pitch limits and prevent I from accumulating when capped
-  if (abs(attitude->roll) > rpLimit || abs(attitude->pitch) > rpLimit) {
-      this.pidVX.pid.iCapped = true;
-      this.pidVY.pid.iCapped = true;
-  } else {
-      this.pidVX.pid.iCapped = false;
-      this.pidVY.pid.iCapped = false;
-  }
-
   attitude->roll  = constrain(attitude->roll,  -rpLimit, rpLimit);
   attitude->pitch = constrain(attitude->pitch, -rpLimit, rpLimit);
 
   // Thrust
   float thrustRaw = pidUpdate(&this.pidVZ.pid, state->velocity.z, true);
   // Scale the thrust and add feed forward term
-  *thrust = thrustRaw*1000 + this.thrustBase;
-  // Check for minimum thrust, prevent I from accumulating when capped
+  *thrust = thrustRaw*thrustScale + this.thrustBase;
+  // Check for minimum thrust
   if (*thrust < this.thrustMin) {
     *thrust = this.thrustMin;
-    this.pidVZ.pid.iCapped = true;
-  } else {
-    this.pidVZ.pid.iCapped = false;
   }
 }
 
@@ -281,6 +279,8 @@ PARAM_ADD(PARAM_FLOAT, zKd, &this.pidZ.pid.kd)
 PARAM_ADD(PARAM_UINT16, thrustBase, &this.thrustBase)
 PARAM_ADD(PARAM_UINT16, thrustMin, &this.thrustMin)
 
-PARAM_ADD(PARAM_FLOAT, rpLimit, &rpLimit)
+PARAM_ADD(PARAM_FLOAT, rpLimit,  &rpLimit)
+PARAM_ADD(PARAM_FLOAT, xyVelMax, &xyVelMax)
+PARAM_ADD(PARAM_FLOAT, zVelMax,  &zVelMax)
 
 PARAM_GROUP_STOP(posCtlPid)

src/utils/interface/num.h

@@ -37,6 +37,9 @@
 #undef abs
 #define abs(a) ((a) > 0 ? (a) : (-1*(a)))
 
+#undef isnan
+#define isnan(n) ((n != n) ? 1 : 0)
+
 
 uint16_t single2half(float number);
 float half2single(uint16_t number);