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

Plane: added dynamic harmonic notch support

Browse Source
This commit is contained in:
Andrew Tridgell 2019-11-17 16:06:34 +11:00 committed by Randy Mackay
parent 68bb7a516b
commit d872ca27ce
3 changed files with 45 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduPlane/ArduPlane.cpp
View File

@ -107,8 +107,9 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
SCHED_TASK(landing_gear_update, 5, 50),
#endif
#if EFI_ENABLED
SCHED_TASK(efi_update, 10, 200)
SCHED_TASK(efi_update, 10, 200),
#endif
SCHED_TASK(update_dynamic_notch, 50, 200),
};
constexpr int8_t Plane::_failsafe_priorities[7];

1
ArduPlane/Plane.h
View File

@ -916,6 +916,7 @@ private:
void startup_INS_ground(void);
bool should_log(uint32_t mask);
int8_t throttle_percentage(void);
void update_dynamic_notch();
bool auto_takeoff_check(void);
void takeoff_calc_roll(void);
void takeoff_calc_pitch(void);

42
ArduPlane/system.cpp
View File

@ -475,3 +475,45 @@ int8_t Plane::throttle_percentage(void)
}
return constrain_int16(throttle, -100, 100);
}
// update the harmonic notch filter center frequency dynamically
void Plane::update_dynamic_notch()
{
if (!ins.gyro_harmonic_notch_enabled()) {
return;
}
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;
}
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
if (quadplane.available()) {
ins.update_harmonic_notch_freq_hz(ref_freq * MAX(1.0f, sqrtf(quadplane.motors->get_throttle_out() / ref)));
}
break;
case HarmonicNotchDynamicMode::UpdateRPM: // rpm sensor based tracking
if (rpm_sensor.healthy(0)) {
// set the harmonic notch filter frequency from the main rotor rpm
ins.update_harmonic_notch_freq_hz(MAX(ref_freq, rpm_sensor.get_rpm(0) * ref / 60.0f));
} else {
ins.update_harmonic_notch_freq_hz(ref_freq);
}
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));
break;
#endif
case HarmonicNotchDynamicMode::Fixed: // static
default:
ins.update_harmonic_notch_freq_hz(ref_freq);
break;
}
}