Browse Source

AC_PID: update pid_info when integrator set

master
Randy Mackay 5 years ago
parent
commit
0052dcc8f8
  1. 7
      libraries/AC_PID/AC_PID.cpp
  2. 2
      libraries/AC_PID/AC_PID.h

7
libraries/AC_PID/AC_PID.cpp

@ -327,4 +327,11 @@ void AC_PID::set_integrator(float target, float measurement, float i) @@ -327,4 +327,11 @@ void AC_PID::set_integrator(float target, float measurement, float i)
void AC_PID::set_integrator(float error, float i)
{
_integrator = constrain_float(i - error * _kp, -_kimax, _kimax);
_pid_info.I = _integrator;
}
void AC_PID::set_integrator(float i)
{
_integrator = constrain_float(i, -_kimax, _kimax);
_pid_info.I = _integrator;
}

2
libraries/AC_PID/AC_PID.h

@ -101,7 +101,7 @@ public: @@ -101,7 +101,7 @@ public:
// integrator setting functions
void set_integrator(float target, float measurement, float i);
void set_integrator(float error, float i);
void set_integrator(float i) { _integrator = constrain_float(i, -_kimax, _kimax); }
void set_integrator(float i);
const AP_Logger::PID_Info& get_pid_info(void) const { return _pid_info; }

Loading…
Cancel
Save