From 61e741e7d0c296ed0d586035213635805c01dace Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 25 Jul 2025 11:19:55 +0200 Subject: [PATCH] FlightTask manual acc: handle ekf position reset properly --- .../ManualAcceleration/FlightTaskManualAcceleration.cpp | 2 +- .../tasks/Utility/StickAccelerationXY.cpp | 5 +++++ .../tasks/Utility/StickAccelerationXY.hpp | 1 + 3 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp index 6b79aa1833..788b7cf989 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp @@ -106,7 +106,7 @@ bool FlightTaskManualAcceleration::update() void FlightTaskManualAcceleration::_ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) { - _stick_acceleration_xy.resetPosition(); + _stick_acceleration_xy.addToPositionSetpoint(delta_xy); } void FlightTaskManualAcceleration::_ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp index c36d024ead..052378244b 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp @@ -55,6 +55,11 @@ void StickAccelerationXY::resetPosition(const matrix::Vector2f &position) _position_setpoint = position; } +void StickAccelerationXY::addToPositionSetpoint(const matrix::Vector2f &delta) +{ + _position_setpoint += delta; +} + void StickAccelerationXY::resetVelocity(const matrix::Vector2f &velocity) { if (velocity.isAllFinite()) { diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp index 1b29580c4f..ad6f4afec1 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp @@ -56,6 +56,7 @@ public: void resetPosition(); void resetPosition(const matrix::Vector2f &position); + void addToPositionSetpoint(const matrix::Vector2f &delta); void resetVelocity(const matrix::Vector2f &velocity); void resetAcceleration(const matrix::Vector2f &acceleration); void generateSetpoints(matrix::Vector2f stick_xy, const float yaw, const float yaw_sp, const matrix::Vector3f &pos,