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

src/modules/navigator/mission.cpp

@@ -79,7 +79,6 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	_mission_type(MISSION_TYPE_NONE),
 	_inited(false),
 	_home_inited(false),
-	_dist_1wp_ok(false),
 	_missionFeasiblityChecker(),
 	_min_current_sp_distance_xy(FLT_MAX),
 	_mission_item_previous_alt(NAN),
@@ -119,7 +118,9 @@ Mission::on_inactive()
 
 			_navigator->get_mission_result()->valid = _missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing,
 					dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
-					_navigator->get_home_position()->alt, _navigator->home_position_valid());
+					_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);
 
 			_navigator->increment_mission_instance_count();
 			_navigator->set_mission_result_updated();
@@ -255,7 +256,9 @@ Mission::update_offboard_mission()
 
 		failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing,
 				dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
-				_navigator->get_home_position()->alt, _navigator->home_position_valid());
+				_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);
 
 		_navigator->get_mission_result()->valid = !failed;
 		_navigator->increment_mission_instance_count();
@@ -308,73 +311,6 @@ Mission::get_absolute_altitude_for_item(struct mission_item_s &mission_item)
 	}
 }
 
-bool
-Mission::check_dist_1wp()
-{
-	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 (_param_dist_1wp.get() > 0.0f) {
-		if (_navigator->get_vstatus()->condition_home_position_valid) {
-			struct mission_item_s mission_item;
-
-			/* find first waypoint (with lat/lon) item in datamanager */
-			for (unsigned i = 0; i < _offboard_mission.count; i++) {
-				if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id), 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,
-								_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
-
-						if (dist_to_1wp < _param_dist_1wp.get()) {
-							_dist_1wp_ok = true;
-							if (dist_to_1wp > ((_param_dist_1wp.get() * 3) / 2)) {
-								/* allow at 2/3 distance, but warn */
-								mavlink_log_critical(_navigator->get_mavlink_fd(), "Warning: First waypoint very far: %d m", (int)dist_to_1wp);
-							}
-							return true;
-
-						} else {
-							/* item is too far from home */
-							mavlink_log_critical(_navigator->get_mavlink_fd(), "Waypoint too far: %d m,[MIS_DIST_1WP=%d]", (int)dist_to_1wp, (int)_param_dist_1wp.get());
-							return false;
-						}
-					}
-
-				} else {
-					/* error reading, mission is invalid */
-					mavlink_log_info(_navigator->get_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 {
-			mavlink_log_info(_navigator->get_mavlink_fd(), "no home position");
-			return false;
-		}
-
-	} else {
-		return true;
-	}
-}
-
 void
 Mission::set_mission_items()
 {
@@ -394,10 +330,8 @@ Mission::set_mission_items()
 		_mission_item_previous_alt = get_absolute_altitude_for_item(_mission_item);
 	}
 
-	/* get home distance state */
-	bool home_dist_ok = check_dist_1wp();
 	/* the home dist check provides user feedback, so we initialize it to this */
-	bool user_feedback_done = !home_dist_ok;
+	bool user_feedback_done = false;
 
 	/* try setting onboard mission item */
 	if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) {
@@ -409,7 +343,7 @@ Mission::set_mission_items()
 		_mission_type = MISSION_TYPE_ONBOARD;
 
 	/* try setting offboard mission item */
-	} else if (home_dist_ok && read_mission_item(false, true, &_mission_item)) {
+	} else if (read_mission_item(false, true, &_mission_item)) {
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_OFFBOARD) {
 			mavlink_log_info(_navigator->get_mavlink_fd(), "offboard mission now running");

src/modules/navigator/mission.h

@@ -187,7 +187,6 @@ private:
 
 	bool _inited;
 	bool _home_inited;
-	bool _dist_1wp_ok;
 
 	MissionFeasibilityChecker _missionFeasiblityChecker; /**< class that checks if a mission is feasible */
 

src/modules/navigator/mission_feasibility_checker.cpp

@@ -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);

src/modules/navigator/mission_feasibility_checker.h

@@ -57,12 +57,14 @@ private:
 	struct navigation_capabilities_s _nav_caps;
 
 	bool _initDone;
+	bool _dist_1wp_ok;
 	void init();
 
 	/* Checks for all airframes */
 	bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence);
-	bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool throw_error = false);
+	bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool &warning_issued, bool throw_error = false);
 	bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems);
+	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);
 
 	/* Checks specific to fixedwing airframes */
 	bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid);
@@ -79,8 +81,9 @@ public:
 	/*
 	 * Returns true if mission is feasible and false otherwise
 	 */
-	bool checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence,
-				  float home_alt, bool home_valid);
+	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);
 
 };