From b34dd2d96a6985dd84260fcc607cc57b25c5ea48 Mon Sep 17 00:00:00 2001 From: raiden00pl Date: Sun, 28 Feb 2021 22:20:54 +0100 Subject: [PATCH] libdsp/lib_pid.c: add anti-windup protection with decay coefficient and add interface to select anti-windup mechanism --- include/dsp.h | 32 +++++++++----- libs/libdsp/lib_pid.c | 99 +++++++++++++++++++++++++++++++++---------- 2 files changed, 97 insertions(+), 34 deletions(-) diff --git a/include/dsp.h b/include/dsp.h index 763a7777f4..58640b596e 100644 --- a/include/dsp.h +++ b/include/dsp.h @@ -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 */ diff --git a/libs/libdsp/lib_pid.c b/libs/libdsp/lib_pid.c index 51ba66a6f0..b0e9ea735d 100644 --- a/libs/libdsp/lib_pid.c +++ b/libs/libdsp/lib_pid.c @@ -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) {