|
|
@ -1048,6 +1048,16 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) |
|
|
|
switch (_mission_type) { |
|
|
|
switch (_mission_type) { |
|
|
|
|
|
|
|
|
|
|
|
case MAV_MISSION_TYPE_MISSION: { |
|
|
|
case MAV_MISSION_TYPE_MISSION: { |
|
|
|
|
|
|
|
// 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); |
|
|
|
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, |
|
|
|
write_failed = dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, |
|
|
@ -1060,6 +1070,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
} |
|
|
|
break; |
|
|
|
break; |
|
|
|
|
|
|
|
|
|
|
|
case MAV_MISSION_TYPE_FENCE: { // Write a geofence point
|
|
|
|
case MAV_MISSION_TYPE_FENCE: { // Write a geofence point
|
|
|
|