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

AP_InertialSensor: allow up to four distinct notch center frequencies to be updated 

apply the LPF after the notch filters to reduced notch-induced noise
This commit is contained in:
Andy Piper 2020-05-29 17:28:06 +01:00 committed by Andrew Tridgell
parent bd7190767e
commit 61bb42cc0c
4 changed files with 60 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -723,13 +723,22 @@ AP_InertialSensor::init(uint16_t sample_rate)
// the center frequency of the harmonic notch is always taken from the calculated value so that it can be updated
// dynamically, the calculated value is always some multiple of the configured center frequency, so start with the
// configured value
_calculated_harmonic_notch_freq_hz = _harmonic_notch_filter.center_freq_hz();
_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(_harmonic_notch_filter.harmonics(),
_harmonic_notch_filter.hasOption(HarmonicNotchFilterParams::Options::DoubleNotch));
_gyro_harmonic_notch_filter[i].allocate_filters(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,
_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());
}
}
@ -1792,8 +1801,21 @@ void AP_InertialSensor::acal_update()
void AP_InertialSensor::update_harmonic_notch_freq_hz(float scaled_freq) {
// protect against zero as the scaled frequency
if (is_positive(scaled_freq)) {
_calculated_harmonic_notch_freq_hz = scaled_freq;
_calculated_harmonic_notch_freq_hz[0] = scaled_freq;
}
_num_calculated_harmonic_notch_frequencies = 1;
}
// Update the harmonic notch frequency
void AP_InertialSensor::update_harmonic_notch_frequencies_hz(uint8_t num_freqs, const float scaled_freq[]) {
// protect against zero as the scaled frequency
for (uint8_t i = 0; i < num_freqs; i++) {
if (is_positive(scaled_freq[i])) {
_calculated_harmonic_notch_freq_hz[i] = scaled_freq[i];
}
}
// any uncalculated frequencies will float at the previous value or the initialized freq if none
_num_calculated_harmonic_notch_frequencies = num_freqs;
}
/*

19
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -13,6 +13,7 @@
*/
#define INS_MAX_INSTANCES 3
#define INS_MAX_BACKENDS 6
#define INS_MAX_NOTCHES 4
#define INS_VIBRATION_CHECK_INSTANCES 2
#define XYZ_AXIS_COUNT 3
// The maximum we need to store is gyro-rate / loop-rate, worst case ArduCopter with BMI088 is 2000/400
@ -218,6 +219,8 @@ public:
// Update the harmonic notch frequency
void update_harmonic_notch_freq_hz(float scaled_freq);
// Update the harmonic notch frequencies
void update_harmonic_notch_frequencies_hz(uint8_t num_freqs, const float scaled_freq[]);
// enable HIL mode
void set_hil_mode(void) { _hil_mode = true; }
@ -229,7 +232,13 @@ public:
uint16_t get_accel_filter_hz(void) const { return _accel_filter_cutoff; }
// harmonic notch current center frequency
float get_gyro_dynamic_notch_center_freq_hz(void) const { return _calculated_harmonic_notch_freq_hz; }
float get_gyro_dynamic_notch_center_freq_hz(void) const { return _calculated_harmonic_notch_freq_hz[0]; }
// set of harmonic notch current center frequencies
const float* get_gyro_dynamic_notch_center_frequencies_hz(void) const { return _calculated_harmonic_notch_freq_hz; }
// number of harmonic notch current center frequencies
uint8_t get_num_gyro_dynamic_notch_center_frequencies(void) const { return _num_calculated_harmonic_notch_frequencies; }
// harmonic notch reference center frequency
float get_gyro_harmonic_notch_center_freq_hz(void) const { return _harmonic_notch_filter.center_freq_hz(); }
@ -243,6 +252,11 @@ public:
// harmonic notch harmonics
uint8_t get_gyro_harmonic_notch_harmonics(void) const { return _harmonic_notch_filter.harmonics(); }
// harmonic notch options
bool has_harmonic_option(HarmonicNotchFilterParams::Options option) {
return _harmonic_notch_filter.hasOption(option);
}
// indicate which bit in LOG_BITMASK indicates raw logging enabled
void set_log_raw_bit(uint32_t log_raw_bit) { _log_raw_bit = log_raw_bit; }
@ -456,7 +470,8 @@ private:
HarmonicNotchFilterParams _harmonic_notch_filter;
HarmonicNotchFilterVector3f _gyro_harmonic_notch_filter[INS_MAX_INSTANCES];
// the current center frequency for the notch
float _calculated_harmonic_notch_freq_hz;
float _calculated_harmonic_notch_freq_hz[INS_MAX_NOTCHES];
uint8_t _num_calculated_harmonic_notch_frequencies;
// Most recent gyro reading
Vector3f _gyro[INS_MAX_INSTANCES];

12
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -234,8 +234,7 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
_imu._gyro_window[instance][2].push(scaled_gyro.z);
}
#endif
// apply the low pass filter
Vector3f gyro_filtered = _imu._gyro_filter[instance].apply(gyro);
Vector3f gyro_filtered = gyro;
// apply the notch filter
if (_gyro_notch_enabled()) {
@ -247,6 +246,9 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
gyro_filtered = _imu._gyro_harmonic_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);
// if the filtering failed in any way then reset the filters and keep the old value
if (gyro_filtered.is_nan() || gyro_filtered.is_inf()) {
_imu._gyro_filter[instance].reset();
@ -539,7 +541,11 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance)
_last_harmonic_notch_bandwidth_hz = gyro_harmonic_notch_bandwidth_hz();
_last_harmonic_notch_attenuation_dB = gyro_harmonic_notch_attenuation_dB();
} else if (!is_equal(_last_harmonic_notch_center_freq_hz, gyro_harmonic_notch_center_freq_hz())) {
_imu._gyro_harmonic_notch_filter[instance].update(gyro_harmonic_notch_center_freq_hz());
if (num_gyro_harmonic_notch_center_frequencies() > 1) {
_imu._gyro_harmonic_notch_filter[instance].update(num_gyro_harmonic_notch_center_frequencies(), gyro_harmonic_notch_center_frequencies_hz());
} else {
_imu._gyro_harmonic_notch_filter[instance].update(gyro_harmonic_notch_center_freq_hz());
}
_last_harmonic_notch_center_freq_hz = gyro_harmonic_notch_center_freq_hz();
}
// possily update the notch filter parameters

8
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -248,7 +248,13 @@ protected:
bool _gyro_notch_enabled(void) const { return _imu._notch_filter.enabled(); }
// return the harmonic notch filter center in Hz for the sample rate
float gyro_harmonic_notch_center_freq_hz() const { return _imu._calculated_harmonic_notch_freq_hz; }
float gyro_harmonic_notch_center_freq_hz() const { return _imu.get_gyro_dynamic_notch_center_freq_hz(); }
// set of harmonic notch current center frequencies
const float* gyro_harmonic_notch_center_frequencies_hz(void) const { return _imu.get_gyro_dynamic_notch_center_frequencies_hz(); }
// number of harmonic notch current center frequencies
uint8_t num_gyro_harmonic_notch_center_frequencies(void) const { return _imu.get_num_gyro_dynamic_notch_center_frequencies(); }
// return the harmonic notch filter bandwidth in Hz for the sample rate
float gyro_harmonic_notch_bandwidth_hz(void) const { return _imu._harmonic_notch_filter.bandwidth_hz(); }