Merge pull request #3518 from PX4/mc_checks

multirotor mission feasibility checks:

src/modules/navigator/mission.cpp

@@ -259,7 +259,7 @@ Mission::update_offboard_mission()
 				dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
 				_navigator->get_home_position()->alt, _navigator->home_position_valid(),
 				_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
-				_param_dist_1wp.get(), _navigator->get_mission_result()->warning);
+				_param_dist_1wp.get(), _navigator->get_mission_result()->warning, _navigator->get_acceptance_radius());
 
 		_navigator->get_mission_result()->valid = !failed;
 		_navigator->increment_mission_instance_count();
@@ -843,7 +843,7 @@ Mission::check_mission_valid()
 				dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
 				_navigator->get_home_position()->alt, _navigator->home_position_valid(),
 				_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
-				_param_dist_1wp.get(), _navigator->get_mission_result()->warning);
+				_param_dist_1wp.get(), _navigator->get_mission_result()->warning, _navigator->get_acceptance_radius());
 
 		_navigator->increment_mission_instance_count();
 		_navigator->set_mission_result_updated();

src/modules/navigator/mission_feasibility_checker.cpp

@@ -64,7 +64,8 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() :
 
 bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRotarywing,
 	dm_item_t dm_current, size_t nMissionItems, Geofence &geofence,
-	float home_alt, bool home_valid, double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued)
+	float home_alt, bool home_valid, double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued,
+	float default_acceptance_rad)
 {
 	bool failed = false;
 	bool warned = false;
@@ -88,7 +89,7 @@ bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRota
 	failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned);
 
 	if (isRotarywing) {
-		failed = failed || !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid);
+		failed = failed || !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid, default_acceptance_rad);
 	} else {
 		failed = failed || !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid);
 	}
@@ -96,9 +97,41 @@ bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRota
 	return !failed;
 }
 
-bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid)
+bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems,
+	Geofence &geofence, float home_alt, bool home_valid, float default_acceptance_rad)
 {
-	/* no custom rotary wing checks yet */
+	/* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */
+	for (size_t i = 0; i < nMissionItems; i++) {
+		struct mission_item_s missionitem;
+		const ssize_t len = sizeof(struct mission_item_s);
+
+		if (dm_read(dm_current, i, &missionitem, len) != len) {
+			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+			return false;
+		}
+
+		// look for a takeoff waypoint
+		if (missionitem.nav_cmd == NAV_CMD_TAKEOFF) {
+			// make sure that the altitude of the waypoint is at least one meter larger than the acceptance radius
+			// this makes sure that the takeoff waypoint is not reached before we are at least one meter in the air
+			float takeoff_alt = missionitem.altitude_is_relative
+				      ? missionitem.altitude
+			              : missionitem.altitude - home_alt;
+			// check if we should use default acceptance radius
+			float acceptance_radius = default_acceptance_rad;
+
+			if (missionitem.acceptance_radius > NAV_EPSILON_POSITION) {
+				acceptance_radius = missionitem.acceptance_radius;
+			}
+
+			if (takeoff_alt - 1.0f < acceptance_radius) {
+				mavlink_log_critical(_mavlink_fd, "Mission rejected: Takeoff altitude too low!");
+				return false;
+			}
+		}
+	}
+
+	// all checks have passed
 	return true;
 }
 

src/modules/navigator/mission_feasibility_checker.h

@@ -72,7 +72,7 @@ private:
 	void updateNavigationCapabilities();
 
 	/* Checks specific to rotarywing airframes */
-	bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid);
+	bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid, float default_acceptance_rad);
 public:
 
 	MissionFeasibilityChecker();
@@ -83,7 +83,7 @@ public:
 	 */
 	bool checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current,
 		size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid,
-		double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued);
+		double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued, float default_acceptance_rad);
 
 };