FlightTask manual acc: handle ekf position reset properly

Author: bresch

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)

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

src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp

@@ -56,6 +56,7 @@ class StickAccelerationXY : public ModuleParams
 
 	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,