Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 09:58:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: catch NAN errors in accel and gyro filters

Browse Source
This commit is contained in:
Andrew Tridgell 2015-11-21 17:55:51 +11:00
parent 5b7f2a3de3
commit 9cdd5f3944
1 changed files with 6 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -93,6 +93,9 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
_imu._last_raw_gyro[instance] = gyro;
_imu._gyro_filtered[instance] = _imu._gyro_filter[instance].apply(gyro);
if (_imu._gyro_filtered[instance].is_nan() || _imu._gyro_filtered[instance].is_inf()) {
_imu._gyro_filter[instance].reset();
}
_imu._new_gyro_data[instance] = true;
@ -148,6 +151,9 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
_imu._delta_velocity_acc_dt[instance] += dt;
_imu._accel_filtered[instance] = _imu._accel_filter[instance].apply(accel);
if (_imu._accel_filtered[instance].is_nan() || _imu._accel_filtered[instance].is_inf()) {
_imu._accel_filter[instance].reset();
}
_imu._new_accel_data[instance] = true;