diff --git a/libraries/AC_PID/AC_P.cpp b/libraries/AC_PID/AC_P.cpp new file mode 100644 index 0000000000..a8d3054d69 --- /dev/null +++ b/libraries/AC_PID/AC_P.cpp @@ -0,0 +1,30 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/// @file AC_P.cpp +/// @brief Generic P algorithm + +#include +#include "AC_P.h" + +const AP_Param::GroupInfo AC_P::var_info[] PROGMEM = { + // @Param: P + // @DisplayName: PI Proportional Gain + // @Description: P Gain which produces an output value that is proportional to the current error value + AP_GROUPINFO("P", 0, AC_P, _kp, 0), + AP_GROUPEND +}; + +float AC_P::get_p(float error) const +{ + return (float)error * _kp; +} + +void AC_P::load_gains() +{ + _kp.load(); +} + +void AC_P::save_gains() +{ + _kp.save(); +} diff --git a/libraries/AC_PID/AC_P.h b/libraries/AC_PID/AC_P.h new file mode 100644 index 0000000000..be310b7380 --- /dev/null +++ b/libraries/AC_PID/AC_P.h @@ -0,0 +1,71 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/// @file AC_PD.h +/// @brief Generic PID algorithm, with EEPROM-backed storage of constants. + +#ifndef __AC_P_H__ +#define __AC_P_H__ + +#include +#include +#include +#include + +/// @class AC_P +/// @brief Object managing one P controller +class AC_P { +public: + + /// Constructor for P that saves its settings to EEPROM + /// + /// @note PIs 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. + /// + AC_P( + const float & initial_p = 0.0) + { + AP_Param::setup_object_defaults(this, var_info); + _kp = initial_p; + } + + /// Iterate the P controller, 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). + /// + /// @returns The updated control output. + /// + float get_p(float error) const; + + /// 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) { _kp = p; } + + // accessors + float kP() const { return _kp.get(); } + void kP(const float v) { _kp.set(v); } + + static const struct AP_Param::GroupInfo var_info[]; + +private: + AP_Float _kp; +}; + +#endif // __AC_P_H__