Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: use common MISSION_ITEM handling

Browse Source
This commit is contained in:
Andrew Tridgell 2014-03-19 10:06:48 +11:00 committed by Randy Mackay
parent b74fddade3
commit 69252868a8
1 changed files with 21 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

114
ArduCopter/GCS_Mavlink.pde
View File

@ -1071,10 +1071,28 @@ GCS_MAVLINK::send_message(enum ap_message id)
mavlink_send_message(chan,id, packet_drops);
}
void GCS_MAVLINK::handle_guided_request(AP_Mission::Mission_Command &cmd)
{
do_guided(cmd);
}
void GCS_MAVLINK::handle_change_alt_request(AP_Mission::Mission_Command &cmd)
{
// add home alt if needed
if (cmd.content.location.options & LOCATION_MASK_OPTIONS_RELATIVE_ALT) {
cmd.content.location.alt += home.alt;
}
// To-Do: update target altitude for loiter or waypoint controller depending upon nav mode
// similar to how do_change_alt works
wp_nav.set_desired_alt(cmd.content.location.alt);
}
void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
{
uint8_t result = MAV_RESULT_FAILED; // assume failure. Each messages id is responsible for return ACK or NAK if required
struct AP_Mission::Mission_Command cmd = {}; // general purpose mission command
switch (msg->msgid) {
@ -1146,104 +1164,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// GCS has sent us a command from GCS, store to EEPROM
case MAVLINK_MSG_ID_MISSION_ITEM: // MAV ID: 39
{
// decode
mavlink_mission_item_t packet;
mavlink_msg_mission_item_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;
}
// convert mavlink packet to mission command
if (!AP_Mission::mavlink_to_mission_cmd(packet, cmd)) {
result = MAV_MISSION_ERROR;
goto mission_item_receive_failed;
}
if(packet.current == 2) { // current = 2 is a flag to tell us this is a "guided mode" waypoint and not for the mission
// initiate guided mode
if (do_guided(cmd)) {
result = MAV_MISSION_ACCEPTED;
}
// send ACK or NAK
mavlink_msg_mission_ack_send_buf(msg, chan, msg->sysid, msg->compid, result);
} else if(packet.current == 3) { //current = 3 is a flag to tell us this is a alt change only
// add home alt if needed
if (cmd.content.location.options & LOCATION_MASK_OPTIONS_RELATIVE_ALT) {
cmd.content.location.alt += home.alt;
}
// To-Do: update target altitude for loiter or waypoint controller depending upon nav mode
// similar to how do_change_alt works
wp_nav.set_desired_alt(cmd.content.location.alt);
// verify we received the command
mavlink_msg_mission_ack_send_buf(msg, chan, msg->sysid, msg->compid, MAV_MISSION_ACCEPTED);
} else {
// Check if receiving waypoints (mission upload expected)
if (!waypoint_receiving) {
result = MAV_MISSION_ERROR;
goto mission_item_receive_failed;
}
// check if this is the requested waypoint
if (packet.seq != waypoint_request_i) {
result = MAV_MISSION_INVALID_SEQUENCE;
goto mission_item_receive_failed;
}
// if command index is within the existing list, replace the command
if (packet.seq < mission.num_commands()) {
if (mission.replace_cmd(packet.seq,cmd)) {
result = MAV_MISSION_ACCEPTED;
}else{
result = MAV_MISSION_ERROR;
goto mission_item_receive_failed;
}
// if command is at the end of command list, add the command
}else if (packet.seq == mission.num_commands()) {
if (mission.add_cmd(cmd)) {
result = MAV_MISSION_ACCEPTED;
}else{
result = MAV_MISSION_ERROR;
goto mission_item_receive_failed;
}
// if beyond the end of the command list, return an error
}else{
result = MAV_MISSION_ERROR;
goto mission_item_receive_failed;
}
// update waypoint receiving state machine
waypoint_timelast_receive = millis();
waypoint_timelast_request = 0;
waypoint_request_i++;
// send mission ACK after receiving the last command
if (waypoint_request_i >= waypoint_request_last) {
mavlink_msg_mission_ack_send_buf(msg, chan, msg->sysid, msg->compid, result);
send_text_P(SEVERITY_LOW,PSTR("flight plan received"));
waypoint_receiving = false;
}
}
break;
mission_item_receive_failed:
// we are rejecting the mission/waypoint
mavlink_msg_mission_ack_send_buf(msg, chan, msg->sysid, msg->compid, result);
handle_mission_item(msg, mission);
break;
}
// read an individual command from EEPROM and send it to the GCS
case MAVLINK_MSG_ID_MISSION_REQUEST: // MAV ID: 40
{
handle_mission_request(mission, msg, cmd);
handle_mission_request(mission, msg);
break;
}