From 465ddddbfd1fc64de7052d8d1c035d5d91a7d2a0 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 8 Mar 2023 10:51:05 +0100 Subject: [PATCH] Navigator: take min of vehicle position or Home distance for first WP check To prevent mission rejections when vehicle is currently far away from Mission start, but planned home is close to it. Signed-off-by: Silvan Fuhrer --- .../MissionFeasibility/FeasibilityChecker.cpp | 40 ++++++++++--------- .../FeasibilityCheckerTest.cpp | 13 ++++++ 2 files changed, 35 insertions(+), 18 deletions(-) diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp index 81d8564414..d20aab3609 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp @@ -604,29 +604,33 @@ bool FeasibilityChecker::checkTakeoffLandAvailable() bool FeasibilityChecker::checkHorizontalDistanceToFirstWaypoint(mission_item_s &mission_item) { - matrix::Vector2d position_reference = matrix::Vector2d((double)NAN, (double)NAN); - - // take last known vehicle global_position, or Home position if not available - if (_current_position_lat_lon.isAllFinite()) { - position_reference = _current_position_lat_lon; - - } else if (_home_lat_lon.isAllFinite()) { - position_reference = _home_lat_lon; - } - if (_param_mis_dist_1wp > FLT_EPSILON && - position_reference.isAllFinite() && + (_current_position_lat_lon.isAllFinite() || _home_lat_lon.isAllFinite()) && !_first_waypoint_found && MissionBlock::item_contains_position(mission_item)) { _first_waypoint_found = true; - /* check distance from current position to item */ - const float dist_to_1wp = get_distance_to_next_waypoint( - mission_item.lat, mission_item.lon, - position_reference(0), position_reference(1)); + /* check distance from current position or Home (whatever is closer) to item */ + float dist_to_1wp_from_current_pos = 1e6f; - if (dist_to_1wp < _param_mis_dist_1wp) { + 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)); + } + + float dist_to_1wp_from_home = 1e6f; + + 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; @@ -634,9 +638,9 @@ bool FeasibilityChecker::checkHorizontalDistanceToFirstWaypoint(mission_item_s & /* item is too far from home */ mavlink_log_critical(_mavlink_log_pub, "First waypoint too far away: %dm, %d max\t", - (int)dist_to_1wp, (int)_param_mis_dist_1wp); + (int)min_dist, (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, (uint32_t)_param_mis_dist_1wp); + "First waypoint too far away: {1m} (maximum: {2m})", (uint32_t)min_dist, (uint32_t)_param_mis_dist_1wp); return false; } diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp index d750958ed4..c0ca5dd39b 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp @@ -199,6 +199,19 @@ 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)