fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 09:13: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/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};