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

Copter: added support for partial mission load 

this allows individual waypoints to be updated in missions
This commit is contained in:
Andrew Tridgell 2013-05-29 16:25:05 +10:00
parent 24a84a4777
commit 903d5636bb
2 changed files with 47 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/GCS.h
View File

@ -167,6 +167,7 @@ private:
// waypoints
uint16_t waypoint_request_i; // request index
uint16_t waypoint_request_last; // last request index
uint16_t waypoint_dest_sysid; // where to send requests
uint16_t waypoint_dest_compid; // "
bool waypoint_sending; // currently in send process

59
ArduCopter/GCS_Mavlink.pde
View File

@ -895,7 +895,7 @@ GCS_MAVLINK::update(void)
uint32_t tnow = millis();
if (waypoint_receiving &&
waypoint_request_i <= (unsigned)g.command_total &&
waypoint_request_i <= waypoint_request_last &&
tnow > waypoint_timelast_request + 500 + (stream_slowdown*20)) {
waypoint_timelast_request = tnow;
send_message(MSG_NEXT_WAYPOINT);
@ -1573,10 +1573,34 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
waypoint_receiving = true;
waypoint_sending = false;
waypoint_request_i = 0;
waypoint_request_last= g.command_total;
waypoint_timelast_request = 0;
break;
}
case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST:
{
// decode
mavlink_mission_write_partial_list_t packet;
mavlink_msg_mission_write_partial_list_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
// start waypoint receiving
if (packet.start_index > g.command_total ||
packet.end_index > g.command_total ||
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;
break;
}
#ifdef MAVLINK_MSG_ID_SET_MAG_OFFSETS
case MAVLINK_MSG_ID_SET_MAG_OFFSETS:
{
@ -1592,6 +1616,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAVLINK_MSG_ID_MISSION_ITEM: //39
{
// decode
uint8_t result = MAV_MISSION_ACCEPTED;
mavlink_mission_item_t packet;
mavlink_msg_mission_item_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
@ -1733,16 +1758,17 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
} else {
// Check if receiving waypoints (mission upload expected)
if (!waypoint_receiving) break;
//cliSerial->printf("req: %d, seq: %d, total: %d\n", waypoint_request_i,packet.seq, g.command_total.get());
if (!waypoint_receiving) {
result = MAV_MISSION_ERROR;
goto mission_failed;
}
// check if this is the requested waypoint
if (packet.seq != waypoint_request_i)
break;
if (packet.seq != waypoint_request_i) {
result = MAV_MISSION_INVALID_SEQUENCE;
goto mission_failed;
}
if(packet.seq != 0)
set_cmd_with_index(tell_command, packet.seq);
// update waypoint receiving state machine
@ -1750,14 +1776,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
waypoint_timelast_request = 0;
waypoint_request_i++;
if (waypoint_request_i == (uint16_t)g.command_total) {
uint8_t type = 0; // ok (0), error(1)
if (waypoint_request_i > waypoint_request_last) {
mavlink_msg_mission_ack_send(
chan,
msg->sysid,
msg->compid,
type);
result);
send_text_P(SEVERITY_LOW,PSTR("flight plan received"));
waypoint_receiving = false;
@ -1766,6 +1790,15 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
}
}
break;
mission_failed:
// we are rejecting the mission/waypoint
mavlink_msg_mission_ack_send(
chan,
msg->sysid,
msg->compid,
result);
break;
}
case MAVLINK_MSG_ID_PARAM_SET: // 23
@ -2101,7 +2134,7 @@ void
GCS_MAVLINK::queued_waypoint_send()
{
if (waypoint_receiving &&
waypoint_request_i < (unsigned)g.command_total) {
waypoint_request_i <= waypoint_request_last) {
mavlink_msg_mission_request_send(
chan,
waypoint_dest_sysid,