AP_InertialSensor: allow HarmonicNotches to be compiled out of the code

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -564,6 +564,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
     // @Bitmask: 0:FirstIMU,1:SecondIMU,2:ThirdIMU,3:FourthIMU,4:FifthIMU,5:SixthIMU,6:SeventhIMU
     AP_GROUPINFO("_ENABLE_MASK",  40, AP_InertialSensor, _enable_mask, 0x7F),
 
+#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
     // @Group: _HNTCH_
     // @Path: ../Filter/HarmonicNotchFilter.cpp
     AP_SUBGROUPINFO(harmonic_notches[0].params, "_HNTCH_",  41, AP_InertialSensor, HarmonicNotchFilterParams),
@@ -572,6 +573,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
     // @Group: _HNTC2_
     // @Path: ../Filter/HarmonicNotchFilter.cpp
     AP_SUBGROUPINFO(harmonic_notches[1].params, "_HNTC2_",  53, AP_InertialSensor, HarmonicNotchFilterParams),
+#endif
 #endif
 
     // @Param: _GYRO_RATE
@@ -895,7 +897,7 @@ bool AP_InertialSensor::set_gyro_window_size(uint16_t size) {
     return true;
 }
 
-#if HAL_WITH_DSP
+#if HAL_WITH_DSP && AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
 bool AP_InertialSensor::has_fft_notch() const
 {
     for (auto &notch : harmonic_notches) {
@@ -919,10 +921,12 @@ AP_InertialSensor::init(uint16_t loop_rate)
     // cause divergence of state estimators
     _loop_delta_t_max = 10 * _loop_delta_t;
 
+#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
     // Initialize notch params
     for (auto &notch : harmonic_notches) {
         notch.params.init();
     }
+#endif
 
     if (_gyro_count == 0 && _accel_count == 0) {
         _start_backends();
@@ -965,6 +969,7 @@ AP_InertialSensor::init(uint16_t loop_rate)
     //  Gyro -> Notch1/2 -> LPF -> Filtered Gyro
 
     if (_post_filter_fft) {
+#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
         for (auto &notch : harmonic_notches) {
             if (!notch.params.enabled()) {
                 continue;
@@ -975,10 +980,14 @@ AP_InertialSensor::init(uint16_t loop_rate)
             }
             _fft_window_phase++;
         }
+#endif
     }
 #else
     bool fft_enabled = false;
+    (void)fft_enabled;
 #endif
+
+#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
     // the center frequency of the harmonic notch is always taken from the calculated value so that it can be updated
     // dynamically, the calculated value is always some multiple of the configured center frequency, so start with the
     // configured value
@@ -1006,12 +1015,15 @@ AP_InertialSensor::init(uint16_t loop_rate)
         }
 #endif
     }
+#endif  // AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
+
     // count number of used sensors
     uint8_t sensors_used = 0;
     for (uint8_t i = 0; i < INS_MAX_INSTANCES; i++) {
         sensors_used += _use(i);
     }
 
+#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
     uint16_t num_filters = 0;
     for (auto &notch : harmonic_notches) {
         // calculate number of notches we might want to use for harmonic notch
@@ -1041,6 +1053,7 @@ AP_InertialSensor::init(uint16_t loop_rate)
             }
         }
     }
+#endif
 
 #if HAL_INS_TEMPERATURE_CAL_ENABLE
     /*
@@ -1822,6 +1835,7 @@ void AP_InertialSensor::_save_gyro_calibration()
     }
 }
 
+#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
 /*
   update harmonic notch parameters
  */
@@ -1844,6 +1858,7 @@ void AP_InertialSensor::HarmonicNotch::update_params(uint8_t instance, bool conv
         }
     }
 }
