From 310de1df963143da40ff6551d5823d40df282af4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 25 Nov 2013 23:19:27 +0400 Subject: pid lib update --- .../multirotor_attitude_control.c | 4 +- .../multirotor_rate_control.c | 6 +- .../multirotor_pos_control.c | 6 +- src/modules/systemlib/pid/pid.c | 111 ++++++++------------- src/modules/systemlib/pid/pid.h | 41 ++++---- 5 files changed, 69 insertions(+), 99 deletions(-) (limited to 'src/modules') diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 8245aa560..b7df0433a 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -194,8 +194,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_init(&h); parameters_update(&h, &p); - pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET, 0.0f); - pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET, 0.0f); + pid_init(&pitch_controller, PID_MODE_DERIVATIV_SET, 0.0f); + pid_init(&roll_controller, PID_MODE_DERIVATIV_SET, 0.0f); initialized = true; } diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 86ac0e4ff..eb41bf93e 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -161,9 +161,9 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, parameters_update(&h, &p); initialized = true; - pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); - pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); - pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); + pid_init(&pitch_rate_controller, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); + pid_init(&roll_rate_controller, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); + pid_init(&yaw_rate_controller, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); } /* load new parameters with lower rate */ diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 3d23d0c09..c0e1eb523 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -246,11 +246,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) for (int i = 0; i < 2; i++) { - pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); - pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + pid_init(&(xy_pos_pids[i]), PID_MODE_DERIVATIV_SET, 0.02f); + pid_init(&(xy_vel_pids[i]), PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); } - pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f); + pid_init(&z_pos_pid, PID_MODE_DERIVATIV_SET, 0.02f); thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); while (!thread_should_exit) { diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 77c952f52..cab33282c 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -39,7 +39,7 @@ /** * @file pid.c * - * Implementation of generic PID control interface. + * Implementation of generic PID controller. * * @author Laurens Mackay * @author Tobias Naegeli @@ -53,24 +53,21 @@ #define SIGMA 0.000001f -__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, - float limit, uint8_t mode, float dt_min) +__EXPORT void pid_init(PID_t *pid, uint8_t mode, float dt_min) { - pid->kp = kp; - pid->ki = ki; - pid->kd = kd; - pid->intmax = intmax; - pid->limit = limit; pid->mode = mode; pid->dt_min = dt_min; - pid->count = 0.0f; - pid->saturated = 0.0f; - pid->last_output = 0.0f; - pid->sp = 0.0f; - pid->error_previous = 0.0f; + pid->kp = 0.0f; + pid->ki = 0.0f; + pid->kd = 0.0f; pid->integral = 0.0f; + pid->integral_limit = 0.0f; + pid->output_limit = 0.0f; + pid->error_previous = 0.0f; + pid->last_output = 0.0f; } -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit) + +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit) { int ret = 0; @@ -95,15 +92,15 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float ret = 1; } - if (isfinite(intmax)) { - pid->intmax = intmax; + if (isfinite(integral_limit)) { + pid->integral_limit = integral_limit; } else { ret = 1; } - if (isfinite(limit)) { - pid->limit = limit; + if (isfinite(output_limit)) { + pid->output_limit = output_limit; } else { ret = 1; @@ -112,42 +109,18 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float return ret; } -//void pid_set(PID_t *pid, float sp) -//{ -// pid->sp = sp; -// pid->error_previous = 0; -// pid->integral = 0; -//} - -/** - * - * @param pid - * @param val - * @param dt - * @return - */ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt) { - /* error = setpoint - actual_position - integral = integral + (error*dt) - derivative = (error - previous_error)/dt - output = (Kp*error) + (Ki*integral) + (Kd*derivative) - previous_error = error - wait(dt) - goto start - */ - if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt)) { return pid->last_output; } float i, d; - pid->sp = sp; - // Calculated current error value - float error = pid->sp - val; + /* current error value */ + float error = sp - val; - // Calculate or measured current error derivative + /* current error derivative */ if (pid->mode == PID_MODE_DERIVATIV_CALC) { d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min); pid->error_previous = error; @@ -167,39 +140,39 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo d = 0.0f; } - if (pid->ki > 0.0f) { - // Calculate the error integral and check for saturation - i = pid->integral + (error * dt); + /* calculate PD output */ + float output = (error * pid->kp) + (d * pid->kd); - if ((pid->limit > SIGMA && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) || - fabsf(i) > pid->intmax) { - i = pid->integral; // If saturated then do not update integral value - pid->saturated = 1; + if (pid->ki > SIGMA) { + /* calculate error integral and check for saturation */ + float i = pid->integral + (error * dt); - } else { - if (!isfinite(i)) { - i = 0.0f; - } + /* fail-safe */ + if (!isfinite(i)) { + i = 0.0f; + } + + if ((pid->output_limit > SIGMA && (fabsf(output + (i * pid->ki)) > pid->output_limit)) || + fabsf(i) > pid->integral_limit) { + /* saturated, do not update integral value */ + i = pid->integral; + } else { pid->integral = i; - pid->saturated = 0; } - } else { - i = 0.0f; - pid->saturated = 0; + /* add I component to output */ + output += i * pid->ki; } - // Calculate the output. Limit output magnitude to pid->limit - float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); - + /* limit output */ if (isfinite(output)) { - if (pid->limit > SIGMA) { - if (output > pid->limit) { - output = pid->limit; + if (pid->output_limit > SIGMA) { + if (output > pid->output_limit) { + output = pid->output_limit; - } else if (output < -pid->limit) { - output = -pid->limit; + } else if (output < -pid->output_limit) { + output = -pid->output_limit; } } @@ -212,5 +185,5 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo __EXPORT void pid_reset_integral(PID_t *pid) { - pid->integral = 0; + pid->integral = 0.0f; } diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h index eca228464..e8b1aac4f 100644 --- a/src/modules/systemlib/pid/pid.h +++ b/src/modules/systemlib/pid/pid.h @@ -39,7 +39,7 @@ /** * @file pid.h * - * Definition of generic PID control interface. + * Definition of generic PID controller. * * @author Laurens Mackay * @author Tobias Naegeli @@ -55,38 +55,35 @@ __BEGIN_DECLS -/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error - * val_dot in pid_calculate() will be ignored */ -#define PID_MODE_DERIVATIV_CALC 0 -/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored - * val_dot in pid_calculate() will be ignored */ -#define PID_MODE_DERIVATIV_CALC_NO_SP 1 -/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */ -#define PID_MODE_DERIVATIV_SET 2 -// Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) -#define PID_MODE_DERIVATIV_NONE 9 +typedef enum PID_MODE { + /* Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) */ + PID_MODE_DERIVATIV_NONE = 0, + /* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error, + * val_dot in pid_calculate() will be ignored */ + PID_MODE_DERIVATIV_CALC, + /* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, + * setpoint derivative will be ignored, val_dot in pid_calculate() will be ignored */ + PID_MODE_DERIVATIV_CALC_NO_SP, + /* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */ + PID_MODE_DERIVATIV_SET +} pid_mode_t; typedef struct { + pid_mode_t mode; + float dt_min; float kp; float ki; float kd; - float intmax; - float sp; float integral; + float integral_limit; + float output_limit; float error_previous; float last_output; - float limit; - float dt_min; - uint8_t mode; - uint8_t count; - uint8_t saturated; } PID_t; -__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode, float dt_min); -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit); -//void pid_set(PID_t *pid, float sp); +__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min); +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit); __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); - __EXPORT void pid_reset_integral(PID_t *pid); __END_DECLS -- cgit v1.2.3