fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-02 04:21:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Mission feasibility checks: make adding new check less error prone (#24241)

Browse Source 
* make adding new feasibility checks less prone to errors

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* Update src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp

Co-authored-by: Silvan Fuhrer <silvan@auterion.com>

---------

Signed-off-by: RomanBapst <bapstroman@gmail.com>
Co-authored-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Roman Bapst
2025-01-22 16:56:31 +01:00
committed by GitHub
co-authored by Silvan Fuhrer
parent 57fdda597b
commit 045c8d9831
2 changed files with 30 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp
+17 -22
View File
@@ -46,12 +46,7 @@ FeasibilityChecker::FeasibilityChecker() :
void FeasibilityChecker::reset() void FeasibilityChecker::reset()
{ {
_mission_validity_failed = false; _checks_failed.value = 0;
_takeoff_failed = false;
_land_pattern_validity_failed = false;
_distance_between_waypoints_failed = false;
_fixed_wing_land_approach_failed = false;
_takeoff_land_available_failed = false;
_found_item_with_position = false; _found_item_with_position = false;
_has_vtol_takeoff = false; _has_vtol_takeoff = false;
@@ -155,11 +150,11 @@ bool FeasibilityChecker::processNextItem(mission_item_s &mission_item, const int
updateData(); updateData();
} }
if (!_mission_validity_failed) { if (!_checks_failed.flags.mission_validity_failed) {
_mission_validity_failed = !checkMissionItemValidity(mission_item, current_index); _checks_failed.flags.mission_validity_failed = !checkMissionItemValidity(mission_item, current_index);
} }
if (_mission_validity_failed) { if (_checks_failed.flags.mission_validity_failed) {
// if a mission item is not valid then abort the other checks // if a mission item is not valid then abort the other checks
return false; return false;
} }
@@ -177,7 +172,7 @@ bool FeasibilityChecker::processNextItem(mission_item_s &mission_item, const int
} }
if (current_index == total_count - 1) { if (current_index == total_count - 1) {
_takeoff_land_available_failed = !checkTakeoffLandAvailable(); _checks_failed.flags.takeoff_land_available_failed = !checkTakeoffLandAvailable();
} }
_mission_item_previous = mission_item; _mission_item_previous = mission_item;
@@ -188,39 +183,39 @@ bool FeasibilityChecker::processNextItem(mission_item_s &mission_item, const int
void FeasibilityChecker::doCommonChecks(mission_item_s &mission_item, const int current_index) void FeasibilityChecker::doCommonChecks(mission_item_s &mission_item, const int current_index)
{ {
if (!_distance_between_waypoints_failed) { if (!_checks_failed.flags.distance_between_waypoints_failed) {
_distance_between_waypoints_failed = !checkDistancesBetweenWaypoints(mission_item); _checks_failed.flags.distance_between_waypoints_failed = !checkDistancesBetweenWaypoints(mission_item);
} }
if (!_first_waypoint_found) { if (!_first_waypoint_found) {
checkHorizontalDistanceToFirstWaypoint(mission_item); checkHorizontalDistanceToFirstWaypoint(mission_item);
} }
if (!_takeoff_failed) { if (!_checks_failed.flags.takeoff_failed) {
_takeoff_failed = !checkTakeoff(mission_item); _checks_failed.flags.takeoff_failed = !checkTakeoff(mission_item);
} }
if (!_items_fit_to_vehicle_type_failed) { if (!_checks_failed.flags.items_fit_to_vehicle_type_failed) {
_items_fit_to_vehicle_type_failed = !checkItemsFitToVehicleType(mission_item); _checks_failed.flags.items_fit_to_vehicle_type_failed = !checkItemsFitToVehicleType(mission_item);
} }
} }
void FeasibilityChecker::doVtolChecks(mission_item_s &mission_item, const int current_index, const int last_index) void FeasibilityChecker::doVtolChecks(mission_item_s &mission_item, const int current_index, const int last_index)
{ {
if (!_land_pattern_validity_failed) { if (!_checks_failed.flags.land_pattern_validity_failed) {
_land_pattern_validity_failed = !checkLandPatternValidity(mission_item, current_index, last_index); _checks_failed.flags.land_pattern_validity_failed = !checkLandPatternValidity(mission_item, current_index, last_index);
} }
} }
void FeasibilityChecker::doFixedWingChecks(mission_item_s &mission_item, const int current_index, const int last_index) void FeasibilityChecker::doFixedWingChecks(mission_item_s &mission_item, const int current_index, const int last_index)
{ {
if (!_land_pattern_validity_failed) { if (!_checks_failed.flags.land_pattern_validity_failed) {
_land_pattern_validity_failed = !checkLandPatternValidity(mission_item, current_index, last_index); _checks_failed.flags.land_pattern_validity_failed = !checkLandPatternValidity(mission_item, current_index, last_index);
} }
if (!_fixed_wing_land_approach_failed) { if (!_checks_failed.flags.fixed_wing_land_approach_failed) {
_fixed_wing_land_approach_failed = !checkFixedWindLandApproach(mission_item, current_index); _checks_failed.flags.fixed_wing_land_approach_failed = !checkFixedWindLandApproach(mission_item, current_index);
} }
} }
src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp
+13 -14
View File
@@ -77,12 +77,7 @@ public:
*/ */
bool someCheckFailed() bool someCheckFailed()
{ {
return _takeoff_failed || return _checks_failed.value != 0;
_distance_between_waypoints_failed ||
_land_pattern_validity_failed ||
_fixed_wing_land_approach_failed ||
_mission_validity_failed ||
_takeoff_land_available_failed;
} }
/** /**
@@ -110,14 +105,18 @@ private:
matrix::Vector2d _home_lat_lon = matrix::Vector2d((double)NAN, (double)NAN); matrix::Vector2d _home_lat_lon = matrix::Vector2d((double)NAN, (double)NAN);
VehicleType _vehicle_type{VehicleType::RotaryWing}; VehicleType _vehicle_type{VehicleType::RotaryWing};
// internal flags to keep track of which checks failed union checks_failed_u {
bool _mission_validity_failed{false}; struct {
bool _takeoff_failed{false}; bool mission_validity_failed : 1;
bool _land_pattern_validity_failed{false}; bool takeoff_failed : 1;
bool _distance_between_waypoints_failed{false}; bool land_pattern_validity_failed : 1;
bool _fixed_wing_land_approach_failed{false}; bool distance_between_waypoints_failed : 1;
bool _takeoff_land_available_failed{false}; bool fixed_wing_land_approach_failed : 1;
bool _items_fit_to_vehicle_type_failed{false}; bool takeoff_land_available_failed : 1;
bool items_fit_to_vehicle_type_failed : 1;
} flags;
uint16_t value {0};
} _checks_failed{};
// internal checkTakeoff related variables // internal checkTakeoff related variables
bool _found_item_with_position{false}; bool _found_item_with_position{false};