diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp index dbaf173efa..c35d2c1bde 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp @@ -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); } } diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp index d0905d3635..1c002baf9a 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp @@ -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};