From c3ba2687ac3ebd0d35166bad50404b15d8f75204 Mon Sep 17 00:00:00 2001
From: Dennis Mannhart <dennis@yuneecresearch.com>
Date: Wed, 12 Sep 2018 10:24:53 +0200
Subject: [PATCH] FlightTask: if not in air, reActivate the task which will set
 the setpoints to current vehicle state.

---
 src/lib/FlightTasks/FlightTasks.cpp                | 8 ++++++++
 src/lib/FlightTasks/FlightTasks.hpp                | 5 +++++
 src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 ++
 3 files changed, 15 insertions(+)

diff --git a/src/lib/FlightTasks/FlightTasks.cpp b/src/lib/FlightTasks/FlightTasks.cpp
index d9c4926e97..1b8b7e4baf 100644
--- a/src/lib/FlightTasks/FlightTasks.cpp
+++ b/src/lib/FlightTasks/FlightTasks.cpp
@@ -130,6 +130,14 @@ const char *FlightTasks::errorToString(const int error)
 	return "This error is not mapped to a string or is unknown.";
 }
 
+void FlightTasks::reActivate()
+{
+
+	if (_current_task.task) {
+		_current_task.task->activate();
+	}
+}
+
 void FlightTasks::_updateCommand()
 {
 	// lazy subscription to command topic
diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp
index da29a1be58..5eb228aa7b 100644
--- a/src/lib/FlightTasks/FlightTasks.hpp
+++ b/src/lib/FlightTasks/FlightTasks.hpp
@@ -131,6 +131,11 @@ public:
 	 */
 	void setYawHandler(WeatherVane *ext_yaw_handler) {_current_task.task->setYawHandler(ext_yaw_handler);}
 
+	/**
+	 *   This method will re-activate current task.
+	 */
+	void reActivate();
+
 private:
 
 	/**
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 2bb1ecb0ec..a2d616720c 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -692,6 +692,8 @@ MulticopterPositionControl::run()
 				setpoint.yawspeed = NAN;
 				setpoint.yaw = _states.yaw;
 				constraints.landing_gear = vehicle_constraints_s::GEAR_KEEP;
+				// reactivate the task which will reset the setpoint to current state
+				_flight_tasks.reActivate();
 			}
 
 			// limit altitude only if local position is valid