Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_PID: AC_PID_2D integrates SlewCalculator2D

This commit is contained in:
Randy Mackay 2023-06-02 13:24:09 +09:00 committed by Andrew Tridgell
parent 07ecbd18ec
commit 99bfcb7498
2 changed files with 10 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AC_PID/AC_PID_2D.cpp
View File

@ -97,6 +97,10 @@ Vector2f AC_PID_2D::update_all(const Vector2f &target, const Vector2f &measureme
// update I term // update I term
update_i(dt, limit); update_i(dt, limit);
// calculate slew limit
_slew_calc.update(Vector2f{_pid_info_x.P + _pid_info_x.D, _pid_info_y.P + _pid_info_y.D}, dt);
_pid_info_x.slew_rate = _pid_info_y.slew_rate = _slew_calc.get_slew_rate();
_pid_info_x.target = _target.x; _pid_info_x.target = _target.x;
_pid_info_x.actual = measurement.x; _pid_info_x.actual = measurement.x;
_pid_info_x.error = _error.x; _pid_info_x.error = _error.x;

6
libraries/AC_PID/AC_PID_2D.h
View File

@ -8,6 +8,7 @@
#include <stdlib.h> #include <stdlib.h>
#include <cmath> #include <cmath>
#include <AC_PID/AP_PIDInfo.h> #include <AC_PID/AP_PIDInfo.h>
#include <Filter/SlewCalculator2D.h>
/// @class AC_PID_2D /// @class AC_PID_2D
/// @brief Copter PID control class /// @brief Copter PID control class
@ -72,6 +73,9 @@ public:
void set_integrator(const Vector3f& i) { set_integrator(Vector2f{i.x, i.y}); } void set_integrator(const Vector3f& i) { set_integrator(Vector2f{i.x, i.y}); }
void set_integrator(const Vector2f& i); void set_integrator(const Vector2f& i);
// return current slew rate of slew limiter. Will return 0 if SMAX is zero
float get_slew_rate(void) const { return _slew_calc.get_slew_rate(); }
const AP_PIDInfo& get_pid_info_x(void) const { return _pid_info_x; } const AP_PIDInfo& get_pid_info_x(void) const { return _pid_info_x; }
const AP_PIDInfo& get_pid_info_y(void) const { return _pid_info_y; } const AP_PIDInfo& get_pid_info_y(void) const { return _pid_info_y; }
@ -99,6 +103,8 @@ protected:
AP_PIDInfo _pid_info_x; AP_PIDInfo _pid_info_x;
AP_PIDInfo _pid_info_y; AP_PIDInfo _pid_info_y;
SlewCalculator2D _slew_calc; // 2D slew rate calculator
private: private:
const float default_kp; const float default_kp;
const float default_ki; const float default_ki;