navigator: check distances between waypoints

Instead of just checking whether the first waypoint is too far away from
home it makes sense to also check between waypoints.
This can prevent
- flyaways due to user errors, or
- catch the corner case where a takeoff waypoint is added to a mission
  and therefore the first waypoint is not too far away, however, the
  subsequent waypoints are still too far away.

src/modules/navigator/mission.cpp

@@ -63,6 +63,7 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	_param_onboard_enabled(this, "MIS_ONBOARD_EN", false),
 	_param_takeoff_alt(this, "MIS_TAKEOFF_ALT", false),
 	_param_dist_1wp(this, "MIS_DIST_1WP", false),
+	_param_dist_between_wps(this, "MIS_DIST_WPS", false),
 	_param_altmode(this, "MIS_ALTMODE", false),
 	_param_yawmode(this, "MIS_YAWMODE", false),
 	_param_force_vtol(this, "NAV_FORCE_VT", false),
@@ -1439,7 +1440,10 @@ Mission::check_mission_valid(bool force)
 	if ((!_home_inited && _navigator->home_position_valid()) || force) {
 
 		_navigator->get_mission_result()->valid =
-			_missionFeasibilityChecker.checkMissionFeasible(_offboard_mission, _param_dist_1wp.get(), false);
+			_missionFeasibilityChecker.checkMissionFeasible(_offboard_mission,
+					_param_dist_1wp.get(),
+					_param_dist_between_wps.get(),
+					false);
 
 		_navigator->get_mission_result()->seq_total = _offboard_mission.count;
 		_navigator->increment_mission_instance_count();

src/modules/navigator/mission.h

@@ -239,6 +239,7 @@ private:
 	control::BlockParamInt _param_onboard_enabled;
 	control::BlockParamFloat _param_takeoff_alt;
 	control::BlockParamFloat _param_dist_1wp;
+	control::BlockParamFloat _param_dist_between_wps;
 	control::BlockParamInt _param_altmode;
 	control::BlockParamInt _param_yawmode;
 	control::BlockParamInt _param_force_vtol;

src/modules/navigator/mission_feasibility_checker.cpp

@@ -51,7 +51,8 @@
 #include <systemlib/mavlink_log.h>
 
 bool
-MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission, float max_waypoint_distance,
+MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
+		float max_distance_to_1st_waypoint, float max_distance_between_waypoints,
 		bool land_start_req)
 {
 	dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(mission.dataman_id);
@@ -78,15 +79,14 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission, float
 		mavlink_log_info(_navigator->get_mavlink_log_pub(), "Not yet ready for mission, no position lock.");
 
 	} else {
-		const double lat = _navigator->get_home_position()->lat;
-		const double lon = _navigator->get_home_position()->lon;
-
-		failed = failed
-			 || !check_dist_1wp(dm_current, nMissionItems, lat, lon, max_waypoint_distance, warning_issued);
+		failed = failed ||
+			 !checkDistanceToFirstWaypoint(dm_current, nMissionItems, max_distance_to_1st_waypoint, warning_issued);
 	}
 
 	// check if all mission item commands are supported
 	failed = failed || !checkMissionItemValidity(dm_current, nMissionItems, landed);
+	failed = failed
+		 || !checkDistancesBetweenWaypoints(dm_current, nMissionItems, max_distance_between_waypoints, warning_issued);
 	failed = failed || !checkGeofence(dm_current, nMissionItems, geofence, home_alt, home_valid);
 	failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned);
 
@@ -476,51 +476,120 @@ MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nM
 }
 
 bool
-MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon,
-		float dist_first_wp, bool &warning_issued)
+MissionFeasibilityChecker::checkDistanceToFirstWaypoint(dm_item_t dm_current, size_t nMissionItems,
+		float max_distance, bool &warning_issued)
 {
-	/* check if first waypoint is not too far from home */
-	if (dist_first_wp > 0.0f) {
-		struct mission_item_s mission_item = {};
+	if (max_distance <= 0.0f) {
+		/* param not set, check is ok */
+		return true;
+	}
 
-		/* find first waypoint (with lat/lon) item in datamanager */
-		for (size_t i = 0; i < nMissionItems; i++) {
-			if (dm_read(dm_current, i, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) {
-				if (MissionBlock::item_contains_position(mission_item)) {
-					/* check only items with valid lat/lon */
-
-					/* check distance from current position to item */
-					float dist_to_1wp = get_distance_to_next_waypoint(mission_item.lat, mission_item.lon, curr_lat, curr_lon);
-
-					if (dist_to_1wp < dist_first_wp) {
-						if (dist_to_1wp > ((dist_first_wp * 3) / 2)) {
-							/* allow at 2/3 distance, but warn */
-							mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission entry point very far: %d m.", (int)dist_to_1wp);
-							warning_issued = true;
-						}
+	/* find first waypoint (with lat/lon) item in datamanager */
+	for (size_t i = 0; i < nMissionItems; i++) {
 
-						return true;
+		struct mission_item_s mission_item {};
 
-					} else {
-						/* item is too far from home */
-						mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission entry point too far: %dm, %dm max.",
-								     (int)dist_to_1wp, (int)dist_first_wp);
-						warning_issued = true;
-						return false;
-					}
+		if (!(dm_read(dm_current, i, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s))) {
+			/* error reading, mission is invalid */
+			mavlink_log_info(_navigator->get_mavlink_log_pub(), "Error reading offboard mission.");
+			return false;
+		}
+
+		/* check only items with valid lat/lon */
+		if (!MissionBlock::item_contains_position(mission_item)) {
+			continue;
+		}
+
+		/* check distance from current position to item */
+		float dist_to_1wp = get_distance_to_next_waypoint(
+					    mission_item.lat, mission_item.lon,
+					    _navigator->get_home_position()->lat, _navigator->get_home_position()->lon);
+
+		if (dist_to_1wp < max_distance) {
+
+			if (dist_to_1wp > ((max_distance * 3) / 2)) {
+				/* allow at 2/3 distance, but warn */
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(),
+						     "First waypoint far away: %d meters.", (int)dist_to_1wp);
+				warning_issued = true;
+			}
+
+			return true;
+
+		} else {
+			/* item is too far from home */
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(),
+					     "First waypoint too far away: %d meters, %d max.",
+					     (int)dist_to_1wp, (int)max_distance);
+			warning_issued = true;
+			return false;
+		}
+	}
+
+	/* no waypoints found in mission, then we will not fly far away */
+	return true;
+}
+
+bool
+MissionFeasibilityChecker::checkDistancesBetweenWaypoints(dm_item_t dm_current, size_t nMissionItems,
+		float max_distance, bool &warning_issued)
+{
+	if (max_distance <= 0.0f) {
+		/* param not set, check is ok */
+		return true;
+	}
+
+	double last_lat = NAN;
+	double last_lon = NAN;
+
+	/* Go through all waypoints */
+	for (size_t i = 0; i < nMissionItems; i++) {
+
+		struct mission_item_s mission_item {};
+
+		if (!(dm_read(dm_current, i, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s))) {
+			/* error reading, mission is invalid */
+			mavlink_log_info(_navigator->get_mavlink_log_pub(), "Error reading offboard mission.");
+			return false;
+		}
+
+		/* check only items with valid lat/lon */
+		if (!MissionBlock::item_contains_position(mission_item)) {
+			continue;
+		}
+
+		/* Compare it to last waypoint if already available. */
+		if (PX4_ISFINITE(last_lat) && PX4_ISFINITE(last_lon)) {
+
+			/* check distance from current position to item */
+			float dist_between_waypoints = get_distance_to_next_waypoint(
+							       mission_item.lat, mission_item.lon,
+							       last_lat, last_lon);
+
+			if (dist_between_waypoints < max_distance) {
+
+				if (dist_between_waypoints > ((max_distance * 3) / 2)) {
+					/* allow at 2/3 distance, but warn */
+					mavlink_log_critical(_navigator->get_mavlink_log_pub(),
+							     "Distance between waypoints very far: %d meters.",
+							     (int)dist_between_waypoints);
+					warning_issued = true;
 				}
 
 			} else {
-				/* error reading, mission is invalid */
-				mavlink_log_info(_navigator->get_mavlink_log_pub(), "Error reading offboard mission.");
+				/* item is too far from home */
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(),
+						     "Distance between waypoints too far: %d meters, %d max.",
+						     (int)dist_between_waypoints, (int)max_distance);
+				warning_issued = true;
 				return false;
 			}
 		}
 
-		/* no waypoints found in mission, then we will not fly far away */
-		return true;
-
-	} else {
-		return true;
+		last_lat = mission_item.lat;
+		last_lon = mission_item.lon;
 	}
+
+	/* We ran through all waypoints and have not found any distances between waypoints that are too far. */
+	return true;
 }

src/modules/navigator/mission_feasibility_checker.h

@@ -62,8 +62,9 @@ private:
 
 	bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems, bool condition_landed);
 
-	bool check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon,
-			    float dist_first_wp, bool &warning_issued);
+	bool checkDistanceToFirstWaypoint(dm_item_t dm_current, size_t nMissionItems, float max_distance, bool &warning_issued);
+	bool checkDistancesBetweenWaypoints(dm_item_t dm_current, size_t nMissionItems, float max_distance,
+					    bool &warning_issued);
 
 	/* Checks specific to fixedwing airframes */
 	bool checkFixedwing(dm_item_t dm_current, size_t nMissionItems, fw_pos_ctrl_status_s *fw_pos_ctrl_status,
@@ -88,7 +89,9 @@ public:
 	/*
 	 * Returns true if mission is feasible and false otherwise
 	 */
-	bool checkMissionFeasible(const mission_s &mission, float max_waypoint_distance, bool land_start_req);
+	bool checkMissionFeasible(const mission_s &mission,
+				  float max_distance_to_1st_waypoint, float max_distance_between_waypoints,
+				  bool land_start_req);
 
 };
 

src/modules/navigator/mission_params.c

@@ -99,6 +99,22 @@ PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1);
  */
 PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 900);
 
+/**
+ * Maximal horizontal distance between waypoint
+ *
+ * Failsafe check to prevent running missions which are way too big.
+ * Set a value of zero or less to disable. The mission will not be started if any distance between
+ * two subsequent waypoints is greater than MIS_DIST_WPS.
+ *
+ * @unit m
+ * @min 0
+ * @max 10000
+ * @decimal 1
+ * @increment 100
+ * @group Mission
+ */
+PARAM_DEFINE_FLOAT(MIS_DIST_WPS, 900);
+
 /**
  * Altitude setpoint mode
  *