Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Revert "AP_Vehicle: make sure that if ESC telemetry is missing notch updates are not misordered"

Browse Source 
This reverts commit 27998db12e.

reverted as it breaks non-contiguous motors for quadplanes
This commit is contained in:
Andrew Tridgell 2022-08-18 18:08:57 +10:00
parent e545cf7c51
commit ed9c28bec5
1 changed files with 4 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_Vehicle/AP_Vehicle.cpp
View File

@ -443,14 +443,12 @@ void AP_Vehicle::update_dynamic_notch(AP_InertialSensor::HarmonicNotch &notch)
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
for (uint8_t i = 0; i < notch.num_dynamic_notches; i++) {
if (!is_zero(notches[i])) {
notches[i] = MAX(ref_freq, notches[i]);
}
for (uint8_t i = 0; i < num_notches; i++) {
notches[i] = MAX(ref_freq, notches[i]);
}
if (num_notches > 0) {
notch.update_frequencies_hz(notch.num_dynamic_notches, notches);
notch.update_frequencies_hz(num_notches, notches);
} else { // throttle fallback
notch.update_freq_hz(throttle_freq);
}