ardupilot/libraries/AC_PID/AC_PID.h

123 lines
3.9 KiB
C
Raw Normal View History

2013-05-29 20:54:53 -03:00
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/// @file AC_PID.h
/// @brief Generic PID algorithm, with EEPROM-backed storage of constants.
2012-10-09 18:01:58 -03:00
#ifndef __AC_PID_H__
#define __AC_PID_H__
#include <AP_Common.h>
#include <AP_Param.h>
2012-10-09 18:01:58 -03:00
#include <stdlib.h>
2012-08-17 02:37:26 -03:00
#include <math.h> // for fabs()
2014-02-14 02:52:24 -04:00
// Examples for _filter:
// f_cut = 10 Hz -> _filter = 15.9155e-3
// f_cut = 15 Hz -> _filter = 10.6103e-3
// f_cut = 20 Hz -> _filter = 7.9577e-3
// f_cut = 25 Hz -> _filter = 6.3662e-3
// f_cut = 30 Hz -> _filter = 5.3052e-3
#define AC_PID_D_TERM_FILTER 0.00795770f // 20hz filter on D term
/// @class AC_PID
/// @brief Object managing one PID control
class AC_PID {
public:
2012-08-17 02:37:26 -03:00
/// Constructor for PID that saves its settings to EEPROM
///
/// @note PIDs must be named to avoid either multiple parameters with the
/// same name, or an overly complex constructor.
///
/// @param initial_p Initial value for the P term.
/// @param initial_i Initial value for the I term.
/// @param initial_d Initial value for the D term.
/// @param initial_imax Initial value for the imax term.4
2012-08-17 02:37:26 -03:00
///
AC_PID(
const float & initial_p = 0.0,
const float & initial_i = 0.0,
const float & initial_d = 0.0,
const int16_t & initial_imax = 0.0)
{
2012-12-12 17:41:28 -04:00
AP_Param::setup_object_defaults(this, var_info);
2012-08-17 02:37:26 -03:00
_kp = initial_p;
_ki = initial_i;
_kd = initial_d;
_imax = abs(initial_imax);
// derivative is invalid on startup
_last_derivative = NAN;
2012-08-17 02:37:26 -03:00
}
/// Iterate the PID, return the new control value
///
/// Positive error produces positive output.
///
/// @param error The measured error value
/// @param dt The time delta in milliseconds (note
/// that update interval cannot be more
/// than 65.535 seconds due to limited range
/// of the data type).
/// @param scaler An arbitrary scale factor
///
/// @returns The updated control output.
///
2014-02-14 02:52:24 -04:00
float get_pid(float error, float dt);
float get_pi(float error, float dt);
float get_p(float error) const;
float get_i(float error, float dt);
float get_d(float error, float dt);
float get_leaky_i(float error, float dt, float leak_rate);
2012-08-17 02:37:26 -03:00
/// Reset the PID integrator
///
void reset_I();
/// Load gain properties
///
void load_gains();
/// Save gain properties
///
void save_gains();
/// @name parameter accessors
//@{
/// Overload the function call operator to permit relatively easy initialisation
void operator () (const float p,
const float i,
const float d,
const int16_t imaxval) {
_kp = p; _ki = i; _kd = d; _imax = abs(imaxval);
}
2014-02-14 02:52:24 -04:00
// accessors
float kP() const { return _kp.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 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; }
void set_integrator(float i) { _integrator = i; }
2012-08-17 02:37:26 -03:00
static const struct AP_Param::GroupInfo var_info[];
2012-02-12 07:25:50 -04:00
private:
2012-08-17 02:37:26 -03:00
AP_Float _kp;
AP_Float _ki;
AP_Float _kd;
AP_Int16 _imax;
float _integrator; ///< integrator value
2014-02-14 02:52:24 -04:00
float _last_input; ///< last input for derivative
2012-08-17 02:37:26 -03:00
float _last_derivative; ///< last derivative for low-pass filter
};
2012-10-09 18:01:58 -03:00
#endif // __AC_PID_H__