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

AP_InertialSensor: Rework notch filter allocation and sampling

Browse Source 
https://github.com/ArduPilot/ardupilot/issues/11346

Allocate a notch filter per-IMU.
Update the notch filters in the backend at the sensor sample rate.
Allow raw logging of post-filtered gyro and accel values.
This commit is contained in:
Andy Piper 2019-05-17 16:57:43 +01:00 committed by Andrew Tridgell
parent 045bdf3478
commit 18d403c3ac
5 changed files with 55 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -439,7 +439,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
// @Group: NOTCH_
// @Path: ../Filter/NotchFilter.cpp
AP_SUBGROUPINFO(_notch_filter, "NOTCH_", 37, AP_InertialSensor, NotchFilterVector3fParam),
AP_SUBGROUPINFO(_notch_filter, "NOTCH_", 37, AP_InertialSensor, NotchFilterParams),
// @Group: LOG_
// @Path: ../AP_InertialSensor/BatchSampler.cpp
@ -646,8 +646,6 @@ AP_InertialSensor::init(uint16_t sample_rate)
_sample_period_usec = 1000*1000UL / _sample_rate;
_notch_filter.init(sample_rate);
// establish the baseline time between samples
_delta_time = 0;
_next_sample_usec = 0;
@ -1339,9 +1337,6 @@ void AP_InertialSensor::update(void)
}
}
// apply notch filter to primary gyro
_gyro[_primary_gyro] = _notch_filter.apply(_gyro[_primary_gyro]);
_last_update_usec = AP_HAL::micros();
_have_sample = false;

6
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -294,6 +294,7 @@ public:
void periodic();
bool doing_sensor_rate_logging() const { return _doing_sensor_rate_logging; }
bool doing_post_filter_logging() const { return _doing_post_filter_logging; }
// class level parameters
static const struct AP_Param::GroupInfo var_info[];
@ -320,6 +321,7 @@ public:
enum batch_opt_t {
BATCH_OPT_SENSOR_RATE = (1<<0),
BATCH_OPT_POST_FILTER = (1<<1),
};
void rotate_to_next_sensor();
@ -333,6 +335,7 @@ public:
bool initialised : 1;
bool isbh_sent : 1;
bool _doing_sensor_rate_logging : 1;
bool _doing_post_filter_logging : 1;
uint8_t instance : 3; // instance we are sending data for
AP_InertialSensor::IMU_SENSOR_TYPE type : 1;
uint16_t isb_seqnum;
@ -402,7 +405,8 @@ private:
bool _new_gyro_data[INS_MAX_INSTANCES];
// optional notch filter on gyro
NotchFilterVector3fParam _notch_filter;
NotchFilterParams _notch_filter;
NotchFilterVector3f _gyro_notch_filter[INS_MAX_INSTANCES];
// Most recent gyro reading
Vector3f _gyro[INS_MAX_INSTANCES];

28
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -204,14 +204,25 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
_imu._last_delta_angle[instance] = delta_angle;
_imu._last_raw_gyro[instance] = gyro;
// apply the low pass filter
_imu._gyro_filtered[instance] = _imu._gyro_filter[instance].apply(gyro);
// apply the notch filter
if (_gyro_notch_enabled()) {
_imu._gyro_filtered[instance] = _imu._gyro_notch_filter[instance].apply(gyro);
}
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._new_gyro_data[instance] = true;
}
log_gyro_raw(instance, sample_us, gyro);
if (!_imu.batchsampler.doing_post_filter_logging()) {
log_gyro_raw(instance, sample_us, gyro);
}
else {
log_gyro_raw(instance, sample_us, _imu._gyro_filtered[instance]);
}
}
void AP_InertialSensor_Backend::log_gyro_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &gyro)
@ -326,7 +337,11 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
_imu._new_accel_data[instance] = true;
}
log_accel_raw(instance, sample_us, accel);
if (!_imu.batchsampler.doing_post_filter_logging()) {
log_accel_raw(instance, sample_us, accel);
} else {
log_accel_raw(instance, sample_us, _imu._accel_filtered[instance]);
}
}
void AP_InertialSensor_Backend::_notify_new_accel_sensor_rate_sample(uint8_t instance, const Vector3f &accel)
@ -438,6 +453,15 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance)
_imu._gyro_filter[instance].set_cutoff_frequency(_gyro_raw_sample_rate(instance), _gyro_filter_cutoff());
_last_gyro_filter_hz[instance] = _gyro_filter_cutoff();
}
// 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())) {
_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();
}
}
/*

18
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -232,6 +232,17 @@ protected:
// return the requested sample rate in Hz
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(); }
// return the notch filter bandwidth in Hz for the sample rate
uint8_t _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(); }
// common gyro update function for all backends
void update_gyro(uint8_t instance);
@ -239,8 +250,11 @@ protected:
void update_accel(uint8_t instance);
// support for updating filter at runtime
int8_t _last_accel_filter_hz[INS_MAX_INSTANCES];
int8_t _last_gyro_filter_hz[INS_MAX_INSTANCES];
uint8_t _last_accel_filter_hz[INS_MAX_INSTANCES];
uint8_t _last_gyro_filter_hz[INS_MAX_INSTANCES];
uint16_t _last_notch_center_freq_hz[INS_MAX_INSTANCES];
uint8_t _last_notch_bandwidth_hz[INS_MAX_INSTANCES];
float _last_notch_attenuation_dB[INS_MAX_INSTANCES];
void set_gyro_orientation(uint8_t instance, enum Rotation rotation) {
_imu._gyro_orientation[instance] = rotation;

8
libraries/AP_InertialSensor/BatchSampler.cpp
View File

@ -22,7 +22,7 @@ const AP_Param::GroupInfo AP_InertialSensor::BatchSampler::var_info[] = {
// @Param: BAT_OPT
// @DisplayName: Batch Logging Options Mask
// @Description: Options for the BatchSampler
// @Bitmask: 0:Sensor-Rate Logging (sample at full sensor rate seen by AP)
// @Bitmask: 0:Sensor-Rate Logging (sample at full sensor rate seen by AP), 1: Sample post-filtering
// @User: Advanced
AP_GROUPINFO("BAT_OPT", 3, AP_InertialSensor::BatchSampler, _batch_options_mask, 0),
@ -87,8 +87,14 @@ void AP_InertialSensor::BatchSampler::periodic()
void AP_InertialSensor::BatchSampler::update_doing_sensor_rate_logging()
{
if (((batch_opt_t)(_batch_options_mask.get()) & BATCH_OPT_POST_FILTER)
&& type == IMU_SENSOR_TYPE_GYRO) {
_doing_post_filter_logging = true;
return;
}
if (!((batch_opt_t)(_batch_options_mask.get()) & BATCH_OPT_SENSOR_RATE)) {
_doing_sensor_rate_logging = false;
_doing_post_filter_logging = false;
return;
}
const uint8_t bit = (1<<instance);