Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: support two full harmonic notch filters

This commit is contained in:
Andrew Tridgell 2022-04-13 13:36:18 +10:00 committed by Randy Mackay
parent 49bf16853c
commit 027f924e16
2 changed files with 20 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Copter.h
View File

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

38
ArduCopter/system.cpp
View File

@ -234,24 +234,24 @@ void Copter::startup_INS_ground()
} }
// update the harmonic notch filter center frequency dynamically // update the harmonic notch filter center frequency dynamically
void Copter::update_dynamic_notch() void Copter::update_dynamic_notch(uint8_t idx)
{ {
if (!ins.gyro_harmonic_notch_enabled()) { if (!ins.gyro_harmonic_notch_enabled(idx)) {
return; return;
} }
const float ref_freq = ins.get_gyro_harmonic_notch_center_freq_hz(); const float ref_freq = ins.get_gyro_harmonic_notch_center_freq_hz(idx);
const float ref = ins.get_gyro_harmonic_notch_reference(); const float ref = ins.get_gyro_harmonic_notch_reference(idx);
if (is_zero(ref)) { if (is_zero(ref)) {
ins.update_harmonic_notch_freq_hz(ref_freq); ins.update_harmonic_notch_freq_hz(idx, ref_freq);
return; return;
} }
const float throttle_freq = ref_freq * MAX(1.0f, sqrtf(motors->get_throttle_out() / ref)); const float throttle_freq = ref_freq * MAX(1.0f, sqrtf(motors->get_throttle_out() / ref));
switch (ins.get_gyro_harmonic_notch_tracking_mode()) { switch (ins.get_gyro_harmonic_notch_tracking_mode(idx)) {
case HarmonicNotchDynamicMode::UpdateThrottle: // throttle based tracking case HarmonicNotchDynamicMode::UpdateThrottle: // throttle based tracking
// set the harmonic notch filter frequency approximately scaled on motor rpm implied by throttle // set the harmonic notch filter frequency approximately scaled on motor rpm implied by throttle
ins.update_harmonic_notch_freq_hz(throttle_freq); ins.update_harmonic_notch_freq_hz(idx, throttle_freq);
break; break;
#if RPM_ENABLED == ENABLED #if RPM_ENABLED == ENABLED
@ -259,48 +259,48 @@ void Copter::update_dynamic_notch()
float rpm; float rpm;
if (rpm_sensor.get_rpm(0, rpm)) { if (rpm_sensor.get_rpm(0, rpm)) {
// set the harmonic notch filter frequency from the main rotor rpm // set the harmonic notch filter frequency from the main rotor rpm
ins.update_harmonic_notch_freq_hz(MAX(ref_freq, rpm * ref / 60.0f)); ins.update_harmonic_notch_freq_hz(idx, MAX(ref_freq, rpm * ref / 60.0f));
} else { } else {
ins.update_harmonic_notch_freq_hz(ref_freq); ins.update_harmonic_notch_freq_hz(idx, ref_freq);
} }
break; break;
#endif #endif
#if HAL_WITH_ESC_TELEM #if HAL_WITH_ESC_TELEM
case HarmonicNotchDynamicMode::UpdateBLHeli: // BLHeli based tracking case HarmonicNotchDynamicMode::UpdateBLHeli: // BLHeli based tracking
// set the harmonic notch filter frequency scaled on measured frequency // set the harmonic notch filter frequency scaled on measured frequency
if (ins.has_harmonic_option(HarmonicNotchFilterParams::Options::DynamicHarmonic)) { if (ins.has_harmonic_option(idx, HarmonicNotchFilterParams::Options::DynamicHarmonic)) {
float notches[INS_MAX_NOTCHES]; float notches[INS_MAX_NOTCHES];
const uint8_t num_notches = AP::esc_telem().get_motor_frequencies_hz(ins.get_num_gyro_dynamic_notches(), 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++) { for (uint8_t i = 0; i < num_notches; i++) {
notches[i] = MAX(ref_freq, notches[i]); notches[i] = MAX(ref_freq, notches[i]);
} }
if (num_notches > 0) { if (num_notches > 0) {
ins.update_harmonic_notch_frequencies_hz(num_notches, notches); ins.update_harmonic_notch_frequencies_hz(idx, num_notches, notches);
} else { // throttle fallback } else { // throttle fallback
ins.update_harmonic_notch_freq_hz(throttle_freq); ins.update_harmonic_notch_freq_hz(idx, throttle_freq);
} }
} else { } else {
ins.update_harmonic_notch_freq_hz(MAX(ref_freq, AP::esc_telem().get_average_motor_frequency_hz() * ref)); ins.update_harmonic_notch_freq_hz(idx, MAX(ref_freq, AP::esc_telem().get_average_motor_frequency_hz() * ref));
} }
break; break;
#endif #endif
#if HAL_GYROFFT_ENABLED #if HAL_GYROFFT_ENABLED
case HarmonicNotchDynamicMode::UpdateGyroFFT: // FFT based tracking case HarmonicNotchDynamicMode::UpdateGyroFFT: // FFT based tracking
// set the harmonic notch filter frequency scaled on measured frequency // set the harmonic notch filter frequency scaled on measured frequency
if (ins.has_harmonic_option(HarmonicNotchFilterParams::Options::DynamicHarmonic)) { if (ins.has_harmonic_option(idx, HarmonicNotchFilterParams::Options::DynamicHarmonic)) {
float notches[INS_MAX_NOTCHES]; float notches[INS_MAX_NOTCHES];
const uint8_t peaks = gyro_fft.get_weighted_noise_center_frequencies_hz(ins.get_num_gyro_dynamic_notches(), 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(peaks, notches); ins.update_harmonic_notch_frequencies_hz(idx, peaks, notches);
} else { } else {
ins.update_harmonic_notch_freq_hz(gyro_fft.get_weighted_noise_center_freq_hz()); ins.update_harmonic_notch_freq_hz(idx, gyro_fft.get_weighted_noise_center_freq_hz());
} }
break; break;
#endif #endif
case HarmonicNotchDynamicMode::Fixed: // static case HarmonicNotchDynamicMode::Fixed: // static
default: default:
ins.update_harmonic_notch_freq_hz(ref_freq); ins.update_harmonic_notch_freq_hz(idx, ref_freq);
break; break;
} }
} }