dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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 <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-03-08 10:51:05 +01:00
parent d8ed35c422
commit 465ddddbfd
2 changed files with 35 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp
View File

@ -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<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, (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;
}

13
src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp
View File

@ -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)