fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 18:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

FeasibilityChecker: Fixed bug and added unit test for it

Browse Source 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst
2023-02-16 11:28:41 +01:00
committed by Roman Bapst
parent 2e50277695
commit 7ef2bff0a2
3 changed files with 29 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp
+9 -8
View File
@@ -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");
src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp
+3 -3
View File
@@ -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);
};
src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp
+17
View File
@@ -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, &param_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)