|
|
|
@ -7,49 +7,67 @@
@@ -7,49 +7,67 @@
|
|
|
|
|
#define AP_PID_h |
|
|
|
|
|
|
|
|
|
#include <AP_Common.h> |
|
|
|
|
#include <math.h> // for fabs() |
|
|
|
|
#include <math.h> // for fabs() |
|
|
|
|
|
|
|
|
|
/// @class AP_PID
|
|
|
|
|
/// @brief Object managing one PID control
|
|
|
|
|
class AP_PID { |
|
|
|
|
public: |
|
|
|
|
|
|
|
|
|
AP_PID(); |
|
|
|
|
AP_PID(); |
|
|
|
|
|
|
|
|
|
long get_pid(int32_t error, uint16_t dt, float scaler = 1.0); |
|
|
|
|
long get_pid(int32_t error, uint16_t dt, float scaler = 1.0); |
|
|
|
|
|
|
|
|
|
/// Reset the PID integrator
|
|
|
|
|
///
|
|
|
|
|
void reset_I(); |
|
|
|
|
/// 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; } |
|
|
|
|
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 kP() { |
|
|
|
|
return _kp; |
|
|
|
|
} |
|
|
|
|
float kI() { |
|
|
|
|
return _ki; |
|
|
|
|
} |
|
|
|
|
float kD() { |
|
|
|
|
return _kd; |
|
|
|
|
} |
|
|
|
|
float imax() { |
|
|
|
|
return _imax; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
float get_integrator() const { return _integrator; } |
|
|
|
|
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; |
|
|
|
|
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 |
|
|
|
|