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

Copter: integrate replace_cmd

This commit is contained in:
Randy Mackay 2014-03-01 12:57:11 +09:00
parent 4c0ae63169
commit aade894014
1 changed files with 18 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
ArduCopter/GCS_Mavlink.pde
View File

@ -1375,14 +1375,28 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
} }
// check if this is the requested waypoint // check if this is the requested waypoint
if ((packet.seq != waypoint_request_i) || (packet.seq != mission.num_commands())) { if (packet.seq != waypoint_request_i) {
result = MAV_MISSION_INVALID_SEQUENCE; result = MAV_MISSION_INVALID_SEQUENCE;
goto mission_item_receive_failed; goto mission_item_receive_failed;
} }
// add new command at end of command list // if command index is within the existing list, replace the command
if (mission.add_cmd(cmd)) { if (packet.seq < mission.num_commands()) {
result = MAV_MISSION_ACCEPTED; 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{ }else{
result = MAV_MISSION_ERROR; result = MAV_MISSION_ERROR;
goto mission_item_receive_failed; goto mission_item_receive_failed;