aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialSensor: use num_composite_notches()

Browse Source 
this prevents duplication of the logic for the priority of the double
notch vs the triple notch option
This commit is contained in:
Andrew Tridgell 2024-01-22 07:55:03 +11:00
parent 2286f2ce27
commit 25ffcc1580
1 changed files with 2 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -1016,11 +1016,9 @@ AP_InertialSensor::init(uint16_t loop_rate)
for (auto &notch : harmonic_notches) {
// calculate number of notches we might want to use for harmonic notch
if (notch.params.enabled() || fft_enabled) {
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);
num_filters += __builtin_popcount(notch.params.harmonics())
* notch.num_dynamic_notches * (double_notch ? 2 : triple_notch ? 3 : 1)
* notch.num_dynamic_notches * notch.params.num_composite_notches()
* (all_sensors?sensors_used:1);
}
}
@ -1035,10 +1033,8 @@ AP_InertialSensor::init(uint16_t loop_rate)
if (_use(i)) {
for (auto &notch : harmonic_notches) {
if (notch.params.enabled() || fft_enabled) {
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.params.harmonics(), double_notch ? 2 : triple_notch ? 3 : 1);
notch.params.harmonics(), notch.params.num_composite_notches());
// initialise default settings, these will be subsequently changed in AP_InertialSensor_Backend::update_gyro()
notch.filter[i].init(_gyro_raw_sample_rates[i], notch.params);
}