navigator: mission feasibility checker: check for a takeoff waypoint on...

navigator: mission feasibility checker: check for a takeoff waypoint on mission on specific conditions

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

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
  *

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
 	)