mirror of https://github.com/ArduPilot/ardupilot
AP_GyroFFT: added defaults for FFT with no notch
allow for testing with FFT enabled, and defaulting number of frequencies to look for
This commit is contained in:
parent
dff0e5ecc1
commit
bccca9ed2e
|
@ -253,11 +253,15 @@ void AP_GyroFFT::init(uint16_t loop_rate_hz)
|
||||||
uint8_t harmonics = 0;
|
uint8_t harmonics = 0;
|
||||||
uint8_t num_notches = 0;
|
uint8_t num_notches = 0;
|
||||||
for (auto ¬ch : _ins->harmonic_notches) {
|
for (auto ¬ch : _ins->harmonic_notches) {
|
||||||
if (notch.params.enabled() && notch.params.tracking_mode() == HarmonicNotchDynamicMode::UpdateGyroFFT) {
|
if (notch.params.enabled()) {
|
||||||
harmonics |= notch.params.harmonics();
|
harmonics |= notch.params.harmonics();
|
||||||
num_notches = MAX(num_notches, notch.num_dynamic_notches);
|
num_notches = MAX(num_notches, notch.num_dynamic_notches);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
if (harmonics == 0) {
|
||||||
|
// this allows use of FFT to find peaks with all notch filters disabled
|
||||||
|
harmonics = 3;
|
||||||
|
}
|
||||||
// count the number of active harmonics or dynamic notchs
|
// count the number of active harmonics or dynamic notchs
|
||||||
_tracked_peaks = constrain_int16(MAX(__builtin_popcount(harmonics),
|
_tracked_peaks = constrain_int16(MAX(__builtin_popcount(harmonics),
|
||||||
num_notches), 1, FrequencyPeak::MAX_TRACKED_PEAKS);
|
num_notches), 1, FrequencyPeak::MAX_TRACKED_PEAKS);
|
||||||
|
@ -383,7 +387,7 @@ void AP_GyroFFT::update()
|
||||||
if (!_rpy_health.x && !_rpy_health.y) {
|
if (!_rpy_health.x && !_rpy_health.y) {
|
||||||
_health = 0;
|
_health = 0;
|
||||||
} else {
|
} else {
|
||||||
uint8_t num_notches = 0;
|
uint8_t num_notches = 1;
|
||||||
for (auto ¬ch : _ins->harmonic_notches) {
|
for (auto ¬ch : _ins->harmonic_notches) {
|
||||||
if (notch.params.enabled()) {
|
if (notch.params.enabled()) {
|
||||||
num_notches = MAX(num_notches, notch.num_dynamic_notches);
|
num_notches = MAX(num_notches, notch.num_dynamic_notches);
|
||||||
|
|
Loading…
Reference in New Issue