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

AP_InertialSensor: delegate active harmonic calculation to the filter

Browse Source
This commit is contained in:
Andy Piper 2020-07-12 20:09:43 +01:00 committed by Andrew Tridgell
parent a93821110e
commit 05366e9aa9
1 changed files with 1 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -742,17 +742,8 @@ AP_InertialSensor::init(uint16_t loop_rate)
_calculated_harmonic_notch_freq_hz[0] = _harmonic_notch_filter.center_freq_hz();
_num_calculated_harmonic_notch_frequencies = 1;
uint8_t harmonics = _harmonic_notch_filter.harmonics();
// allocate INS_MAX_NOTCHES as harmonics if using DynamicHarmonic
if (_harmonic_notch_filter.hasOption(HarmonicNotchFilterParams::Options::DynamicHarmonic)) {
harmonics = 0;
for (uint8_t h = 0; h < INS_MAX_NOTCHES; h++) {
harmonics = (harmonics << 1) + 1;
}
}
for (uint8_t i=0; i<get_gyro_count(); i++) {
_gyro_harmonic_notch_filter[i].allocate_filters(harmonics, _harmonic_notch_filter.hasOption(HarmonicNotchFilterParams::Options::DoubleNotch));
_gyro_harmonic_notch_filter[i].allocate_filters(_harmonic_notch_filter.harmonics(), _harmonic_notch_filter.hasOption(HarmonicNotchFilterParams::Options::DoubleNotch));
// initialise default settings, these will be subsequently changed in AP_InertialSensor_Backend::update_gyro()
_gyro_harmonic_notch_filter[i].init(_gyro_raw_sample_rates[i], _calculated_harmonic_notch_freq_hz[0],
_harmonic_notch_filter.bandwidth_hz(), _harmonic_notch_filter.attenuation_dB());