diff options
Diffstat (limited to 'src/modules/systemlib/pid')
-rw-r--r-- | src/modules/systemlib/pid/pid.c | 70 | ||||
-rw-r--r-- | src/modules/systemlib/pid/pid.h | 31 |
2 files changed, 55 insertions, 46 deletions
diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 2aafe0636..9308100b0 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -1,9 +1,11 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Laurens Mackay <mackayl@student.ethz.ch> - * @author Tobias Naegeli <naegelit@student.ethz.ch> - * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Laurens Mackay <mackayl@student.ethz.ch> + * Tobias Naegeli <naegelit@student.ethz.ch> + * Martin Rutschmann <rutmarti@student.ethz.ch> + * Anton Babushkin <anton.babushkin@me.com> + * Julian Oes <joes@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,14 +38,21 @@ /** * @file pid.c - * Implementation of generic PID control interface + * + * Implementation of generic PID control interface. + * + * @author Laurens Mackay <mackayl@student.ethz.ch> + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + * @author Julian Oes <joes@student.ethz.ch> */ #include "pid.h" #include <math.h> __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, - float limit, float diff_filter_factor, uint8_t mode) + float limit, uint8_t mode, float dt_min) { pid->kp = kp; pid->ki = ki; @@ -51,17 +60,15 @@ __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, pid->intmax = intmax; pid->limit = limit; pid->mode = mode; - pid->diff_filter_factor = diff_filter_factor; + 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_filtered = 0.0f; - pid->control_previous = 0.0f; + pid->error_previous = 0.0f; pid->integral = 0.0f; } -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor) +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit) { int ret = 0; @@ -100,13 +107,6 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float ret = 1; } - if (isfinite(diff_filter_factor)) { - pid->diff_filter_factor = diff_filter_factor; - - } else { - ret = 1; - } - return ret; } @@ -144,20 +144,16 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculated current error value float error = pid->sp - val; - float error_filtered; // Calculate or measured current error derivative - if (pid->mode == PID_MODE_DERIVATIV_CALC) { + d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min); + pid->error_previous = error; - error_filtered = pid->error_previous_filtered + (error - pid->error_previous_filtered) * pid->diff_filter_factor; - d = (error_filtered - pid->error_previous_filtered) / fmaxf(dt, 0.003f); // fail-safe for too low dt - pid->error_previous_filtered = error_filtered; } else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) { + d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min); + pid->error_previous = -val; - error_filtered = pid->error_previous_filtered + (- val - pid->error_previous_filtered) * pid->diff_filter_factor; - d = (error_filtered - pid->error_previous_filtered) / fmaxf(dt, 0.003f); // fail-safe for too low dt - pid->error_previous_filtered = error_filtered; } else if (pid->mode == PID_MODE_DERIVATIV_SET) { d = -val_dot; @@ -165,6 +161,10 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo d = 0.0f; } + if (!isfinite(d)) { + d = 0.0f; + } + // Calculate the error integral and check for saturation i = pid->integral + (error * dt); @@ -175,7 +175,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo } else { if (!isfinite(i)) { - i = 0; + i = 0.0f; } pid->integral = i; @@ -184,19 +184,17 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculate the output. Limit output magnitude to pid->limit float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); - if (output > pid->limit) output = pid->limit; - - if (output < -pid->limit) output = -pid->limit; if (isfinite(output)) { - pid->last_output = output; - } + if (output > pid->limit) { + output = pid->limit; - pid->control_previous = pid->last_output; + } else if (output < -pid->limit) { + output = -pid->limit; + } - // if (isfinite(error)) { // Why is this necessary? DEW - // pid->error_previous = error; - // } + pid->last_output = output; + } *ctrl_p = (error * pid->kp); *ctrl_i = (i * pid->ki); diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h index eb9df96cc..5f2650f69 100644 --- a/src/modules/systemlib/pid/pid.h +++ b/src/modules/systemlib/pid/pid.h @@ -1,9 +1,11 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Laurens Mackay <mackayl@student.ethz.ch> - * @author Tobias Naegeli <naegelit@student.ethz.ch> - * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Laurens Mackay <mackayl@student.ethz.ch> + * Tobias Naegeli <naegelit@student.ethz.ch> + * Martin Rutschmann <rutmarti@student.ethz.ch> + * Anton Babushkin <anton.babushkin@me.com> + * Julian Oes <joes@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,7 +38,14 @@ /** * @file pid.h - * Definition of generic PID control interface + * + * Definition of generic PID control interface. + * + * @author Laurens Mackay <mackayl@student.ethz.ch> + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + * @author Julian Oes <joes@student.ethz.ch> */ #ifndef PID_H_ @@ -44,6 +53,8 @@ #include <stdint.h> +__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 @@ -62,22 +73,22 @@ typedef struct { float intmax; float sp; float integral; - float error_previous_filtered; - float control_previous; + float error_previous; float last_output; float limit; + float dt_min; uint8_t mode; - float diff_filter_factor; 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, float diff_filter_factor, uint8_t mode); -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor); +__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 float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt, float *ctrl_p, float *ctrl_i, float *ctrl_d); __EXPORT void pid_reset_integral(PID_t *pid); +__END_DECLS #endif /* PID_H_ */ |