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

ekf2: drag fusion don't use any accel samples that are clipping

This commit is contained in:
Daniel Agar 2023-08-17 16:16:10 -04:00
parent b485b1a12a
commit 2c76109a94
1 changed files with 11 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/modules/ekf2/EKF/estimator_interface.cpp
View File

@ -494,6 +494,17 @@ void EstimatorInterface::setDragData(const imuSample &imu)
}
}
// don't use any accel samples that are clipping
if (imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2]) {
// reset accumulators
_drag_sample_count = 0;
_drag_down_sampled.accelXY.zero();
_drag_down_sampled.time_us = 0;
_drag_sample_time_dt = 0.0f;
return;
}
_drag_sample_count++;
// note acceleration is accumulated as a delta velocity
_drag_down_sampled.accelXY(0) += imu.delta_vel(0);