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

sensors/vehicle_angular_velocity: add IMU_GYRO_DNF_HMC to configure number of ESC RPM notch filter harmonics

Browse Source
This commit is contained in:
Daniel Agar 2022-01-12 23:39:09 -05:00
parent 4c6e746360
commit 5ded1aedcb
3 changed files with 83 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -57,6 +57,7 @@ VehicleAngularVelocity::~VehicleAngularVelocity()
perf_free(_selection_changed_perf);
#if !defined(CONSTRAINED_FLASH)
delete[] _dynamic_notch_filter_esc_rpm;
perf_free(_dynamic_notch_filter_esc_rpm_disable_perf);
perf_free(_dynamic_notch_filter_esc_rpm_update_perf);
@ -387,11 +388,41 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
#if !defined(CONSTRAINED_FLASH)
if (_param_imu_gyro_dnf_en.get() & DynamicNotch::EscRpm) {
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_update_perf = perf_alloc(PC_COUNT,
MODULE_NAME": gyro dynamic notch filter ESC RPM update");
const int32_t esc_rpm_harmonics = math::constrain(_param_imu_gyro_dnf_hmc.get(), (int32_t)1, (int32_t)10);
if (_dynamic_notch_filter_esc_rpm && (esc_rpm_harmonics != _esc_rpm_harmonics)) {
delete[] _dynamic_notch_filter_esc_rpm;
_dynamic_notch_filter_esc_rpm = nullptr;
_esc_rpm_harmonics = 0;
}
if (_dynamic_notch_filter_esc_rpm == nullptr) {
_dynamic_notch_filter_esc_rpm = new NotchFilterHarmonic[esc_rpm_harmonics];
if (_dynamic_notch_filter_esc_rpm) {
_esc_rpm_harmonics = esc_rpm_harmonics;
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");
}
if (_dynamic_notch_filter_esc_rpm_update_perf == nullptr) {
_dynamic_notch_filter_esc_rpm_update_perf = perf_alloc(PC_COUNT,
MODULE_NAME": gyro dynamic notch filter ESC RPM update");
}
} else {
_esc_rpm_harmonics = 0;
perf_free(_dynamic_notch_filter_esc_rpm_disable_perf);
perf_free(_dynamic_notch_filter_esc_rpm_update_perf);
_dynamic_notch_filter_esc_rpm_disable_perf = nullptr;
_dynamic_notch_filter_esc_rpm_update_perf = nullptr;
}
}
} else {
@ -452,19 +483,16 @@ void VehicleAngularVelocity::DisableDynamicNotchEscRpm()
{
#if !defined(CONSTRAINED_FLASH)
if (_dynamic_notch_esc_rpm_available) {
for (int axis = 0; axis < 3; axis++) {
for (int esc = 0; esc < MAX_NUM_ESC_RPM; esc++) {
for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
_dynamic_notch_filter_esc_rpm[axis][esc][harmonic].disable();
if (_dynamic_notch_filter_esc_rpm) {
for (int harmonic = 0; harmonic < _esc_rpm_harmonics; harmonic++) {
for (int axis = 0; axis < 3; axis++) {
for (int esc = 0; esc < MAX_NUM_ESCS; esc++) {
_dynamic_notch_filter_esc_rpm[harmonic][axis][esc].disable();
_esc_available.set(esc, false);
perf_count(_dynamic_notch_filter_esc_rpm_disable_perf);
}
_esc_available.set(esc, false);
}
}
_dynamic_notch_esc_rpm_available = false;
}
#endif // !CONSTRAINED_FLASH
@ -491,22 +519,17 @@ void VehicleAngularVelocity::DisableDynamicNotchFFT()
void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(const hrt_abstime &time_now_us, bool force)
{
#if !defined(CONSTRAINED_FLASH)
const bool enabled = _param_imu_gyro_dnf_en.get() & DynamicNotch::EscRpm;
const bool enabled = _dynamic_notch_filter_esc_rpm && (_param_imu_gyro_dnf_en.get() & DynamicNotch::EscRpm);
if (enabled && (_esc_status_sub.updated() || force)) {
if (!_dynamic_notch_esc_rpm_available) {
// force update filters if previously disabled
force = true;
}
esc_status_s esc_status;
if (_esc_status_sub.copy(&esc_status) && (time_now_us < esc_status.timestamp + DYNAMIC_NOTCH_FITLER_TIMEOUT)) {
static constexpr float FREQ_MIN = 10.f; // TODO: configurable
for (size_t esc = 0; esc < math::min(esc_status.esc_count, (uint8_t)MAX_NUM_ESC_RPM); esc++) {
for (size_t esc = 0; esc < math::min(esc_status.esc_count, (uint8_t)MAX_NUM_ESCS); esc++) {
const esc_report_s &esc_report = esc_status.esc[esc];
// only update if ESC RPM range seems valid
@ -517,17 +540,17 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(const hrt_abstime &time_no
const float esc_hz = abs(esc_report.esc_rpm) / 60.f;
for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
for (int harmonic = 0; harmonic < _esc_rpm_harmonics; harmonic++) {
const float frequency_hz = esc_hz * (harmonic + 1);
// for each ESC harmonic determine if enabled/disabled from first notch (x axis)
auto &nfx = _dynamic_notch_filter_esc_rpm[0][esc][harmonic];
auto &nfx = _dynamic_notch_filter_esc_rpm[harmonic][0][esc];
if (frequency_hz > FREQ_MIN) {
// update filter parameters if frequency changed or forced
if (update || !nfx.initialized() || (fabsf(nfx.getNotchFreq() - frequency_hz) > 0.1f)) {
for (int axis = 0; axis < 3; axis++) {
auto &nf = _dynamic_notch_filter_esc_rpm[axis][esc][harmonic];
auto &nf = _dynamic_notch_filter_esc_rpm[harmonic][axis][esc];
nf.setParameters(_filter_sample_rate_hz, frequency_hz, _param_imu_gyro_dnf_bw.get());
perf_count(_dynamic_notch_filter_esc_rpm_update_perf);
}
@ -539,7 +562,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(const hrt_abstime &time_no
// disable these notch filters (if they aren't already)
if (nfx.getNotchFreq() > 0.f) {
for (int axis = 0; axis < 3; axis++) {
auto &nf = _dynamic_notch_filter_esc_rpm[axis][esc][harmonic];
auto &nf = _dynamic_notch_filter_esc_rpm[harmonic][axis][esc];
nf.disable();
perf_count(_dynamic_notch_filter_esc_rpm_disable_perf);
}
@ -548,7 +571,6 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(const hrt_abstime &time_no
}
if (esc_enabled) {
_dynamic_notch_esc_rpm_available = true;
_esc_available.set(esc, true);
_last_esc_rpm_notch_update[esc] = esc_report.timestamp;
@ -560,14 +582,14 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(const hrt_abstime &time_no
}
// check ESC feedback timeout
for (size_t esc = 0; esc < MAX_NUM_ESC_RPM; esc++) {
for (size_t esc = 0; esc < MAX_NUM_ESCS; esc++) {
if (_esc_available[esc] && (time_now_us > _last_esc_rpm_notch_update[esc] + DYNAMIC_NOTCH_FITLER_TIMEOUT)) {
_esc_available.set(esc, false);
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].disable();
for (int harmonic = 0; harmonic < _esc_rpm_harmonics; harmonic++) {
for (int axis = 0; axis < 3; axis++) {
_dynamic_notch_filter_esc_rpm[harmonic][axis][esc].disable();
perf_count(_dynamic_notch_filter_esc_rpm_disable_perf);
}
}
@ -642,13 +664,11 @@ float VehicleAngularVelocity::FilterAngularVelocity(int axis, float data[], int
#if !defined(CONSTRAINED_FLASH)
// Apply dynamic notch filter from ESC RPM
if (_dynamic_notch_esc_rpm_available) {
for (int esc = 0; esc < MAX_NUM_ESC_RPM; esc++) {
if (_dynamic_notch_filter_esc_rpm) {
for (int esc = 0; esc < MAX_NUM_ESCS; esc++) {
if (_esc_available[esc]) {
// apply notch filters higher -> lowest frequency
for (int harmonic = MAX_NUM_ESC_RPM_HARMONICS - 1; harmonic >= 0; harmonic--) {
_dynamic_notch_filter_esc_rpm[axis][esc][harmonic].applyArray(data, N);
for (int harmonic = 0; harmonic < _esc_rpm_harmonics; harmonic++) {
_dynamic_notch_filter_esc_rpm[harmonic][axis][esc].applyArray(data, N);
}
}
}

26
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -143,25 +143,28 @@ private:
static constexpr hrt_abstime DYNAMIC_NOTCH_FITLER_TIMEOUT = 1_s;
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;
// ESC RPM
static constexpr int MAX_NUM_ESCS = sizeof(esc_status_s::esc) / sizeof(esc_status_s::esc[0]);
static constexpr int MAX_NUM_FFT_PEAKS = sizeof(sensor_gyro_fft_s::peak_frequencies_x) / sizeof(
sensor_gyro_fft_s::peak_frequencies_x[0]);
using NotchFilterHarmonic = math::NotchFilter<float>[3][MAX_NUM_ESCS];
NotchFilterHarmonic *_dynamic_notch_filter_esc_rpm{nullptr};
math::NotchFilter<float> _dynamic_notch_filter_esc_rpm[3][MAX_NUM_ESC_RPM][MAX_NUM_ESC_RPM_HARMONICS] {};
math::NotchFilter<float> _dynamic_notch_filter_fft[3][MAX_NUM_FFT_PEAKS] {};
px4::Bitset<MAX_NUM_ESC_RPM> _esc_available{};
hrt_abstime _last_esc_rpm_notch_update[MAX_NUM_ESC_RPM] {};
int _esc_rpm_harmonics{0};
px4::Bitset<MAX_NUM_ESCS> _esc_available{};
hrt_abstime _last_esc_rpm_notch_update[MAX_NUM_ESCS] {};
perf_counter_t _dynamic_notch_filter_esc_rpm_update_perf{nullptr};
perf_counter_t _dynamic_notch_filter_esc_rpm_disable_perf{nullptr};
// FFT
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::NotchFilter<float> _dynamic_notch_filter_fft[3][MAX_NUM_FFT_PEAKS] {};
perf_counter_t _dynamic_notch_filter_fft_disable_perf{nullptr};
perf_counter_t _dynamic_notch_filter_fft_update_perf{nullptr};
bool _dynamic_notch_esc_rpm_available{false};
bool _dynamic_notch_fft_available{false};
#endif // !CONSTRAINED_FLASH
@ -181,6 +184,7 @@ private:
DEFINE_PARAMETERS(
#if !defined(CONSTRAINED_FLASH)
(ParamInt<px4::params::IMU_GYRO_DNF_EN>) _param_imu_gyro_dnf_en,
(ParamInt<px4::params::IMU_GYRO_DNF_HMC>) _param_imu_gyro_dnf_hmc,
(ParamFloat<px4::params::IMU_GYRO_DNF_BW>) _param_imu_gyro_dnf_bw,
#endif // !CONSTRAINED_FLASH
(ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff,

11
src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c
View File

@ -147,3 +147,14 @@ PARAM_DEFINE_INT32(IMU_GYRO_DNF_EN, 0);
* @max 30
*/
PARAM_DEFINE_FLOAT(IMU_GYRO_DNF_BW, 15.f);
/**
* IMU gyro dynamic notch filter harmonics
*
* ESC RPM number of harmonics (multiples of RPM) for ESC RPM dynamic notch filtering.
*
* @group Sensors
* @min 1
* @max 7
*/
PARAM_DEFINE_INT32(IMU_GYRO_DNF_HMC, 3);