aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_PID: more protection against NaN and Inf

Browse Source
This commit is contained in:
Randy Mackay 2015-04-09 12:06:07 +09:00
parent bdfe8bbc54
commit a095a8c3a1
2 changed files with 26 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
libraries/AC_PID/AC_PID.cpp
View File

@ -84,21 +84,23 @@ void AC_PID::filt_hz(float hz)
// this should be called before any other calls to get_p, get_i or get_d // this should be called before any other calls to get_p, get_i or get_d
void AC_PID::set_input_filter_all(float input) void AC_PID::set_input_filter_all(float input)
{ {
// don't pass in inf or NaN // don't process inf or NaN
if (isfinite(input)){ if (!isfinite(input)) {
// reset input filter to value received return;
if (_flags._reset_filter) { }
_flags._reset_filter = false;
_input = input;
_derivative = 0.0f;
}
// update filter and calculate derivative // reset input filter to value received
float input_filt_change = _filt_alpha * (input - _input); if (_flags._reset_filter) {
_input = _input + input_filt_change; _flags._reset_filter = false;
if (_dt > 0.0f) { _input = input;
_derivative = input_filt_change / _dt; _derivative = 0.0f;
} }
// update filter and calculate derivative
float input_filt_change = _filt_alpha * (input - _input);
_input = _input + input_filt_change;
if (_dt > 0.0f) {
_derivative = input_filt_change / _dt;
} }
} }
@ -107,6 +109,11 @@ void AC_PID::set_input_filter_all(float input)
// this should be called before any other calls to get_p, get_i or get_d // this should be called before any other calls to get_p, get_i or get_d
void AC_PID::set_input_filter_d(float input) void AC_PID::set_input_filter_d(float input)
{ {
// don't process inf or NaN
if (!isfinite(input)) {
return;
}
// reset input filter to value received // reset input filter to value received
if (_flags._reset_filter) { if (_flags._reset_filter) {
_flags._reset_filter = false; _flags._reset_filter = false;

5
libraries/AC_PID/AC_PI_2D.cpp
View File

@ -72,6 +72,11 @@ void AC_PI_2D::filt_hz(float hz)
// this should be called before any other calls to get_p, get_i or get_d // this should be called before any other calls to get_p, get_i or get_d
void AC_PI_2D::set_input(const Vector2f &input) void AC_PI_2D::set_input(const Vector2f &input)
{ {
// don't process inf or NaN
if (!isfinite(input.x) || !isfinite(input.y)) {
return;
}
// reset input filter to value received // reset input filter to value received
if (_flags._reset_filter) { if (_flags._reset_filter) {
_flags._reset_filter = false; _flags._reset_filter = false;