diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index ed774976c3..8952b6db83 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -1175,6 +1175,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // XXX read a WP from EEPROM and send it to the GCS + case MAVLINK_MSG_ID_MISSION_REQUEST_INT: case MAVLINK_MSG_ID_MISSION_REQUEST: { handle_mission_request(rover.mission, msg); @@ -1232,19 +1233,26 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // GCS has sent us a mission item, store to EEPROM case MAVLINK_MSG_ID_MISSION_ITEM: - { - if (handle_mission_item(msg, rover.mission)) { - rover.DataFlash.Log_Write_EntireMission(rover.mission); - } - break; + { + if (handle_mission_item(msg, rover.mission)) { + rover.DataFlash.Log_Write_EntireMission(rover.mission); } + break; + } + case MAVLINK_MSG_ID_MISSION_ITEM_INT: + { + if (handle_mission_item(msg, rover.mission)) { + rover.DataFlash.Log_Write_EntireMission(rover.mission); + } + break; + } case MAVLINK_MSG_ID_PARAM_SET: - { - handle_param_set(msg, &rover.DataFlash); - break; - } + { + handle_param_set(msg, &rover.DataFlash); + break; + } case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: { diff --git a/APMrover2/capabilities.cpp b/APMrover2/capabilities.cpp index 7122eb84f5..f678368d29 100644 --- a/APMrover2/capabilities.cpp +++ b/APMrover2/capabilities.cpp @@ -6,4 +6,5 @@ void Rover::init_capabilities(void) { hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT); hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT); + hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_MISSION_INT); }