From ba17a137e1b5a2d173afef8394b16638523cc88d Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 27 Mar 2023 10:45:39 +0200 Subject: [PATCH] Navigator: MissionFeasibiltyCheck first WP: check only to current vehicle position Signed-off-by: Silvan Fuhrer --- .../MissionFeasibility/FeasibilityChecker.cpp | 23 +++------- .../MissionFeasibility/FeasibilityChecker.hpp | 3 +- .../FeasibilityCheckerTest.cpp | 46 ++----------------- src/modules/navigator/mission_params.c | 6 +-- 4 files changed, 13 insertions(+), 65 deletions(-) diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp index d20aab3609..0ed522bd62 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp +++ b/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()) && - !_first_waypoint_found && + (_current_position_lat_lon.isAllFinite()) && !_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 (_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) { + if (dist_to_1wp_from_current_pos < _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(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; } diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp index a6dff6f89a..48124daef0 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp +++ b/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. - * Use Home position instead of vehicle position if vehicle position is invalid. + * @brief Check distance to first waypoint from current vehicle position (if available). * * @param mission_item The current mission item * @return False if the check failed. diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp index c0ca5dd39b..fa58826905 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp +++ b/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) diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 98ddd0e628..9b411355bc 100644 --- a/src/modules/navigator/mission_params.c +++ b/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