MulticopterPositionControl: reset task when switching mode

to make sure the state is clean between auto mode switches.

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,

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.