fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-20 09:09:05 +08:00
Code Issues 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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -466,7 +466,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force)
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);
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++) {
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);
}