Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 02:36:37 +08:00
Code Issues Actions 32 Packages Projects Releases Wiki Activity

sensors/vehicle_angular_velocity: RPM filter add harmonics

Browse Source
This commit is contained in:
Daniel Agar
2021-03-14 18:26:58 -04:00
parent 8f242ec444
commit b74bdb0250
2 changed files with 40 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
+37 -17
View File
@@ -335,8 +335,10 @@ void VehicleAngularVelocity::DisableDynamicNotchEscRpm()
// device id mismatch, disable all // device id mismatch, disable all
for (auto &dnf : _dynamic_notch_filter_esc_rpm) { for (auto &dnf : _dynamic_notch_filter_esc_rpm) {
for (int axis = 0; axis < 3; axis++) { for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
dnf[axis].setParameters(0, 0, 0); for (int axis = 0; axis < 3; axis++) {
dnf[harmonic][axis].setParameters(0, 0, 0);
}
} }
} }
@@ -376,20 +378,27 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force)
if ((esc_status.esc[i].timestamp != 0) && ((_timestamp_sample_last - esc_status.esc[i].timestamp) < 1_s) if ((esc_status.esc[i].timestamp != 0) && ((_timestamp_sample_last - esc_status.esc[i].timestamp) < 1_s)
&& (esc_status.esc[i].esc_rpm > MIN_ESC_RPM)) { && (esc_status.esc[i].esc_rpm > MIN_ESC_RPM)) {
// TODO: rotor frequency, blade pass frequency, harmonics const float esc_hz = static_cast<float>(esc_status.esc[i].esc_rpm) / 60.f;
const float frequency_hz = static_cast<float>(esc_status.esc[i].esc_rpm) / 60.f;
for (int axis = 0; axis < 3; axis++) { for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
auto &dnf = _dynamic_notch_filter_esc_rpm[i][axis]; const float frequency_hz = esc_hz * (harmonic + 1);
const float change_percent = fabsf(dnf.getNotchFreq() - frequency_hz) / frequency_hz;
auto &dnf0 = _dynamic_notch_filter_esc_rpm[i][harmonic][0];
const float change_percent = fabsf(dnf0.getNotchFreq() - frequency_hz) / frequency_hz;
if (change_percent > 0.001f) { if (change_percent > 0.001f) {
// peak frequency changed by at least 0.1% // peak frequency changed by at least 0.1%
dnf.setParameters(_filter_sample_rate_hz, frequency_hz, 1.f); // TODO: configurable bandwidth for (int axis = 0; axis < 3; axis++) {
auto &dnf = _dynamic_notch_filter_esc_rpm[i][harmonic][axis];
dnf.setParameters(_filter_sample_rate_hz, frequency_hz, 1.f); // TODO: configurable bandwidth
}
// only reset if there's sufficient change (> 1%) // only reset if there's sufficient change (> 1%)
if ((change_percent > 0.01f) && (_last_scale > 0.f)) { if ((change_percent > 0.01f) && (_last_scale > 0.f)) {
dnf.reset(_angular_velocity(axis) / _last_scale); for (int axis = 0; axis < 3; axis++) {
auto &dnf = _dynamic_notch_filter_esc_rpm[i][harmonic][axis];
dnf.reset(_angular_velocity(axis) / _last_scale);
}
} }
perf_count(_dynamic_notch_filter_esc_rpm_update_perf); perf_count(_dynamic_notch_filter_esc_rpm_update_perf);
@@ -399,10 +408,11 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force)
_dynamic_notch_esc_rpm_available = true; _dynamic_notch_esc_rpm_available = true;
} else { } else {
// disable this notch filter // disable all notch filters for this ESC
for (int axis = 0; axis < 3; axis++) { for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
auto &dnf = _dynamic_notch_filter_esc_rpm[i][axis]; for (int axis = 0; axis < 3; axis++) {
dnf.setParameters(0, 0, 0); _dynamic_notch_filter_esc_rpm[i][harmonic][axis].setParameters(0, 0, 0);
}
} }
} }
} }
@@ -525,8 +535,13 @@ void VehicleAngularVelocity::Run()
perf_begin(_dynamic_notch_filter_esc_rpm_perf); perf_begin(_dynamic_notch_filter_esc_rpm_perf);
for (auto &dnf : _dynamic_notch_filter_esc_rpm) { for (auto &dnf : _dynamic_notch_filter_esc_rpm) {
if (dnf[axis].getNotchFreq() > 0.f) { for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
dnf[axis].applyDF1(data, N); if (dnf[harmonic][axis].getNotchFreq() > 0.f) {
dnf[harmonic][axis].applyDF1(data, N);
} else {
break;
}
} }
} }
@@ -621,8 +636,13 @@ void VehicleAngularVelocity::Run()
perf_begin(_dynamic_notch_filter_esc_rpm_perf); perf_begin(_dynamic_notch_filter_esc_rpm_perf);
for (auto &dnf : _dynamic_notch_filter_esc_rpm) { for (auto &dnf : _dynamic_notch_filter_esc_rpm) {
if (dnf[axis].getNotchFreq() > 0.f) { for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
dnf[axis].applyDF1(&angular_velocity(axis), 1); if (dnf[harmonic][axis].getNotchFreq() > 0.f) {
dnf[harmonic][axis].applyDF1(&angular_velocity(axis), 1);
} else {
break;
}
} }
} }
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
+3 -1
View File
@@ -133,10 +133,12 @@ private:
}; };
static constexpr int MAX_NUM_ESC_RPM = sizeof(esc_status_s::esc) / sizeof(esc_status_s::esc[0]); static constexpr int MAX_NUM_ESC_RPM = sizeof(esc_status_s::esc) / sizeof(esc_status_s::esc[0]);
static constexpr int MAX_NUM_ESC_RPM_HARMONICS = 3;
static constexpr int MAX_NUM_FFT_PEAKS = sizeof(sensor_gyro_fft_s::peak_frequencies_x) / sizeof( static constexpr int MAX_NUM_FFT_PEAKS = sizeof(sensor_gyro_fft_s::peak_frequencies_x) / sizeof(
sensor_gyro_fft_s::peak_frequencies_x[0]); sensor_gyro_fft_s::peak_frequencies_x[0]);
math::NotchFilterArray<float> _dynamic_notch_filter_esc_rpm[MAX_NUM_ESC_RPM][3] {}; math::NotchFilterArray<float> _dynamic_notch_filter_esc_rpm[MAX_NUM_ESC_RPM][MAX_NUM_ESC_RPM_HARMONICS][3] {};
math::NotchFilterArray<float> _dynamic_notch_filter_fft[MAX_NUM_FFT_PEAKS][3] {}; math::NotchFilterArray<float> _dynamic_notch_filter_fft[MAX_NUM_FFT_PEAKS][3] {};
perf_counter_t _dynamic_notch_filter_esc_rpm_update_perf{nullptr}; perf_counter_t _dynamic_notch_filter_esc_rpm_update_perf{nullptr};