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

ArduCopter: add support for BLHeli telemetry-based updates to the harmonic notch 

refactor to include RPM for all copter types
This commit is contained in:
Andy Piper 2019-10-12 13:04:45 +01:00 committed by Randy Mackay
parent 3b6598b0e9
commit bf12c686c7
2 changed files with 32 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduCopter/defines.h
View File

@ -172,6 +172,14 @@ 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)

34
ArduCopter/system.cpp
View File

@ -1,4 +1,5 @@
#include "Copter.h"
#include <AP_BLHeli/AP_BLHeli.h>
/*****************************************************************************
* The init_ardupilot function processes everything we need for an in - air restart
@ -294,18 +295,31 @@ void Copter::update_dynamic_notch()
return;
}
if (is_tradheli()) {
switch (ins.get_gyro_harmonic_notch_tracking_mode()) {
case HarmonicNotch_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
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);
}
case HarmonicNotch_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;
#endif
} else {
// 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)));
#ifdef HAVE_AP_BLHELI_SUPPORT
case HarmonicNotch_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
default:
ins.update_harmonic_notch_freq_hz(ref_freq);
break;
}
}