2010-11-25 21:30:21 -04:00
|
|
|
/// @file PID.h
|
|
|
|
/// @brief Generic PID algorithm, with EEPROM-backed storage of constants.
|
2016-02-17 21:25:56 -04:00
|
|
|
#pragma once
|
2010-10-28 01:59:24 -03:00
|
|
|
|
2015-08-11 03:28:46 -03:00
|
|
|
#include <AP_Common/AP_Common.h>
|
|
|
|
#include <AP_Param/AP_Param.h>
|
2022-03-03 23:29:48 -04:00
|
|
|
#include <AC_PID/AC_PID.h> // for AP_PIDInfo
|
2012-11-30 23:35:01 -04:00
|
|
|
#include <stdlib.h>
|
2016-03-31 18:43:36 -03:00
|
|
|
#include <cmath>
|
2010-10-28 01:59:24 -03:00
|
|
|
|
2010-11-25 21:30:21 -04:00
|
|
|
/// @class PID
|
|
|
|
/// @brief Object managing one PID control
|
2010-10-28 01:59:24 -03:00
|
|
|
class PID {
|
|
|
|
public:
|
2010-11-27 16:30:53 -04:00
|
|
|
|
2015-04-24 02:01:39 -03:00
|
|
|
PID(const float & initial_p = 0.0f,
|
|
|
|
const float & initial_i = 0.0f,
|
|
|
|
const float & initial_d = 0.0f,
|
2012-08-17 03:22:44 -03:00
|
|
|
const int16_t & initial_imax = 0)
|
2011-02-14 00:45:31 -04:00
|
|
|
{
|
2012-12-12 17:48:08 -04:00
|
|
|
AP_Param::setup_object_defaults(this, var_info);
|
2022-07-04 23:50:46 -03:00
|
|
|
_kp.set_and_default(initial_p);
|
|
|
|
_ki.set_and_default(initial_i);
|
|
|
|
_kd.set_and_default(initial_d);
|
|
|
|
_imax.set_and_default(initial_imax);
|
2012-11-27 03:41:31 -04:00
|
|
|
|
|
|
|
// set _last_derivative as invalid when we startup
|
|
|
|
_last_derivative = NAN;
|
2012-08-17 03:22:44 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
/// Iterate the PID, return the new control value
|
|
|
|
///
|
|
|
|
/// Positive error produces positive output.
|
|
|
|
///
|
|
|
|
/// @param error The measured error value
|
|
|
|
/// @param scaler An arbitrary scale factor
|
|
|
|
///
|
|
|
|
/// @returns The updated control output.
|
|
|
|
///
|
2013-02-09 05:36:13 -04:00
|
|
|
float get_pid(float error, float scaler = 1.0);
|
2012-08-17 03:22:44 -03:00
|
|
|
|
2017-10-12 19:26:22 -03:00
|
|
|
/// Reset the whole PID state
|
|
|
|
//
|
|
|
|
void reset();
|
|
|
|
|
2012-08-17 03:22:44 -03:00
|
|
|
/// Reset the PID integrator
|
|
|
|
///
|
|
|
|
void reset_I();
|
|
|
|
|
|
|
|
/// Load gain properties
|
|
|
|
///
|
|
|
|
void load_gains();
|
|
|
|
|
|
|
|
/// Save gain properties
|
|
|
|
///
|
|
|
|
void save_gains();
|
|
|
|
|
|
|
|
/// @name parameter accessors
|
|
|
|
//@{
|
|
|
|
|
|
|
|
/// Overload the function call operator to permit relatively easy initialisation
|
|
|
|
void operator () (const float p,
|
|
|
|
const float i,
|
|
|
|
const float d,
|
|
|
|
const int16_t imaxval) {
|
2022-07-05 00:08:57 -03:00
|
|
|
_kp.set(p); _ki.set(i); _kd.set(d); _imax.set(imaxval);
|
2012-08-17 03:22:44 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
float kP() const {
|
|
|
|
return _kp.get();
|
|
|
|
}
|
|
|
|
float kI() const {
|
|
|
|
return _ki.get();
|
|
|
|
}
|
|
|
|
float kD() const {
|
|
|
|
return _kd.get();
|
|
|
|
}
|
|
|
|
int16_t imax() const {
|
|
|
|
return _imax.get();
|
|
|
|
}
|
|
|
|
|
|
|
|
void kP(const float v) {
|
|
|
|
_kp.set(v);
|
|
|
|
}
|
|
|
|
void kI(const float v) {
|
|
|
|
_ki.set(v);
|
|
|
|
}
|
|
|
|
void kD(const float v) {
|
|
|
|
_kd.set(v);
|
|
|
|
}
|
|
|
|
void imax(const int16_t v) {
|
|
|
|
_imax.set(abs(v));
|
|
|
|
}
|
|
|
|
|
|
|
|
float get_integrator() const {
|
|
|
|
return _integrator;
|
|
|
|
}
|
|
|
|
|
|
|
|
static const struct AP_Param::GroupInfo var_info[];
|
2012-02-11 07:54:10 -04:00
|
|
|
|
2022-03-03 23:29:48 -04:00
|
|
|
const AP_PIDInfo& get_pid_info(void) const { return _pid_info; }
|
2016-05-30 03:48:40 -03:00
|
|
|
|
2011-02-14 00:45:31 -04:00
|
|
|
private:
|
2012-08-17 03:22:44 -03:00
|
|
|
AP_Float _kp;
|
|
|
|
AP_Float _ki;
|
|
|
|
AP_Float _kd;
|
|
|
|
AP_Int16 _imax;
|
|
|
|
|
2012-11-30 23:35:01 -04:00
|
|
|
float _integrator;///< integrator value
|
2013-02-09 05:36:13 -04:00
|
|
|
float _last_error;///< last error for derivative
|
2012-11-30 23:35:01 -04:00
|
|
|
float _last_derivative;///< last derivative for low-pass filter
|
|
|
|
uint32_t _last_t;///< last time get_pid() was called in millis
|
2012-08-17 03:22:44 -03:00
|
|
|
|
2013-02-09 05:36:13 -04:00
|
|
|
float _get_pid(float error, uint16_t dt, float scaler);
|
2012-08-17 03:22:44 -03:00
|
|
|
|
2022-03-03 23:29:48 -04:00
|
|
|
AP_PIDInfo _pid_info {};
|
2016-05-30 03:48:40 -03:00
|
|
|
|
2012-08-17 03:22:44 -03:00
|
|
|
/// Low pass filter cut frequency for derivative calculation.
|
|
|
|
///
|
2016-05-12 13:50:41 -03:00
|
|
|
/// 20 Hz because anything over that is probably noise, see
|
2012-08-17 03:22:44 -03:00
|
|
|
/// http://en.wikipedia.org/wiki/Low-pass_filter.
|
|
|
|
///
|
|
|
|
static const uint8_t _fCut = 20;
|
2010-10-28 01:59:24 -03:00
|
|
|
};
|