From 8f40558f41ecc4385ca4f9066f68cebc8c98e3bb Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Thu, 19 Nov 2020 18:04:21 +0100
Subject: [PATCH] MulticopterPositionControl: reset task when switching mode

to make sure the state is clean between auto mode switches.
---
 src/modules/mc_pos_control/MulticopterPositionControl.cpp | 7 +++++++
 src/modules/mc_pos_control/MulticopterPositionControl.hpp | 2 ++
 2 files changed, 9 insertions(+)

diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp
index 683d4142b7..d1ada966d6 100644
--- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp
+++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp
@@ -477,6 +477,11 @@ void MulticopterPositionControl::start_flight_task()
 		return;
 	}
 
+	// Switch to clean new task when mode switches e.g. to reset state when switching between auto modes
+	if (_last_vehicle_nav_state != _vehicle_status.nav_state) {
+		_flight_tasks.switchTask(FlightTaskIndex::None);
+	}
+
 	if (_vehicle_status.in_transition_mode) {
 		should_disable_task = false;
 		FlightTaskError error = _flight_tasks.switchTask(FlightTaskIndex::Transition);
@@ -666,6 +671,8 @@ void MulticopterPositionControl::start_flight_task()
 	} else if (should_disable_task) {
 		_flight_tasks.switchTask(FlightTaskIndex::None);
 	}
+
+	_last_vehicle_nav_state = _vehicle_status.nav_state;
 }
 
 void MulticopterPositionControl::failsafe(const hrt_abstime &now, vehicle_local_position_setpoint_s &setpoint,
diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp
index efb4336863..632edd473a 100644
--- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp
+++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp
@@ -203,6 +203,8 @@ private:
 
 	perf_counter_t _cycle_perf;
 
+	uint8_t _last_vehicle_nav_state{0};
+
 	/**
 	 * Update our local parameter cache.
 	 * Parameter update can be forced when argument is true.