From 7ef2bff0a20b4684fcba36f597067fc4c96718a5 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Thu, 26 Jan 2023 14:05:00 +0300 Subject: [PATCH] FeasibilityChecker: Fixed bug and added unit test for it Signed-off-by: RomanBapst --- .../MissionFeasibility/FeasibilityChecker.cpp | 17 +++++++++-------- .../MissionFeasibility/FeasibilityChecker.hpp | 6 +++--- .../FeasibilityCheckerTest.cpp | 17 +++++++++++++++++ 3 files changed, 29 insertions(+), 11 deletions(-) diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp index 349bca8ce1..a1eb0e51ad 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp @@ -160,10 +160,10 @@ void FeasibilityChecker::processNextItem(mission_item_s &mission_item, const int doCommonChecks(mission_item, current_index); if (_vehicle_type == VehicleType::Vtol) { - doVtolChecks(mission_item, current_index); + doVtolChecks(mission_item, current_index, total_count - 1); } else if (_vehicle_type == VehicleType::FixedWing) { - doFixedWingChecks(mission_item, current_index); + doFixedWingChecks(mission_item, current_index, total_count - 1); } else if (_vehicle_type == VehicleType::RotaryWing) { doMulticopterChecks(mission_item, current_index); @@ -199,18 +199,18 @@ void FeasibilityChecker::doCommonChecks(mission_item_s &mission_item, const int } } -void FeasibilityChecker::doVtolChecks(mission_item_s &mission_item, const int current_index) +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); + _land_pattern_validity_failed = !checkLandPatternValidity(mission_item, current_index, last_index); } } -void FeasibilityChecker::doFixedWingChecks(mission_item_s &mission_item, const int current_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); + _land_pattern_validity_failed = !checkLandPatternValidity(mission_item, current_index, last_index); } if (!_fixed_wing_land_approach_failed) { @@ -488,7 +488,8 @@ bool FeasibilityChecker::checkFixedWindLandApproach(mission_item_s &mission_item return true; } -bool FeasibilityChecker::checkLandPatternValidity(mission_item_s &mission_item, const int current_index) +bool FeasibilityChecker::checkLandPatternValidity(mission_item_s &mission_item, const int current_index, + const int last_index) { // if DO_LAND_START found then require valid landing AFTER @@ -528,7 +529,7 @@ bool FeasibilityChecker::checkLandPatternValidity(mission_item_s &mission_item, } } - if (land_start_found && (_do_land_start_index > _landing_approach_index)) { + if (current_index == last_index && land_start_found && (_do_land_start_index > _landing_approach_index)) { mavlink_log_critical(_mavlink_log_pub, "Mission rejected: invalid land start.\t"); events::send(events::ID("navigator_mis_invalid_land"), {events::Log::Error, events::LogInternal::Info}, "Mission rejected: invalid land start"); diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp index e1f0d01059..facaa25765 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp @@ -128,7 +128,7 @@ private: // methods which are called for each mission item bool checkMissionItemValidity(mission_item_s &mission_item, const int current_index); bool checkTakeoff(mission_item_s &mission_item); - bool checkLandPatternValidity(mission_item_s &mission_item, const int current_index); + bool checkLandPatternValidity(mission_item_s &mission_item, const int current_index, const int last_index); bool checkDistanceToFirstWaypoint(mission_item_s &mission_item); bool checkDistancesBetweenWaypoints(const mission_item_s &mission_item); bool checkIfBelowHomeAltitude(const mission_item_s &mission_item, const int current_index); @@ -138,7 +138,7 @@ private: bool checkTakeoffLandAvailable(); void doCommonChecks(mission_item_s &mission_item, const int current_index); - void doVtolChecks(mission_item_s &mission_item, const int current_index); - void doFixedWingChecks(mission_item_s &mission_item, const int current_index); + void doVtolChecks(mission_item_s &mission_item, const int current_index, const int last_index); + void doFixedWingChecks(mission_item_s &mission_item, const int current_index, const int last_index); void doMulticopterChecks(mission_item_s &mission_item, const int current_index); }; diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp index 115887add9..ba8ea34643 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp @@ -411,6 +411,23 @@ TEST_F(FeasibilityCheckerTest, fixed_wing_landing) mission_item.nav_cmd = NAV_CMD_DO_LAND_START; checker.processNextItem(mission_item, 2, 3); ASSERT_EQ(checker.someCheckFailed(), true); + + + checker.reset(); + param_t param = param_handle(px4::params::MIS_TKO_LAND_REQ); + // not takeoff land requiremntes, check should pass + int param_val = 0; + param_set(param, ¶m_val); + checker.paramsChanged(); + checker.publishHomePosition(0, 0, 100); + checker.publishVehicleType(vehicle_status_s::VEHICLE_TYPE_FIXED_WING); + mission_item.nav_cmd = NAV_CMD_WAYPOINT; + checker.processNextItem(mission_item, 0, 3); + mission_item.nav_cmd = NAV_CMD_DO_LAND_START; + checker.processNextItem(mission_item, 1, 3); + mission_item.nav_cmd = NAV_CMD_LAND; + checker.processNextItem(mission_item, 2, 3); + ASSERT_EQ(checker.someCheckFailed(), false); } TEST_F(FeasibilityCheckerTest, check_takeoff_land_requirements)