mirror of https://github.com/ArduPilot/ardupilot
AP_InertialSensor: support two full harmonic notch filters
This commit is contained in:
Andrew Tridgell
parent
54a5bd007d
commit
25d00e433e
|
|
@ -548,11 +548,13 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
|
|||
|
||||
// @Group: HNTCH_
|
||||
// @Path: ../Filter/HarmonicNotchFilter.cpp
|
||||
AP_SUBGROUPINFO(_harmonic_notch_filter, "HNTCH_", 41, AP_InertialSensor, HarmonicNotchFilterParams),
|
||||
AP_SUBGROUPINFO(_harmonic_notch_filter[0], "HNTCH_", 41, AP_InertialSensor, HarmonicNotchFilterParams),
|
||||
|
||||
#if HAL_INS_NUM_HARMONIC_NOTCH_FILTERS > 1
|
||||
// @Group: HNTC2_
|
||||
// @Path: ../Filter/HarmonicNotchFilter.cpp
|
||||
AP_SUBGROUPINFO(_notch_filter, "HNTC2_", 53, AP_InertialSensor, HarmonicNotchFilterParams),
|
||||
AP_SUBGROUPINFO(_harmonic_notch_filter[1], "HNTC2_", 53, AP_InertialSensor, HarmonicNotchFilterParams),
|
||||
#endif
|
||||
|
||||
// @Param: GYRO_RATE
|
||||
// @DisplayName: Gyro rate for IMUs with Fast Sampling enabled
|
||||
|
|
@ -874,45 +876,44 @@ AP_InertialSensor::init(uint16_t loop_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[0] = _harmonic_notch_filter.center_freq_hz();
|
||||
_num_calculated_harmonic_notch_frequencies = 1;
|
||||
_num_dynamic_harmonic_notches = 1;
|
||||
|
||||
uint8_t num_filters = 0;
|
||||
const bool double_notch = _harmonic_notch_filter.hasOption(HarmonicNotchFilterParams::Options::DoubleNotch);
|
||||
bool double_notch[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS] {};
|
||||
for (uint8_t i=0; i<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS; i++) {
|
||||
_calculated_harmonic_notch_freq_hz[i][0] = _harmonic_notch_filter[i].center_freq_hz();
|
||||
_num_calculated_harmonic_notch_frequencies[i] = 1;
|
||||
_num_dynamic_harmonic_notches[i] = 1;
|
||||
double_notch[i] = _harmonic_notch_filter[i].hasOption(HarmonicNotchFilterParams::Options::DoubleNotch);
|
||||
#if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane)
|
||||
if (_harmonic_notch_filter.hasOption(HarmonicNotchFilterParams::Options::DynamicHarmonic)) {
|
||||
if (_harmonic_notch_filter[i].hasOption(HarmonicNotchFilterParams::Options::DynamicHarmonic)) {
|
||||
#if HAL_WITH_DSP
|
||||
if (get_gyro_harmonic_notch_tracking_mode() == HarmonicNotchDynamicMode::UpdateGyroFFT) {
|
||||
_num_dynamic_harmonic_notches = AP_HAL::DSP::MAX_TRACKED_PEAKS; // only 3 peaks supported currently
|
||||
if (get_gyro_harmonic_notch_tracking_mode(i) == HarmonicNotchDynamicMode::UpdateGyroFFT) {
|
||||
_num_dynamic_harmonic_notches[i] = AP_HAL::DSP::MAX_TRACKED_PEAKS; // only 3 peaks supported currently
|
||||
} else
|
||||
#endif
|
||||
{
|
||||
AP_Motors *motors = AP::motors();
|
||||
_num_dynamic_harmonic_notches = __builtin_popcount(motors->get_motor_mask());
|
||||
if (motors != nullptr) {
|
||||
_num_dynamic_harmonic_notches[i] = __builtin_popcount(motors->get_motor_mask());
|
||||
}
|
||||
}
|
||||
// avoid harmonics unless actually configured by the user
|
||||
_harmonic_notch_filter.set_default_harmonics(1);
|
||||
}
|
||||
_harmonic_notch_filter[i].set_default_harmonics(1);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
// count number of used sensors
|
||||
uint8_t sensors_used = 0;
|
||||
for (uint8_t i = 0; i < INS_MAX_INSTANCES; i++) {
|
||||
sensors_used += _use[i];
|
||||
}
|
||||
|
||||
// calculate number of notches we might want to use for harmonic notch
|
||||
if (_harmonic_notch_filter.enabled()) {
|
||||
num_filters = __builtin_popcount( _harmonic_notch_filter.harmonics())
|
||||
* _num_dynamic_harmonic_notches * (double_notch ? 2 : 1)
|
||||
* sensors_used;
|
||||
}
|
||||
// add filters used by static notch
|
||||
if (_notch_filter.enabled()) {
|
||||
_notch_filter.set_default_harmonics(1);
|
||||
num_filters += __builtin_popcount(_notch_filter.harmonics())
|
||||
* (_notch_filter.hasOption(HarmonicNotchFilterParams::Options::DoubleNotch) ? 2 : 1)
|
||||
* sensors_used;
|
||||
for (uint8_t i=0; i<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS; i++) {
|
||||
// calculate number of notches we might want to use for harmonic notch
|
||||
if (_harmonic_notch_filter[i].enabled()) {
|
||||
num_filters += __builtin_popcount( _harmonic_notch_filter[i].harmonics())
|
||||
* _num_dynamic_harmonic_notches[i] * (double_notch[i] ? 2 : 1)
|
||||
* sensors_used;
|
||||
}
|
||||
}
|
||||
|
||||
if (num_filters > HAL_HNF_MAX_FILTERS) {
|
||||
|
|
@ -923,18 +924,14 @@ AP_InertialSensor::init(uint16_t loop_rate)
|
|||
for (uint8_t i=0; i<get_gyro_count(); i++) {
|
||||
// only allocate notches for IMUs in use
|
||||
if (_use[i]) {
|
||||
if (_harmonic_notch_filter.enabled()) {
|
||||
_gyro_harmonic_notch_filter[i].allocate_filters(_num_dynamic_harmonic_notches,
|
||||
_harmonic_notch_filter.harmonics(), double_notch);
|
||||
// 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[0],
|
||||
_harmonic_notch_filter.bandwidth_hz(), _harmonic_notch_filter.attenuation_dB());
|
||||
}
|
||||
if (_notch_filter.enabled()) {
|
||||
_gyro_notch_filter[i].allocate_filters(1, _notch_filter.harmonics(),
|
||||
_notch_filter.hasOption(HarmonicNotchFilterParams::Options::DoubleNotch));
|
||||
_gyro_notch_filter[i].init(_gyro_raw_sample_rates[i], _notch_filter.center_freq_hz(),
|
||||
_notch_filter.bandwidth_hz(), _notch_filter.attenuation_dB());
|
||||
for (uint8_t j=0; j<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS; j++) {
|
||||
if (_harmonic_notch_filter[j].enabled()) {
|
||||
_gyro_harmonic_notch_filter[j][i].allocate_filters(_num_dynamic_harmonic_notches[j],
|
||||
_harmonic_notch_filter[j].harmonics(), double_notch[j]);
|
||||
// initialise default settings, these will be subsequently changed in AP_InertialSensor_Backend::update_gyro()
|
||||
_gyro_harmonic_notch_filter[j][i].init(_gyro_raw_sample_rates[i], _calculated_harmonic_notch_freq_hz[j][0],
|
||||
_harmonic_notch_filter[j].bandwidth_hz(), _harmonic_notch_filter[j].attenuation_dB());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -2007,24 +2004,30 @@ void AP_InertialSensor::acal_update()
|
|||
}
|
||||
|
||||
// Update the harmonic notch frequency
|
||||
void AP_InertialSensor::update_harmonic_notch_freq_hz(float scaled_freq) {
|
||||
void AP_InertialSensor::update_harmonic_notch_freq_hz(uint8_t idx, float scaled_freq) {
|
||||
if (idx >= HAL_INS_NUM_HARMONIC_NOTCH_FILTERS) {
|
||||
return;
|
||||
}
|
||||
// protect against zero as the scaled frequency
|
||||
if (is_positive(scaled_freq)) {
|
||||
_calculated_harmonic_notch_freq_hz[0] = scaled_freq;
|
||||
_calculated_harmonic_notch_freq_hz[idx][0] = scaled_freq;
|
||||
}
|
||||
_num_calculated_harmonic_notch_frequencies = 1;
|
||||
_num_calculated_harmonic_notch_frequencies[idx] = 1;
|
||||
}
|
||||
|
||||
// Update the harmonic notch frequency
|
||||
void AP_InertialSensor::update_harmonic_notch_frequencies_hz(uint8_t num_freqs, const float scaled_freq[]) {
|
||||
void AP_InertialSensor::update_harmonic_notch_frequencies_hz(uint8_t idx, uint8_t num_freqs, const float scaled_freq[]) {
|
||||
if (idx >= HAL_INS_NUM_HARMONIC_NOTCH_FILTERS) {
|
||||
return;
|
||||
}
|
||||
// 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];
|
||||
_calculated_harmonic_notch_freq_hz[idx][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;
|
||||
_num_calculated_harmonic_notch_frequencies[idx] = num_freqs;
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
|
|||
|
|
@ -35,6 +35,9 @@
|
|||
#define HAL_INS_TEMPERATURE_CAL_ENABLE !HAL_MINIMIZE_FEATURES && BOARD_FLASH_SIZE > 1024
|
||||
#endif
|
||||
|
||||
#ifndef HAL_INS_NUM_HARMONIC_NOTCH_FILTERS
|
||||
#define HAL_INS_NUM_HARMONIC_NOTCH_FILTERS 2
|
||||
#endif
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
|
|
@ -241,9 +244,9 @@ public:
|
|||
uint8_t get_primary_gyro(void) const { return _primary_gyro; }
|
||||
|
||||
// Update the harmonic notch frequency
|
||||
void update_harmonic_notch_freq_hz(float scaled_freq);
|
||||
void update_harmonic_notch_freq_hz(uint8_t idx, float scaled_freq);
|
||||
// Update the harmonic notch frequencies
|
||||
void update_harmonic_notch_frequencies_hz(uint8_t num_freqs, const float scaled_freq[]);
|
||||
void update_harmonic_notch_frequencies_hz(uint8_t idx, uint8_t num_freqs, const float scaled_freq[]);
|
||||
|
||||
// get the gyro filter rate in Hz
|
||||
uint16_t get_gyro_filter_hz(void) const { return _gyro_filter_cutoff; }
|
||||
|
|
@ -252,32 +255,32 @@ 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[0]; }
|
||||
float get_gyro_dynamic_notch_center_freq_hz(uint8_t idx) const { return idx<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS?_calculated_harmonic_notch_freq_hz[idx][0]:0; }
|
||||
|
||||
// number of dynamic harmonic notches
|
||||
uint8_t get_num_gyro_dynamic_notches(void) const { return _num_dynamic_harmonic_notches; }
|
||||
uint8_t get_num_gyro_dynamic_notches(uint8_t idx) const { return idx<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS?_num_dynamic_harmonic_notches[idx]: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; }
|
||||
const float* get_gyro_dynamic_notch_center_frequencies_hz(uint8_t idx) const { return idx<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS?_calculated_harmonic_notch_freq_hz[idx]:nullptr; }
|
||||
|
||||
// number of harmonic notch current center frequencies
|
||||
uint8_t get_num_gyro_dynamic_notch_center_frequencies(void) const { return _num_calculated_harmonic_notch_frequencies; }
|
||||
uint8_t get_num_gyro_dynamic_notch_center_frequencies(uint8_t idx) const { return idx<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS?_num_calculated_harmonic_notch_frequencies[idx]:0; }
|
||||
|
||||
// harmonic notch reference center frequency
|
||||
float get_gyro_harmonic_notch_center_freq_hz(void) const { return _harmonic_notch_filter.center_freq_hz(); }
|
||||
float get_gyro_harmonic_notch_center_freq_hz(uint8_t idx) const { return idx<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS?_harmonic_notch_filter[idx].center_freq_hz():0; }
|
||||
|
||||
// harmonic notch reference scale factor
|
||||
float get_gyro_harmonic_notch_reference(void) const { return _harmonic_notch_filter.reference(); }
|
||||
float get_gyro_harmonic_notch_reference(uint8_t idx) const { return idx<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS?_harmonic_notch_filter[idx].reference():0; }
|
||||
|
||||
// harmonic notch tracking mode
|
||||
HarmonicNotchDynamicMode get_gyro_harmonic_notch_tracking_mode(void) const { return _harmonic_notch_filter.tracking_mode(); }
|
||||
HarmonicNotchDynamicMode get_gyro_harmonic_notch_tracking_mode(uint8_t idx) const { return idx<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS?_harmonic_notch_filter[idx].tracking_mode():HarmonicNotchDynamicMode::Fixed; }
|
||||
|
||||
// harmonic notch harmonics
|
||||
uint8_t get_gyro_harmonic_notch_harmonics(void) const { return _harmonic_notch_filter.harmonics(); }
|
||||
uint8_t get_gyro_harmonic_notch_harmonics(uint8_t idx) const { return idx<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS?_harmonic_notch_filter[idx].harmonics():0; }
|
||||
|
||||
// harmonic notch options
|
||||
bool has_harmonic_option(HarmonicNotchFilterParams::Options option) {
|
||||
return _harmonic_notch_filter.hasOption(option);
|
||||
bool has_harmonic_option(uint8_t idx, HarmonicNotchFilterParams::Options option) {
|
||||
return idx<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS?_harmonic_notch_filter[idx].hasOption(option):false;
|
||||
}
|
||||
|
||||
// write out harmonic notch log messages
|
||||
|
|
@ -307,7 +310,7 @@ public:
|
|||
bool gyro_notch_enabled(void) const { return _notch_filter.enabled(); }
|
||||
|
||||
// return true if harmonic notch enabled
|
||||
bool gyro_harmonic_notch_enabled(void) const { return _harmonic_notch_filter.enabled(); }
|
||||
bool gyro_harmonic_notch_enabled(uint8_t idx) const { return idx<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS?_harmonic_notch_filter[idx].enabled():false; }
|
||||
|
||||
AuxiliaryBus *get_auxiliary_bus(int16_t backend_id) { return get_auxiliary_bus(backend_id, 0); }
|
||||
AuxiliaryBus *get_auxiliary_bus(int16_t backend_id, uint8_t instance);
|
||||
|
|
@ -509,18 +512,14 @@ private:
|
|||
bool _new_accel_data[INS_MAX_INSTANCES];
|
||||
bool _new_gyro_data[INS_MAX_INSTANCES];
|
||||
|
||||
// optional notch filter on gyro
|
||||
HarmonicNotchFilterParams _notch_filter;
|
||||
HarmonicNotchFilterVector3f _gyro_notch_filter[INS_MAX_INSTANCES];
|
||||
|
||||
// optional harmonic notch filter on gyro
|
||||
HarmonicNotchFilterParams _harmonic_notch_filter;
|
||||
HarmonicNotchFilterVector3f _gyro_harmonic_notch_filter[INS_MAX_INSTANCES];
|
||||
HarmonicNotchFilterParams _harmonic_notch_filter[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS];
|
||||
HarmonicNotchFilterVector3f _gyro_harmonic_notch_filter[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS][INS_MAX_INSTANCES];
|
||||
// number of independent notches in the filter
|
||||
uint8_t _num_dynamic_harmonic_notches;
|
||||
uint8_t _num_dynamic_harmonic_notches[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS];
|
||||
// the current center frequency for the notch
|
||||
float _calculated_harmonic_notch_freq_hz[INS_MAX_NOTCHES];
|
||||
uint8_t _num_calculated_harmonic_notch_frequencies;
|
||||
float _calculated_harmonic_notch_freq_hz[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS][INS_MAX_NOTCHES];
|
||||
uint8_t _num_calculated_harmonic_notch_frequencies[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS];
|
||||
|
||||
// Most recent gyro reading
|
||||
Vector3f _gyro[INS_MAX_INSTANCES];
|
||||
|
|
|
|||
|
|
@ -274,14 +274,11 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
|
|||
#endif
|
||||
Vector3f gyro_filtered = gyro;
|
||||
|
||||
// apply the notch filter
|
||||
if (_gyro_notch_enabled()) {
|
||||
gyro_filtered = _imu._gyro_notch_filter[instance].apply(gyro_filtered);
|
||||
}
|
||||
|
||||
// apply the harmonic notch filter
|
||||
if (gyro_harmonic_notch_enabled()) {
|
||||
gyro_filtered = _imu._gyro_harmonic_notch_filter[instance].apply(gyro_filtered);
|
||||
for (uint8_t i=0; i<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS; i++) {
|
||||
if (gyro_harmonic_notch_enabled(i)) {
|
||||
gyro_filtered = _imu._gyro_harmonic_notch_filter[i][instance].apply(gyro_filtered);
|
||||
}
|
||||
}
|
||||
|
||||
// apply the low pass filter last to attentuate any notch induced noise
|
||||
|
|
@ -290,8 +287,9 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
|
|||
// 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();
|
||||
_imu._gyro_notch_filter[instance].reset();
|
||||
_imu._gyro_harmonic_notch_filter[instance].reset();
|
||||
for (uint8_t i=0; i<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS; i++) {
|
||||
_imu._gyro_harmonic_notch_filter[i][instance].reset();
|
||||
}
|
||||
} else {
|
||||
_imu._gyro_filtered[instance] = gyro_filtered;
|
||||
}
|
||||
|
|
@ -395,14 +393,11 @@ void AP_InertialSensor_Backend::_notify_new_delta_angle(uint8_t instance, const
|
|||
#endif
|
||||
Vector3f gyro_filtered = gyro;
|
||||
|
||||
// apply the notch filter
|
||||
if (_gyro_notch_enabled()) {
|
||||
gyro_filtered = _imu._gyro_notch_filter[instance].apply(gyro_filtered);
|
||||
}
|
||||
|
||||
// apply the harmonic notch filter
|
||||
if (gyro_harmonic_notch_enabled()) {
|
||||
gyro_filtered = _imu._gyro_harmonic_notch_filter[instance].apply(gyro_filtered);
|
||||
// apply the harmonic notch filters
|
||||
for (uint8_t i=0; i<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS; i++) {
|
||||
if (gyro_harmonic_notch_enabled(i)) {
|
||||
gyro_filtered = _imu._gyro_harmonic_notch_filter[i][instance].apply(gyro_filtered);
|
||||
}
|
||||
}
|
||||
|
||||
// apply the low pass filter last to attentuate any notch induced noise
|
||||
|
|
@ -411,8 +406,9 @@ void AP_InertialSensor_Backend::_notify_new_delta_angle(uint8_t instance, const
|
|||
// 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();
|
||||
_imu._gyro_notch_filter[instance].reset();
|
||||
_imu._gyro_harmonic_notch_filter[instance].reset();
|
||||
for (uint8_t i=0; i<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS; i++) {
|
||||
_imu._gyro_harmonic_notch_filter[i][instance].reset();
|
||||
}
|
||||
} else {
|
||||
_imu._gyro_filtered[instance] = gyro_filtered;
|
||||
}
|
||||
|
|
@ -734,30 +730,25 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) /* front end */
|
|||
}
|
||||
|
||||
// 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()) ||
|
||||
sensors_converging()) {
|
||||
_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())) {
|
||||
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());
|
||||
for (uint8_t i=0; i<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS; i++) {
|
||||
if (!is_equal(_last_harmonic_notch_bandwidth_hz[i], gyro_harmonic_notch_bandwidth_hz(i)) ||
|
||||
!is_equal(_last_harmonic_notch_attenuation_dB[i], gyro_harmonic_notch_attenuation_dB(i)) ||
|
||||
sensors_converging()) {
|
||||
_imu._gyro_harmonic_notch_filter[i][instance].init(_gyro_raw_sample_rate(instance),
|
||||
gyro_harmonic_notch_center_freq_hz(i),
|
||||
gyro_harmonic_notch_bandwidth_hz(i),
|
||||
gyro_harmonic_notch_attenuation_dB(i));
|
||||
_last_harmonic_notch_center_freq_hz[i] = gyro_harmonic_notch_center_freq_hz(i);
|
||||
_last_harmonic_notch_bandwidth_hz[i] = gyro_harmonic_notch_bandwidth_hz(i);
|
||||
_last_harmonic_notch_attenuation_dB[i] = gyro_harmonic_notch_attenuation_dB(i);
|
||||
} else if (!is_equal(_last_harmonic_notch_center_freq_hz[i], gyro_harmonic_notch_center_freq_hz(i))) {
|
||||
if (num_gyro_harmonic_notch_center_frequencies(i) > 1) {
|
||||
_imu._gyro_harmonic_notch_filter[i][instance].update(num_gyro_harmonic_notch_center_frequencies(i), gyro_harmonic_notch_center_frequencies_hz(i));
|
||||
} else {
|
||||
_imu._gyro_harmonic_notch_filter[i][instance].update(gyro_harmonic_notch_center_freq_hz(i));
|
||||
}
|
||||
_last_harmonic_notch_center_freq_hz[i] = gyro_harmonic_notch_center_freq_hz(i);
|
||||
}
|
||||
_last_harmonic_notch_center_freq_hz = gyro_harmonic_notch_center_freq_hz();
|
||||
}
|
||||
// possily update the notch filter parameters
|
||||
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()) ||
|
||||
sensors_converging()) {
|
||||
_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 = _gyro_notch_center_freq_hz();
|
||||
_last_notch_bandwidth_hz = _gyro_notch_bandwidth_hz();
|
||||
_last_notch_attenuation_dB = _gyro_notch_attenuation_dB();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -257,33 +257,22 @@ protected:
|
|||
return (uint16_t)_imu._loop_rate;
|
||||
}
|
||||
|
||||
// return the notch filter center in Hz for the sample rate
|
||||
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
|
||||
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(); }
|
||||
|
||||
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.get_gyro_dynamic_notch_center_freq_hz(); }
|
||||
float gyro_harmonic_notch_center_freq_hz(uint8_t idx) const { return idx<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS?_imu.get_gyro_dynamic_notch_center_freq_hz(idx):0; }
|
||||
|
||||
// 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(); }
|
||||
const float* gyro_harmonic_notch_center_frequencies_hz(uint8_t idx) const { return idx<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS?_imu.get_gyro_dynamic_notch_center_frequencies_hz(idx):nullptr; }
|
||||
|
||||
// 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(); }
|
||||
uint8_t num_gyro_harmonic_notch_center_frequencies(uint8_t idx) const { return idx<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS?_imu.get_num_gyro_dynamic_notch_center_frequencies(idx):0; }
|
||||
|
||||
// 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(); }
|
||||
float gyro_harmonic_notch_bandwidth_hz(uint8_t idx) const { return idx<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS?_imu._harmonic_notch_filter[idx].bandwidth_hz():0; }
|
||||
|
||||
// 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(); }
|
||||
float gyro_harmonic_notch_attenuation_dB(uint8_t idx) const { return idx<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS?_imu._harmonic_notch_filter[idx].attenuation_dB():0; }
|
||||
|
||||
bool gyro_harmonic_notch_enabled(void) const { return _imu._harmonic_notch_filter.enabled(); }
|
||||
bool gyro_harmonic_notch_enabled(uint8_t idx) const { return idx<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS?_imu._harmonic_notch_filter[idx].enabled():false; }
|
||||
|
||||
// common gyro update function for all backends
|
||||
void update_gyro(uint8_t instance) __RAMFUNC__; /* front end */
|
||||
|
|
@ -294,14 +283,11 @@ protected:
|
|||
// support for updating filter at runtime
|
||||
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;
|
||||
float _last_harmonic_notch_center_freq_hz[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS];
|
||||
float _last_harmonic_notch_bandwidth_hz[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS];
|
||||
float _last_harmonic_notch_attenuation_dB[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS];
|
||||
|
||||
void set_gyro_orientation(uint8_t instance, enum Rotation rotation) {
|
||||
_imu._gyro_orientation[instance] = rotation;
|
||||
|
|
|
|||
|
|
@ -132,6 +132,7 @@ bool AP_InertialSensor::BatchSampler::Write_ISBD() const
|
|||
// @LoggerMessage: FTN
|
||||
// @Description: Filter Tuning Messages
|
||||
// @Field: TimeUS: microseconds since system startup
|
||||
// @Field: I: instance
|
||||
// @Field: NDn: number of active dynamic harmonic notches
|
||||
// @Field: NF1: dynamic harmonic notch centre frequency for motor 1
|
||||
// @Field: NF2: dynamic harmonic notch centre frequency for motor 2
|
||||
|
|
@ -147,10 +148,21 @@ bool AP_InertialSensor::BatchSampler::Write_ISBD() const
|
|||
// @Field: NF12: dynamic harmonic notch centre frequency for motor 12
|
||||
void AP_InertialSensor::write_notch_log_messages() const
|
||||
{
|
||||
const float* notches = get_gyro_dynamic_notch_center_frequencies_hz();
|
||||
AP::logger().WriteStreaming(
|
||||
"FTN", "TimeUS,NDn,NF1,NF2,NF3,NF4,NF5,NF6,NF7,NF8,NF9,NF10,NF11,NF12", "s-zzzzzzzzzzzz", "F-------------", "QBffffffffffff", AP_HAL::micros64(), get_num_gyro_dynamic_notch_center_frequencies(),
|
||||
for (uint8_t i=0; i<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS; i++) {
|
||||
if (!gyro_harmonic_notch_enabled(i)) {
|
||||
continue;
|
||||
}
|
||||
const float* notches = get_gyro_dynamic_notch_center_frequencies_hz(i);
|
||||
if (notches == nullptr) {
|
||||
continue;
|
||||
}
|
||||
AP::logger().WriteStreaming(
|
||||
"FTN", "TimeUS,I,NDn,NF1,NF2,NF3,NF4,NF5,NF6,NF7,NF8,NF9,NF10,NF11,NF12", "s#-zzzzzzzzzzzz", "F--------------", "QBBffffffffffff",
|
||||
AP_HAL::micros64(),
|
||||
i,
|
||||
get_num_gyro_dynamic_notch_center_frequencies(i),
|
||||
notches[0], notches[1], notches[2], notches[3],
|
||||
notches[4], notches[5], notches[6], notches[7],
|
||||
notches[8], notches[9], notches[10], notches[11]);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue