From 3478765c310d3d34fc139dbf62594c41da6f85a9 Mon Sep 17 00:00:00 2001 From: KonradRudin <98741601+KonradRudin@users.noreply.github.com> Date: Wed, 21 Aug 2024 09:08:36 +0200 Subject: [PATCH] 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 * 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 --------- Signed-off-by: Silvan Fuhrer Co-authored-by: Silvan Fuhrer --- .../MissionFeasibility/FeasibilityChecker.cpp | 40 ++++++++----------- .../MissionFeasibility/FeasibilityChecker.hpp | 4 -- .../FeasibilityCheckerTest.cpp | 4 +- .../navigator/mission_feasibility_checker.cpp | 1 - 4 files changed, 19 insertions(+), 30 deletions(-) diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp index c684f0119d9b..c2b3a8119807 100644 --- 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(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 + * + * This check can be configured via MIS_DIST_1WP parameter. + * + */ + events::send(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 index fd18cbcfb68e..d0905d363545 100644 --- 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 index 0de54f33766c..43662441488a 100644 --- 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 index dc2420d55bf5..8049df1236cf 100644 --- 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"