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
const uint16_t waypoint_receive_timeout = 8000; // milliseconds
// number of extra 20ms intervals to add to slow things down for the radio
uint8_t stream_slowdown;
// number of extra ms to add to slow things down for the radio
uint16_t stream_slowdown_ms;
// perf counters
AP_HAL::Util::perf_counter_t _perf_packet;
@ -696,7 +696,7 @@ private:
uint32_t max_retry_deferred_body_us;
uint8_t max_retry_deferred_body_type;
} try_send_message_stats;
uint8_t max_slowdown;
uint16_t max_slowdown_ms;
#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
// control the stream rate, giving us adaptive software
// 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
stream_slowdown += 3;
} else if (packet.txbuf < 50 && stream_slowdown < 100) {
stream_slowdown_ms += 60;
} else if (packet.txbuf < 50 && stream_slowdown_ms < 2000) {
// we are a bit low on space, slow down slightly
stream_slowdown += 1;
} else if (packet.txbuf > 95 && stream_slowdown > 10) {
stream_slowdown_ms += 20;
} else if (packet.txbuf > 95 && stream_slowdown_ms > 200) {
// the buffer has plenty of space, speed up a lot
stream_slowdown -= 2;
} else if (packet.txbuf > 90 && stream_slowdown != 0) {
stream_slowdown_ms -= 40;
} else if (packet.txbuf > 90 && stream_slowdown_ms != 0) {
// the buffer has enough space, speed up a bit
stream_slowdown--;
stream_slowdown_ms -= 20;
}
#if GCS_DEBUG_SEND_MESSAGE_TIMINGS
if (stream_slowdown > max_slowdown) {
max_slowdown = stream_slowdown;
if (stream_slowdown_ms > max_slowdown_ms) {
max_slowdown_ms = stream_slowdown_ms;
}
#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;
// add in adjustments for streamrate-slowdown (e.g. based
// on feedback from telemetry radio on its state).
// slowdown is basically 50ths of a second
interval_ms += stream_slowdown * 20;
interval_ms += stream_slowdown_ms;
// slow most messages down if we're transfering parameters or
// 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 = 0;
}
if (max_slowdown) {
if (max_slowdown_ms) {
gcs().send_text(MAV_SEVERITY_INFO,
"GCS.chan(%u): max slowdown=%u",
chan,
max_slowdown);
max_slowdown = 0;
max_slowdown_ms);
max_slowdown_ms = 0;
}
if (try_send_message_stats.behind) {
gcs().send_text(MAV_SEVERITY_INFO,
@ -1577,7 +1574,7 @@ GCS_MAVLINK::update_receive(uint32_t max_time_us)
#endif
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
if (tnow - waypoint_timelast_receive > wp_recv_time+waypoint_receive_timeout) {