AP_InertialSensor: add set_inactive() on notch filters

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -446,12 +446,22 @@ public:
         // Update the harmonic notch frequencies
         void update_freq_hz(float scaled_freq);
         void update_frequencies_hz(uint8_t num_freqs, const float scaled_freq[]);
-        
+
+        // enable/disable the notch
+        void set_inactive(bool _inactive) {
+            inactive = _inactive;
+        }
+
+        bool is_inactive(void) const {
+            return inactive;
+        }
+
     private:
         // support for updating harmonic filter at runtime
         float last_center_freq_hz[INS_MAX_INSTANCES];
         float last_bandwidth_hz[INS_MAX_INSTANCES];
         float last_attenuation_dB[INS_MAX_INSTANCES];
+        bool inactive;
     } harmonic_notches[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS];
 
 private:

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -186,17 +186,23 @@ void AP_InertialSensor_Backend::apply_gyro_filters(const uint8_t instance, const
         if (!notch.params.enabled()) {
             continue;
         }
+        bool inactive = notch.is_inactive();
 #ifndef HAL_BUILD_AP_PERIPH
         // by default we only run the expensive notch filters on the
         // currently active IMU we reset the inactive notch filters so
         // that if we switch IMUs we're not left with old data
         if (!notch.params.hasOption(HarmonicNotchFilterParams::Options::EnableOnAllIMUs) &&
             instance != AP::ahrs().get_primary_gyro_index()) {
-            notch.filter[instance].reset();
-            continue;
+            inactive = true;
         }
 #endif
-        gyro_filtered = notch.filter[instance].apply(gyro_filtered);
+        if (inactive) {
+            // while inactive we reset the filter so when it activates the first output
+            // will be the first input sample
+            notch.filter[instance].reset();
+        } else {
+            gyro_filtered = notch.filter[instance].apply(gyro_filtered);
+        }
     }
 
     // apply the low pass filter last to attentuate any notch induced noise