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

AC_PID: more protection against NaN and Inf

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
void AC_PID::set_input_filter_all(float input)
{
// don't pass in inf or NaN
if (isfinite(input)){
// reset input filter to value received
if (_flags._reset_filter) {
_flags._reset_filter = false;
_input = input;
_derivative = 0.0f;
}
// don't process inf or NaN
if (!isfinite(input)) {
return;
}
// 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;
}
// reset input filter to value received
if (_flags._reset_filter) {
_flags._reset_filter = false;
_input = input;
_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
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
if (_flags._reset_filter) {
_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
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
if (_flags._reset_filter) {
_flags._reset_filter = false;