Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 10:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

modify the PID library for AP_Param

Browse Source
This commit is contained in:
Andrew Tridgell 2012-02-11 22:54:10 +11:00
parent 78564f6a92
commit 013d66527d
2 changed files with 22 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/PID/PID.cpp
View File

@ -7,6 +7,14 @@
#include "PID.h"
const AP_Param::GroupInfo PID::var_info[] PROGMEM = {
{ AP_PARAM_FLOAT, "P", VAROFFSET(PID, _kp) },
{ AP_PARAM_FLOAT, "I", VAROFFSET(PID, _ki) },
{ AP_PARAM_FLOAT, "D", VAROFFSET(PID, _kd) },
{ AP_PARAM_INT16, "IMAX", VAROFFSET(PID, _imax) },
{ AP_PARAM_NONE, "" }
};
long
PID::get_pid(int32_t error, uint16_t dt, float scaler)
{
@ -22,7 +30,7 @@ PID::get_pid(int32_t error, uint16_t dt, float scaler)
// discrete low pass filter, cuts out the
// high frequency noise that can drive the controller crazy
float RC = 1/(2*M_PI*_fCut);
float RC = 1/(2*M_PI*20);
derivative = _last_derivative +
(delta_time / (RC + delta_time)) * (derivative - _last_derivative);
@ -62,11 +70,15 @@ PID::reset_I()
void
PID::load_gains()
{
_group.load();
_kp.load();
_ki.load();
_kd.load();
}
void
PID::save_gains()
{
_group.save();
_kp.save();
_ki.save();
_kd.save();
}

69
libraries/PID/PID.h
View File

@ -14,59 +14,10 @@
class PID {
public:
/// 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 key Storage key assigned to this PID. Should be unique.
/// @param name Name by which the PID is known, or NULL for an anonymous PID.
/// The name is prefixed to the P, I, D, IMAX variable names when
/// they are reported.
/// @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
///
PID(AP_Var::Key key,
const prog_char_t *name,
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) :
_group(key, name),
// group, index, initial value, name
_kp (&_group, 0, initial_p, PSTR("P")),
_ki (&_group, 1, initial_i, PSTR("I")),
_kd (&_group, 2, initial_d, PSTR("D")),
_imax(&_group, 3, initial_imax, PSTR("IMAX"))
{
// no need for explicit load, assuming that the main code uses AP_Var::load_all.
}
/// Constructor for PID that does not save its settings.
///
/// @param name Name by which the PID is known, or NULL for an anonymous PID.
/// The name is prefixed to the P, I, D, IMAX variable names when
/// they are reported.
/// @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
///
PID(const prog_char_t *name,
const float &initial_p = 0.0,
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) :
_group(AP_Var::k_key_none, name),
// group, index, initial value, name
_kp (&_group, 0, initial_p, PSTR("P")),
_ki (&_group, 1, initial_i, PSTR("I")),
_kd (&_group, 2, initial_d, PSTR("D")),
_imax(&_group, 3, initial_imax, PSTR("IMAX"))
const int16_t &initial_imax = 0.0)
{
}
@ -120,23 +71,17 @@ public:
float get_integrator() const { return _integrator; }
static const struct AP_Param::GroupInfo var_info[];
private:
AP_Var_group _group;
AP_Float16 _kp;
AP_Float16 _ki;
AP_Float16 _kd;
AP_Float _kp;
AP_Float _ki;
AP_Float _kd;
AP_Int16 _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