diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 44a5dfce40..0a280dcd3c 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1662,8 +1662,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } - // 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(plane.mission, msg); @@ -1735,6 +1735,15 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } break; } + + // GCS has sent us a mission item, store to EEPROM + case MAVLINK_MSG_ID_MISSION_ITEM_INT: + { + if (handle_mission_item(msg, plane.mission)) { + plane.DataFlash.Log_Write_EntireMission(plane.mission); + } + break; + } #if GEOFENCE_ENABLED == ENABLED // receive a fence point from GCS and store in EEPROM diff --git a/ArduPlane/capabilities.cpp b/ArduPlane/capabilities.cpp index d7e552944d..8a67220323 100644 --- a/ArduPlane/capabilities.cpp +++ b/ArduPlane/capabilities.cpp @@ -7,4 +7,5 @@ void Plane::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_COMMAND_INT); + hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_MISSION_INT); }