Navigator: MissionFeasibilityCheck: Rework 1st waypoint check (#23568)

* FeasibilityChecker: only warn when first waypoint is too far, but still accept mission as valid * feasiblityChecker: make distance to first waypoint check against home position instead of current position, so it is more constant during a flight * Apply suggestions from code review Co-authored-by: Silvan Fuhrer <[email protected]> * feasibilityCheckerTest: update tests to not fail for first waypoint check * feasibilityChecker: make comment for 1stwaypointcheck event * Feasibility check unit test: fix comment Signed-off-by: Silvan Fuhrer <[email protected]> --------- Signed-off-by: Silvan Fuhrer <[email protected]> Co-authored-by: Silvan Fuhrer <[email protected]>
PX4 · Aug 21, 2024 · 3478765 · 3478765
1 parent f252e20
commit 3478765
Show file tree

Hide file tree

Showing 4 changed files with 19 additions and 30 deletions.
diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp b/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;
@@ -118,12 +117,6 @@ void FeasibilityChecker::updateData()
 		_is_landed = land_detected.landed;
 	}
 
-	if (_vehicle_global_position_sub.updated()) {
-		vehicle_global_position_s vehicle_global_position = {};
-		_vehicle_global_position_sub.copy(&vehicle_global_position);
-		_current_position_lat_lon = matrix::Vector2d(vehicle_global_position.lat, vehicle_global_position.lon);
-	}
-
 	if (_rtl_status_sub.updated()) {
 		rtl_status_s rtl_status = {};
 		_rtl_status_sub.copy(&rtl_status);
@@ -199,8 +192,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,31 +624,32 @@ 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 &&
+	    (_home_lat_lon.isAllFinite()) &&
 	    MissionBlock::item_contains_position(mission_item)) {
 
 		_first_waypoint_found = true;
 
-		float dist_to_1wp_from_current_pos = 1e6f;
-
-		if (_current_position_lat_lon.isAllFinite()) {
-			dist_to_1wp_from_current_pos = get_distance_to_next_waypoint(
-							       mission_item.lat, mission_item.lon,
-							       _current_position_lat_lon(0), _current_position_lat_lon(1));
-		}
+		const float dist_to_1wp_from_home_pos = get_distance_to_next_waypoint(
+				mission_item.lat, mission_item.lon,
+				_home_lat_lon(0), _home_lat_lon(1));
 
-		if (dist_to_1wp_from_current_pos < _param_mis_dist_1wp) {
+		if (dist_to_1wp_from_home_pos < _param_mis_dist_1wp) {
 
 			return true;
 
 		} else {
 			/* item is too far from current position */
 			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);
+					     "First waypoint far away from home: %dm. Correct mission loaded?\t",
+					     (int)dist_to_1wp_from_home_pos);
+			/* EVENT
+			* @description
+			* <profile name="dev">
+			* This check can be configured via <param>MIS_DIST_1WP</param> parameter.
+			* </profile>
+			*/
+			events::send<uint32_t>(events::ID("navigator_mis_first_wp_far"), {events::Log::Warning, events::LogInternal::Info},
+					       "First waypoint far away from Home: {1m} Correct mission loaded?", (uint32_t)dist_to_1wp_from_home_pos);
 
 			return false;
 		}

diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp
@@ -78,7 +78,6 @@ 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 ||
@@ -97,7 +96,6 @@ class FeasibilityChecker : public ModuleParams
 	uORB::Subscription _home_pos_sub{ORB_ID(home_position)};
 	uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
 	uORB::Subscription _land_detector_sub{ORB_ID(vehicle_land_detected)};
-	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
 	uORB::Subscription _rtl_status_sub{ORB_ID(rtl_status)};
 
 	// parameters
@@ -110,14 +108,12 @@ class FeasibilityChecker : public ModuleParams
 	float _home_alt_msl{NAN};
 	bool _has_vtol_approach{false};
 	matrix::Vector2d _home_lat_lon = matrix::Vector2d((double)NAN, (double)NAN);
-	matrix::Vector2d _current_position_lat_lon = matrix::Vector2d((double)NAN, (double)NAN);
 	VehicleType _vehicle_type{VehicleType::RotaryWing};
 
 	// internal flags to keep track of which checks failed
 	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};

diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp
@@ -170,9 +170,9 @@ TEST_F(FeasibilityCheckerTest, check_dist_first_waypoint)
 	mission_item.lat = lat_new;
 	mission_item.lon = lon_new;
 
-	// THEN: fail
+	// THEN: pass
 	checker.processNextItem(mission_item, 0, 1);
-	ASSERT_EQ(checker.someCheckFailed(), true);
+	ASSERT_EQ(checker.someCheckFailed(), false);
 
 	// BUT WHEN: valid current position fist WP 499m away from current
 	checker.reset();

diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/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"