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

AP_InertialSensor: switch to HarmonicNotch class 

this makes the logic much easier to follow, without indexes into
arrays
This commit is contained in:
Andrew Tridgell 2022-04-15 17:38:56 +10:00
parent d7d04bc7cf
commit e2e1e74da5
5 changed files with 117 additions and 147 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

99
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -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)
// 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)
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)
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()
}
}
/*
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)
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()
#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
View File

@ -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:
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:
// 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:
// 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:
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
View File

@ -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,
// 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
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
// 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 */
_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
View File

@ -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:
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:
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
View File

@ -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]);