Disable the PID output constraint for the attitude PIDs (rate/angle) …

…and correct

how the integral limit is computed/enforced
Fixes #185

src/modules/interface/pid.h

@@ -63,7 +63,8 @@
 #define PID_YAW_INTEGRATION_LIMIT     360.0
 
 
-#define DEFAULT_PID_INTEGRATION_LIMIT  5000.0
+#define DEFAULT_PID_INTEGRATION_LIMIT 5000.0
+#define DEFAULT_PID_OUTPUT_LIMIT      0.0
 
 #else
 
@@ -98,7 +99,8 @@
 #define PID_YAW_INTEGRATION_LIMIT     360.0
 
 
-#define DEFAULT_PID_INTEGRATION_LIMIT  10000.0
+#define DEFAULT_PID_INTEGRATION_LIMIT 10000.0
+#define DEFAULT_PID_OUTPUT_LIMIT      0.0
 
 #endif
 
@@ -115,8 +117,8 @@ typedef struct
   float outP;         //< proportional output (debugging)
   float outI;         //< integral output (debugging)
   float outD;         //< derivative output (debugging)
-  float iLimit;       //< integral limit
-  float outputLimit;  //< output limit
+  float iLimit;       //< integral limit, absolute value. '0' means no limit.
+  float outputLimit;  //< total PID output limit, absolute value. '0' means no 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

@@ -60,10 +60,6 @@ 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;
 
 void attitudeControllerInit(const float updateDt)
@@ -94,10 +90,6 @@ 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;
 }
 
@@ -125,10 +117,6 @@ 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);
 
@@ -201,8 +189,6 @@ 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

@@ -43,7 +43,7 @@ void pidInit(PidObject* pid, const float desired, const float kp,
   pid->ki            = ki;
   pid->kd            = kd;
   pid->iLimit        = DEFAULT_PID_INTEGRATION_LIMIT;
-  pid->outputLimit   = DEFAULT_PID_INTEGRATION_LIMIT;
+  pid->outputLimit   = DEFAULT_PID_OUTPUT_LIMIT;
   pid->dt            = dt;
   pid->enableDFilter = enableDFilter;
   if (pid->enableDFilter)
@@ -77,16 +77,23 @@ float pidUpdate(PidObject* pid, const float measured, const bool updateError)
     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->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);
     }
 
     pid->outI = pid->ki * pid->integ;
     output += pid->outI;
 
-    output = constrain(output, -pid->outputLimit, pid->outputLimit);
+    // 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;