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

vehicle_angular_velocity: ESC RPM notch filters minimize filter resets (#20449) 

- vehicle_angular_velocity: ESC RPM notch filters minimize filter resets
 - only allow one filter init per axis per cycle
 - "park" ESC notch filters at min frequency instead of full disable
 - relax timeout before a notch filter is disabled
 - add new parameter IMU_GYRO_DNF_MIN for configuring the minimum notch filter frequency
This commit is contained in:
Daniel Agar 2022-10-20 09:05:23 -04:00 committed by GitHub
parent 89bc28e836
commit b400b7fcc4
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 82 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
posix-configs/SITL/init/test/test_imu_filtering
View File

@ -25,6 +25,10 @@ param set IMU_GYRO_FFT_SNR 10
#param set IMU_GYRO_DNF_EN 2
param set IMU_GYRO_DNF_EN 3
param set IMU_GYRO_DNF_BW 15
param set IMU_GYRO_DNF_HMC 3
param set IMU_GYRO_DNF_MIN 25
# test values
param set IMU_GYRO_CUTOFF 60
param set IMU_DGYRO_CUTOFF 40

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

@ -61,6 +61,7 @@ VehicleAngularVelocity::~VehicleAngularVelocity()
#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_init_perf);
perf_free(_dynamic_notch_filter_esc_rpm_update_perf);
perf_free(_dynamic_notch_filter_fft_disable_perf);
@ -458,6 +459,11 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
MODULE_NAME": gyro dynamic notch filter ESC RPM disable");
}
if (_dynamic_notch_filter_esc_rpm_init_perf == nullptr) {
_dynamic_notch_filter_esc_rpm_init_perf = perf_alloc(PC_COUNT,
MODULE_NAME": gyro dynamic notch filter ESC RPM init");
}
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");
@ -467,9 +473,11 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
_esc_rpm_harmonics = 0;
perf_free(_dynamic_notch_filter_esc_rpm_disable_perf);
perf_free(_dynamic_notch_filter_esc_rpm_init_perf);
perf_free(_dynamic_notch_filter_esc_rpm_update_perf);
_dynamic_notch_filter_esc_rpm_disable_perf = nullptr;
_dynamic_notch_filter_esc_rpm_init_perf = nullptr;
_dynamic_notch_filter_esc_rpm_update_perf = nullptr;
}
}
@ -566,76 +574,89 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(const hrt_abstime &time_no
if (enabled && (_esc_status_sub.updated() || force)) {
bool axis_init[3] {false, false, false};
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
const float bandwidth_hz = _param_imu_gyro_dnf_bw.get();
const float freq_min = math::max(_param_imu_gyro_dnf_min.get(), bandwidth_hz);
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
if ((esc_report.esc_rpm != 0) && (time_now_us < esc_report.timestamp + DYNAMIC_NOTCH_FITLER_TIMEOUT)) {
const bool esc_connected = (esc_status.esc_online_flags & (1 << esc)) || (esc_report.esc_rpm != 0);
bool esc_enabled = false;
bool update = force || !_esc_available[esc]; // force parameter update or notch was previously disabled
// only update if ESC RPM range seems valid
if (esc_connected && (time_now_us < esc_report.timestamp + DYNAMIC_NOTCH_FITLER_TIMEOUT)) {
const float esc_hz = abs(esc_report.esc_rpm) / 60.f;
const bool force_update = force || !_esc_available[esc]; // force parameter update or notch was previously disabled
for (int harmonic = 0; harmonic < _esc_rpm_harmonics; harmonic++) {
const float frequency_hz = esc_hz * (harmonic + 1);
// as RPM drops leave the notch filter "parked" at the minimum rather than disabling
// keep harmonics separated by half the notch filter bandwidth
const float frequency_hz = math::max(esc_hz * (harmonic + 1), freq_min + (harmonic * 0.5f * bandwidth_hz));
// for each ESC harmonic determine if enabled/disabled from first notch (x axis)
auto &nfx = _dynamic_notch_filter_esc_rpm[harmonic][0][esc];
// update filter parameters if frequency changed or forced
for (int axis = 0; axis < 3; axis++) {
auto &nf = _dynamic_notch_filter_esc_rpm[harmonic][axis][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[harmonic][axis][esc];
nf.setParameters(_filter_sample_rate_hz, frequency_hz, _param_imu_gyro_dnf_bw.get());
const float notch_freq_delta = fabsf(nf.getNotchFreq() - frequency_hz);
const bool notch_freq_changed = (notch_freq_delta > 0.1f);
// only allow initializing one new filter per axis each iteration
const bool allow_update = !axis_init[axis] || (nf.initialized() && notch_freq_delta < nf.getBandwidth());
if ((force_update || notch_freq_changed) && allow_update) {
if (nf.setParameters(_filter_sample_rate_hz, frequency_hz, bandwidth_hz)) {
perf_count(_dynamic_notch_filter_esc_rpm_update_perf);
}
}
esc_enabled = true;
} else {
// 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[harmonic][axis][esc];
nf.disable();
perf_count(_dynamic_notch_filter_esc_rpm_disable_perf);
if (!nf.initialized()) {
perf_count(_dynamic_notch_filter_esc_rpm_init_perf);
axis_init[axis] = true;
}
}
}
}
}
if (esc_enabled) {
_esc_available.set(esc, true);
_last_esc_rpm_notch_update[esc] = esc_report.timestamp;
} else {
_esc_available.set(esc, false);
}
_esc_available.set(esc, true);
_last_esc_rpm_notch_update[esc] = esc_report.timestamp;
}
}
}
// check ESC feedback timeout
// check notch filter timeout
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)) {
bool all_disabled = true;
_esc_available.set(esc, false);
for (int harmonic = 0; harmonic < _esc_rpm_harmonics; harmonic++) {
// disable notch filters from highest frequency to lowest
for (int harmonic = _esc_rpm_harmonics - 1; harmonic >= 0; 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);
auto &nf = _dynamic_notch_filter_esc_rpm[harmonic][axis][esc];
if (nf.getNotchFreq() > 0.f) {
if (nf.initialized() && !axis_init[axis]) {
nf.disable();
perf_count(_dynamic_notch_filter_esc_rpm_disable_perf);
axis_init[axis] = true;
}
}
if (nf.getNotchFreq() > 0.f) {
all_disabled = false;
}
}
}
if (all_disabled) {
_esc_available.set(esc, false);
}
}
}
}
@ -711,7 +732,9 @@ float VehicleAngularVelocity::FilterAngularVelocity(int axis, float data[], int
for (int esc = 0; esc < MAX_NUM_ESCS; esc++) {
if (_esc_available[esc]) {
for (int harmonic = 0; harmonic < _esc_rpm_harmonics; harmonic++) {
_dynamic_notch_filter_esc_rpm[harmonic][axis][esc].applyArray(data, N);
if (_dynamic_notch_filter_esc_rpm[harmonic][axis][esc].getNotchFreq() > 0.f) {
_dynamic_notch_filter_esc_rpm[harmonic][axis][esc].applyArray(data, N);
}
}
}
}
@ -941,6 +964,7 @@ 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_init_perf);
perf_print_counter(_dynamic_notch_filter_esc_rpm_update_perf);
perf_print_counter(_dynamic_notch_filter_fft_disable_perf);

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

