From 8d296a50f9b3cf2842dfb5e49037fbbd3ecbe532 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Wed, 5 Feb 2025 12:14:19 +0100
Subject: [PATCH] FlightTask: properly initialize EKF reset counters

This fixes a race condition when switching from a flight mode that is
not a flight task (e.g.: stabilized). In this case, the reset counters
were initialized to 0 and deltas were applied to the first setpoints if
the EKF had any of its reset counters different from 0.
---
 .../flight_mode_manager/FlightModeManager.cpp |  3 ---
 .../tasks/FlightTask/FlightTask.cpp           | 11 +++++++++
 .../tasks/FlightTask/FlightTask.hpp           | 23 ++++++++-----------
 3 files changed, 21 insertions(+), 16 deletions(-)

diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp
index c58a7fa157..f17e00aa96 100644
--- a/src/modules/flight_mode_manager/FlightModeManager.cpp
+++ b/src/modules/flight_mode_manager/FlightModeManager.cpp
@@ -374,11 +374,9 @@ FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index)
 
 	// Save current setpoints for the next FlightTask
 	trajectory_setpoint_s last_setpoint = FlightTask::empty_trajectory_setpoint;
-	ekf_reset_counters_s last_reset_counters{};
 
 	if (isAnyTaskActive()) {
 		last_setpoint = _current_task.task->getTrajectorySetpoint();
-		last_reset_counters = _current_task.task->getResetCounters();
 	}
 
 	if (_initTask(new_task_index)) {
@@ -399,7 +397,6 @@ FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index)
 		return FlightTaskError::ActivationFailed;
 	}
 
-	_current_task.task->setResetCounters(last_reset_counters);
 	_command_failed = false;
 
 	return FlightTaskError::NoError;
diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp
index cbecf517f6..fa2943f147 100644
--- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp
+++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp
@@ -11,6 +11,7 @@ bool FlightTask::activate(const trajectory_setpoint_s &last_setpoint)
 {
 	_resetSetpoints();
 	_setDefaultConstraints();
+	initEkfResetCounters();
 	_time_stamp_activate = hrt_absolute_time();
 	_gear = empty_landing_gear_default_keep;
 	return true;
@@ -24,6 +25,16 @@ void FlightTask::reActivate()
 	activate(setpoint_preserve_vertical);
 }
 
+void FlightTask::initEkfResetCounters()
+{
+	_reset_counters.xy = _sub_vehicle_local_position.get().xy_reset_counter;
+	_reset_counters.vxy = _sub_vehicle_local_position.get().vxy_reset_counter;
+	_reset_counters.z = _sub_vehicle_local_position.get().z_reset_counter;
+	_reset_counters.hagl = _sub_vehicle_local_position.get().dist_bottom_reset_counter;
+	_reset_counters.vz = _sub_vehicle_local_position.get().vz_reset_counter;
+	_reset_counters.heading = _sub_vehicle_local_position.get().heading_reset_counter;
+}
+
 bool FlightTask::updateInitialize()
 {
 	_time_stamp_current = hrt_absolute_time();
diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp
index 6ee861f3cd..06327d8ca8 100644
--- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp
+++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp
@@ -55,15 +55,6 @@
 #include <uORB/topics/home_position.h>
 #include <lib/geo/geo.h>
 
-struct ekf_reset_counters_s {
-	uint8_t xy;
-	uint8_t vxy;
-	uint8_t z;
-	uint8_t vz;
-	uint8_t heading;
-	uint8_t hagl;
-};
-
 class FlightTask : public ModuleParams
 {
 public:
@@ -88,6 +79,8 @@ public:
 	 */
 	virtual void reActivate();
 
+	virtual void initEkfResetCounters();
+
 	/**
 	 * To be called to adopt parameters from an arrived vehicle command
 	 * @param command received command message containing the parameters
@@ -115,9 +108,6 @@ public:
 	 */
 	const trajectory_setpoint_s getTrajectorySetpoint();
 
-	const ekf_reset_counters_s getResetCounters() const { return _reset_counters; }
-	void setResetCounters(const ekf_reset_counters_s &counters) { _reset_counters = counters; }
-
 	/**
 	 * Get vehicle constraints.
 	 * The constraints can vary with task.
@@ -236,7 +226,14 @@ protected:
 	float _yaw_setpoint{};
 	float _yawspeed_setpoint{};
 
-	ekf_reset_counters_s _reset_counters{}; ///< Counters for estimator local position resets
+	struct ekf_reset_counters_s {
+		uint8_t xy;
+		uint8_t vxy;
+		uint8_t z;
+		uint8_t vz;
+		uint8_t heading;
+		uint8_t hagl;
+	} _reset_counters{}; ///< Counters for estimator local position resets
 
 	/**
 	 * Vehicle constraints.