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

do not update mission if it has unsupported mission items

Browse Source
This commit is contained in:
Roman Bapst
2015-05-21 20:08:49 +02:00
committed by Lorenz Meier
parent 9179fcefc9
commit afa8266255
3 changed files with 47 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_feasibility_checker.cpp
+40 -2
View File
@@ -65,6 +65,7 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab
bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
{
bool failed = false;
/* Init if not done yet */
init();
@@ -74,11 +75,16 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
}
// check if all mission item commands are supported
failed |= !checkMissionItemValidity(dm_current, nMissionItems);
if (isRotarywing)
return checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt);
failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt);
else
return checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt);
failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt);
return !failed;
}
bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
@@ -163,6 +169,38 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
return true;
}
bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems) {
// do not allow mission if we find unsupported item
for (size_t i = 0; i < nMissionItems; i++) {
struct mission_item_s missionitem;
const ssize_t len = sizeof(struct mission_item_s);
if (dm_read(dm_current, i, &missionitem, len) != len) {
// not supposed to happen unless the datamanager can't access the SD card, etc.
mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Cannot access mission from SD card");
return false;
}
// check if we find unsupported item and reject mission if so
if (missionitem.nav_cmd != NAV_CMD_IDLE &&
missionitem.nav_cmd != NAV_CMD_WAYPOINT &&
missionitem.nav_cmd != NAV_CMD_LOITER_UNLIMITED &&
missionitem.nav_cmd != NAV_CMD_LOITER_TURN_COUNT &&
missionitem.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT &&
missionitem.nav_cmd != NAV_CMD_LAND &&
missionitem.nav_cmd != NAV_CMD_TAKEOFF &&
missionitem.nav_cmd != NAV_CMD_ROI &&
missionitem.nav_cmd != NAV_CMD_PATHPLANNING &&
missionitem.nav_cmd != NAV_CMD_DO_JUMP) {
mavlink_log_critical(_mavlink_fd, "Rejecting mission item %i: unsupported action.", (int)(i+1));
return false;
}
}
mavlink_log_critical(_mavlink_fd, "Mission is valid!");
return true;
}
bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems)
{
/* Go through all mission items and search for a landing waypoint