Navigator: MissionFeasibiltyCheck first WP: check only to current vehicle position

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp

@@ -605,13 +605,11 @@ bool FeasibilityChecker::checkTakeoffLandAvailable()
 bool FeasibilityChecker::checkHorizontalDistanceToFirstWaypoint(mission_item_s &mission_item)
 {
 	if (_param_mis_dist_1wp > FLT_EPSILON &&
-	    (_current_position_lat_lon.isAllFinite() || _home_lat_lon.isAllFinite()) &&
+	    (_current_position_lat_lon.isAllFinite()) && !_first_waypoint_found &&
-	    !_first_waypoint_found &&
 	    MissionBlock::item_contains_position(mission_item)) {
 
 		_first_waypoint_found = true;
 
-		/* check distance from current position or Home (whatever is closer) to item */
 		float dist_to_1wp_from_current_pos = 1e6f;
 
 		if (_current_position_lat_lon.isAllFinite()) {
@@ -620,27 +618,18 @@ bool FeasibilityChecker::checkHorizontalDistanceToFirstWaypoint(mission_item_s &
 							       _current_position_lat_lon(0), _current_position_lat_lon(1));
 		}
 
-		float dist_to_1wp_from_home = 1e6f;
+		if (dist_to_1wp_from_current_pos < _param_mis_dist_1wp) {
-
-		if (_home_lat_lon.isAllFinite()) {
-			dist_to_1wp_from_home = get_distance_to_next_waypoint(
-							mission_item.lat, mission_item.lon,
-							_home_lat_lon(0), _home_lat_lon(1));
-		}
-
-		const float min_dist = math::min(dist_to_1wp_from_current_pos, dist_to_1wp_from_home);
-
-		if (min_dist < _param_mis_dist_1wp) {
 
 			return true;
 
 		} else {
-			/* item is too far from home */
+			/* item is too far from current position */
 			mavlink_log_critical(_mavlink_log_pub,
 					     "First waypoint too far away: %dm, %d max\t",
-					     (int)min_dist, (int)_param_mis_dist_1wp);
+					     (int)dist_to_1wp_from_current_pos, (int)_param_mis_dist_1wp);
 			events::send<uint32_t, uint32_t>(events::ID("navigator_mis_first_wp_too_far"), {events::Log::Error, events::LogInternal::Info},
-							 "First waypoint too far away: {1m} (maximum: {2m})", (uint32_t)min_dist, (uint32_t)_param_mis_dist_1wp);
+							 "First waypoint too far away: {1m} (maximum: {2m})", (uint32_t)dist_to_1wp_from_current_pos,
+							 (uint32_t)_param_mis_dist_1wp);
 
 			return false;
 		}

src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp

@@ -174,8 +174,7 @@ private:
 	bool checkLandPatternValidity(mission_item_s &mission_item, const int current_index, const int last_index);
 
 	/**
-	 * @brief Check distance to first waypoint from current vehicle position.
+	 * @brief Check distance to first waypoint from current vehicle position (if available).
-	 * 	  Use Home position instead of vehicle position if vehicle position is invalid.
 	 *
 	 * @param mission_item The current mission item
 	 * @return False if the check failed.

src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp

@@ -143,41 +143,15 @@ TEST_F(FeasibilityCheckerTest, check_dist_first_waypoint)
 	mission_item.nav_cmd = NAV_CMD_WAYPOINT;
 	double lat_new, lon_new;
 
-	// GIVEN: no valid Current position, no valid Home
+	// GIVEN: no valid Current position
-
 
 	// THEN: always pass
 	checker.processNextItem(mission_item, 0, 1);
 	ASSERT_EQ(checker.someCheckFailed(), false);
 
-	// GIVEN: no valid current position, but valid Home. First WP 501m away from Home
+	// BUT WHEN: valid current position, first WP 501m away from current pos
 	checker.reset();
 	checker.publishLanded(true);
-	checker.publishHomePosition(0, 0, 0);
-	waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 501, &lat_new, &lon_new);
-	mission_item.lat = lat_new;
-	mission_item.lon = lon_new;
-
-	// THEN: check should fail
-	checker.processNextItem(mission_item, 0, 1);
-	ASSERT_EQ(checker.someCheckFailed(), true);
-
-	// BUT WHEN: no valid current position, but valid Home. First WP 499m away from Home
-	checker.reset();
-	checker.publishLanded(true);
-	checker.publishHomePosition(0, 0, 0);
-	waypoint_from_heading_and_distance(0, 0, 0, 499, &lat_new, &lon_new);
-	mission_item.lat = lat_new;
-	mission_item.lon = lon_new;
-
-	// THEN: pass
-	checker.processNextItem(mission_item, 0, 1);
-	ASSERT_EQ(checker.someCheckFailed(), false);
-
-	// BUT WHEN: valid current position, valid Home, first WP 501m away from current pos
-	checker.reset();
-	checker.publishLanded(true);
-	checker.publishHomePosition(0, 0, 0);
 	checker.publishCurrentPosition(0, 0);
 	waypoint_from_heading_and_distance(0, 0, 0, 501, &lat_new, &lon_new);
 	mission_item.lat = lat_new;
@@ -187,10 +161,9 @@ TEST_F(FeasibilityCheckerTest, check_dist_first_waypoint)
 	checker.processNextItem(mission_item, 0, 1);
 	ASSERT_EQ(checker.someCheckFailed(), true);
 
-	// BUT WHEN: valid current position, valid Home, fist WP 499m away from current but more from Home
+	// BUT WHEN: valid current position fist WP 499m away from current
 	checker.reset();
 	checker.publishLanded(true);
-	checker.publishHomePosition(10, 20, 0); // random position far away
 	checker.publishCurrentPosition(0, 0);
 	waypoint_from_heading_and_distance(0, 0, 0, 499, &lat_new, &lon_new);
 	mission_item.lat = lat_new;
@@ -199,19 +172,6 @@ TEST_F(FeasibilityCheckerTest, check_dist_first_waypoint)
 	// THEN: pass
 	checker.processNextItem(mission_item, 0, 1);
 	ASSERT_EQ(checker.someCheckFailed(), false);
-
-	// BUT WHEN: valid current position (far away), valid Home, first WP 499 away from Home
-	checker.reset();
-	checker.publishLanded(true);
-	checker.publishHomePosition(0, 0, 0); // random position far away
-	checker.publishCurrentPosition(10, 0);
-	waypoint_from_heading_and_distance(0, 0, 0, 499, &lat_new, &lon_new);
-	mission_item.lat = lat_new;
-	mission_item.lon = lon_new;
-
-	// THEN: pass
-	checker.processNextItem(mission_item, 0, 1);
-	ASSERT_EQ(checker.someCheckFailed(), false);
 }
 
 TEST_F(FeasibilityCheckerTest, check_below_home)

src/modules/navigator/mission_params.c

@@ -73,14 +73,14 @@ PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 2.5f);
 PARAM_DEFINE_INT32(MIS_TKO_LAND_REQ, 0);
 
 /**
- * Maximal horizontal distance from home to first waypoint
+ * Maximal horizontal distance from current position to first waypoint
  *
  * Failsafe check to prevent running mission stored from previous flight at a new takeoff location.
  * Set a value of zero or less to disable. The mission will not be started if the current
- * waypoint is more distant than MIS_DIST_1WP from the home position.
+ * waypoint is more distant than MIS_DIST_1WP from the current position.
  *
  * @unit m
- * @min 0
+ * @min -1
  * @max 10000
  * @decimal 1
  * @increment 100