forked from Archive/PX4-Autopilot
Navigator: MissionFeasibiltyCheck first WP: check only to current vehicle position
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
parent
465ddddbfd
commit
ba17a137e1
|
@ -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<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)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;
|
||||
}
|
||||
|
|
|
@ -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.
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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
|
||||
|
|
Loading…
Reference in New Issue