OrgApache/incubator-nuttx
OrgApache
/
incubator-nuttx
mirror of https://github.com/apache/incubator-nuttx.git
1
0
Fork 0
Code Issues Releases Wiki Activity

libdsp/lib_pid.c: add anti-windup protection with decay coefficient and add interface to select anti-windup mechanism

This commit is contained in:
raiden00pl 2021-02-28 22:20:54 +01:00 committed by Xiang Xiao
parent 15b0ae716c
commit b34dd2d96a
2 changed files with 97 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
include/dsp.h
View File

@ -177,17 +177,24 @@ typedef struct float_sat_f32_s float_sat_f32_t;
struct pid_controller_f32_s
{
float out; /* Controller output */
float_sat_f32_t sat; /* Output saturation */
float err; /* Current error value */
float err_prev; /* Previous error value */
float KP; /* Proportional coefficient */
float KI; /* Integral coefficient */
float KD; /* Derivative coefficient */
float part[3]; /* 0 - proporitonal part
* 1 - integral part
* 2 - derivative part
*/
bool aw_en; /* Integral part decay if saturated */
bool ireset_en; /* Intergral part reset if saturated */
bool pisat_en; /* PI saturation enabled */
bool pidsat_en; /* PID saturation enabled */
bool _res; /* Reserved */
float out; /* Controller output */
float_sat_f32_t sat; /* Output saturation */
float err; /* Current error value */
float err_prev; /* Previous error value */
float KP; /* Proportional coefficient */
float KI; /* Integral coefficient */
float KD; /* Derivative coefficient */
float part[3]; /* 0 - proporitonal part
* 1 - integral part
* 2 - derivative part
*/
float KC; /* Integral anti-windup decay coefficient */
float aw; /* Integral anti-windup decay part */
};
typedef struct pid_controller_f32_s pid_controller_f32_t;
@ -390,6 +397,9 @@ void pid_integral_reset(FAR pid_controller_f32_t *pid);
void pi_integral_reset(FAR pid_controller_f32_t *pid);
float pi_controller(FAR pid_controller_f32_t *pid, float err);
float pid_controller(FAR pid_controller_f32_t *pid, float err);
void pi_antiwindup_enable(FAR pid_controller_f32_t *pid, float KC,
bool enable);
void pi_ireset_enable(FAR pid_controller_f32_t *pid, bool enable);
/* Transformation functions */

99
libs/libdsp/lib_pid.c
View File

@ -60,6 +60,7 @@ void pid_controller_init(FAR pid_controller_f32_t *pid, float KP, float KI,
pid->KP = KP;
pid->KI = KI;
pid->KD = KD;
pid->KC = 0.0;
}
/****************************************************************************
@ -92,6 +93,12 @@ void pi_controller_init(FAR pid_controller_f32_t *pid, float KP, float KI)
pid->KP = KP;
pid->KI = KI;
pid->KD = 0.0f;
pid->KC = 0.0f;
/* No windup-protection at default */
pid->aw_en = false;
pid->ireset_en = false;
}
/****************************************************************************
@ -119,6 +126,10 @@ void pid_saturation_set(FAR pid_controller_f32_t *pid, float min, float max)
pid->sat.max = max;
pid->sat.min = min;
/* Enable saturation in PID controller */
pid->pidsat_en = true;
}
/****************************************************************************
@ -141,7 +152,32 @@ void pi_saturation_set(FAR pid_controller_f32_t *pid, float min, float max)
LIBDSP_DEBUGASSERT(pid != NULL);
LIBDSP_DEBUGASSERT(min < max);
pid_saturation_set(pid, min, max);
pid->sat.max = max;
pid->sat.min = min;
/* Enable saturation in PI controller */
pid->pisat_en = true;
}
/****************************************************************************
* Name: pid_antiwindup_enable
****************************************************************************/
void pi_antiwindup_enable(FAR pid_controller_f32_t *pid, float KC,
bool enable)
{
pid->aw_en = enable;
pid->KC = KC;
}
/****************************************************************************
* Name: pid_ireset_enable
****************************************************************************/
void pi_ireset_enable(FAR pid_controller_f32_t *pid, bool enable)
{
pid->ireset_en = enable;
}
/****************************************************************************
@ -151,6 +187,7 @@ void pi_saturation_set(FAR pid_controller_f32_t *pid, float min, float max)
void pid_integral_reset(FAR pid_controller_f32_t *pid)
{
pid->part[1] = 0.0f;
pid->aw = 0.0f;
}
/****************************************************************************
@ -181,6 +218,8 @@ float pi_controller(FAR pid_controller_f32_t *pid, float err)
{
LIBDSP_DEBUGASSERT(pid != NULL);
float tmp = 0.0f;
/* Store error in controller structure */
pid->err = err;
@ -191,47 +230,61 @@ float pi_controller(FAR pid_controller_f32_t *pid, float err)
/* Get intergral part */
pid->part[1] += pid->KI * err;
pid->part[1] += pid->KI * (err - pid->aw);
/* Add proportional, integral */
pid->out = pid->part[0] + pid->part[1];
/* Saturate output only if we are not in a PID calculation and only
* if some limits are set. Saturation for a PID controller are done later
* in PID routine.
*/
/* Store not saturated output */
if (pid->sat.max != pid->sat.min && pid->KD == 0.0f)
tmp = pid->out;
/* Saturate output if enabled */
if (pid->pisat_en == true)
{
if (pid->out > pid->sat.max)
{
if (pid->ireset_en == true)
{
/* Reset I part */
if (err > 0.0f)
{
pi_integral_reset(pid);
}
}
/* Limit output to the upper limit */
pid->out = pid->sat.max;
/* Integral anti-windup - reset integral part */
if (err > 0.0f)
{
pi_integral_reset(pid);
}
}
else if (pid->out < pid->sat.min)
{
if (pid->ireset_en == true)
{
/* Reset I part */
if (err < 0.0f)
{
pi_integral_reset(pid);
}
}
/* Limit output to the lower limit */
pid->out = pid->sat.min;
/* Integral anti-windup - reset integral part */
if (err < 0.0f)
{
pi_integral_reset(pid);
}
}
}
/* Anti-windup I-part decay if enabled */
if (pid->aw_en == true)
{
pid->aw = pid->KC * (tmp - pid->out);
}
/* Return regulator output */
return pid->out;
@ -272,9 +325,9 @@ float pid_controller(FAR pid_controller_f32_t *pid, float err)
pid->err_prev = err;
/* Saturate output if limits are set */
/* Saturate output if enabled */
if (pid->sat.max != pid->sat.min)
if (pid->pidsat_en == true)
{
if (pid->out > pid->sat.max)
{