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

AP_InertialSensor: implement triple notches

This commit is contained in:
Andy Piper 2022-06-23 17:53:39 +01:00 committed by Andrew Tridgell
parent dfba938e63
commit 193375a7e5
1 changed files with 4 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -930,9 +930,10 @@ AP_InertialSensor::init(uint16_t loop_rate)
// calculate number of notches we might want to use for harmonic notch // calculate number of notches we might want to use for harmonic notch
if (notch.params.enabled() || fft_enabled) { if (notch.params.enabled() || fft_enabled) {
const bool double_notch = notch.params.hasOption(HarmonicNotchFilterParams::Options::DoubleNotch); const bool double_notch = notch.params.hasOption(HarmonicNotchFilterParams::Options::DoubleNotch);
const bool triple_notch = notch.params.hasOption(HarmonicNotchFilterParams::Options::TripleNotch);
const bool all_sensors = notch.params.hasOption(HarmonicNotchFilterParams::Options::EnableOnAllIMUs); const bool all_sensors = notch.params.hasOption(HarmonicNotchFilterParams::Options::EnableOnAllIMUs);
num_filters += __builtin_popcount(notch.params.harmonics()) num_filters += __builtin_popcount(notch.params.harmonics())
* notch.num_dynamic_notches * (double_notch ? 2 : 1) * notch.num_dynamic_notches * (double_notch ? 2 : triple_notch ? 3 : 1)
* (all_sensors?sensors_used:1); * (all_sensors?sensors_used:1);
} }
} }
@ -948,8 +949,9 @@ AP_InertialSensor::init(uint16_t loop_rate)
for (auto &notch : harmonic_notches) { for (auto &notch : harmonic_notches) {
if (notch.params.enabled() || fft_enabled) { if (notch.params.enabled() || fft_enabled) {
const bool double_notch = notch.params.hasOption(HarmonicNotchFilterParams::Options::DoubleNotch); const bool double_notch = notch.params.hasOption(HarmonicNotchFilterParams::Options::DoubleNotch);
const bool triple_notch = notch.params.hasOption(HarmonicNotchFilterParams::Options::TripleNotch);
notch.filter[i].allocate_filters(notch.num_dynamic_notches, notch.filter[i].allocate_filters(notch.num_dynamic_notches,
notch.params.harmonics(), double_notch); notch.params.harmonics(), double_notch ? 2 : triple_notch ? 3 : 1);
// initialise default settings, these will be subsequently changed in AP_InertialSensor_Backend::update_gyro() // initialise default settings, these will be subsequently changed in AP_InertialSensor_Backend::update_gyro()
notch.filter[i].init(_gyro_raw_sample_rates[i], notch.calculated_notch_freq_hz[0], notch.filter[i].init(_gyro_raw_sample_rates[i], notch.calculated_notch_freq_hz[0],
notch.params.bandwidth_hz(), notch.params.attenuation_dB()); notch.params.bandwidth_hz(), notch.params.attenuation_dB());