+#endif
 
 /*
   update gyro and accel values from backends
@@ -2285,6 +2300,7 @@ void AP_InertialSensor::acal_update()
 }
 #endif
 
+#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
 /*
   Update the harmonic notch frequency
 
@@ -2327,6 +2343,7 @@ bool AP_InertialSensor::setup_throttle_gyro_harmonic_notch(float center_freq_hz,
 
     return false;
 }
+#endif  // AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
 
 /*
     set and save accelerometer bias along with trim calculation

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -163,7 +163,9 @@ public:
     FloatBuffer&  get_raw_gyro_window(uint8_t axis) { return get_raw_gyro_window(_primary_gyro, axis); }
     uint16_t get_raw_gyro_rate_hz() const { return get_raw_gyro_rate_hz(_primary_gyro); }
     uint16_t get_raw_gyro_rate_hz(uint8_t instance) const { return _gyro_raw_sample_rates[_primary_gyro]; }
+#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
     bool has_fft_notch() const;
+#endif
 #endif
     bool set_gyro_window_size(uint16_t size);
     // get accel offsets in m/s/s
@@ -229,11 +231,13 @@ public:
     // get the accel filter rate in Hz
     uint16_t get_accel_filter_hz(void) const { return _accel_filter_cutoff; }
 
+#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
     // setup the notch for throttle based tracking
     bool setup_throttle_gyro_harmonic_notch(float center_freq_hz, float lower_freq_hz, float ref, uint8_t harmonics);
 
     // write out harmonic notch log messages
     void write_notch_log_messages() const;
+#endif
 
     // indicate which bit in LOG_BITMASK indicates raw logging enabled
     void set_log_raw_bit(uint32_t log_raw_bit) { _log_raw_bit = log_raw_bit; }
@@ -431,6 +435,7 @@ public:
     // force save of current calibration as valid
     void force_save_calibration(void);
 
+#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
     // structure per harmonic notch filter. This is public to allow for
     // easy iteration
     class HarmonicNotch {
@@ -473,6 +478,7 @@ public:
         float last_attenuation_dB[INS_MAX_INSTANCES];
         bool inactive;
     } harmonic_notches[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS];
+#endif  // AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
 
 private:
     // load backend drivers

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -213,6 +213,7 @@ void AP_InertialSensor_Backend::apply_gyro_filters(const uint8_t instance, const
 
     Vector3f gyro_filtered = gyro;
 
+#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
     // apply the harmonic notch filters
     for (auto &notch : _imu.harmonic_notches) {
         if (!notch.params.enabled()) {
@@ -237,6 +238,7 @@ void AP_InertialSensor_Backend::apply_gyro_filters(const uint8_t instance, const
         }
         save_gyro_window(instance, gyro_filtered, filter_phase++);
     }
+#endif  // AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
 
     // apply the low pass filter last to attenuate any notch induced noise
     gyro_filtered = _imu._gyro_filter[instance].apply(gyro_filtered);
@@ -247,9 +249,11 @@ void AP_InertialSensor_Backend::apply_gyro_filters(const uint8_t instance, const
 #if HAL_GYROFFT_ENABLED
         _imu._post_filter_gyro_filter[instance].reset();
 #endif
+#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
         for (auto &notch : _imu.harmonic_notches) {
             notch.filter[instance].reset();
         }
+#endif
     } else {
         _imu._gyro_filtered[instance] = gyro_filtered;
     }
@@ -788,11 +792,13 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) /* front end */
         _last_gyro_filter_hz = _gyro_filter_cutoff();
     }
 
+#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
     for (auto &notch : _imu.harmonic_notches) {
         if (notch.params.enabled()) {
             notch.update_params(instance, sensors_converging(), gyro_rate);
         }
     }
+#endif
 }
 
 /*

libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp

@@ -140,6 +140,7 @@ bool AP_InertialSensor::BatchSampler::Write_ISBD() const
 }
 #endif
 
+#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
 // @LoggerMessage: FTN
 // @Description: Filter Tuning Message - per motor
 // @Field: TimeUS: microseconds since system startup
@@ -198,5 +199,6 @@ void AP_InertialSensor::write_notch_log_messages() const
         notch.filter[primary_gyro].log_notch_centers(i, now_us);
     }
 }
+#endif  // AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
 
 #endif  // HAL_LOGGING_ENABLED

libraries/AP_InertialSensor/AP_InertialSensor_config.h

@@ -8,6 +8,10 @@
    than 1 then redundant sensors may be available
  */
 
+#ifndef AP_INERTIALSENSOR_ENABLED
+#define AP_INERTIALSENSOR_ENABLED 1
+#endif
+
 #ifndef INS_AUX_INSTANCES
 #define INS_AUX_INSTANCES 0
 #endif
@@ -43,19 +47,21 @@
 #define HAL_INS_TEMPERATURE_CAL_ENABLE BOARD_FLASH_SIZE > 1024
 #endif
 
+#ifndef AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
+#define AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED AP_INERTIALSENSOR_ENABLED
+#endif
+
+#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
 #ifndef HAL_INS_NUM_HARMONIC_NOTCH_FILTERS
 #define HAL_INS_NUM_HARMONIC_NOTCH_FILTERS 2
 #endif
+#endif
 
 // time for the estimated gyro rates to converge
 #ifndef HAL_INS_CONVERGANCE_MS
 #define HAL_INS_CONVERGANCE_MS 30000
 #endif
 
-#ifndef AP_INERTIALSENSOR_ENABLED
-#define AP_INERTIALSENSOR_ENABLED 1
-#endif
-
 #ifndef AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
 #define AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED (AP_INERTIALSENSOR_ENABLED && HAL_LOGGING_ENABLED)
 #endif