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

AP_InertialSensor: add harmonic notch filter to gyro filter chain

Browse Source 
Allow dynamic updates to the calculated frequency. Convert bandwidth and frequency to floats.
backend variables do not need to be indexed per-backed
This commit is contained in:
Andy Piper 2019-06-17 09:44:12 +01:00 committed by Andrew Tridgell
parent 366bc06089
commit 50f7e50634
4 changed files with 96 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -453,6 +453,10 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
// @Bitmask: 0:FirstIMU,1:SecondIMU,2:ThirdIMU
AP_GROUPINFO("ENABLE_MASK", 40, AP_InertialSensor, _enable_mask, 0x7F),
// @Group: HNTCH_
// @Path: ../Filter/HarmonicNotchFilter.cpp
AP_SUBGROUPINFO(_harmonic_notch_filter, "HNTCH_", 41, AP_InertialSensor, HarmonicNotchFilterParams),
/*
NOTE: parameter indexes have gaps above. When adding new
parameters check for conflicts carefully
@ -471,6 +475,7 @@ AP_InertialSensor::AP_InertialSensor() :
}
_singleton = this;
AP_Param::setup_object_defaults(this, var_info);
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
_gyro_cal_ok[i] = true;
_accel_max_abs_offsets[i] = 3.5f;
@ -654,6 +659,15 @@ AP_InertialSensor::init(uint16_t sample_rate)
// initialise IMU batch logging
batchsampler.init();
// 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();
for (uint8_t i=0; i<get_gyro_count(); i++) {
_gyro_harmonic_notch_filter[i].create(_harmonic_notch_filter.harmonics());
}
}
bool AP_InertialSensor::_add_backend(AP_InertialSensor_Backend *backend)
@ -1709,6 +1723,14 @@ void AP_InertialSensor::acal_update()
}
}
// Update the harmonic notch frequency
void AP_InertialSensor::update_harmonic_notch_freq_hz(float scaled_freq) {
// When disarmed, throttle is zero
if (is_positive(scaled_freq)) {
_calculated_harmonic_notch_freq_hz = scaled_freq;
}
}
/*
set and save accelerometer bias along with trim calculation
*/

19
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -25,6 +25,7 @@
#include <Filter/LowPassFilter2p.h>
#include <Filter/LowPassFilter.h>
#include <Filter/NotchFilter.h>
#include <Filter/HarmonicNotchFilter.h>
class AP_InertialSensor_Backend;
class AuxiliaryBus;
@ -202,6 +203,9 @@ public:
uint8_t get_primary_accel(void) const { return _primary_accel; }
uint8_t get_primary_gyro(void) const { return _primary_gyro; }
// Update the harmonic notch frequency
void update_harmonic_notch_freq_hz(float scaled_freq);
// enable HIL mode
void set_hil_mode(void) { _hil_mode = true; }
@ -211,6 +215,15 @@ public:
// get the accel filter rate in Hz
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; }
// harmonic notch reference center frequency
float get_gyro_harmonic_notch_center_freq_hz(void) const { return _harmonic_notch_filter.center_freq_hz(); }
// harmonic notch reference scale factor
float get_gyro_harmonic_notch_reference(void) const { return _harmonic_notch_filter.reference(); }
// 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; }
@ -411,6 +424,12 @@ private:
NotchFilterParams _notch_filter;
NotchFilterVector3f _gyro_notch_filter[INS_MAX_INSTANCES];
// optional harmonic notch filter on gyro
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;
// Most recent gyro reading
Vector3f _gyro[INS_MAX_INSTANCES];
Vector3f _delta_angle[INS_MAX_INSTANCES];

