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