From ac2701b1bf888952da899af3895502a79a2c4c88 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 18 Aug 2022 17:20:58 +1000 Subject: [PATCH] AP_Vehicle: removed num_dynamic_notches limit in dynamic harmonic use INS_MAX_NOTCHES instead, allowing for more ESCs to be added by lua scripts --- libraries/AP_Vehicle/AP_Vehicle.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index fc84ba4032..65b1255f92 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -517,8 +517,8 @@ void AP_Vehicle::update_dynamic_notch(AP_InertialSensor::HarmonicNotch ¬ch) // set the harmonic notch filter frequency scaled on measured frequency if (notch.params.hasOption(HarmonicNotchFilterParams::Options::DynamicHarmonic)) { float notches[INS_MAX_NOTCHES]; - const uint8_t num_notches = AP::esc_telem().get_motor_frequencies_hz(notch.num_dynamic_notches, notches); // ESC telemetry will return 0 for missing data, but only after 1s + 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++) { if (!is_zero(notches[i])) { notches[i] = MAX(ref_freq, notches[i]);