mirror of https://github.com/ArduPilot/ardupilot
AP_InertialSensor: add set_inactive() on notch filters
This commit is contained in:
parent
c085b713ac
commit
9ac3472b47
|
@ -447,11 +447,21 @@ public:
|
|||
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:
|
||||
|
|
|
@ -186,18 +186,24 @@ 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
|
||||
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
|
||||
gyro_filtered = _imu._gyro_filter[instance].apply(gyro_filtered);
|
||||
|
|
Loading…
Reference in New Issue