From 09d3392aee82fa7f937e2fcc93fd7539b78aaac1 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 5 Jul 2019 11:51:30 +1000
Subject: [PATCH] AP_InertialSensor: zero accumulators on time step

this zeros the delta angle and delta velocity accumulators when a
sensor is unavailable for a period of 0.1s. This prevents garbage
values being passed into the EKF when a sensor dies and then becomes
available again some time later
---
 .../AP_InertialSensor_Backend.cpp             | 32 +++++++++++++++++--
 1 file changed, 29 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
index be069f9cd2..1b51f6ceb0 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
@@ -155,6 +155,8 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
     _update_sensor_rate(_imu._sample_gyro_count[instance], _imu._sample_gyro_start_us[instance],
                         _imu._gyro_raw_sample_rates[instance]);
 
+    uint64_t last_sample_us = _imu._gyro_last_sample_us[instance];
+
     /*
       we have two classes of sensors. FIFO based sensors produce data
       at a very predictable overall rate, but the data comes in
@@ -165,6 +167,7 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
      */
     if (sample_us != 0 && _imu._gyro_last_sample_us[instance] != 0) {
         dt = (sample_us - _imu._gyro_last_sample_us[instance]) * 1.0e-6f;
+        _imu._gyro_last_sample_us[instance] = sample_us;
     } else {
         // don't accept below 100Hz
         if (_imu._gyro_raw_sample_rates[instance] < 100) {
@@ -172,8 +175,8 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
         }
 
         dt = 1.0f / _imu._gyro_raw_sample_rates[instance];
+        _imu._gyro_last_sample_us[instance] = AP_HAL::micros64();
     }
-    _imu._gyro_last_sample_us[instance] = sample_us;
 
 #if AP_MODULE_SUPPORTED
     // call gyro_sample hook if any
@@ -181,8 +184,9 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
 #endif
 
     // push gyros if optical flow present
-    if (hal.opticalflow)
+    if (hal.opticalflow) {
         hal.opticalflow->push_gyro(gyro.x, gyro.y, dt);
+    }
     
     // compute delta angle
     Vector3f delta_angle = (gyro + _imu._last_raw_gyro[instance]) * 0.5f * dt;
@@ -199,6 +203,16 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
 
     {
         WITH_SEMAPHORE(_sem);
+        uint64_t now = AP_HAL::micros64();
+
+        if (now - last_sample_us > 100000U) {
+            // zero accumulator if sensor was unhealthy for 0.1s
+            _imu._delta_angle_acc[instance].zero();
+            _imu._delta_angle_acc_dt[instance] = 0;
+            dt = 0;
+            delta_angle.zero();
+        }
+
         // integrate delta angle accumulator
         // the angles and coning corrections are accumulated separately in the
         // referenced paper, but in simulation little difference was found between
@@ -305,6 +319,8 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
     _update_sensor_rate(_imu._sample_accel_count[instance], _imu._sample_accel_start_us[instance],
                         _imu._accel_raw_sample_rates[instance]);
 
+    uint64_t last_sample_us = _imu._accel_last_sample_us[instance];
+
     /*
       we have two classes of sensors. FIFO based sensors produce data
       at a very predictable overall rate, but the data comes in
@@ -315,6 +331,7 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
      */
     if (sample_us != 0 && _imu._accel_last_sample_us[instance] != 0) {
         dt = (sample_us - _imu._accel_last_sample_us[instance]) * 1.0e-6f;
+        _imu._accel_last_sample_us[instance] = sample_us;
     } else {
         // don't accept below 100Hz
         if (_imu._accel_raw_sample_rates[instance] < 100) {
@@ -322,8 +339,8 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
         }
 
         dt = 1.0f / _imu._accel_raw_sample_rates[instance];
+        _imu._accel_last_sample_us[instance] = AP_HAL::micros64();
     }
-    _imu._accel_last_sample_us[instance] = sample_us;
 
 #if AP_MODULE_SUPPORTED
     // call accel_sample hook if any
@@ -335,6 +352,15 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
     {
         WITH_SEMAPHORE(_sem);
 
+        uint64_t now = AP_HAL::micros64();
+
+        if (now - last_sample_us > 100000U) {
+            // zero accumulator if sensor was unhealthy for 0.1s
+            _imu._delta_velocity_acc[instance].zero();
+            _imu._delta_velocity_acc_dt[instance] = 0;
+            dt = 0;
+        }
+        
         // delta velocity
         _imu._delta_velocity_acc[instance] += accel * dt;
         _imu._delta_velocity_acc_dt[instance] += dt;