Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-04 04:33:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_PID: Protect against NaN and INF

Browse Source
This commit is contained in:
Leonard Hall 2015-03-29 20:32:46 +10:30 committed by Randy Mackay
parent 132cdc4916
commit ae77c18a19
1 changed files with 14 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AC_PID/AC_PID.cpp
View File

@ -84,18 +84,21 @@ 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)
{
// reset input filter to value received
if (_flags._reset_filter) {
_flags._reset_filter = false;
_input = input;
_derivative = 0.0f;
}
// 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;
}
// 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;
// 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;
}
}
}