From 25cc64947ab291769642ab954e4806333b95eae2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 6 Jun 2017 13:39:07 +0200 Subject: [PATCH] mavlink_mission: add check for fence & safe point on regular mission upload --- src/modules/mavlink/mavlink_mission.cpp | 25 ++++++++++++++++++------- 1 file changed, 18 insertions(+), 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index f8fcb7f6cc..344c18b21e 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -1048,15 +1048,26 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) switch (_mission_type) { case MAV_MISSION_TYPE_MISSION: { - dm_item_t dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_transfer_dataman_id); + // check that we don't get a wrong item (hardening against wrong client implementations, the list here + // does not need to be complete) + if (mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION || + mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION || + mission_item.nav_cmd == MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION || + mission_item.nav_cmd == MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION || + mission_item.nav_cmd == MAV_CMD_NAV_RALLY_POINT) { + check_failed = true; + + } else { + dm_item_t dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_transfer_dataman_id); - write_failed = dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, - sizeof(struct mission_item_s)) != sizeof(struct mission_item_s); + write_failed = dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, + sizeof(struct mission_item_s)) != sizeof(struct mission_item_s); - if (!write_failed) { - /* waypoint marked as current */ - if (wp.current) { - _transfer_current_seq = wp.seq; + if (!write_failed) { + /* waypoint marked as current */ + if (wp.current) { + _transfer_current_seq = wp.seq; + } } } }