40
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -226,6 +226,12 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
// apply the low pass filter
_imu._gyro_filtered[instance] = _imu._gyro_filter[instance].apply(gyro);
// apply the harmonic notch filter
if (_gyro_harmonic_notch_enabled()) {
_imu._gyro_filtered[instance] = _imu._gyro_harmonic_notch_filter[instance].apply(_imu._gyro_filtered[instance]);
}
// apply the notch filter
if (_gyro_notch_enabled()) {
_imu._gyro_filtered[instance] = _imu._gyro_notch_filter[instance].apply(_imu._gyro_filtered[instance]);
@ -233,6 +239,7 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
if (_imu._gyro_filtered[instance].is_nan() || _imu._gyro_filtered[instance].is_inf()) {
_imu._gyro_filter[instance].reset();
_imu._gyro_notch_filter[instance].reset();
_imu._gyro_harmonic_notch_filter[instance].reset();
}
_imu._new_gyro_data[instance] = true;
}
@ -493,18 +500,31 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance)
}
// possibly update filter frequency
if (_last_gyro_filter_hz[instance] != _gyro_filter_cutoff()) {
if (_last_gyro_filter_hz != _gyro_filter_cutoff()) {
_imu._gyro_filter[instance].set_cutoff_frequency(_gyro_raw_sample_rate(instance), _gyro_filter_cutoff());
_last_gyro_filter_hz[instance] = _gyro_filter_cutoff();
_last_gyro_filter_hz = _gyro_filter_cutoff();
}
// possily update the harmonic notch filter parameters
if (!is_equal(_last_harmonic_notch_bandwidth_hz, _gyro_harmonic_notch_bandwidth_hz()) ||
!is_equal(_last_harmonic_notch_attenuation_dB, _gyro_harmonic_notch_attenuation_dB())) {
_imu._gyro_harmonic_notch_filter[instance].init(_gyro_raw_sample_rate(instance), _gyro_harmonic_notch_center_freq_hz(), _gyro_harmonic_notch_bandwidth_hz(), _gyro_harmonic_notch_attenuation_dB());
_last_harmonic_notch_center_freq_hz = _gyro_harmonic_notch_center_freq_hz();
_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());
_last_harmonic_notch_center_freq_hz = _gyro_harmonic_notch_center_freq_hz();
}
// possily update the notch filter parameters
if (_last_notch_center_freq_hz[instance] != _gyro_notch_center_freq_hz() ||
_last_notch_bandwidth_hz[instance] != _gyro_notch_bandwidth_hz() ||
!is_equal(_last_notch_attenuation_dB[instance], _gyro_notch_attenuation_dB())) {
if (!is_equal(_last_notch_center_freq_hz, _gyro_notch_center_freq_hz()) ||
!is_equal(_last_notch_bandwidth_hz, _gyro_notch_bandwidth_hz()) ||
!is_equal(_last_notch_attenuation_dB, _gyro_notch_attenuation_dB())) {
_imu._gyro_notch_filter[instance].init(_gyro_raw_sample_rate(instance), _gyro_notch_center_freq_hz(), _gyro_notch_bandwidth_hz(), _gyro_notch_attenuation_dB());
_last_notch_center_freq_hz[instance] = _gyro_notch_center_freq_hz();
_last_notch_bandwidth_hz[instance] = _gyro_notch_bandwidth_hz();
_last_notch_attenuation_dB[instance] = _gyro_notch_attenuation_dB();
_last_notch_center_freq_hz = _gyro_notch_center_freq_hz();
_last_notch_bandwidth_hz = _gyro_notch_bandwidth_hz();
_last_notch_attenuation_dB = _gyro_notch_attenuation_dB();
}
}
@ -524,9 +544,9 @@ void AP_InertialSensor_Backend::update_accel(uint8_t instance)
}
// possibly update filter frequency
if (_last_accel_filter_hz[instance] != _accel_filter_cutoff()) {
if (_last_accel_filter_hz != _accel_filter_cutoff()) {
_imu._accel_filter[instance].set_cutoff_frequency(_accel_raw_sample_rate(instance), _accel_filter_cutoff());
_last_accel_filter_hz[instance] = _accel_filter_cutoff();
_last_accel_filter_hz = _accel_filter_cutoff();
}
}

34
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -75,7 +75,7 @@ public:
// notify of a fifo reset
void notify_fifo_reset(void);
/*
device driver IDs. These are used to fill in the devtype field
of the device ID, which shows up as INS*ID* parameters to
@ -179,7 +179,7 @@ protected:
// update the sensor rate for FIFO sensors
void _update_sensor_rate(uint16_t &count, uint32_t &start_us, float &rate_hz) const;
// set accelerometer max absolute offset for calibration
void _set_accel_max_abs_offset(uint8_t instance, float offset);
@ -233,16 +233,27 @@ protected:
uint16_t get_sample_rate_hz(void) const;
// return the notch filter center in Hz for the sample rate
uint16_t _gyro_notch_center_freq_hz(void) const { return _imu._notch_filter.center_freq_hz(); }
float _gyro_notch_center_freq_hz(void) const { return _imu._notch_filter.center_freq_hz(); }
// return the notch filter bandwidth in Hz for the sample rate
uint16_t _gyro_notch_bandwidth_hz(void) const { return _imu._notch_filter.bandwidth_hz(); }
float _gyro_notch_bandwidth_hz(void) const { return _imu._notch_filter.bandwidth_hz(); }
// return the notch filter attenuation in dB for the sample rate
float _gyro_notch_attenuation_dB(void) const { return _imu._notch_filter.attenuation_dB(); }
uint8_t _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; }
// 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(); }
// return the harmonic notch filter attenuation in dB for the sample rate
float _gyro_harmonic_notch_attenuation_dB(void) const { return _imu._harmonic_notch_filter.attenuation_dB(); }
uint8_t _gyro_harmonic_notch_enabled(void) const { return _imu._harmonic_notch_filter.enabled(); }
// common gyro update function for all backends
void update_gyro(uint8_t instance);
@ -250,12 +261,17 @@ protected:
void update_accel(uint8_t instance);
// support for updating filter at runtime
uint16_t _last_accel_filter_hz[INS_MAX_INSTANCES];
uint16_t _last_gyro_filter_hz[INS_MAX_INSTANCES];
uint16_t _last_notch_center_freq_hz[INS_MAX_INSTANCES];
uint16_t _last_notch_bandwidth_hz[INS_MAX_INSTANCES];
float _last_notch_attenuation_dB[INS_MAX_INSTANCES];
uint16_t _last_accel_filter_hz;
uint16_t _last_gyro_filter_hz;
float _last_notch_center_freq_hz;
float _last_notch_bandwidth_hz;
float _last_notch_attenuation_dB;
// support for updating harmonic filter at runtime
float _last_harmonic_notch_center_freq_hz;
float _last_harmonic_notch_bandwidth_hz;
float _last_harmonic_notch_attenuation_dB;
void set_gyro_orientation(uint8_t instance, enum Rotation rotation) {
_imu._gyro_orientation[instance] = rotation;
}