AC_PID.h 4.8 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136
  1. #pragma once
  2. /// @file AC_PID.h
  3. /// @brief Generic PID algorithm, with EEPROM-backed storage of constants.
  4. #include <AP_Common/AP_Common.h>
  5. #include <AP_Param/AP_Param.h>
  6. #include <stdlib.h>
  7. #include <cmath>
  8. #include <AP_Logger/AP_Logger.h>
  9. #define AC_PID_TFILT_HZ_DEFAULT 0.0f // default input filter frequency
  10. #define AC_PID_EFILT_HZ_DEFAULT 0.0f // default input filter frequency
  11. #define AC_PID_DFILT_HZ_DEFAULT 20.0f // default input filter frequency
  12. /// @class AC_PID
  13. /// @brief Copter PID control class
  14. class AC_PID {
  15. public:
  16. // Constructor for PID
  17. AC_PID(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_T_hz, float initial_filt_E_hz, float initial_filt_D_hz, float dt);
  18. // set_dt - set time step in seconds
  19. void set_dt(float dt);
  20. // update_all - set target and measured inputs to PID controller and calculate outputs
  21. // target and error are filtered
  22. // the derivative is then calculated and filtered
  23. // the integral is then updated based on the setting of the limit flag
  24. float update_all(float target, float measurement, bool limit = false);
  25. // update_error - set error input to PID controller and calculate outputs
  26. // target is set to zero and error is set and filtered
  27. // the derivative then is calculated and filtered
  28. // the integral is then updated based on the setting of the limit flag
  29. // Target and Measured must be set manually for logging purposes.
  30. // todo: remove function when it is no longer used.
  31. float update_error(float error, bool limit = false);
  32. // update_i - update the integral
  33. // if the limit flag is set the integral is only allowed to shrink
  34. void update_i(bool limit);
  35. // get_pid - get results from pid controller
  36. float get_pid() const;
  37. float get_pi() const;
  38. float get_p() const;
  39. float get_i() const;
  40. float get_d() const;
  41. float get_ff();
  42. // todo: remove function when it is no longer used.
  43. float get_ff(float target);
  44. // reset_I - reset the integrator
  45. void reset_I();
  46. // reset_filter - input filter will be reset to the next value provided to set_input()
  47. void reset_filter() {
  48. _flags._reset_filter = true;
  49. }
  50. // load gain from eeprom
  51. void load_gains();
  52. // save gain to eeprom
  53. void save_gains();
  54. /// operator function call for easy initialisation
  55. void operator()(float p_val, float i_val, float d_val, float ff_val, float imax_val, float input_filt_T_hz, float input_filt_E_hz, float input_filt_D_hz, float dt);
  56. // get accessors
  57. AP_Float &kP() { return _kp; }
  58. AP_Float &kI() { return _ki; }
  59. AP_Float &kD() { return _kd; }
  60. AP_Float &ff() { return _kff;}
  61. AP_Float &filt_T_hz() { return _filt_T_hz; }
  62. AP_Float &filt_E_hz() { return _filt_E_hz; }
  63. AP_Float &filt_D_hz() { return _filt_D_hz; }
  64. float imax() const { return _kimax.get(); }
  65. float get_filt_alpha(float filt_hz) const;
  66. float get_filt_T_alpha() const;
  67. float get_filt_E_alpha() const;
  68. float get_filt_D_alpha() const;
  69. // set accessors
  70. void kP(const float v) { _kp.set(v); }
  71. void kI(const float v) { _ki.set(v); }
  72. void kD(const float v) { _kd.set(v); }
  73. void ff(const float v) { _kff.set(v); }
  74. void imax(const float v) { _kimax.set(fabsf(v)); }
  75. void filt_T_hz(const float v);
  76. void filt_E_hz(const float v);
  77. void filt_D_hz(const float v);
  78. // set the desired and actual rates (for logging purposes)
  79. void set_target_rate(float target) { _pid_info.target = target; }
  80. void set_actual_rate(float actual) { _pid_info.actual = actual; }
  81. // integrator setting functions
  82. void set_integrator(float target, float measurement, float i);
  83. void set_integrator(float error, float i);
  84. void set_integrator(float i) { _integrator = constrain_float(i, -_kimax, _kimax); }
  85. const AP_Logger::PID_Info& get_pid_info(void) const { return _pid_info; }
  86. // parameter var table
  87. static const struct AP_Param::GroupInfo var_info[];
  88. protected:
  89. // parameters
  90. AP_Float _kp;
  91. AP_Float _ki;
  92. AP_Float _kd;
  93. AP_Float _kff;
  94. AP_Float _kimax;
  95. AP_Float _filt_T_hz; // PID target filter frequency in Hz
  96. AP_Float _filt_E_hz; // PID error filter frequency in Hz
  97. AP_Float _filt_D_hz; // PID derivative filter frequency in Hz
  98. // flags
  99. struct ac_pid_flags {
  100. bool _reset_filter :1; // true when input filter should be reset during next call to set_input
  101. } _flags;
  102. // internal variables
  103. float _dt; // timestep in seconds
  104. float _integrator; // integrator value
  105. float _target; // target value to enable filtering
  106. float _error; // error value to enable filtering
  107. float _derivative; // derivative value to enable filtering
  108. AP_Logger::PID_Info _pid_info;
  109. };