zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

uncrustify libraries/AC_PID/AC_PID.h

mission-4.1.18
uncrustify 13 years ago committed by Pat Hickey
parent
fc9f1a8216
commit
6c5757e775
1 changed files with 117 additions and 97 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 214
      libraries/AC_PID/AC_PID.h

214
libraries/AC_PID/AC_PID.h
Unescape View File

@ -7,114 +7,134 @@
#define AC_PID_h #define AC_PID_h
#include <AP_Common.h> #include <AP_Common.h>
#include <math.h> // for fabs() #include <math.h> // for fabs()
/// @class AC_PID /// @class AC_PID
/// @brief Object managing one PID control /// @brief Object managing one PID control
class AC_PID { class AC_PID {
public: public:
/// Constructor for PID that saves its settings to EEPROM /// Constructor for PID that saves its settings to EEPROM
/// ///
/// @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 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( AC_PID(
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)
{ {
_kp = initial_p; _kp = initial_p;
_ki = initial_i; _ki = initial_i;
_kd = initial_d; _kd = initial_d;
_imax = abs(initial_imax); _imax = abs(initial_imax);
} }
/// Iterate the PID, return the new control value /// Iterate the PID, return the new control value
/// ///
/// Positive error produces positive output. /// Positive error produces positive output.
/// ///
/// @param error The measured error value /// @param error The measured error value
/// @param dt The time delta in milliseconds (note /// @param dt The time delta in milliseconds (note
/// that update interval cannot be more /// that update interval cannot be more
/// than 65.535 seconds due to limited range /// than 65.535 seconds due to limited range
/// of the data type). /// of the data type).
/// @param scaler An arbitrary scale factor /// @param scaler An arbitrary scale factor
/// ///
/// @returns The updated control output. /// @returns The updated control output.
/// ///
int32_t get_pid(int32_t error, float dt); int32_t get_pid(int32_t error, float dt);
int32_t get_pi(int32_t error, float dt); int32_t get_pi(int32_t error, float dt);
int32_t get_p(int32_t error); int32_t get_p(int32_t error);
int32_t get_i(int32_t error, float dt); int32_t get_i(int32_t error, float dt);
int32_t get_d(int32_t error, float dt); int32_t get_d(int32_t error, float dt);
/// Reset the PID integrator /// Reset the PID integrator
/// ///
void reset_I(); void reset_I();
/// Load gain properties /// Load gain properties
/// ///
void load_gains(); void load_gains();
/// Save gain properties /// Save gain properties
/// ///
void save_gains(); void save_gains();
/// @name parameter accessors /// @name parameter accessors
//@{ //@{
/// Overload the function call operator to permit relatively easy initialisation /// Overload the function call operator to permit relatively easy initialisation
void operator() (const float p, void operator () (const float p,
const float i, const float i,
const float d, const float d,
const int16_t imaxval) { const int16_t imaxval) {
_kp = p; _ki = i; _kd = d; _imax = abs(imaxval); _kp = p; _ki = i; _kd = d; _imax = abs(imaxval);
} }
float kP() const { return _kp.get(); } float kP() const {
float kI() const { return _ki.get(); } return _kp.get();
float kD() const { return _kd.get(); } }
int16_t imax() const { return _imax.get(); } float kI() const {
return _ki.get();
void kP(const float v) { _kp.set(v); } }
void kI(const float v) { _ki.set(v); } float kD() const {
void kD(const float v) { _kd.set(v); } return _kd.get();
void imax(const int16_t v) { _imax.set(abs(v)); } }
int16_t imax() const {
float get_integrator() const { return _integrator; } return _imax.get();
void set_integrator(float i) { _integrator = i; } }
static const struct AP_Param::GroupInfo var_info[]; 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;
}
static const struct AP_Param::GroupInfo var_info[];
private: private:
AP_Float _kp; AP_Float _kp;
AP_Float _ki; AP_Float _ki;
AP_Float _kd; AP_Float _kd;
AP_Int16 _imax; AP_Int16 _imax;
float _integrator; ///< integrator value float _integrator; ///< integrator value
int32_t _last_input; ///< last input for derivative int32_t _last_input; ///< last input for derivative
float _last_derivative; ///< last derivative for low-pass filter float _last_derivative; ///< last derivative for low-pass filter
float _output; float _output;
float _derivative; float _derivative;
/// Low pass filter cut frequency for derivative calculation. /// Low pass filter cut frequency for derivative calculation.
/// ///
static const float _filter = 7.9577e-3; // Set to "1 / ( 2 * PI * f_cut )"; static const float _filter = 7.9577e-3; // Set to "1 / ( 2 * PI * f_cut )";
// Examples for _filter: // Examples for _filter:
// f_cut = 10 Hz -> _filter = 15.9155e-3 // f_cut = 10 Hz -> _filter = 15.9155e-3
// f_cut = 15 Hz -> _filter = 10.6103e-3 // f_cut = 15 Hz -> _filter = 10.6103e-3
// f_cut = 20 Hz -> _filter = 7.9577e-3 // f_cut = 20 Hz -> _filter = 7.9577e-3
// f_cut = 25 Hz -> _filter = 6.3662e-3 // f_cut = 25 Hz -> _filter = 6.3662e-3
// f_cut = 30 Hz -> _filter = 5.3052e-3 // f_cut = 30 Hz -> _filter = 5.3052e-3
}; };
#endif #endif

Write Preview
Loading…
Cancel
Save