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

18
ArduCopter/GCS_Mavlink.pde
View File

@ -1375,18 +1375,32 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
}
// 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;
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 (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();