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

GCS: fixed timeout loading waypoints (ArduCopter) 

prevents link saturation
This commit is contained in:
Andrew Tridgell 2011-10-31 21:25:58 +11:00
parent ac4c604a6b
commit 3bf4cb52b1
2 changed files with 14 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/GCS.h
View File

@ -152,6 +152,7 @@ private:
uint16_t waypoint_count; uint16_t waypoint_count;
uint32_t waypoint_timelast_send; // milliseconds uint32_t waypoint_timelast_send; // milliseconds
uint32_t waypoint_timelast_receive; // milliseconds uint32_t waypoint_timelast_receive; // milliseconds
uint32_t waypoint_timelast_request; // milliseconds
uint16_t waypoint_send_timeout; // milliseconds uint16_t waypoint_send_timeout; // milliseconds
uint16_t waypoint_receive_timeout; // milliseconds uint16_t waypoint_receive_timeout; // milliseconds

16
ArduCopter/GCS_Mavlink.pde
View File

@ -572,18 +572,26 @@ GCS_MAVLINK::update(void)
send_message(MSG_NEXT_PARAM); send_message(MSG_NEXT_PARAM);
} }
if (!waypoint_receiving && !waypoint_sending) {
return;
}
uint32_t tnow = millis();
if (waypoint_receiving && if (waypoint_receiving &&
waypoint_request_i <= (unsigned)g.waypoint_total) { waypoint_request_i <= (unsigned)g.waypoint_total &&
tnow > waypoint_timelast_request + 500) {
waypoint_timelast_request = tnow;
send_message(MSG_NEXT_WAYPOINT); send_message(MSG_NEXT_WAYPOINT);
} }
// stop waypoint sending if timeout // stop waypoint sending if timeout
if (waypoint_sending && (millis() - waypoint_timelast_send) > waypoint_send_timeout){ if (waypoint_sending && (tnow - waypoint_timelast_send) > waypoint_send_timeout){
waypoint_sending = false; waypoint_sending = false;
} }
// stop waypoint receiving if timeout // stop waypoint receiving if timeout
if (waypoint_receiving && (millis() - waypoint_timelast_receive) > waypoint_receive_timeout){ if (waypoint_receiving && (tnow - waypoint_timelast_receive) > waypoint_receive_timeout){
waypoint_receiving = false; waypoint_receiving = false;
} }
} }
@ -1151,6 +1159,7 @@ 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_timelast_request = 0;
break; break;
} }
@ -1296,6 +1305,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// update waypoint receiving state machine // update waypoint receiving state machine
waypoint_timelast_receive = millis(); waypoint_timelast_receive = millis();
waypoint_timelast_request = 0;
waypoint_request_i++; waypoint_request_i++;
if (waypoint_request_i > (uint16_t)g.waypoint_total){ if (waypoint_request_i > (uint16_t)g.waypoint_total){