aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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" #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 long
PID::get_pid(int32_t error, uint16_t dt, float scaler) 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 // discrete low pass filter, cuts out the
// high frequency noise that can drive the controller crazy // 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 + derivative = _last_derivative +
(delta_time / (RC + delta_time)) * (derivative - _last_derivative); (delta_time / (RC + delta_time)) * (derivative - _last_derivative);
@ -62,11 +70,15 @@ PID::reset_I()
void void
PID::load_gains() PID::load_gains()
{ {
_group.load(); _kp.load();
_ki.load();
_kd.load();
} }
void void
PID::save_gains() PID::save_gains()
{ {
_group.save(); _kp.save();
_ki.save();
_kd.save();
} }

69
libraries/PID/PID.h
View File

@ -14,59 +14,10 @@
class PID { class PID {
public: public:
/// Constructor for PID that saves its settings to EEPROM PID(const float &initial_p = 0.0,
///
/// @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_i = 0.0,
const float &initial_d = 0.0, const float &initial_d = 0.0,
const int16_t &initial_imax = 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,
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"))
{ {
} }
@ -120,23 +71,17 @@ public:
float get_integrator() const { return _integrator; } float get_integrator() const { return _integrator; }
static const struct AP_Param::GroupInfo var_info[];
private: private:
AP_Var_group _group; AP_Float _kp;
AP_Float16 _kp; AP_Float _ki;
AP_Float16 _ki; AP_Float _kd;
AP_Float16 _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
/// 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 #endif