aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: add support for individually tracking fft peaks and motor rpms with harmonic notches

Browse Source 
log harmonic notch even if FFT is disabled. Fallback to throttle notch for BLHeli
move harmonic notch update to AP_Vehicle
This commit is contained in:
Andy Piper 2020-05-29 17:30:15 +01:00 committed by Andrew Tridgell
parent d402761a4b
commit 25dcc1a623
3 changed files with 30 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/Copter.cpp
View File

@ -354,8 +354,6 @@ void Copter::throttle_loop()
// compensate for ground effect (if enabled)
update_ground_effect_detector();
update_dynamic_notch();
}
// update_batt_compass - read battery and compass
@ -631,6 +629,8 @@ void Copter::update_altitude()
Log_Write_Control_Tuning();
#if HAL_GYROFFT_ENABLED
gyro_fft.write_log_messages();
#else
write_notch_log_messages();
#endif
}
}

2
ArduCopter/Copter.h
View File

@ -863,7 +863,7 @@ private:
// system.cpp
void init_ardupilot() override;
void startup_INS_ground();
void update_dynamic_notch();
void update_dynamic_notch() override;
bool position_ok() const;
bool ekf_position_ok() const;
bool optflow_position_ok() const;

31
ArduCopter/system.cpp
View File

@ -248,16 +248,17 @@ void Copter::update_dynamic_notch()
}
const float ref_freq = ins.get_gyro_harmonic_notch_center_freq_hz();
const float ref = ins.get_gyro_harmonic_notch_reference();
if (is_zero(ref)) {
ins.update_harmonic_notch_freq_hz(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()) {
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(ref_freq * MAX(1.0f, sqrtf(motors->get_throttle_out() / ref)));
ins.update_harmonic_notch_freq_hz(throttle_freq);
break;
#if RPM_ENABLED == ENABLED
@ -273,13 +274,35 @@ void Copter::update_dynamic_notch()
#endif
#ifdef HAVE_AP_BLHELI_SUPPORT
case HarmonicNotchDynamicMode::UpdateBLHeli: // BLHeli based tracking
ins.update_harmonic_notch_freq_hz(MAX(ref_freq, AP_BLHeli::get_singleton()->get_average_motor_frequency_hz() * ref));
// 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);
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(num_notches, notches);
} else { // throttle fallback
ins.update_harmonic_notch_freq_hz(throttle_freq);
}
} else {
ins.update_harmonic_notch_freq_hz(MAX(ref_freq, AP_BLHeli::get_singleton()->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
ins.update_harmonic_notch_freq_hz(gyro_fft.get_weighted_noise_center_freq_hz());
if (ins.has_harmonic_option(HarmonicNotchFilterParams::Options::DynamicHarmonic)) {
float notches[INS_MAX_NOTCHES];
const uint8_t peaks = gyro_fft.get_weighted_noise_center_frequencies_hz(INS_MAX_NOTCHES, notches);
ins.update_harmonic_notch_frequencies_hz(peaks, notches);
} else {
ins.update_harmonic_notch_freq_hz(gyro_fft.get_weighted_noise_center_freq_hz());
}
break;
#endif
case HarmonicNotchDynamicMode::Fixed: // static