zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_InertialSensor: switch to HarmonicNotch class 

this makes the logic much easier to follow, without indexes into
arrays
apm_2208
Andrew Tridgell 3 years ago
parent
d7d04bc7cf
commit
e2e1e74da5
5 changed files with 117 additions and 147 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 99
      libraries/AP_InertialSensor/AP_InertialSensor.cpp
  2. 87
      libraries/AP_InertialSensor/AP_InertialSensor.h
  3. 42
      libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
  4. 24
      libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
  5. 12
      libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp

99
libraries/AP_InertialSensor/AP_InertialSensor.cpp
Unescape View File

@ -548,12 +548,12 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = { @@ -548,12 +548,12 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
// @Group: HNTCH_
// @Path: ../Filter/HarmonicNotchFilter.cpp
AP_SUBGROUPINFO(_harmonic_notch_filter[0], "HNTCH_", 41, AP_InertialSensor, HarmonicNotchFilterParams),
AP_SUBGROUPINFO(harmonic_notches[0].params, "HNTCH_", 41, AP_InertialSensor, HarmonicNotchFilterParams),
#if HAL_INS_NUM_HARMONIC_NOTCH_FILTERS > 1
// @Group: HNTC2_
// @Path: ../Filter/HarmonicNotchFilter.cpp
AP_SUBGROUPINFO(_harmonic_notch_filter[1], "HNTC2_", 53, AP_InertialSensor, HarmonicNotchFilterParams),
AP_SUBGROUPINFO(harmonic_notches[1].params, "HNTC2_", 53, AP_InertialSensor, HarmonicNotchFilterParams),
#endif
// @Param: GYRO_RATE
@ -876,28 +876,25 @@ AP_InertialSensor::init(uint16_t loop_rate) @@ -876,28 +876,25 @@ 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
uint8_t num_filters = 0;
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);
for (auto &notch : harmonic_notches) {
notch.calculated_notch_freq_hz[0] = notch.params.center_freq_hz();
notch.num_calculated_notch_frequencies = 1;
notch.num_dynamic_notches = 1;
#if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane)
if (_harmonic_notch_filter[i].hasOption(HarmonicNotchFilterParams::Options::DynamicHarmonic)) {
if (notch.params.hasOption(HarmonicNotchFilterParams::Options::DynamicHarmonic)) {
#if HAL_WITH_DSP
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
if (notch.params.tracking_mode() == HarmonicNotchDynamicMode::UpdateGyroFFT) {
notch.num_dynamic_notches = AP_HAL::DSP::MAX_TRACKED_PEAKS; // only 3 peaks supported currently
} else
#endif
{
AP_Motors *motors = AP::motors();
if (motors != nullptr) {
_num_dynamic_harmonic_notches[i] = __builtin_popcount(motors->get_motor_mask());
notch.num_dynamic_notches = __builtin_popcount(motors->get_motor_mask());
}
}
// avoid harmonics unless actually configured by the user
_harmonic_notch_filter[i].set_default_harmonics(1);
notch.params.set_default_harmonics(1);
}
#endif
}
@ -907,11 +904,13 @@ AP_InertialSensor::init(uint16_t loop_rate) @@ -907,11 +904,13 @@ AP_InertialSensor::init(uint16_t loop_rate)
sensors_used += _use[i];
}
for (uint8_t i=0; i<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS; i++) {
uint8_t num_filters = 0;
for (auto &notch : harmonic_notches) {
// 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)
if (notch.params.enabled()) {
const bool double_notch = notch.params.hasOption(HarmonicNotchFilterParams::Options::DoubleNotch);
num_filters += __builtin_popcount(notch.params.harmonics())
* notch.num_dynamic_notches * (double_notch ? 2 : 1)
* sensors_used;
}
}
@ -924,13 +923,14 @@ AP_InertialSensor::init(uint16_t loop_rate) @@ -924,13 +923,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]) {
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]);
for (auto &notch : harmonic_notches) {
if (notch.params.enabled()) {
const bool double_notch = notch.params.hasOption(HarmonicNotchFilterParams::Options::DoubleNotch);
notch.filter[i].allocate_filters(notch.num_dynamic_notches,
notch.params.harmonics(), double_notch);
// 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());
notch.filter[i].init(_gyro_raw_sample_rates[i], notch.calculated_notch_freq_hz[0],
notch.params.bandwidth_hz(), notch.params.attenuation_dB());
}
}
}
@ -1584,6 +1584,31 @@ void AP_InertialSensor::_save_gyro_calibration() @@ -1584,6 +1584,31 @@ void AP_InertialSensor::_save_gyro_calibration()
}
}
/*
update harmonic notch parameters
*/
void AP_InertialSensor::HarmonicNotch::update_params(uint8_t instance, bool converging, float gyro_rate)
{
const float center_freq = calculated_notch_freq_hz[0];
if (!is_equal(last_bandwidth_hz, params.bandwidth_hz()) ||
!is_equal(last_attenuation_dB, params.attenuation_dB()) ||
converging) {
filter[instance].init(gyro_rate,
center_freq,
params.bandwidth_hz(),
params.attenuation_dB());
last_center_freq_hz = center_freq;
last_bandwidth_hz = params.bandwidth_hz();
last_attenuation_dB = params.attenuation_dB();
} else if (!is_equal(last_center_freq_hz, center_freq)) {
if (num_calculated_notch_frequencies > 1) {
filter[instance].update(num_calculated_notch_frequencies, calculated_notch_freq_hz);
} else {
filter[instance].update(center_freq);
}
last_center_freq_hz = center_freq;
}
}
/*
update gyro and accel values from backends
@ -1606,6 +1631,13 @@ void AP_InertialSensor::update(void) @@ -1606,6 +1631,13 @@ void AP_InertialSensor::update(void)
for (uint8_t i=0; i<_backend_count; i++) {
_backends[i]->update();
}
for (uint8_t i=0; i<_gyro_count; i++) {
const bool converging = AP_HAL::millis() < HAL_INS_CONVERGANCE_MS;
const float gyro_rate = _gyro_raw_sample_rates[i];
for (auto &notch : harmonic_notches) {
notch.update_params(i, converging, gyro_rate);
}
}
if (!_startup_error_counts_set) {
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
@ -2014,30 +2046,25 @@ void AP_InertialSensor::acal_update() @@ -2014,30 +2046,25 @@ void AP_InertialSensor::acal_update()
#endif
// Update the harmonic notch frequency
void AP_InertialSensor::update_harmonic_notch_freq_hz(uint8_t idx, float scaled_freq) {
if (idx >= HAL_INS_NUM_HARMONIC_NOTCH_FILTERS) {
return;
}
void AP_InertialSensor::HarmonicNotch::update_freq_hz(float scaled_freq)
{
// protect against zero as the scaled frequency
if (is_positive(scaled_freq)) {
_calculated_harmonic_notch_freq_hz[idx][0] = scaled_freq;
calculated_notch_freq_hz[0] = scaled_freq;
}
_num_calculated_harmonic_notch_frequencies[idx] = 1;
num_calculated_notch_frequencies = 1;
}
// Update the harmonic notch frequency
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;
}
void AP_InertialSensor::HarmonicNotch::update_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[idx][i] = scaled_freq[i];
calculated_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[idx] = num_freqs;
num_calculated_notch_frequencies = num_freqs;
}
/*

87
libraries/AP_InertialSensor/AP_InertialSensor.h
Unescape View File

@ -39,6 +39,11 @@ @@ -39,6 +39,11 @@
#define HAL_INS_NUM_HARMONIC_NOTCH_FILTERS 2
#endif
// time for the estimated gyro rates to converge
#ifndef HAL_INS_CONVERGANCE_MS
#define HAL_INS_CONVERGANCE_MS 30000
#endif
#include <stdint.h>
#include <AP_AccelCal/AP_AccelCal.h>
@ -242,46 +247,12 @@ public: @@ -242,46 +247,12 @@ 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(uint8_t idx, float scaled_freq);
// Update the harmonic notch frequencies
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; }
// 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(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(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(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(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(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(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(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(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(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
void write_notch_log_messages() const;
@ -305,12 +276,6 @@ public: @@ -305,12 +276,6 @@ public:
// check for vibration movement. True when all axis show nearly zero movement
bool is_still();
// return true if notch enabled
bool gyro_notch_enabled(void) const { return _notch_filter.enabled(); }
// return true if harmonic notch 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);
@ -453,6 +418,39 @@ public: @@ -453,6 +418,39 @@ public:
// force save of current calibration as valid
void force_save_calibration(void);
// structure per harmonic notch filter. This is public to allow for
// easy iteration
class HarmonicNotch {
public:
HarmonicNotchFilterParams params;
HarmonicNotchFilterVector3f filter[INS_MAX_INSTANCES];
uint8_t num_dynamic_notches;
// the current center frequency for the notch
float calculated_notch_freq_hz[INS_MAX_NOTCHES];
uint8_t num_calculated_notch_frequencies;
// Update the harmonic notch frequency
void update_notch_freq_hz(float scaled_freq);
// Update the harmonic notch frequencies
void update_notch_frequencies_hz(uint8_t num_freqs, const float scaled_freq[]);
// runtime update of notch parameters
void update_params(uint8_t instance, bool converging, float gyro_rate);
// Update the harmonic notch frequencies
void update_freq_hz(float scaled_freq);
void update_frequencies_hz(uint8_t num_freqs, const float scaled_freq[]);
private:
// support for updating harmonic filter at runtime
float last_center_freq_hz;
float last_bandwidth_hz;
float last_attenuation_dB;
} harmonic_notches[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS];
private:
// load backend drivers
bool _add_backend(AP_InertialSensor_Backend *backend);
@ -513,15 +511,6 @@ private: @@ -513,15 +511,6 @@ private:
bool _new_accel_data[INS_MAX_INSTANCES];
bool _new_gyro_data[INS_MAX_INSTANCES];
// optional harmonic notch filter on gyro
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[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS];
// the current center frequency for the notch
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];
Vector3f _delta_angle[INS_MAX_INSTANCES];

42
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
Unescape View File

@ -267,9 +267,9 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance, @@ -267,9 +267,9 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
Vector3f gyro_filtered = gyro;
// apply the harmonic notch filter
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);
for (auto &notch : _imu.harmonic_notches) {
if (notch.params.enabled()) {
gyro_filtered = notch.filter[instance].apply(gyro_filtered);
}
}
@ -279,8 +279,8 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance, @@ -279,8 +279,8 @@ 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();
for (uint8_t i=0; i<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS; i++) {
_imu._gyro_harmonic_notch_filter[i][instance].reset();
for (auto &notch : _imu.harmonic_notches) {
notch.filter[instance].reset();
}
} else {
_imu._gyro_filtered[instance] = gyro_filtered;
@ -386,9 +386,9 @@ void AP_InertialSensor_Backend::_notify_new_delta_angle(uint8_t instance, const @@ -386,9 +386,9 @@ void AP_InertialSensor_Backend::_notify_new_delta_angle(uint8_t instance, const
Vector3f gyro_filtered = gyro;
// 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);
for (auto &notch : _imu.harmonic_notches) {
if (notch.params.enabled()) {
gyro_filtered = notch.filter[instance].apply(gyro_filtered);
}
}
@ -398,8 +398,8 @@ void AP_InertialSensor_Backend::_notify_new_delta_angle(uint8_t instance, const @@ -398,8 +398,8 @@ 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();
for (uint8_t i=0; i<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS; i++) {
_imu._gyro_harmonic_notch_filter[i][instance].reset();
for (auto &notch : _imu.harmonic_notches) {
notch.filter[instance].reset();
}
} else {
_imu._gyro_filtered[instance] = gyro_filtered;
@ -720,28 +720,6 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) /* front end */ @@ -720,28 +720,6 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) /* front end */
_imu._gyro_filter[instance].set_cutoff_frequency(_gyro_raw_sample_rate(instance), _gyro_filter_cutoff());
_last_gyro_filter_hz = _gyro_filter_cutoff();
}
// possily update the harmonic notch filter parameters
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);
}
}
}
/*

24
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
Unescape View File

@ -200,7 +200,7 @@ protected: @@ -200,7 +200,7 @@ protected:
void _update_sensor_rate(uint16_t &count, uint32_t &start_us, float &rate_hz) const __RAMFUNC__;
// return true if the sensors are still converging and sampling rates could change significantly
bool sensors_converging() const { return AP_HAL::millis() < 30000; }
bool sensors_converging() const { return AP_HAL::millis() < HAL_INS_CONVERGANCE_MS; }
// set accelerometer max absolute offset for calibration
void _set_accel_max_abs_offset(uint8_t instance, float offset);
@ -257,23 +257,6 @@ protected: @@ -257,23 +257,6 @@ protected:
return (uint16_t)_imu._loop_rate;
}
// return the harmonic notch filter center in Hz for the sample rate
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(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(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(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(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(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 */
@ -284,11 +267,6 @@ protected: @@ -284,11 +267,6 @@ protected:
uint16_t _last_accel_filter_hz;
uint16_t _last_gyro_filter_hz;
// support for updating harmonic filter at runtime
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;
}

12
libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp
Unescape View File

@ -148,19 +148,17 @@ bool AP_InertialSensor::BatchSampler::Write_ISBD() const @@ -148,19 +148,17 @@ 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
{
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) {
for (auto &notch : harmonic_notches) {
const uint8_t i = &notch - &harmonic_notches[0];
if (!notch.params.enabled()) {
continue;
}
const float* notches = notch.calculated_notch_freq_hz;
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),
notch.num_calculated_notch_frequencies,
notches[0], notches[1], notches[2], notches[3],
notches[4], notches[5], notches[6], notches[7],
notches[8], notches[9], notches[10], notches[11]);

Write Preview
Loading…
Cancel
Save