fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

flight_mode_manager: StickAccelerationXY minimize setpoint oscillations around 0

Browse Source
This commit is contained in:
Daniel Agar 2021-07-11 20:42:28 -04:00 committed by Matthias Grob
parent f300ec1da2
commit 085ec0e8ef
1 changed files with 5 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
View File

@ -99,8 +99,11 @@ void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw,
// Don't allow the drag to change the sign of the velocity, otherwise we might get into oscillations around 0, due
// to discretization
if (_acceleration_setpoint.norm_squared() < FLT_EPSILON
&& _velocity_setpoint.norm_squared() < drag.norm_squared() * dt * dt) {
if (((_acceleration_setpoint.norm_squared() < FLT_EPSILON)
|| (sign(_acceleration_setpoint_prev(0)) != sign(_acceleration_setpoint(0)))
|| (sign(_acceleration_setpoint_prev(1)) != sign(_acceleration_setpoint(1))))
&& (_velocity_setpoint.norm_squared() < (drag.norm_squared() * dt * dt))) {
drag.setZero();
_velocity_setpoint.setZero();
}