Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 19:07:45 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

sensors/vehicle_angular_velocity: fix ESC RPM notch filter update

Browse Source
This commit is contained in:
Daniel Agar
2021-11-03 11:41:19 -04:00
parent 376b72fb2f
commit edce30c6de
1 changed files with 2 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
+2 -1
View File
@@ -466,7 +466,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force)
reset = true; reset = true;
} }
for (int harmonic = MAX_NUM_ESC_RPM_HARMONICS; harmonic >= 0; harmonic--) { for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
const float frequency_hz = esc_hz * (harmonic + 1); const float frequency_hz = esc_hz * (harmonic + 1);
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
@@ -599,6 +599,7 @@ float VehicleAngularVelocity::FilterAngularVelocity(int axis, float data[], int
for (int esc = 0; esc < MAX_NUM_ESC_RPM; esc++) { for (int esc = 0; esc < MAX_NUM_ESC_RPM; esc++) {
if (_esc_available[esc]) { if (_esc_available[esc]) {
// apply notch filters higher -> lowest frequency
for (int harmonic = MAX_NUM_ESC_RPM_HARMONICS - 1; harmonic >= 0; harmonic--) { for (int harmonic = MAX_NUM_ESC_RPM_HARMONICS - 1; harmonic >= 0; harmonic--) {
_dynamic_notch_filter_esc_rpm[axis][esc][harmonic].applyArray(data, N); _dynamic_notch_filter_esc_rpm[axis][esc][harmonic].applyArray(data, N);
} }