Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-07 00:13:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

uncrustify libraries/PID/PID.h

Browse Source
This commit is contained in:
uncrustify 2012-08-16 23:22:44 -07:00 committed by Pat Hickey
parent 330c6c07e3
commit 13e0dd2c70
1 changed files with 85 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

152
libraries/PID/PID.h
View File

@ -7,95 +7,113 @@
#define PID_h #define PID_h
#include <AP_Common.h> #include <AP_Common.h>
#include <math.h> // for fabs() #include <math.h> // for fabs()
/// @class PID /// @class PID
/// @brief Object managing one PID control /// @brief Object managing one PID control
class PID { class PID {
public: public:
PID(const float &initial_p = 0.0, PID(const float & initial_p = 0.0,
const float &initial_i = 0.0, const float & initial_i = 0.0,
const float &initial_d = 0.0, const float & initial_d = 0.0,
const int16_t &initial_imax = 0) const int16_t & initial_imax = 0)
{ {
_kp = initial_p; _kp = initial_p;
_ki = initial_i; _ki = initial_i;
_kd = initial_d; _kd = initial_d;
_imax = initial_imax; _imax = initial_imax;
} }
/// Iterate the PID, return the new control value /// Iterate the PID, return the new control value
/// ///
/// Positive error produces positive output. /// Positive error produces positive output.
/// ///
/// @param error The measured error value /// @param error The measured error value
/// @param dt The time delta in milliseconds (note /// @param dt The time delta in milliseconds (note
/// that update interval cannot be more /// that update interval cannot be more
/// than 65.535 seconds due to limited range /// than 65.535 seconds due to limited range
/// of the data type). /// of the data type).
/// @param scaler An arbitrary scale factor /// @param scaler An arbitrary scale factor
/// ///
/// @returns The updated control output. /// @returns The updated control output.
/// ///
int32_t get_pid(int32_t error, float scaler = 1.0); int32_t get_pid(int32_t error, float scaler = 1.0);
/// Reset the PID integrator /// Reset the PID integrator
/// ///
void reset_I(); void reset_I();
/// Load gain properties /// Load gain properties
/// ///
void load_gains(); void load_gains();
/// Save gain properties /// Save gain properties
/// ///
void save_gains(); void save_gains();
/// @name parameter accessors /// @name parameter accessors
//@{ //@{
/// Overload the function call operator to permit relatively easy initialisation /// Overload the function call operator to permit relatively easy initialisation
void operator() (const float p, void operator () (const float p,
const float i, const float i,
const float d, const float d,
const int16_t imaxval) { const int16_t imaxval) {
_kp = p; _ki = i; _kd = d; _imax = imaxval; _kp = p; _ki = i; _kd = d; _imax = imaxval;
} }
float kP() const { return _kp.get(); } float kP() const {
float kI() const { return _ki.get(); } return _kp.get();
float kD() const { return _kd.get(); } }
int16_t imax() const { return _imax.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 kP(const float v) {
void kI(const float v) { _ki.set(v); } _kp.set(v);
void kD(const float v) { _kd.set(v); } }
void imax(const int16_t v) { _imax.set(abs(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; } float get_integrator() const {
return _integrator;
}
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
private: private:
AP_Float _kp; AP_Float _kp;
AP_Float _ki; AP_Float _ki;
AP_Float _kd; AP_Float _kd;
AP_Int16 _imax; AP_Int16 _imax;
float _integrator; ///< integrator value float _integrator; ///< integrator value
int32_t _last_error; ///< last error for derivative int32_t _last_error; ///< last error for derivative
float _last_derivative; ///< last derivative for low-pass filter float _last_derivative; ///< last derivative for low-pass filter
uint32_t _last_t; uint32_t _last_t;
int32_t _get_pid(int32_t error, uint16_t dt, float scaler); int32_t _get_pid(int32_t error, uint16_t dt, float scaler);
/// Low pass filter cut frequency for derivative calculation. /// Low pass filter cut frequency for derivative calculation.
/// ///
/// 20 Hz becasue anything over that is probably noise, see /// 20 Hz becasue anything over that is probably noise, see
/// http://en.wikipedia.org/wiki/Low-pass_filter. /// http://en.wikipedia.org/wiki/Low-pass_filter.
/// ///
static const uint8_t _fCut = 20; static const uint8_t _fCut = 20;
}; };
#endif #endif