dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

sensors/vehicle_angular_velocity: RPM filter add harmonics

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
View File

@ -335,8 +335,10 @@ void VehicleAngularVelocity::DisableDynamicNotchEscRpm()
// device id mismatch, disable all
for (auto &dnf : _dynamic_notch_filter_esc_rpm) {
for (int axis = 0; axis < 3; axis++) {
dnf[axis].setParameters(0, 0, 0);
for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
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)
&& (esc_status.esc[i].esc_rpm > MIN_ESC_RPM)) {
// TODO: rotor frequency, blade pass frequency, harmonics
const float frequency_hz = static_cast<float>(esc_status.esc[i].esc_rpm) / 60.f;
const float esc_hz = static_cast<float>(esc_status.esc[i].esc_rpm) / 60.f;
for (int axis = 0; axis < 3; axis++) {
auto &dnf = _dynamic_notch_filter_esc_rpm[i][axis];
const float change_percent = fabsf(dnf.getNotchFreq() - frequency_hz) / frequency_hz;
for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
const float frequency_hz = esc_hz * (harmonic + 1);
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) {
// 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%)
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);
@ -399,10 +408,11 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force)
_dynamic_notch_esc_rpm_available = true;
} else {
// disable this notch filter
for (int axis = 0; axis < 3; axis++) {
auto &dnf = _dynamic_notch_filter_esc_rpm[i][axis];
dnf.setParameters(0, 0, 0);
// disable all notch filters for this ESC
for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
for (int axis = 0; axis < 3; axis++) {
_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);
for (auto &dnf : _dynamic_notch_filter_esc_rpm) {
if (dnf[axis].getNotchFreq() > 0.f) {
dnf[axis].applyDF1(data, N);
for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
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);
for (auto &dnf : _dynamic_notch_filter_esc_rpm) {
if (dnf[axis].getNotchFreq() > 0.f) {
dnf[axis].applyDF1(&angular_velocity(axis), 1);
for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
if (dnf[harmonic][axis].getNotchFreq() > 0.f) {
dnf[harmonic][axis].applyDF1(&angular_velocity(axis), 1);
} else {
break;
}
}
}

4
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
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_HARMONICS = 3;
static constexpr int MAX_NUM_FFT_PEAKS = sizeof(sensor_gyro_fft_s::peak_frequencies_x) / sizeof(
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] {};
perf_counter_t _dynamic_notch_filter_esc_rpm_update_perf{nullptr};