@ -142,7 +142,7 @@ private:
FFT = 2,
};
static constexpr hrt_abstime DYNAMIC_NOTCH_FITLER_TIMEOUT = 1_s;
static constexpr hrt_abstime DYNAMIC_NOTCH_FITLER_TIMEOUT = 3_s;
// ESC RPM
static constexpr int MAX_NUM_ESCS = sizeof(esc_status_s::esc) / sizeof(esc_status_s::esc[0]);
@ -154,8 +154,9 @@ private:
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};
perf_counter_t _dynamic_notch_filter_esc_rpm_init_perf{nullptr};
perf_counter_t _dynamic_notch_filter_esc_rpm_update_perf{nullptr};
// FFT
static constexpr int MAX_NUM_FFT_PEAKS = sizeof(sensor_gyro_fft_s::peak_frequencies_x)
@ -187,6 +188,7 @@ private:
(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,
(ParamFloat<px4::params::IMU_GYRO_DNF_MIN>) _param_imu_gyro_dnf_min,
#endif // !CONSTRAINED_FLASH
(ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff,
(ParamFloat<px4::params::IMU_GYRO_NF0_FRQ>) _param_imu_gyro_nf0_frq,

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

@ -178,6 +178,7 @@ PARAM_DEFINE_INT32(IMU_GYRO_DNF_EN, 0);
* Bandwidth per notch filter when using dynamic notch filtering with ESC RPM.
*
* @group Sensors
* @unit Hz
* @min 5
* @max 30
*/
@ -193,3 +194,13 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_DNF_BW, 15.f);
* @max 7
*/
PARAM_DEFINE_INT32(IMU_GYRO_DNF_HMC, 3);
/**
* IMU gyro dynamic notch filter minimum frequency
*
* Minimum notch filter frequency in Hz.
*
* @group Sensors
* @unit Hz
*/
PARAM_DEFINE_FLOAT(IMU_GYRO_DNF_MIN, 25.f);