fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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 18:56:31 +03:00 committed by GitHub
parent 57fdda597b
commit 045c8d9831
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 30 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -46,12 +46,7 @@ FeasibilityChecker::FeasibilityChecker() :
void FeasibilityChecker::reset()
{
_mission_validity_failed = false;
_takeoff_failed = false;
_land_pattern_validity_failed = false;
_distance_between_waypoints_failed = false;
_fixed_wing_land_approach_failed = false;
_takeoff_land_available_failed = false;
_checks_failed.value = 0;
_found_item_with_position = false;
_has_vtol_takeoff = false;
@ -155,11 +150,11 @@ bool FeasibilityChecker::processNextItem(mission_item_s &mission_item, const int
updateData();
}
if (!_mission_validity_failed) {
_mission_validity_failed = !checkMissionItemValidity(mission_item, current_index);
if (!_checks_failed.flags.mission_validity_failed) {
_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
return false;
}
@ -177,7 +172,7 @@ bool FeasibilityChecker::processNextItem(mission_item_s &mission_item, const int
}
if (current_index == total_count - 1) {
_takeoff_land_available_failed = !checkTakeoffLandAvailable();
_checks_failed.flags.takeoff_land_available_failed = !checkTakeoffLandAvailable();
}
_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)
{
if (!_distance_between_waypoints_failed) {
_distance_between_waypoints_failed = !checkDistancesBetweenWaypoints(mission_item);
if (!_checks_failed.flags.distance_between_waypoints_failed) {
_checks_failed.flags.distance_between_waypoints_failed = !checkDistancesBetweenWaypoints(mission_item);
}
if (!_first_waypoint_found) {
checkHorizontalDistanceToFirstWaypoint(mission_item);
}
if (!_takeoff_failed) {
_takeoff_failed = !checkTakeoff(mission_item);
if (!_checks_failed.flags.takeoff_failed) {
_checks_failed.flags.takeoff_failed = !checkTakeoff(mission_item);
}
if (!_items_fit_to_vehicle_type_failed) {
_items_fit_to_vehicle_type_failed = !checkItemsFitToVehicleType(mission_item);
if (!_checks_failed.flags.items_fit_to_vehicle_type_failed) {
_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)
{
if (!_land_pattern_validity_failed) {
_land_pattern_validity_failed = !checkLandPatternValidity(mission_item, current_index, last_index);
if (!_checks_failed.flags.land_pattern_validity_failed) {
_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)
{
if (!_land_pattern_validity_failed) {
_land_pattern_validity_failed = !checkLandPatternValidity(mission_item, current_index, last_index);
if (!_checks_failed.flags.land_pattern_validity_failed) {
_checks_failed.flags.land_pattern_validity_failed = !checkLandPatternValidity(mission_item, current_index, last_index);
}
if (!_fixed_wing_land_approach_failed) {
_fixed_wing_land_approach_failed = !checkFixedWindLandApproach(mission_item, current_index);
if (!_checks_failed.flags.fixed_wing_land_approach_failed) {
_checks_failed.flags.fixed_wing_land_approach_failed = !checkFixedWindLandApproach(mission_item, current_index);
}
}

27
src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp
View File

@ -77,12 +77,7 @@ public:
*/
bool someCheckFailed()
{
return _takeoff_failed ||
_distance_between_waypoints_failed ||
_land_pattern_validity_failed ||
_fixed_wing_land_approach_failed ||
_mission_validity_failed ||
_takeoff_land_available_failed;
return _checks_failed.value != 0;
}
/**
@ -110,14 +105,18 @@ private:
matrix::Vector2d _home_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_between_waypoints_failed{false};
bool _fixed_wing_land_approach_failed{false};
bool _takeoff_land_available_failed{false};
bool _items_fit_to_vehicle_type_failed{false};
union checks_failed_u {
struct {
bool mission_validity_failed : 1;
bool takeoff_failed : 1;
bool land_pattern_validity_failed : 1;
bool distance_between_waypoints_failed : 1;
bool fixed_wing_land_approach_failed : 1;
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
bool _found_item_with_position{false};