FeasibilityChecker: only warn when first waypoint is too far, but sti…

…ll accept mission as valid

src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp

@@ -49,7 +49,6 @@ void FeasibilityChecker::reset()
 	_mission_validity_failed = false;
 	_takeoff_failed = false;
 	_land_pattern_validity_failed = false;
-	_distance_first_waypoint_failed = false;
 	_distance_between_waypoints_failed = false;
 	_fixed_wing_land_approach_failed = false;
 	_takeoff_land_available_failed = false;
@@ -199,8 +198,8 @@ void FeasibilityChecker::doCommonChecks(mission_item_s &mission_item, const int
 		_distance_between_waypoints_failed = !checkDistancesBetweenWaypoints(mission_item);
 	}
 
-	if (!_distance_first_waypoint_failed) {
-		_distance_first_waypoint_failed = !checkHorizontalDistanceToFirstWaypoint(mission_item);
+	if (!_first_waypoint_found) {
+		checkHorizontalDistanceToFirstWaypoint(mission_item);
 	}
 
 	if (!_takeoff_failed) {
@@ -631,7 +630,7 @@ bool FeasibilityChecker::hasMissionBothOrNeitherTakeoffAndLanding()
 bool FeasibilityChecker::checkHorizontalDistanceToFirstWaypoint(mission_item_s &mission_item)
 {
 	if (_param_mis_dist_1wp > FLT_EPSILON &&
-	    (_current_position_lat_lon.isAllFinite()) && !_first_waypoint_found &&
+	    (_current_position_lat_lon.isAllFinite()) &&
 	    MissionBlock::item_contains_position(mission_item)) {
 
 		_first_waypoint_found = true;
@@ -653,9 +652,8 @@ bool FeasibilityChecker::checkHorizontalDistanceToFirstWaypoint(mission_item_s &
 			mavlink_log_critical(_mavlink_log_pub,
 					     "First waypoint too far away: %dm, %d max\t",
 					     (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)dist_to_1wp_from_current_pos,
-							 (uint32_t)_param_mis_dist_1wp);
+			events::send<uint32_t>(events::ID("navigator_mis_first_wp_far"), {events::Log::Warning, events::LogInternal::Info},
+					       "First waypoint far away: {1m} Correct mission loaded?", (uint32_t)dist_to_1wp_from_current_pos);
 
 			return false;
 		}

src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp

@@ -78,19 +78,13 @@ class FeasibilityChecker : public ModuleParams
 	bool someCheckFailed()
 	{
 		return _takeoff_failed ||
-		       _distance_first_waypoint_failed ||
 		       _distance_between_waypoints_failed ||
 		       _land_pattern_validity_failed ||
 		       _fixed_wing_land_approach_failed ||
 		       _mission_validity_failed ||
 		       _takeoff_land_available_failed;
 	}
 
-	/**
-	 * @brief Reset all data
-	*/
-	void reset();
-
 private:
 	orb_advert_t *_mavlink_log_pub{nullptr};
 
@@ -117,7 +111,6 @@ class FeasibilityChecker : public ModuleParams
 	bool _mission_validity_failed{false};
 	bool _takeoff_failed{false};
 	bool _land_pattern_validity_failed{false};
-	bool _distance_first_waypoint_failed{false};
 	bool _distance_between_waypoints_failed{false};
 	bool _fixed_wing_land_approach_failed{false};
 	bool _takeoff_land_available_failed{false};
@@ -142,6 +135,11 @@ class FeasibilityChecker : public ModuleParams
 	double _last_lon{(double)NAN};
 	int _last_cmd{-1};
 
+	/**
+	 * @brief Reset all data
+	*/
+	void reset();
+
 	/**
 	 * @brief Update data from external topics, e.g home position
 	*/

src/modules/navigator/mission_feasibility_checker.cpp

@@ -41,7 +41,6 @@
  */
 
 #include "mission_feasibility_checker.h"
-#include "MissionFeasibility/FeasibilityChecker.hpp"
 
 #include "mission_block.h"
 #include "navigator.h"