Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: use common GCS_MAVLink handle_mission methods

This commit is contained in:
Randy Mackay 2014-03-18 22:34:10 +09:00
parent 1ad743c4e1
commit 6ef2b55602
1 changed files with 5 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

98
ArduCopter/GCS_Mavlink.pde
View File

@ -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;
}