Merge pull request #13 from uLipe/uLipe/pid_windup

pid: added basic anti-windup mechanism on pid controller

pid/pid.c

@@ -20,6 +20,7 @@ pid_control_t pid_create(void)
 
     new_pid->maximum = 100;
     new_pid->minimum = 20;
+    new_pid->anti_windup_value = new_pid->maximum * 2.0f;
 
     new_pid->p_error = 10.0f;
     new_pid->i_error = 1.0f;
@@ -75,6 +76,12 @@ rt_err_t pid_set_target(pid_control_t pid, rt_int16_t set_point)
     return RT_EOK;
 }
 
+rt_err_t pid_set_anti_windup_value(pid_control_t pid, float anti_windup_value)
+{
+    pid->anti_windup_value = anti_windup_value;
+    return RT_EOK;
+}
+
 rt_err_t pid_set_sample_time(pid_control_t pid, rt_uint16_t sample_time)
 {
     // TODO
@@ -96,6 +103,14 @@ float pid_update(pid_control_t pid, rt_int16_t current_point)
 
     pid->integral += pid->error;
 
+    //Perform integral value capping to avoid internal PID state to blows up
+    //when actuators saturate:
+    if(pid->integral > pid->anti_windup_value) {
+        pid->integral = pid->anti_windup_value;
+    } else if (pid->integral < -pid->anti_windup_value) {
+        pid->integral = -pid->anti_windup_value;
+    }
+
     pid->p_error = pid->kp * pid->error;
     pid->i_error = pid->ki * pid->integral;
     pid->d_error = pid->kd * (pid->error - 2 * pid->error_l + pid->error_ll);

pid/pid.h

@@ -13,6 +13,7 @@ struct pid_control
 
     float       minimum;
     float       maximum;
+    float       anti_windup_value;
     rt_int16_t  set_point;
     rt_uint16_t sample_time;    // unit:ms
 
@@ -39,7 +40,7 @@ rt_err_t        pid_set_ki(pid_control_t pid, float ki);
 rt_err_t        pid_set_kd(pid_control_t pid, float kd);
 rt_err_t        pid_set_target(pid_control_t pid, rt_int16_t set_point);
 rt_err_t        pid_set_sample_time(pid_control_t pid, rt_uint16_t sample_time);
-
+rt_err_t        pid_set_anti_windup_value(pid_control_t pid, float anti_windup_value);
 float           pid_update(pid_control_t pid, rt_int16_t current_point);
 
 // PID Thread