From 3e97782bc5c569d3e7f9482ab2214761acbca28b Mon Sep 17 00:00:00 2001 From: Balduin Date: Wed, 23 Jul 2025 11:12:16 +0200 Subject: [PATCH] StickAccelerationXY: if invalid pos, do not give pos sp --- .../tasks/Utility/StickAccelerationXY.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp index c36d024ead..c8b45af35d 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp @@ -204,6 +204,14 @@ void StickAccelerationXY::lockPosition(const Vector3f &pos, const matrix::Vector const bool moving = _velocity_setpoint.norm_squared() > FLT_EPSILON; const bool position_locked = Vector2f(_position_setpoint).isAllFinite(); + if (!pos.isAllFinite()) { + // position is invalid. We may still enter here through a mode + // requiring only relaxed local position. In this case, no + // position locking can happen, so we give no position setpoint. + _position_setpoint.setNaN(); + return; + } + // lock position if (!moving && !position_locked) { _position_setpoint = pos.xy();