Copter: moved harmonic notch update code to AP_Vehicle

ArduCopter/Copter.h

@@ -879,7 +879,6 @@ private:
     // system.cpp
     void init_ardupilot() override;
     void startup_INS_ground();
-    void update_dynamic_notch(uint8_t idx) override;
     bool position_ok() const;
     bool ekf_has_absolute_position() const;
     bool ekf_has_relative_position() const;

ArduCopter/Parameters.cpp

@@ -1388,7 +1388,7 @@ void Copter::convert_pid_parameters(void)
     }
 
 #if HAL_INS_NUM_HARMONIC_NOTCH_FILTERS > 1
-    if (!ins.gyro_harmonic_notch_enabled(1)) {
+    if (!ins.harmonic_notches[1].params.enabled()) {
         // notch filter parameter conversions (moved to INS_HNTC2) for 4.2.x, converted from fixed notch
         const AP_Param::ConversionInfo notchfilt_conversion_info[] {
             { Parameters::k_param_ins, 101, AP_PARAM_INT8,  "INS_HNTC2_ENABLE" },
@@ -1400,10 +1400,8 @@ void Copter::convert_pid_parameters(void)
         for (uint8_t i=0; i<notchfilt_table_size; i++) {
             AP_Param::convert_old_parameters(&notchfilt_conversion_info[i], 1.0f);
         }
-        if (ins.gyro_harmonic_notch_enabled(1)) {
-            AP_Param::set_default_by_name("INS_HNTC2_MODE", 0);
-            AP_Param::set_default_by_name("INS_HNTC2_HMNCS", 1);
-        }
+        AP_Param::set_default_by_name("INS_HNTC2_MODE", 0);
+        AP_Param::set_default_by_name("INS_HNTC2_HMNCS", 1);
     }
 #endif
 

ArduCopter/system.cpp

@@ -233,81 +233,6 @@ void Copter::startup_INS_ground()
     ahrs.reset();
 }
 
-// update the harmonic notch filter center frequency dynamically
-void Copter::update_dynamic_notch(uint8_t idx)
-{
-    if (!ins.gyro_harmonic_notch_enabled(idx)) {
-        return;
-    }
-    const float ref_freq = ins.get_gyro_harmonic_notch_center_freq_hz(idx);
-    const float ref = ins.get_gyro_harmonic_notch_reference(idx);
-    if (is_zero(ref)) {
-        ins.update_harmonic_notch_freq_hz(idx, ref_freq);
-        return;
-    }
-
-    const float throttle_freq = ref_freq * MAX(1.0f, sqrtf(motors->get_throttle_out() / ref));
-
-    switch (ins.get_gyro_harmonic_notch_tracking_mode(idx)) {
-        case HarmonicNotchDynamicMode::UpdateThrottle: // throttle based tracking
-            // set the harmonic notch filter frequency approximately scaled on motor rpm implied by throttle
-            ins.update_harmonic_notch_freq_hz(idx, throttle_freq);
-            break;
-
-#if RPM_ENABLED == ENABLED
-        case HarmonicNotchDynamicMode::UpdateRPM: // rpm sensor based tracking
-        case HarmonicNotchDynamicMode::UpdateRPM2: {
-            uint8_t sensor = (ins.get_gyro_harmonic_notch_tracking_mode(idx)==HarmonicNotchDynamicMode::UpdateRPM?0:1);
-            float rpm;
-            if (rpm_sensor.get_rpm(sensor, rpm)) {
-                // set the harmonic notch filter frequency from the main rotor rpm
-                ins.update_harmonic_notch_freq_hz(idx, MAX(ref_freq, rpm * ref * (1.0/60)));
-            } else {
-                ins.update_harmonic_notch_freq_hz(idx, ref_freq);
-            }
-            break;
-        }
-#endif
-#if HAL_WITH_ESC_TELEM
-        case HarmonicNotchDynamicMode::UpdateBLHeli: // BLHeli based tracking
-            // set the harmonic notch filter frequency scaled on measured frequency
-            if (ins.has_harmonic_option(idx, HarmonicNotchFilterParams::Options::DynamicHarmonic)) {
-                float notches[INS_MAX_NOTCHES];
-                const uint8_t num_notches = AP::esc_telem().get_motor_frequencies_hz(ins.get_num_gyro_dynamic_notches(idx), notches);
-
-                for (uint8_t i = 0; i < num_notches; i++) {
-                    notches[i] =  MAX(ref_freq, notches[i]);
-                }
-                if (num_notches > 0) {
-                    ins.update_harmonic_notch_frequencies_hz(idx, num_notches, notches);
-                } else {    // throttle fallback
-                    ins.update_harmonic_notch_freq_hz(idx, throttle_freq);
-                }
-            } else {
-                ins.update_harmonic_notch_freq_hz(idx, MAX(ref_freq, AP::esc_telem().get_average_motor_frequency_hz() * ref));
-            }
-            break;
-#endif
-#if HAL_GYROFFT_ENABLED
-        case HarmonicNotchDynamicMode::UpdateGyroFFT: // FFT based tracking
-            // set the harmonic notch filter frequency scaled on measured frequency
-            if (ins.has_harmonic_option(idx, HarmonicNotchFilterParams::Options::DynamicHarmonic)) {
-                float notches[INS_MAX_NOTCHES];
-                const uint8_t peaks = gyro_fft.get_weighted_noise_center_frequencies_hz(ins.get_num_gyro_dynamic_notches(idx), notches);
-
-                ins.update_harmonic_notch_frequencies_hz(idx, peaks, notches);
-            } else {
-                ins.update_harmonic_notch_freq_hz(idx, gyro_fft.get_weighted_noise_center_freq_hz());
-            }
-            break;
-#endif
-        case HarmonicNotchDynamicMode::Fixed: // static
-        default:
-            ins.update_harmonic_notch_freq_hz(idx, ref_freq);
-            break;
-    }
-}
-
 // position_ok - returns true if the horizontal absolute position is ok and home position is set
 bool Copter::position_ok() const
 {