Copter: use common GCS_MAVLink handle_mission methods

ArduCopter/GCS_Mavlink.pde

@@ -1286,28 +1286,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
     case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: // MAV ID: 38
     {
-        // decode
-        mavlink_mission_write_partial_list_t packet;
-        mavlink_msg_mission_write_partial_list_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;
-        }
-
-        // start waypoint receiving
-        if (packet.start_index > mission.num_commands() ||
-            packet.end_index > mission.num_commands() ||
-            packet.end_index < packet.start_index) {
-            send_text_P(SEVERITY_LOW,PSTR("flight plan update rejected"));
-            break;
-        }
-
-        waypoint_timelast_receive = millis();
-        waypoint_timelast_request = 0;
-        waypoint_receiving   = true;
-        waypoint_request_i   = packet.start_index;
-        waypoint_request_last= packet.end_index;
+        handle_mission_write_partial_list(mission, msg);
         break;
     }
 
@@ -1431,41 +1410,14 @@ mission_item_receive_failed:
 
     case MAVLINK_MSG_ID_MISSION_SET_CURRENT:    // MAV ID: 41
     {
-        // decode
-        mavlink_mission_set_current_t packet;
-        mavlink_msg_mission_set_current_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;
-        }
-
-        // set current command
-        if (mission.set_current_cmd(packet.seq)) {
-            mavlink_msg_mission_current_send(chan, mission.get_current_nav_cmd().index);
-        }
+        handle_mission_set_current(mission, msg);
         break;
     }
 
     // GCS request the full list of commands, we return just the number and leave the GCS to then request each command individually
     case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:       // MAV ID: 43
     {
-        // decode
-        mavlink_mission_request_list_t packet;
-        mavlink_msg_mission_request_list_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;
-        }
-
-        // reply with number of commands in the mission.  The GCS will then request each command separately
-        mavlink_msg_mission_count_send(chan,msg->sysid, msg->compid, mission.num_commands());
-
-        // set variables to help handle the expected sending of commands to the GCS
-        waypoint_receiving = false;             // record that we are sending commands (i.e. not receiving)
-        waypoint_dest_sysid = msg->sysid;       // record system id of GCS who has requested the commands
-        waypoint_dest_compid = msg->compid;     // record component id of GCS who has requested the commands
+        handle_mission_request_list(mission, msg);
         break;
     }
 
@@ -1473,53 +1425,13 @@ mission_item_receive_failed:
     //  individual commands will then be sent from the GCS using the MAVLINK_MSG_ID_MISSION_ITEM message
     case MAVLINK_MSG_ID_MISSION_COUNT:          // MAV ID: 44
     {
-        // decode
-        mavlink_mission_count_t packet;
-        mavlink_msg_mission_count_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;
-        }
-
-        // start waypoint receiving
-        if (packet.count > mission.num_commands_max()) {
-            // send NAK
-            mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, MAV_MISSION_NO_SPACE);
-            break;
-        }
-
-        // new mission arriving, truncate mission to be the same length
-        mission.truncate(packet.count);
-
-        // set variables to help handle the expected receiving of commands from the GCS
-        waypoint_timelast_receive = millis();   // set time we last received commands to now
-        waypoint_receiving = true;              // record that we expect to receive commands
-        waypoint_request_i = 0;                 // reset the next expected command number to zero
-        waypoint_request_last = packet.count;   // record how many commands we expect to receive
-        waypoint_timelast_request = 0;          // set time we last requested commands to zero
+        handle_mission_count(mission, msg);
         break;
     }
 
     case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:      // MAV ID: 45
     {
-        // decode
-        mavlink_mission_clear_all_t packet;
-        mavlink_msg_mission_clear_all_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;
-        }
-
-        // clear all waypoints
-        if (mission.clear()) {
-            // send ack
-            mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, MAV_RESULT_ACCEPTED);
-        }else{
-            // send nack
-            mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, 1);
-        }
+        handle_mission_clear_all(mission, msg);
         break;
     }