diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index cfe31e155e..8333d6b5ce 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1425,58 +1425,7 @@ mission_item_receive_failed: // read an individual command from EEPROM and send it to the GCS case MAVLINK_MSG_ID_MISSION_REQUEST: // MAV ID: 40 { - // decode - mavlink_mission_request_t packet; - mavlink_msg_mission_request_decode(msg, &packet); - - // exit immediately if this command is not meant for this vehicle - if (mavlink_check_target(packet.target_system, packet.target_component)) { - break; - } - - // retrieve mission from eeprom - if (!mission.read_cmd_from_storage(packet.seq, cmd)) { - result = MAV_MISSION_ERROR; - goto mission_item_send_failed; - } - - // convert mission command to mavlink mission item packet - mavlink_mission_item_t ret_packet; - memset(&ret_packet, 0, sizeof(ret_packet)); - if (!AP_Mission::mission_cmd_to_mavlink(cmd, ret_packet)) { - result = MAV_MISSION_ERROR; - goto mission_item_send_failed; - } - - // set packet's current field to 1 if this is the command being executed - if (cmd.id == (uint16_t)mission.get_current_nav_cmd().index) { - ret_packet.current = 1; - }else{ - ret_packet.current = 0; - } - - // set auto continue to 1 - ret_packet.autocontinue = 1; - - mavlink_msg_mission_item_send(chan,msg->sysid, - msg->compid, - packet.seq, - ret_packet.frame, - cmd.id, - ret_packet.current, - ret_packet.autocontinue, - ret_packet.param1, - ret_packet.param2, - ret_packet.param3, - ret_packet.param4, - ret_packet.x, - ret_packet.y, - ret_packet.z); - break; - -mission_item_send_failed: - // send failure message - mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, result); + handle_mission_request(mission, msg, cmd); break; }