From 8e744a4a251e3529337f6128d59454e92a48a766 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 23 Feb 2021 22:07:13 +0000 Subject: [PATCH] Plane: generalise ESC telemetry to allow harmonic notch handling with other ESCs --- ArduPlane/system.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index c9436a89af..23205951c8 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -477,12 +477,12 @@ void Plane::update_dynamic_notch() ins.update_harmonic_notch_freq_hz(ref_freq); } break; -#ifdef HAVE_AP_BLHELI_SUPPORT +#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(HarmonicNotchFilterParams::Options::DynamicHarmonic)) { float notches[INS_MAX_NOTCHES]; - const uint8_t num_notches = AP_BLHeli::get_singleton()->get_motor_frequencies_hz(INS_MAX_NOTCHES, notches); + const uint8_t num_notches = AP::esc_telem().get_motor_frequencies_hz(INS_MAX_NOTCHES, notches); for (uint8_t i = 0; i < num_notches; i++) { notches[i] = MAX(ref_freq, notches[i]); @@ -495,7 +495,7 @@ void Plane::update_dynamic_notch() ins.update_harmonic_notch_freq_hz(ref_freq); } } else { - ins.update_harmonic_notch_freq_hz(MAX(ref_freq, AP_BLHeli::get_singleton()->get_average_motor_frequency_hz() * ref)); + ins.update_harmonic_notch_freq_hz(MAX(ref_freq, AP::esc_telem().get_average_motor_frequency_hz() * ref)); } break; #endif