From a2d5485c7f7f14bf818c4440ec41709c2162b826 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Thu, 13 Dec 2018 15:30:11 +0100
Subject: [PATCH] Trajectory manual - Handle EKF xy reset
---
.../FlightTaskManualPositionSmoothVel.cpp | 10 ++++++++++
.../FlightTaskManualPositionSmoothVel.hpp | 1 +
2 files changed, 11 insertions(+)
diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
index fc4ffa7027..5e0109ee5f 100644
--- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
+++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
@@ -147,6 +147,16 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
VelocitySmoothing::timeSynchronization(_smoothing, 2); // Synchronize x and y only
+ if (_position_lock_xy_active) {
+ // Check if a reset event has happened.
+ if (_sub_vehicle_local_position->get().xy_reset_counter != _reset_counter) {
+ // Reset the XY axes
+ _smoothing[0].setCurrentPosition(_position(0));
+ _smoothing[1].setCurrentPosition(_position(1));
+ _reset_counter = _sub_vehicle_local_position->get().xy_reset_counter;
+ }
+ }
+
Vector3f pos_sp_smooth;
Vector3f accel_sp_smooth;
diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
index 9892f59cda..945c76fe45 100644
--- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
+++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
@@ -70,4 +70,5 @@ private:
matrix::Vector3f _vel_sp_smooth;
bool _position_lock_xy_active{false};
matrix::Vector2f _position_setpoint_xy_locked;
+ uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */
};
--
GitLab