You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
55 lines
1.3 KiB
55 lines
1.3 KiB
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- |
|
|
|
/// @file AP_PID.h |
|
/// @brief Generic PID algorithm, with EEPROM-backed storage of constants. |
|
|
|
#ifndef AP_PID_h |
|
#define AP_PID_h |
|
|
|
#include <AP_Common.h> |
|
#include <math.h> // for fabs() |
|
|
|
/// @class AP_PID |
|
/// @brief Object managing one PID control |
|
class AP_PID { |
|
public: |
|
|
|
AP_PID(); |
|
|
|
long get_pid(int32_t error, uint16_t dt, float scaler = 1.0); |
|
|
|
/// Reset the PID integrator |
|
/// |
|
void reset_I(); |
|
|
|
void kP(const float v) { _kp = v; } |
|
void kI(const float v) { _ki = v; } |
|
void kD(const float v) { _kd = v; } |
|
void imax(const int16_t v) { _imax = v; } |
|
|
|
float kP() { return _kp; } |
|
float kI() { return _ki; } |
|
float kD() { return _kd; } |
|
float imax() { return _imax; } |
|
|
|
float get_integrator() const { return _integrator; } |
|
|
|
private: |
|
float _kp; |
|
float _ki; |
|
float _kd; |
|
float _imax; |
|
|
|
float _integrator; ///< integrator value |
|
int32_t _last_error; ///< last error for derivative |
|
float _last_derivative; ///< last derivative for low-pass filter |
|
|
|
/// Low pass filter cut frequency for derivative calculation. |
|
/// |
|
/// 20 Hz becasue anything over that is probably noise, see |
|
/// http://en.wikipedia.org/wiki/Low-pass_filter. |
|
/// |
|
static const uint8_t _fCut = 20; |
|
}; |
|
|
|
#endif
|
|
|