From ef363edfcd810e3b153609f0a723f53db0cd3885 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Sat, 19 Jul 2014 17:37:13 +0200
Subject: [PATCH] mavlink: TX/RX rate counters added

---
 src/modules/mavlink/mavlink_main.cpp     | 28 ++++++++++++++++++++++++
 src/modules/mavlink/mavlink_main.h       | 23 +++++++++++++++++++
 src/modules/mavlink/mavlink_receiver.cpp |  3 +++
 3 files changed, 54 insertions(+)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index cb8c298c04..0c6f8c42f8 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -197,6 +197,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
 			if (buf_free < desired) {
 				/* we don't want to send anything just in half, so return */
 				instance->count_txerr();
+				instance->count_txerrbytes(desired);
 				return;
 			}
 		}
@@ -205,9 +206,11 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
 
 		if (ret != desired) {
 			instance->count_txerr();
+			instance->count_txerrbytes(desired);
 
 		} else {
 			last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel];
+			instance->count_txbytes(desired);
 		}
 	}
 }
@@ -249,6 +252,13 @@ Mavlink::Mavlink() :
 	   _subscribe_to_stream(nullptr),
 	   _subscribe_to_stream_rate(0.0f),
 	   _flow_control_enabled(true),
+	   _bytes_tx(0),
+	   _bytes_txerr(0),
+	   _bytes_rx(0),
+	   _bytes_timestamp(0),
+	   _rate_tx(0.0f),
+	   _rate_txerr(0.0f),
+	   _rate_rx(0.0f),
 	   _rstatus {},
 	   _message_buffer {},
 	   _message_buffer_mutex {},
@@ -1570,6 +1580,20 @@ Mavlink::task_main(int argc, char *argv[])
 			}
 		}
 
+		/* update TX/RX rates*/
+		if (t > _bytes_timestamp + 1000000) {
+			if (_bytes_timestamp != 0) {
+				float dt = (t - _bytes_timestamp) / 1000.0f;
+				_rate_tx = _bytes_tx / dt;
+				_rate_txerr = _bytes_txerr / dt;
+				_rate_rx = _bytes_rx / dt;
+				_bytes_tx = 0;
+				_bytes_txerr = 0;
+				_bytes_rx = 0;
+			}
+			_bytes_timestamp = t;
+		}
+
 		perf_end(_loop_perf);
 	}
 
@@ -1732,6 +1756,10 @@ Mavlink::display_status()
 	} else {
 		printf("\tno telem status.\n");
 	}
+	printf("\trates:\n");
+	printf("\ttx: %.3f kB/s\n", (double)_rate_tx);
+	printf("\ttxerr: %.3f kB/s\n", (double)_rate_txerr);
+	printf("\trx: %.3f kB/s\n", (double)_rate_rx);
 }
 
 int
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index d51120462a..70d13acb09 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -232,6 +232,21 @@ public:
 	 */
 	void			count_txerr();
 
+	/**
+	 * Count transmitted bytes
+	 */
+	void			count_txbytes(unsigned n) { _bytes_tx += n; };
+
+	/**
+	 * Count bytes not transmitted because of errors
+	 */
+	void			count_txerrbytes(unsigned n) { _bytes_txerr += n; };
+
+	/**
+	 * Count received bytes
+	 */
+	void			count_rxbytes(unsigned n) { _bytes_rx += n; };
+
 	/**
 	 * Get the receive status of this MAVLink link
 	 */
@@ -293,6 +308,14 @@ private:
 
 	bool			_flow_control_enabled;
 
+	unsigned		_bytes_tx;
+	unsigned		_bytes_txerr;
+	unsigned		_bytes_rx;
+	uint64_t		_bytes_timestamp;
+	float		_rate_tx;
+	float		_rate_txerr;
+	float		_rate_rx;
+
 	struct telemetry_status_s	_rstatus;			///< receive status
 
 	struct mavlink_message_buffer {
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 454d730d83..54c412ce7c 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -958,6 +958,9 @@ MavlinkReceiver::receive_thread(void *arg)
 					_mavlink->handle_message(&msg);
 				}
 			}
+
+			/* count received bytes */
+			_mavlink->count_rxbytes(nread);
 		}
 	}