mirror of https://github.com/ArduPilot/ardupilot
74 lines
1.8 KiB
C++
74 lines
1.8 KiB
C++
// -*- 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
|