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

Plane: add support for tracking fft peaks and individual motor rpms with harmonic notches 

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:31:46 +01:00 committed by Andrew Tridgell
parent 25dcc1a623
commit 9077d41df1
3 changed files with 30 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduPlane/ArduPlane.cpp
View File

@ -110,7 +110,6 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
#if EFI_ENABLED
SCHED_TASK(efi_update, 10, 200),
#endif
SCHED_TASK(update_dynamic_notch, 50, 200),
};
void Plane::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
@ -218,6 +217,8 @@ void Plane::update_logging2(void)
Log_Write_Control_Tuning();
#if HAL_GYROFFT_ENABLED
gyro_fft.write_log_messages();
#else
write_notch_log_messages();
#endif
}

2
ArduPlane/Plane.h
View File

@ -1014,7 +1014,7 @@ private:
void startup_INS_ground(void);
bool should_log(uint32_t mask);
int8_t throttle_percentage(void);
void update_dynamic_notch();
void update_dynamic_notch() override;
void notify_mode(const Mode& mode);
// takeoff.cpp

30
ArduPlane/system.cpp
View File

@ -456,7 +456,7 @@ void Plane::update_dynamic_notch()
}
switch (ins.get_gyro_harmonic_notch_tracking_mode()) {
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
if (quadplane.available()) {
ins.update_harmonic_notch_freq_hz(ref_freq * MAX(1.0f, sqrtf(quadplane.motors->get_throttle_out() / ref)));
@ -474,13 +474,37 @@ void Plane::update_dynamic_notch()
break;
#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 if (quadplane.available()) { // throttle fallback
ins.update_harmonic_notch_freq_hz(ref_freq * MAX(1.0f, sqrtf(quadplane.motors->get_throttle_out() / ref)));
} else {
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));
}
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