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

mission feasibility checker: move checks for VTOL landing into separate function

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2019-12-17 10:16:52 +01:00 committed by Lorenz Meier
parent 3a37d0b560
commit e6b18fe2da
2 changed files with 89 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

85
src/modules/navigator/mission_feasibility_checker.cpp
View File

@ -83,8 +83,7 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
failed = failed || !checkHomePositionAltitude(mission, home_alt, home_alt_valid, warned);
if (_navigator->get_vstatus()->is_vtol) {
failed = failed || !checkRotarywing(mission, home_alt);
failed = failed || !checkFixedwing(mission, home_alt, false);
failed = failed || !checkVTOL(mission, home_alt, false);
} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
failed = failed || !checkRotarywing(mission, home_alt);
@ -117,6 +116,17 @@ MissionFeasibilityChecker::checkFixedwing(const mission_s &mission, float home_a
return (resTakeoff && resLanding);
}
bool
MissionFeasibilityChecker::checkVTOL(const mission_s &mission, float home_alt, bool land_start_req)
{
/* Perform checks and issue feedback to the user for all checks */
bool resTakeoff = checkTakeoff(mission, home_alt);
bool resLanding = checkVTOLLanding(mission, land_start_req);
/* Mission is only marked as feasible if all checks return true */
return (resTakeoff && resLanding);
}
bool
MissionFeasibilityChecker::checkGeofence(const mission_s &mission, float home_alt, bool home_valid)
{
@ -527,6 +537,77 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool
return true;
}
bool
MissionFeasibilityChecker::checkVTOLLanding(const mission_s &mission, bool land_start_req)
{
/* Go through all mission items and search for a landing waypoint
* if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */
bool land_start_found = false;
size_t do_land_start_index = 0;
size_t landing_approach_index = 0;
for (size_t i = 0; i < mission.count; i++) {
struct mission_item_s missionitem;
const ssize_t len = sizeof(missionitem);
if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return false;
}
// if DO_LAND_START found then require valid landing AFTER
if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) {
if (land_start_found) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: more than one land start.");
return false;
} else {
land_start_found = true;
do_land_start_index = i;
}
}
if (missionitem.nav_cmd == NAV_CMD_LAND) {
mission_item_s missionitem_previous {};
if (i > 0) {
landing_approach_index = i - 1;
if (dm_read((dm_item_t)mission.dataman_id, landing_approach_index, &missionitem_previous, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return false;
}
} else {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: starts with land waypoint.");
return false;
}
} else if (missionitem.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
if (land_start_found && do_land_start_index < i) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(),
"Mission rejected: land start item before RTL item not possible.");
return false;
}
}
}
if (land_start_req && !land_start_found) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: landing pattern required.");
return false;
}
if (land_start_found && (do_land_start_index > landing_approach_index)) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: invalid land start.");
return false;
}
/* No landing waypoints or no waypoints */
return true;
}
bool
MissionFeasibilityChecker::checkDistanceToFirstWaypoint(const mission_s &mission, float max_distance)
{

7
src/modules/navigator/mission_feasibility_checker.h
View File

@ -63,14 +63,19 @@ private:
bool checkDistanceToFirstWaypoint(const mission_s &mission, float max_distance);
bool checkDistancesBetweenWaypoints(const mission_s &mission, float max_distance);
bool checkTakeoff(const mission_s &mission, float home_alt);
/* Checks specific to fixedwing airframes */
bool checkFixedwing(const mission_s &mission, float home_alt, bool land_start_req);
bool checkTakeoff(const mission_s &mission, float home_alt);
bool checkFixedWingLanding(const mission_s &mission, bool land_start_req);
/* Checks specific to rotarywing airframes */
bool checkRotarywing(const mission_s &mission, float home_alt);
/* Checks specific to VTOL airframes */
bool checkVTOL(const mission_s &mission, float home_alt, bool land_start_req);
bool checkVTOLLanding(const mission_s &mission, bool land_start_req);
public:
MissionFeasibilityChecker(Navigator *navigator) : _navigator(navigator) {}
~MissionFeasibilityChecker() = default;