navigator: Include distance to first waypoint in mission check, provide warning feedback

2026-10-26 02:53:34 +08:00 · 2015-06-15 17:28:05 +02:00
parent b11e133318
commit 41f535ae26
4 changed files with 107 additions and 98 deletions
@@ -57,23 +57,40 @@
 #endif
 static const int ERROR = -1;

-MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capabilities_sub(-1), _initDone(false)
+MissionFeasibilityChecker::MissionFeasibilityChecker() :
+	_mavlink_fd(-1),
+	_capabilities_sub(-1),
+	_initDone(false),
+	_dist_1wp_ok(false)
 {
 	_nav_caps = {0};
 }


-bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid)
+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)
 {
 	bool failed = false;
+	bool warned = false;
 	/* Init if not done yet */
 	init();

 	_mavlink_fd = mavlink_fd;

+	// first check if we have a valid position
+	if (!home_valid /* can later use global / local pos for finer granularity */) {
+		failed = true;
+		warned = true;
+		mavlink_log_info(_mavlink_fd, "Not yet ready for mission, no position lock.");
+	} else {
+		failed |= !check_dist_1wp(dm_current, nMissionItems, curr_lat, curr_lon, max_waypoint_distance, warning_issued);
+	}
+
 	// check if all mission item commands are supported
 	failed |= !checkMissionItemValidity(dm_current, nMissionItems);
-
+	failed |= !checkGeofence(dm_current, nMissionItems, geofence);
+	failed |= !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned);

 	if (isRotarywing) {
 		failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid);
@@ -90,28 +107,20 @@ bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRota

 bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid)
 {
-
-	/* Perform checks and issue feedback to the user for all checks */
-	bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence);
-	bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid);
-
-	/* Mission is only marked as feasible if all checks return true */
-	return (resGeofence && resHomeAltitude);
+	/* no custom rotary wing checks yet */
+	return true;
 }

 bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid)
 {
 	/* Update fixed wing navigation capabilites */
 	updateNavigationCapabilities();
-//	warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement);

 	/* Perform checks and issue feedback to the user for all checks */
 	bool resLanding = checkFixedWingLanding(dm_current, nMissionItems);
-	bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence);
-	bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid);

 	/* Mission is only marked as feasible if all checks return true */
-	return (resLanding && resGeofence && resHomeAltitude);
+	return resLanding;
 }

 bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
@@ -137,7 +146,8 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss
 	return true;
 }

-bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool throw_error)
+bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems,
+	float home_alt, bool home_valid, bool &warning_issued, bool throw_error)
 {
 	/* 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++) {
@@ -145,17 +155,15 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
 		const ssize_t len = sizeof(struct mission_item_s);

 		if (dm_read(dm_current, i, &missionitem, len) != len) {
+			warning_issued = true;
 			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
-			if (throw_error) {
-				return false;
-			} else	{
-				return true;
-			}
+			return false;
 		}

 		/* always reject relative alt without home set */
 		if (missionitem.altitude_is_relative && !home_valid) {
 			mavlink_log_critical(_mavlink_fd, "Rejecting Mission: No home pos, WP %d uses rel alt", i);
+			warning_issued = true;
 			return false;
 		}

@@ -163,6 +171,9 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
 		float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude;

 		if (home_alt > wp_alt) {
+
+			warning_issued = true;
+
 			if (throw_error) {
 				mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Waypoint %d below home", i);
 				return false;
@@ -275,6 +286,68 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
 	return true;
 }

+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)
+{
+	if (_dist_1wp_ok) {
+		/* always return true after at least one successful check */
+		return true;
+	}
+
+	/* check if first waypoint is not too far from home */
+	if (dist_first_wp > 0.0f) {
+		struct mission_item_s mission_item;
+
+		/* find first waypoint (with lat/lon) item in datamanager */
+		for (unsigned i = 0; i < nMissionItems; i++) {
+			if (dm_read(dm_current, i,
+					&mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) {
+
+				/* check only items with valid lat/lon */
+				if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
+						mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+						mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
+						mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
+						mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
+						mission_item.nav_cmd == NAV_CMD_PATHPLANNING) {
+
+					/* 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) {
+						_dist_1wp_ok = true;
+						if (dist_to_1wp > ((dist_first_wp * 3) / 2)) {
+							/* allow at 2/3 distance, but warn */
+							mavlink_log_critical(_mavlink_fd, "Warning: First waypoint very far: %d m", (int)dist_to_1wp);
+							warning_issued = true;
+						}
+						return true;
+
+					} else {
+						/* item is too far from home */
+						mavlink_log_critical(_mavlink_fd, "Waypoint too far: %d m,[MIS_DIST_1WP=%d]", (int)dist_to_1wp, (int)dist_first_wp);
+						warning_issued = true;
+						return false;
+					}
+				}
+
+			} else {
+				/* error reading, mission is invalid */
+				mavlink_log_info(_mavlink_fd, "error reading offboard mission");
+				return false;
+			}
+		}
+
+		/* no waypoints found in mission, then we will not fly far away */
+		_dist_1wp_ok = true;
+		return true;
+
+	} else {
+		return true;
+	}
+}
+
 void MissionFeasibilityChecker::updateNavigationCapabilities()
 {
 	(void)orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);