From db14ba46dc3fb1a8e0e87996329d0b93fb480ea5 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 17 Apr 2022 17:35:57 +1000
Subject: [PATCH] AP_InertialSensor: call notch param update with semaphore
 held

---
 libraries/AP_InertialSensor/AP_InertialSensor.cpp      |  9 ---------
 .../AP_InertialSensor/AP_InertialSensor_Backend.cpp    | 10 +++++++++-
 2 files changed, 9 insertions(+), 10 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index 99517dbf68..f3e90daf34 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -1634,15 +1634,6 @@ void AP_InertialSensor::update(void)
         for (uint8_t i=0; i<_backend_count; i++) {
             _backends[i]->update();
         }
-        for (uint8_t i=0; i<_gyro_count; i++) {
-            const bool converging = AP_HAL::millis() < HAL_INS_CONVERGANCE_MS;
-            const float gyro_rate = _gyro_raw_sample_rates[i];
-            for (auto &notch : harmonic_notches) {
-                if (notch.params.enabled()) {
-                    notch.update_params(i, converging, gyro_rate);
-                }
-            }
-        }
 
         if (!_startup_error_counts_set) {
             for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
index 97ff40d64e..e1c2d89aa8 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
@@ -716,10 +716,18 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) /* front end */
     }
 
     // possibly update filter frequency
+    const float gyro_rate = _gyro_raw_sample_rate(instance);
+
     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());
+        _imu._gyro_filter[instance].set_cutoff_frequency(gyro_rate, _gyro_filter_cutoff());
         _last_gyro_filter_hz = _gyro_filter_cutoff();
     }
+
+    for (auto &notch : _imu.harmonic_notches) {
+        if (notch.params.enabled()) {
+            notch.update_params(instance, sensors_converging(), gyro_rate);
+        }
+    }
 }
 
 /*