From bb9f167d79e8c085ba774def8ac81cf10a385348 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Tue, 5 Oct 2021 11:43:53 +0200
Subject: [PATCH] StickAccelerationXY: add reseting position to specified
 coordinates

---
 .../tasks/Utility/StickAccelerationXY.cpp                    | 5 +++++
 .../tasks/Utility/StickAccelerationXY.hpp                    | 1 +
 2 files changed, 6 insertions(+)

diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
index e5a7af4252..b6a33e9bce 100644
--- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
+++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
@@ -54,6 +54,11 @@ void StickAccelerationXY::resetPosition()
 	_position_setpoint.setNaN();
 }
 
+void StickAccelerationXY::resetPosition(const matrix::Vector2f &position)
+{
+	_position_setpoint = position;
+}
+
 void StickAccelerationXY::resetVelocity(const matrix::Vector2f &velocity)
 {
 	_velocity_setpoint = velocity;
diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp
index 347dbaf049..94637217d6 100644
--- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp
+++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp
@@ -54,6 +54,7 @@ public:
 	~StickAccelerationXY() = default;
 
 	void resetPosition();
+	void resetPosition(const matrix::Vector2f &position);
 	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,