From 5b7f9f6bea81bf1d8d1992dc4e1ca7e1a281ab50 Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Fri, 30 Aug 2019 08:33:42 +0100
Subject: [PATCH] AP_InertialSensor: while sensors are converging update the
 filters sample rates. if gyro filtering produces invalid output, keep the
 previous value

---
 .../AP_InertialSensor_Backend.cpp             | 30 ++++++++++++-------
 .../AP_InertialSensor_Backend.h               |  3 ++
 2 files changed, 22 insertions(+), 11 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
index 443982f115..4f6dbce53a 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
@@ -69,7 +69,7 @@ void AP_InertialSensor_Backend::_update_sensor_rate(uint16_t &count, uint32_t &s
             float filter_constant = 0.98f;
             float upper_limit = 1.05f;
             float lower_limit = 0.95f;
-            if (AP_HAL::millis() < 30000) {
+            if (sensors_converging()) {
                 // converge quickly for first 30s, then more slowly
                 filter_constant = 0.8f;
                 upper_limit = 2.0f;
@@ -225,22 +225,28 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
         _imu._last_raw_gyro[instance] = gyro;
 
         // apply the low pass filter
-        _imu._gyro_filtered[instance] = _imu._gyro_filter[instance].apply(gyro);
+        Vector3f gyro_filtered = _imu._gyro_filter[instance].apply(gyro);
+
+        // apply the notch filter
+        if (_gyro_notch_enabled()) {
+            gyro_filtered = _imu._gyro_notch_filter[instance].apply(gyro_filtered);
+        }
 
         // apply the harmonic notch filter
         if (gyro_harmonic_notch_enabled()) {
-            _imu._gyro_filtered[instance] = _imu._gyro_harmonic_notch_filter[instance].apply(_imu._gyro_filtered[instance]);
+            gyro_filtered = _imu._gyro_harmonic_notch_filter[instance].apply(gyro_filtered);
         }
 
-        // apply the notch filter
-        if (_gyro_notch_enabled()) {
-            _imu._gyro_filtered[instance] = _imu._gyro_notch_filter[instance].apply(_imu._gyro_filtered[instance]);
-        }
-        if (_imu._gyro_filtered[instance].is_nan() || _imu._gyro_filtered[instance].is_inf()) {
+        // if the filtering failed in any way then reset the filters and keep the old value
+        if (gyro_filtered.is_nan() || gyro_filtered.is_inf()) {
             _imu._gyro_filter[instance].reset();
             _imu._gyro_notch_filter[instance].reset();
             _imu._gyro_harmonic_notch_filter[instance].reset();
         }
+        else {
+            _imu._gyro_filtered[instance] = gyro_filtered;
+        }
+
         _imu._new_gyro_data[instance] = true;
     }
 
@@ -500,14 +506,15 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance)
     }
 
     // possibly update filter frequency
-    if (_last_gyro_filter_hz != _gyro_filter_cutoff()) {
+    if (_last_gyro_filter_hz != _gyro_filter_cutoff() || sensors_converging()) {
         _imu._gyro_filter[instance].set_cutoff_frequency(_gyro_raw_sample_rate(instance), _gyro_filter_cutoff());
         _last_gyro_filter_hz = _gyro_filter_cutoff();
     }
 
     // possily update the harmonic notch filter parameters
     if (!is_equal(_last_harmonic_notch_bandwidth_hz, gyro_harmonic_notch_bandwidth_hz()) ||
-        !is_equal(_last_harmonic_notch_attenuation_dB, gyro_harmonic_notch_attenuation_dB())) {
+        !is_equal(_last_harmonic_notch_attenuation_dB, gyro_harmonic_notch_attenuation_dB()) ||
+        sensors_converging()) {
         _imu._gyro_harmonic_notch_filter[instance].init(_gyro_raw_sample_rate(instance), gyro_harmonic_notch_center_freq_hz(), gyro_harmonic_notch_bandwidth_hz(), gyro_harmonic_notch_attenuation_dB());
         _last_harmonic_notch_center_freq_hz = gyro_harmonic_notch_center_freq_hz();
         _last_harmonic_notch_bandwidth_hz = gyro_harmonic_notch_bandwidth_hz();
@@ -519,7 +526,8 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance)
     // possily update the notch filter parameters
     if (!is_equal(_last_notch_center_freq_hz, _gyro_notch_center_freq_hz()) ||
         !is_equal(_last_notch_bandwidth_hz, _gyro_notch_bandwidth_hz()) ||
-        !is_equal(_last_notch_attenuation_dB, _gyro_notch_attenuation_dB())) {
+        !is_equal(_last_notch_attenuation_dB, _gyro_notch_attenuation_dB()) ||
+        sensors_converging()) {
         _imu._gyro_notch_filter[instance].init(_gyro_raw_sample_rate(instance), _gyro_notch_center_freq_hz(), _gyro_notch_bandwidth_hz(), _gyro_notch_attenuation_dB());
         _last_notch_center_freq_hz = _gyro_notch_center_freq_hz();
         _last_notch_bandwidth_hz = _gyro_notch_bandwidth_hz();
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
index f5c12f0960..8befc03edf 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
@@ -180,6 +180,9 @@ protected:
     // update the sensor rate for FIFO sensors
     void _update_sensor_rate(uint16_t &count, uint32_t &start_us, float &rate_hz) const;
 
+    // return true if the sensors are still converging and sampling rates could change significantly
+    bool sensors_converging() const { return AP_HAL::millis() < 30000; }
+
     // set accelerometer max absolute offset for calibration
     void _set_accel_max_abs_offset(uint8_t instance, float offset);