aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

convert AC_PID library to AP_Param

Browse Source
This commit is contained in:
Andrew Tridgell 2012-02-12 22:25:50 +11:00
parent f9eed9bd9e
commit 452749149f
2 changed files with 26 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AC_PID/AC_PID.cpp
View File

@ -6,6 +6,13 @@
#include <math.h> #include <math.h>
#include "AC_PID.h" #include "AC_PID.h"
const AP_Param::GroupInfo AC_PID::var_info[] PROGMEM = {
AP_GROUPINFO("P", AC_PID, _kp),
AP_GROUPINFO("I", AC_PID, _ki),
AP_GROUPINFO("D", AC_PID, _kd),
AP_GROUPINFO("IMAX", AC_PID, _imax),
AP_GROUPEND
};
int32_t AC_PID::get_p(int32_t error) int32_t AC_PID::get_p(int32_t error)
{ {
@ -109,11 +116,17 @@ AC_PID::reset_I()
void void
AC_PID::load_gains() AC_PID::load_gains()
{ {
_group.load(); _kp.load();
_ki.load();
_kd.load();
_imax.load();
} }
void void
AC_PID::save_gains() AC_PID::save_gains()
{ {
_group.save(); _kp.save();
_ki.save();
_kd.save();
_imax.save();
} }

53
libraries/AC_PID/AC_PID.h
View File

@ -19,55 +19,23 @@ public:
/// @note PIDs must be named to avoid either multiple parameters with the /// @note PIDs must be named to avoid either multiple parameters with the
/// same name, or an overly complex constructor. /// 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_p Initial value for the P term.
/// @param initial_i Initial value for the I term. /// @param initial_i Initial value for the I term.
/// @param initial_d Initial value for the D term. /// @param initial_d Initial value for the D term.
/// @param initial_imax Initial value for the imax term.4 /// @param initial_imax Initial value for the imax term.4
/// ///
AC_PID(AP_Var::Key key, AC_PID(
const prog_char_t *name,
const float &initial_p = 0.0, 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), _kp (initial_p),
// group, index, initial value, name _ki (initial_i),
_kp (&_group, 0, initial_p, PSTR("P")), _kd (initial_d),
_ki (&_group, 1, initial_i, PSTR("I")), _imax(initial_imax)
_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. // no need for explicit load, assuming that the main code uses AP_Param::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
///
AC_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"))
{
} }
/// Iterate the PID, return the new control value /// Iterate the PID, return the new control value
@ -125,11 +93,12 @@ 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