diff --git a/src/modules/gyro_calibration/GyroCalibration.cpp b/src/modules/gyro_calibration/GyroCalibration.cpp index ced9d66c7f..169652a3e9 100644 --- a/src/modules/gyro_calibration/GyroCalibration.cpp +++ b/src/modules/gyro_calibration/GyroCalibration.cpp @@ -155,6 +155,7 @@ void GyroCalibration::Run() if (_gyro_calibration[gyro].device_id() == sensor_gyro.device_id) { const Vector3f val{Vector3f{sensor_gyro.x, sensor_gyro.y, sensor_gyro.z} - _gyro_calibration[gyro].thermal_offset()}; _gyro_mean[gyro].update(val); + _gyro_last_update[gyro] = sensor_gyro.timestamp; } else { // setting device id, reset all @@ -162,6 +163,13 @@ void GyroCalibration::Run() Reset(); } } + + if ((_gyro_last_update[gyro] != 0) && (hrt_elapsed_time(&_gyro_last_update[gyro]) > 100_ms)) { + // reset on any timeout + Reset(); + _gyro_last_update[gyro] = 0; + return; + } } diff --git a/src/modules/gyro_calibration/GyroCalibration.hpp b/src/modules/gyro_calibration/GyroCalibration.hpp index 1b2b925b5e..9d1047d6b3 100644 --- a/src/modules/gyro_calibration/GyroCalibration.hpp +++ b/src/modules/gyro_calibration/GyroCalibration.hpp @@ -95,6 +95,7 @@ private: calibration::Gyroscope _gyro_calibration[MAX_SENSORS] {}; math::WelfordMean _gyro_mean[MAX_SENSORS] {}; float _temperature[MAX_SENSORS] {}; + hrt_abstime _gyro_last_update[MAX_SENSORS] {}; matrix::Vector3f _acceleration[MAX_SENSORS] {};