From 9eb27c257362dd57b1b57d5ef2c1a61f27fb2039 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 31 Oct 2011 21:25:35 +1100 Subject: [PATCH] GCS: fixed timeout loading waypoints while loading waypoints we could saturate the link --- ArduPlane/GCS.h | 1 + ArduPlane/GCS_Mavlink.pde | 11 ++++++++++- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/ArduPlane/GCS.h b/ArduPlane/GCS.h index 5f999f6ca2..7172eeaf52 100644 --- a/ArduPlane/GCS.h +++ b/ArduPlane/GCS.h @@ -152,6 +152,7 @@ private: uint16_t waypoint_count; uint32_t waypoint_timelast_send; // milliseconds uint32_t waypoint_timelast_receive; // milliseconds + uint32_t waypoint_timelast_request; // milliseconds uint16_t waypoint_send_timeout; // milliseconds uint16_t waypoint_receive_timeout; // milliseconds diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 173400c571..8b9e4abc63 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -777,8 +777,15 @@ GCS_MAVLINK::update(void) send_message(MSG_NEXT_PARAM); } + if (!waypoint_receiving && !waypoint_sending) { + return; + } + + uint32_t tnow = millis(); + if (waypoint_receiving && - waypoint_request_i <= (unsigned)g.command_total) { + waypoint_request_i <= (unsigned)g.command_total && + tnow > waypoint_timelast_request + 500) { send_message(MSG_NEXT_WAYPOINT); } @@ -1413,6 +1420,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) g.command_total.set_and_save(packet.count - 1); waypoint_timelast_receive = millis(); + waypoint_timelast_request = 0; waypoint_receiving = true; waypoint_sending = false; waypoint_request_i = 0; @@ -1589,6 +1597,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // update waypoint receiving state machine waypoint_timelast_receive = millis(); + waypoint_timelast_request = 0; waypoint_request_i++; if (waypoint_request_i > (uint16_t)g.command_total){