Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: add set_inactive() on notch filters

Browse Source
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

10
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -447,11 +447,21 @@ public:
void update_freq_hz(float scaled_freq); void update_freq_hz(float scaled_freq);
void update_frequencies_hz(uint8_t num_freqs, const 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: private:
// support for updating harmonic filter at runtime // support for updating harmonic filter at runtime
float last_center_freq_hz[INS_MAX_INSTANCES]; float last_center_freq_hz[INS_MAX_INSTANCES];
float last_bandwidth_hz[INS_MAX_INSTANCES]; float last_bandwidth_hz[INS_MAX_INSTANCES];
float last_attenuation_dB[INS_MAX_INSTANCES]; float last_attenuation_dB[INS_MAX_INSTANCES];
bool inactive;
} harmonic_notches[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS]; } harmonic_notches[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS];
private: private:

10
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -186,18 +186,24 @@ void AP_InertialSensor_Backend::apply_gyro_filters(const uint8_t instance, const
if (!notch.params.enabled()) { if (!notch.params.enabled()) {
continue; continue;
} }
bool inactive = notch.is_inactive();
#ifndef HAL_BUILD_AP_PERIPH #ifndef HAL_BUILD_AP_PERIPH
// by default we only run the expensive notch filters on the // by default we only run the expensive notch filters on the
// currently active IMU we reset the inactive notch filters so // currently active IMU we reset the inactive notch filters so
// that if we switch IMUs we're not left with old data // that if we switch IMUs we're not left with old data
if (!notch.params.hasOption(HarmonicNotchFilterParams::Options::EnableOnAllIMUs) && if (!notch.params.hasOption(HarmonicNotchFilterParams::Options::EnableOnAllIMUs) &&
instance != AP::ahrs().get_primary_gyro_index()) { instance != AP::ahrs().get_primary_gyro_index()) {
notch.filter[instance].reset(); inactive = true;
continue;
} }
#endif #endif
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); gyro_filtered = notch.filter[instance].apply(gyro_filtered);
} }
}
// apply the low pass filter last to attentuate any notch induced noise // apply the low pass filter last to attentuate any notch induced noise
gyro_filtered = _imu._gyro_filter[instance].apply(gyro_filtered); gyro_filtered = _imu._gyro_filter[instance].apply(gyro_filtered);