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

GCS_MAVLink: measure stream slowdown in ms 

This costs us a couple of extra bytes of RAM and gains us not doing a
whole heap of multiplies
This commit is contained in:
Peter Barker 2019-02-16 11:56:22 +11:00 committed by Peter Barker
parent ff90ee3eab
commit 4a0887b807
2 changed files with 18 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/GCS_MAVLink/GCS.h
View File

@ -487,8 +487,8 @@ private:
uint32_t waypoint_timelast_request; // milliseconds uint32_t waypoint_timelast_request; // milliseconds
const uint16_t waypoint_receive_timeout = 8000; // milliseconds const uint16_t waypoint_receive_timeout = 8000; // milliseconds
// number of extra 20ms intervals to add to slow things down for the radio // number of extra ms to add to slow things down for the radio
uint8_t stream_slowdown; uint16_t stream_slowdown_ms;
// perf counters // perf counters
AP_HAL::Util::perf_counter_t _perf_packet; AP_HAL::Util::perf_counter_t _perf_packet;
@ -696,7 +696,7 @@ private:
uint32_t max_retry_deferred_body_us; uint32_t max_retry_deferred_body_us;
uint8_t max_retry_deferred_body_type; uint8_t max_retry_deferred_body_type;
} try_send_message_stats; } try_send_message_stats;
uint8_t max_slowdown; uint16_t max_slowdown_ms;
#endif #endif
}; };

33
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -706,23 +706,23 @@ void GCS_MAVLINK::handle_radio_status(mavlink_message_t *msg, AP_Logger &datafla
// use the state of the transmit buffer in the radio to // use the state of the transmit buffer in the radio to
// control the stream rate, giving us adaptive software // control the stream rate, giving us adaptive software
// flow control // flow control
if (packet.txbuf < 20 && stream_slowdown < 100) { if (packet.txbuf < 20 && stream_slowdown_ms < 2000) {
// we are very low on space - slow down a lot // we are very low on space - slow down a lot
stream_slowdown += 3; stream_slowdown_ms += 60;
} else if (packet.txbuf < 50 && stream_slowdown < 100) { } else if (packet.txbuf < 50 && stream_slowdown_ms < 2000) {
// we are a bit low on space, slow down slightly // we are a bit low on space, slow down slightly
stream_slowdown += 1; stream_slowdown_ms += 20;
} else if (packet.txbuf > 95 && stream_slowdown > 10) { } else if (packet.txbuf > 95 && stream_slowdown_ms > 200) {
// the buffer has plenty of space, speed up a lot // the buffer has plenty of space, speed up a lot
stream_slowdown -= 2; stream_slowdown_ms -= 40;
} else if (packet.txbuf > 90 && stream_slowdown != 0) { } else if (packet.txbuf > 90 && stream_slowdown_ms != 0) {
// the buffer has enough space, speed up a bit // the buffer has enough space, speed up a bit
stream_slowdown--; stream_slowdown_ms -= 20;
} }
#if GCS_DEBUG_SEND_MESSAGE_TIMINGS #if GCS_DEBUG_SEND_MESSAGE_TIMINGS
if (stream_slowdown > max_slowdown) { if (stream_slowdown_ms > max_slowdown_ms) {
max_slowdown = stream_slowdown; max_slowdown_ms = stream_slowdown_ms;
} }
#endif #endif
@ -991,10 +991,7 @@ uint16_t GCS_MAVLINK::get_reschedule_interval_ms(const deferred_message_bucket_t
{ {
uint32_t interval_ms = deferred.interval_ms; uint32_t interval_ms = deferred.interval_ms;
// add in adjustments for streamrate-slowdown (e.g. based interval_ms += stream_slowdown_ms;
// on feedback from telemetry radio on its state).
// slowdown is basically 50ths of a second
interval_ms += stream_slowdown * 20;
// slow most messages down if we're transfering parameters or // slow most messages down if we're transfering parameters or
// waypoints: // waypoints:
@ -1530,12 +1527,12 @@ GCS_MAVLINK::update_receive(uint32_t max_time_us)
try_send_message_stats.no_space_for_message); try_send_message_stats.no_space_for_message);
try_send_message_stats.no_space_for_message = 0; try_send_message_stats.no_space_for_message = 0;
} }
if (max_slowdown) { if (max_slowdown_ms) {
gcs().send_text(MAV_SEVERITY_INFO, gcs().send_text(MAV_SEVERITY_INFO,
"GCS.chan(%u): max slowdown=%u", "GCS.chan(%u): max slowdown=%u",
chan, chan,
max_slowdown); max_slowdown_ms);
max_slowdown = 0; max_slowdown_ms = 0;
} }
if (try_send_message_stats.behind) { if (try_send_message_stats.behind) {
gcs().send_text(MAV_SEVERITY_INFO, gcs().send_text(MAV_SEVERITY_INFO,
@ -1577,7 +1574,7 @@ GCS_MAVLINK::update_receive(uint32_t max_time_us)
#endif #endif
if (waypoint_receiving) { if (waypoint_receiving) {
const uint32_t wp_recv_time = 1000U + (stream_slowdown*20); const uint32_t wp_recv_time = 1000U + stream_slowdown_ms;
// stop waypoint receiving if timeout // stop waypoint receiving if timeout
if (tnow - waypoint_timelast_receive > wp_recv_time+waypoint_receive_timeout) { if (tnow - waypoint_timelast_receive > wp_recv_time+waypoint_receive_timeout) {