mirror of https://github.com/ArduPilot/ardupilot
Copter: use common GCS_MAVLink handle_mission methods
This commit is contained in:
parent
1ad743c4e1
commit
6ef2b55602
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue