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

AP_InertialSensor: add set_inactive() on notch filters

This commit is contained in:
Andrew Tridgell 2022-06-12 12:32:46 +10:00
parent c085b713ac
commit 9ac3472b47
2 changed files with 20 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -446,12 +446,22 @@ public:
// Update the harmonic notch frequencies
void update_freq_hz(float scaled_freq);
void update_frequencies_hz(uint8_t num_freqs, const float scaled_freq[]);
// enable/disable the notch
void set_inactive(bool _inactive) {
inactive = _inactive;
}
bool is_inactive(void) const {
return inactive;
}
private:
// support for updating harmonic filter at runtime
float last_center_freq_hz[INS_MAX_INSTANCES];
float last_bandwidth_hz[INS_MAX_INSTANCES];
float last_attenuation_dB[INS_MAX_INSTANCES];
bool inactive;
} harmonic_notches[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS];
private:

12
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -186,17 +186,23 @@ void AP_InertialSensor_Backend::apply_gyro_filters(const uint8_t instance, const
if (!notch.params.enabled()) {
continue;
}
bool inactive = notch.is_inactive();
#ifndef HAL_BUILD_AP_PERIPH
// by default we only run the expensive notch filters on the
// currently active IMU we reset the inactive notch filters so
// that if we switch IMUs we're not left with old data
if (!notch.params.hasOption(HarmonicNotchFilterParams::Options::EnableOnAllIMUs) &&
instance != AP::ahrs().get_primary_gyro_index()) {
notch.filter[instance].reset();
continue;
inactive = true;
}
#endif
gyro_filtered = notch.filter[instance].apply(gyro_filtered);
if (inactive) {
// while inactive we reset the filter so when it activates the first output
// will be the first input sample
notch.filter[instance].reset();
} else {
gyro_filtered = notch.filter[instance].apply(gyro_filtered);
}
}
// apply the low pass filter last to attentuate any notch induced noise