Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-08 08:53:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: added partial update of waypoints

Browse Source
This commit is contained in:
Andrew Tridgell 2012-11-28 09:35:09 +11:00
parent 691d19dd98
commit ff41e057d9
2 changed files with 28 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
APMrover2/GCS.h
View File

@ -165,6 +165,7 @@ private:
// waypoints // waypoints
uint16_t waypoint_request_i; // request index 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_sysid; // where to send requests
uint16_t waypoint_dest_compid; // " uint16_t waypoint_dest_compid; // "
bool waypoint_sending; // currently in send process bool waypoint_sending; // currently in send process

30
APMrover2/GCS_Mavlink.pde
View File

@ -761,7 +761,7 @@ GCS_MAVLINK::update(void)
uint32_t tnow = millis(); uint32_t tnow = millis();
if (waypoint_receiving && if (waypoint_receiving &&
waypoint_request_i <= (unsigned)g.command_total && waypoint_request_i <= waypoint_request_last &&
tnow > waypoint_timelast_request + 500 + (stream_slowdown*20)) { tnow > waypoint_timelast_request + 500 + (stream_slowdown*20)) {
waypoint_timelast_request = tnow; waypoint_timelast_request = tnow;
send_message(MSG_NEXT_WAYPOINT); send_message(MSG_NEXT_WAYPOINT);
@ -1259,6 +1259,30 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
waypoint_receiving = true; waypoint_receiving = true;
waypoint_sending = false; waypoint_sending = false;
waypoint_request_i = 0; waypoint_request_i = 0;
waypoint_request_last= g.command_total;
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(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; break;
} }
@ -1426,7 +1450,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
waypoint_timelast_request = 0; waypoint_timelast_request = 0;
waypoint_request_i++; waypoint_request_i++;
if (waypoint_request_i > (uint16_t)g.command_total){ if (waypoint_request_i > waypoint_request_last) {
mavlink_msg_mission_ack_send( mavlink_msg_mission_ack_send(
chan, chan,
msg->sysid, msg->sysid,
@ -1777,7 +1801,7 @@ void
GCS_MAVLINK::queued_waypoint_send() GCS_MAVLINK::queued_waypoint_send()
{ {
if (waypoint_receiving && if (waypoint_receiving &&
waypoint_request_i <= (unsigned)g.command_total) { waypoint_request_i <= waypoint_request_last) {
mavlink_msg_mission_request_send( mavlink_msg_mission_request_send(
chan, chan,
waypoint_dest_sysid, waypoint_dest_sysid,