fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 17:23:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Navigator: MissionFeasibilityCheck: Rework 1st waypoint check (#23568)

Browse Source 
* 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 <silvan@auterion.com>

* 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 <silvan@auterion.com>

---------

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
Co-authored-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
KonradRudin
2024-08-21 09:08:36 +02:00
committed by GitHub
co-authored by Silvan Fuhrer
parent f252e20eae
commit 3478765c31
4 changed files with 19 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp
+17 -23
View File
@@ -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;
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 (_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));
}
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<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_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
* <profile name="dev">
* This check can be configured via <param>MIS_DIST_1WP</param> parameter.
* </profile>
*/
events::send<uint32_t>(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;
}
src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp
-4
View File
@@ -78,7 +78,6 @@ public:
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 @@ private:
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 @@ private:
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};
src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp
+2 -2
View File
@@ -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();
src/modules/navigator/mission_feasibility_checker.cpp
-1
View File
@@ -41,7 +41,6 @@
*/
#include "mission_feasibility_checker.h"
#include "MissionFeasibility/FeasibilityChecker.hpp"
#include "mission_block.h"
#include "navigator.h"