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();