Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 17:38:32 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: add support for tracking fft peaks and individual 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: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 #if EFI_ENABLED
SCHED_TASK(efi_update, 10, 200), SCHED_TASK(efi_update, 10, 200),
#endif #endif
SCHED_TASK(update_dynamic_notch, 50, 200),
}; };
void Plane::get_scheduler_tasks(const AP_Scheduler::Task *&tasks, void Plane::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
@ -218,6 +217,8 @@ void Plane::update_logging2(void)
Log_Write_Control_Tuning(); Log_Write_Control_Tuning();
#if HAL_GYROFFT_ENABLED #if HAL_GYROFFT_ENABLED
gyro_fft.write_log_messages(); gyro_fft.write_log_messages();
#else
write_notch_log_messages();
#endif #endif
} }

2
ArduPlane/Plane.h
View File

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

24
ArduPlane/system.cpp
View File

@ -474,13 +474,37 @@ void Plane::update_dynamic_notch()
break; break;
#ifdef HAVE_AP_BLHELI_SUPPORT #ifdef HAVE_AP_BLHELI_SUPPORT
case HarmonicNotchDynamicMode::UpdateBLHeli: // BLHeli based tracking case HarmonicNotchDynamicMode::UpdateBLHeli: // BLHeli based tracking
// 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)); ins.update_harmonic_notch_freq_hz(MAX(ref_freq, AP_BLHeli::get_singleton()->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)) {
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()); ins.update_harmonic_notch_freq_hz(gyro_fft.get_weighted_noise_center_freq_hz());
}
break; break;
#endif #endif
case HarmonicNotchDynamicMode::Fixed: // static case HarmonicNotchDynamicMode::Fixed: // static