diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp index a1eb0e51ad..349bca8ce1 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, total_count - 1); + doVtolChecks(mission_item, current_index); } else if (_vehicle_type == VehicleType::FixedWing) { - doFixedWingChecks(mission_item, current_index, total_count - 1); + doFixedWingChecks(mission_item, current_index); } 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, const int last_index) +void FeasibilityChecker::doVtolChecks(mission_item_s &mission_item, const int current_index) { if (!_land_pattern_validity_failed) { - _land_pattern_validity_failed = !checkLandPatternValidity(mission_item, current_index, last_index); + _land_pattern_validity_failed = !checkLandPatternValidity(mission_item, current_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) { if (!_land_pattern_validity_failed) { - _land_pattern_validity_failed = !checkLandPatternValidity(mission_item, current_index, last_index); + _land_pattern_validity_failed = !checkLandPatternValidity(mission_item, current_index); } if (!_fixed_wing_land_approach_failed) { @@ -488,8 +488,7 @@ bool FeasibilityChecker::checkFixedWindLandApproach(mission_item_s &mission_item return true; } -bool FeasibilityChecker::checkLandPatternValidity(mission_item_s &mission_item, const int current_index, - const int last_index) +bool FeasibilityChecker::checkLandPatternValidity(mission_item_s &mission_item, const int current_index) { // if DO_LAND_START found then require valid landing AFTER @@ -529,7 +528,7 @@ bool FeasibilityChecker::checkLandPatternValidity(mission_item_s &mission_item, } } - if (current_index == last_index && land_start_found && (_do_land_start_index > _landing_approach_index)) { + if (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 facaa25765..e1f0d01059 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, const int last_index); + bool checkLandPatternValidity(mission_item_s &mission_item, const int current_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, const int last_index); - void doFixedWingChecks(mission_item_s &mission_item, const int current_index, const int last_index); + void doVtolChecks(mission_item_s &mission_item, const int current_index); + void doFixedWingChecks(mission_item_s &mission_item, const int current_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 ba8ea34643..115887add9 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp @@ -411,23 +411,6 @@ 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)