From 63a35ccabd7eb1916b772ac3849ebb62758e5668 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 9 Mar 2021 19:27:33 +0100 Subject: [PATCH] StickAccelerationXY: fix losing the internal velocity setpoint state because of a failing position unlock with NAN velocity setpoint feedback. --- .../tasks/Utility/StickAccelerationXY.cpp | 21 ++++++++++++------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp index 332a194f5d..821ffdface 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp @@ -149,15 +149,20 @@ void StickAccelerationXY::applyTiltLimit(Vector2f &acceleration) void StickAccelerationXY::lockPosition(const Vector3f &pos, const matrix::Vector2f &vel_sp_feedback, const float dt) { - if (_velocity_setpoint.norm_squared() < FLT_EPSILON) { - if (!PX4_ISFINITE(_position_setpoint(0))) { - _position_setpoint = pos.xy(); - } + const bool moving = _velocity_setpoint.norm_squared() > FLT_EPSILON; + const bool position_locked = PX4_ISFINITE(_position_setpoint(0)) || PX4_ISFINITE(_position_setpoint(1)); - } else { - if (PX4_ISFINITE(_position_setpoint(0))) { - _position_setpoint.setNaN(); - // avoid velocity control jump because of remaining position error when unlocking + // lock position + if (!moving && !position_locked) { + _position_setpoint = pos.xy(); + } + + // open position loop + if (moving && position_locked) { + _position_setpoint.setNaN(); + + // avoid velocity setpoint jump caused by ignoring remaining position error + if (PX4_ISFINITE(vel_sp_feedback(0)) && PX4_ISFINITE(vel_sp_feedback(1))) { _velocity_setpoint = vel_sp_feedback; } }