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

navigator: mission feasibility checker: check for a takeoff waypoint on mission on specific conditions

Browse Source
This commit is contained in:
TSC21
2019-02-27 13:25:47 +01:00
committed by Julian Oes
parent 26811978e1
commit 5b151a5df7
3 changed files with 42 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_feasibility_checker.cpp
+30 -4
View File
@@ -95,6 +95,8 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
bool
MissionFeasibilityChecker::checkRotarywing(const mission_s &mission, float home_alt, bool home_alt_valid)
{
bool has_takeoff = false;
for (size_t i = 0; i < mission.count; i++) {
struct mission_item_s missionitem = {};
const ssize_t len = sizeof(struct mission_item_s);
@@ -122,11 +124,22 @@ MissionFeasibilityChecker::checkRotarywing(const mission_s &mission, float home_
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Takeoff altitude too low!");
return false;
}
has_takeoff = true;
}
}
// all checks have passed
return true;
// checks if the mission has at least a takeoff waypoint
// MIS_TAKEOFF_REQ param has to be set and the vehicle has to be landed - one can load a mission
// while the vehicle is flying and it does not require a takeoff waypoint
if (!has_takeoff && _navigator->get_takeoff_required() && _navigator->get_land_detected()->landed) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: No takeoff waypoint found!");
return false;
} else {
// all checks have passed
return true;
}
}
bool
@@ -315,6 +328,8 @@ MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission)
bool
MissionFeasibilityChecker::checkFixedWingTakeoff(const mission_s &mission, float home_alt, bool home_alt_valid)
{
bool has_takeoff = false;
for (size_t i = 0; i < mission.count; i++) {
struct mission_item_s missionitem = {};
const ssize_t len = sizeof(struct mission_item_s);
@@ -344,11 +359,22 @@ MissionFeasibilityChecker::checkFixedWingTakeoff(const mission_s &mission, float
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Takeoff altitude too low!");
return false;
}
has_takeoff = true;
}
}
// all checks have passed
return true;
// checks if the mission has at least one takeoff waypoint;
// MIS_TAKEOFF_REQ param has to be set and the vehicle has to be landed - one can load a mission
// while the vehicle is flying and it does not require a takeoff waypoint
if (!has_takeoff && _navigator->get_takeoff_required() && _navigator->get_land_detected()->landed) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: No takeoff waypoint found!");
return false;
} else {
// all checks have passed
return true;
}
}
bool