From 5b151a5df777d9296176bc6fa0f80568263ace4d Mon Sep 17 00:00:00 2001
From: TSC21 <n.marques21@hotmail.com>
Date: Wed, 20 Feb 2019 19:49:35 +0000
Subject: [PATCH] navigator: mission feasibility checker: check for a takeoff
 waypoint on mission on specific conditions

---
 .../navigator/mission_feasibility_checker.cpp | 34 ++++++++++++++++---
 src/modules/navigator/mission_params.c        | 10 ++++++
 src/modules/navigator/navigator.h             |  2 ++
 3 files changed, 42 insertions(+), 4 deletions(-)

diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp
index 2ecb7f74fe..80936193c4 100644
--- a/src/modules/navigator/mission_feasibility_checker.cpp
+++ b/src/modules/navigator/mission_feasibility_checker.cpp
@@ -95,6 +95,8 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
 bool
 MissionFeasibilityChecker::checkRotarywing(const mission_s &mission, float home_alt, bool home_alt_valid)
 {
+	bool has_takeoff = false;
+
 	for (size_t i = 0; i < mission.count; i++) {
 		struct mission_item_s missionitem = {};
 		const ssize_t len = sizeof(struct mission_item_s);
@@ -122,11 +124,22 @@ MissionFeasibilityChecker::checkRotarywing(const mission_s &mission, float home_
 				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Takeoff altitude too low!");
 				return false;
 			}
+
+			has_takeoff = true;
 		}
 	}
 
-	// all checks have passed
-	return true;
+	// checks if the mission has at least a takeoff waypoint
+	// MIS_TAKEOFF_REQ param has to be set and the vehicle has to be landed - one can load a mission
+	// while the vehicle is flying and it does not require a takeoff waypoint
+	if (!has_takeoff && _navigator->get_takeoff_required() && _navigator->get_land_detected()->landed) {
+		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: No takeoff waypoint found!");
+		return false;
+
+	} else {
+		// all checks have passed
+		return true;
+	}
 }
 
 bool
@@ -315,6 +328,8 @@ MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission)
 bool
 MissionFeasibilityChecker::checkFixedWingTakeoff(const mission_s &mission, float home_alt, bool home_alt_valid)
 {
+	bool has_takeoff = false;
+
 	for (size_t i = 0; i < mission.count; i++) {
 		struct mission_item_s missionitem = {};
 		const ssize_t len = sizeof(struct mission_item_s);
@@ -344,11 +359,22 @@ MissionFeasibilityChecker::checkFixedWingTakeoff(const mission_s &mission, float
 				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Takeoff altitude too low!");
 				return false;
 			}
+
+			has_takeoff = true;
 		}
 	}
 
-	// all checks have passed
-	return true;
+	// checks if the mission has at least one takeoff waypoint;
+	// MIS_TAKEOFF_REQ param has to be set and the vehicle has to be landed - one can load a mission
+	// while the vehicle is flying and it does not require a takeoff waypoint
+	if (!has_takeoff && _navigator->get_takeoff_required() && _navigator->get_land_detected()->landed) {
+		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: No takeoff waypoint found!");
+		return false;
+
+	} else {
+		// all checks have passed
+		return true;
+	}
 }
 
 bool
diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c
index d1b6f48f01..17e6bba7ab 100644
--- a/src/modules/navigator/mission_params.c
+++ b/src/modules/navigator/mission_params.c
@@ -57,6 +57,16 @@
  */
 PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 2.5f);
 
+/**
+ * Take-off waypoint required
+ *
+ * If set, the mission feasibility checker will check for a takeoff waypoint on the mission.
+ *
+ * @boolean
+ * @group Mission
+ */
+PARAM_DEFINE_INT32(MIS_TAKEOFF_REQ, 0);
+
 /**
  * Minimum Loiter altitude
  *
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 95bbffc504..c83bb58870 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -281,6 +281,7 @@ public:
 	// Param access
 	float		get_loiter_min_alt() const { return _param_loiter_min_alt.get(); }
 	float		get_takeoff_min_alt() const { return _param_takeoff_min_alt.get(); }
+	bool		get_takeoff_required() const { return _param_takeoff_required.get(); }
 	float		get_yaw_timeout() const { return _param_yaw_timeout.get(); }
 	float		get_yaw_threshold() const { return _param_yaw_err.get(); }
 
@@ -371,6 +372,7 @@ private:
 		// Mission (MIS_*)
 		(ParamFloat<px4::params::MIS_LTRMIN_ALT>) _param_loiter_min_alt,
 		(ParamFloat<px4::params::MIS_TAKEOFF_ALT>) _param_takeoff_min_alt,
+		(ParamBool<px4::params::MIS_TAKEOFF_REQ>) _param_takeoff_required,
 		(ParamFloat<px4::params::MIS_YAW_TMT>) _param_yaw_timeout,
 		(ParamFloat<px4::params::MIS_YAW_ERR>) _param_yaw_err
 	)