fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-13 03:33:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors/vehicle_angular_velocity: notch filters automatically reset on sufficient parameter change

Browse Source
This commit is contained in:
Daniel Agar
2022-01-10 14:51:29 -05:00
parent 7d632254be
commit b58922a5d7
3 changed files with 34 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/mathlib/math/filter/NotchFilter.hpp
+32 -10
View File
@@ -57,7 +57,7 @@ public:
NotchFilter() = default;
~NotchFilter() = default;
void setParameters(float sample_freq, float notch_freq, float bandwidth);
bool setParameters(float sample_freq, float notch_freq, float bandwidth);
/**
* Add a new raw value to the filter using the Direct Form I
@@ -217,13 +217,13 @@ protected:
* conserving the filter's history
*/
template<typename T>
void NotchFilter<T>::setParameters(float sample_freq, float notch_freq, float bandwidth)
bool NotchFilter<T>::setParameters(float sample_freq, float notch_freq, float bandwidth)
{
if ((sample_freq <= 0.f) || (notch_freq <= 0.f) || (bandwidth <= 0.f) || (notch_freq >= sample_freq / 2)
|| !isFinite(sample_freq) || !isFinite(notch_freq) || !isFinite(bandwidth)) {
disable();
return;
return false;
}
const float freq_min = sample_freq * 0.001f;
@@ -231,11 +231,16 @@ void NotchFilter<T>::setParameters(float sample_freq, float notch_freq, float ba
const float notch_freq_new = math::max(notch_freq, freq_min);
const float bandwidth_new = math::max(bandwidth, freq_min);
bool sample_freq_change = (fabsf(sample_freq - _sample_freq) > FLT_EPSILON);
bool bandwidth_change = (fabsf(bandwidth_new - _bandwidth) > FLT_EPSILON);
const float sample_freq_diff = fabsf(sample_freq - _sample_freq);
const float bandwidth_diff = fabsf(bandwidth_new - _bandwidth);
const bool sample_freq_change = (sample_freq_diff > FLT_EPSILON);
const bool bandwidth_change = (bandwidth_diff > FLT_EPSILON);
if (!sample_freq_change && !bandwidth_change) {
if (fabsf(notch_freq_new - _notch_freq) > FLT_EPSILON) {
const float notch_freq_diff = fabsf(notch_freq_new - _notch_freq);
if (notch_freq_diff > FLT_EPSILON) {
// only notch frequency has changed
_notch_freq = notch_freq_new;
@@ -244,15 +249,21 @@ void NotchFilter<T>::setParameters(float sample_freq, float notch_freq, float ba
_b1 = 2.f * beta * _b0;
_a1 = _b1;
if (!isFinite(_b1)) {
disable();
if (notch_freq_diff > _bandwidth) {
// force reset
_initialized = false;
}
return;
if (!isFinite(_b1)) {
disable();
return false;
}
return true;
} else {
// no change, do nothing
return;
return true;
}
}
@@ -273,7 +284,18 @@ void NotchFilter<T>::setParameters(float sample_freq, float notch_freq, float ba
if (!isFinite(_b0) || !isFinite(_b1) || !isFinite(_b2) || !isFinite(_a2)) {
disable();
return false;
}
// force reset if bandwidth or sample frequency changed by more than 1%
if (bandwidth_diff > 0.01f * _bandwidth) {
_initialized = false;
} else if (sample_freq_diff > 0.01f * _sample_freq) {
_initialized = false;
}
return true;
}
} // namespace math
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
+2 -29
View File
@@ -58,11 +58,9 @@ VehicleAngularVelocity::~VehicleAngularVelocity()
#if !defined(CONSTRAINED_FLASH)
perf_free(_dynamic_notch_filter_esc_rpm_disable_perf);
perf_free(_dynamic_notch_filter_esc_rpm_reset_perf);
perf_free(_dynamic_notch_filter_esc_rpm_update_perf);
perf_free(_dynamic_notch_filter_fft_disable_perf);
perf_free(_dynamic_notch_filter_fft_reset_perf);
perf_free(_dynamic_notch_filter_fft_update_perf);
#endif // CONSTRAINED_FLASH
}
@@ -392,7 +390,6 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
if (_dynamic_notch_filter_esc_rpm_disable_perf == nullptr) {
_dynamic_notch_filter_esc_rpm_disable_perf = perf_alloc(PC_COUNT,
MODULE_NAME": gyro dynamic notch filter ESC RPM disable");
_dynamic_notch_filter_esc_rpm_reset_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter ESC RPM reset");
_dynamic_notch_filter_esc_rpm_update_perf = perf_alloc(PC_COUNT,
MODULE_NAME": gyro dynamic notch filter ESC RPM update");
}
@@ -404,7 +401,6 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
if (_param_imu_gyro_dnf_en.get() & DynamicNotch::FFT) {
if (_dynamic_notch_filter_fft_disable_perf == nullptr) {
_dynamic_notch_filter_fft_disable_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT disable");
_dynamic_notch_filter_fft_reset_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT reset");
_dynamic_notch_filter_fft_update_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT update");
}
@@ -530,11 +526,6 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force)
// update filter parameters if frequency changed or forced
if (reset || (notch_freq_diff > 0.1f)) {
// force reset if the notch frequency jumps significantly
if (notch_freq_diff > _param_imu_gyro_dnf_bw.get()) {
reset = true;
}
for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
const float frequency_hz = esc_hz * (harmonic + 1);
@@ -547,16 +538,6 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force)
perf_count(_dynamic_notch_filter_esc_rpm_update_perf);
}
if (reset) {
for (int axis = 0; axis < 3; axis++) {
for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
_dynamic_notch_filter_esc_rpm[axis][esc][harmonic].reset();
}
}
perf_count(_dynamic_notch_filter_esc_rpm_reset_perf);
}
_dynamic_notch_esc_rpm_available = true;
_esc_available.set(esc, true);
_last_esc_rpm_notch_update[esc] = esc_report.timestamp;
@@ -570,7 +551,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force)
for (int axis = 0; axis < 3; axis++) {
for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
_dynamic_notch_filter_esc_rpm[axis][esc][harmonic].setParameters(0, 0, 0);
_dynamic_notch_filter_esc_rpm[axis][esc][harmonic].disable();
}
}
}
@@ -629,18 +610,12 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force)
perf_count(_dynamic_notch_filter_fft_update_perf);
}
// force reset if the notch frequency jumps significantly
if (force || reset || (notch_freq_diff > bandwidth)) {
nf.reset();
perf_count(_dynamic_notch_filter_fft_reset_perf);
}
_dynamic_notch_fft_available = true;
} else {
// disable this notch filter (if it isn't already)
if (force || !reset) {
nf.setParameters(0, 0, 0);
nf.disable();
perf_count(_dynamic_notch_filter_fft_disable_perf);
}
}
@@ -885,11 +860,9 @@ void VehicleAngularVelocity::PrintStatus()
perf_print_counter(_selection_changed_perf);
#if !defined(CONSTRAINED_FLASH)
perf_print_counter(_dynamic_notch_filter_esc_rpm_disable_perf);
perf_print_counter(_dynamic_notch_filter_esc_rpm_reset_perf);
perf_print_counter(_dynamic_notch_filter_esc_rpm_update_perf);
perf_print_counter(_dynamic_notch_filter_fft_disable_perf);
perf_print_counter(_dynamic_notch_filter_fft_reset_perf);
perf_print_counter(_dynamic_notch_filter_fft_update_perf);
#endif // CONSTRAINED_FLASH
}
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
-2
View File
@@ -156,11 +156,9 @@ private:
hrt_abstime _last_esc_rpm_notch_update[MAX_NUM_ESC_RPM] {};
perf_counter_t _dynamic_notch_filter_esc_rpm_update_perf{nullptr};
perf_counter_t _dynamic_notch_filter_esc_rpm_reset_perf{nullptr};
perf_counter_t _dynamic_notch_filter_esc_rpm_disable_perf{nullptr};
perf_counter_t _dynamic_notch_filter_fft_disable_perf{nullptr};
perf_counter_t _dynamic_notch_filter_fft_reset_perf{nullptr};
perf_counter_t _dynamic_notch_filter_fft_update_perf{nullptr};
bool _dynamic_notch_esc_rpm_available{false};