AP_InertialSensor: fixed Q calculation for notch filters

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -1059,8 +1059,10 @@ AP_InertialSensor::init(uint16_t loop_rate)
                     notch.filter[i].allocate_filters(notch.num_dynamic_notches,
                                                      notch.params.harmonics(), double_notch ? 2 : triple_notch ? 3 : 1);
                     // initialise default settings, these will be subsequently changed in AP_InertialSensor_Backend::update_gyro()
-                    notch.filter[i].init(_gyro_raw_sample_rates[i], notch.calculated_notch_freq_hz[0],
-                                         notch.params.bandwidth_hz(), notch.params.attenuation_dB());
+                    notch.filter[i].init(_gyro_raw_sample_rates[i],
+                                         notch.params.center_freq_hz(),
+                                         notch.params.bandwidth_hz(),
+                                         notch.params.attenuation_dB());
                 }
             }
         }
@@ -1783,25 +1785,27 @@ void AP_InertialSensor::_save_gyro_calibration()
  */
 void AP_InertialSensor::HarmonicNotch::update_params(uint8_t instance, bool converging, float gyro_rate)
 {
-    const float center_freq = calculated_notch_freq_hz[0];
     if (!is_equal(last_bandwidth_hz[instance], params.bandwidth_hz()) ||
         !is_equal(last_attenuation_dB[instance], params.attenuation_dB()) ||
-        (params.tracking_mode() == HarmonicNotchDynamicMode::Fixed && !is_equal(last_center_freq_hz[instance], center_freq)) ||
+        !is_equal(last_center_freq_hz[instance], params.center_freq_hz()) ||
         converging) {
         filter[instance].init(gyro_rate,
-                              center_freq,
+                              params.center_freq_hz(),
                               params.bandwidth_hz(),
                               params.attenuation_dB());
-        last_center_freq_hz[instance] = center_freq;
+        last_center_freq_hz[instance] = params.center_freq_hz();
         last_bandwidth_hz[instance] = params.bandwidth_hz();
         last_attenuation_dB[instance] = params.attenuation_dB();
-    } else if (params.tracking_mode() != HarmonicNotchDynamicMode::Fixed) {
+    }
+    if (params.tracking_mode() != HarmonicNotchDynamicMode::Fixed) {
+        // we always call the update for non-fixed notches, even if we
+        // just called init() as the update may use a different
+        // frequency to the init
         if (num_calculated_notch_frequencies > 1) {
             filter[instance].update(num_calculated_notch_frequencies, calculated_notch_freq_hz);
         } else {
-            filter[instance].update(center_freq);
+            filter[instance].update(calculated_notch_freq_hz[0]);
         }
-        last_center_freq_hz[instance] = center_freq;
     }
 }
 

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -54,6 +54,15 @@ void AP_InertialSensor_Backend::_set_gyro_oversampling(uint8_t instance, uint8_t
     _imu._gyro_over_sampling[instance] = n;
 }
 
+/*
+  while sensors are converging to get the true sample rate we re-init the notch filters.
+  stop doing this if the user arms
+ */
+bool AP_InertialSensor_Backend::sensors_converging() const
+{
+    return AP_HAL::millis64() < HAL_INS_CONVERGANCE_MS && !hal.util->get_soft_armed();
+}
+
 /*
   update the sensor rate for FIFO sensors
 

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -212,7 +212,7 @@ class AP_InertialSensor_Backend
     void _update_sensor_rate(uint16_t &count, uint32_t &start_us, float &rate_hz) const __RAMFUNC__;
 
     // return true if the sensors are still converging and sampling rates could change significantly
-    bool sensors_converging() const { return AP_HAL::millis() < HAL_INS_CONVERGANCE_MS; }
+    bool sensors_converging() const;
 
     // set accelerometer max absolute offset for calibration
     void _set_accel_max_abs_offset(uint8_t instance, float offset);