Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 15:40:31 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

sensors/vehicle_angular_velocity: avoid unnecessary ESC notch filter resets

Browse Source
This commit is contained in:
Daniel Agar
2021-11-03 13:32:50 -04:00
parent 78436e706c
commit c60a9e2981
1 changed files with 5 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
+5 -4
View File
@@ -454,15 +454,16 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force)
// for each ESC check determine if enabled/disabled from first notch (x axis, harmonic 0)
auto &nfx0 = _dynamic_notch_filter_esc_rpm[0][esc][0];
bool reset = (nfx0.getNotchFreq() <= FLT_EPSILON); // notch was previously disabled
bool reset = force || (nfx0.getNotchFreq() <= FLT_EPSILON); // notch was previously disabled
const float esc_hz = esc_rpm / 60.f;
const float notch_freq_diff = fabsf(nfx0.getNotchFreq() - esc_hz);
// update filter parameters if frequency changed or forced
if (force || reset || (fabsf(nfx0.getNotchFreq() - esc_hz) > FLT_EPSILON)) {
if (reset || (notch_freq_diff > 0.1f)) {
// force reset if the notch frequency jumps significantly
if (!reset || (fabsf(nfx0.getNotchFreq() - esc_hz) > _param_imu_gyro_dnf_bw.get())) {
if (notch_freq_diff > _param_imu_gyro_dnf_bw.get()) {
reset = true;
}
@@ -478,7 +479,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force)
perf_count(_dynamic_notch_filter_esc_rpm_update_perf);
}
if (force || reset) {
if (reset) {
const Vector3f reset_angular_velocity{GetResetAngularVelocity()};
for (int axis = 0; axis < 3; axis++) {