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

Copter: use HarmonicNotchDynamicMode enum class

This commit is contained in:
Andrew Tridgell 2019-11-17 16:16:41 +11:00 committed by Randy Mackay
parent 9dc618ddd4
commit 68bb7a516b
2 changed files with 7 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduCopter/defines.h
View File

@ -167,14 +167,6 @@ enum LoggingParameters {
LOG_SYSIDS_MSG,
};
// Harmonic notch update mode
enum HarmonicNotchDynamicMode {
HarmonicNotch_Fixed,
HarmonicNotch_UpdateThrottle,
HarmonicNotch_UpdateRPM,
HarmonicNotch_UpdateBLHeli,
};
#define MASK_LOG_ATTITUDE_FAST (1<<0)
#define MASK_LOG_ATTITUDE_MED (1<<1)
#define MASK_LOG_GPS (1<<2)

11
ArduCopter/system.cpp
View File

@ -289,6 +289,9 @@ void Copter::startup_INS_ground()
// update the harmonic notch filter center frequency dynamically
void Copter::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();
@ -298,13 +301,13 @@ void Copter::update_dynamic_notch()
}
switch (ins.get_gyro_harmonic_notch_tracking_mode()) {
case HarmonicNotch_UpdateThrottle: // throttle based tracking
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)));
break;
#if RPM_ENABLED == ENABLED
case HarmonicNotch_UpdateRPM: // rpm sensor based tracking
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));
@ -314,11 +317,11 @@ void Copter::update_dynamic_notch()
break;
#endif
#ifdef HAVE_AP_BLHELI_SUPPORT
case HarmonicNotch_UpdateBLHeli: // BLHeli based tracking
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 HarmonicNotch_Fixed: // static
case HarmonicNotchDynamicMode::Fixed: // static
default:
ins.update_harmonic_notch_freq_hz(ref_freq);
break;