From 64449883928182ca5abb516b91cf9818bd74bb18 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lorenz@px4.io>
Date: Sun, 25 Dec 2016 18:28:28 +0100
Subject: [PATCH] MAVLink app: Send messages on average at a more correct rate
 and send on first call

This patch fixes two issues:
  * It sends the message on the first call, making sure that the first update gets sent out.
  * It improves the rate scheduling. In an experiment with 0.5, 50 and 250 Hz all rates were correct within 0.3% of the intended rate.
---
 src/modules/mavlink/mavlink_main.h     |  4 ++-
 src/modules/mavlink/mavlink_stream.cpp | 43 +++++++++++++++++++-------
 src/modules/mavlink/mavlink_stream.h   |  4 +--
 3 files changed, 36 insertions(+), 15 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index d9e51bc7b4..05261a8010 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -428,9 +428,11 @@ public:
 
 	void			set_logging_enabled(bool logging) { _logging_enabled = logging; }
 
-	int			get_data_rate() { return _datarate; }
+	int				get_data_rate() { return _datarate; }
 	void			set_data_rate(int rate) { if (rate > 0) { _datarate = rate; } }
 
+	uint64_t		get_main_loop_delay() { return _main_loop_delay; }
+
 	/** get the Mavlink shell. Create a new one if there isn't one. It is *always* created via MavlinkReceiver thread.
 	 *  Returns nullptr if shell cannot be created */
 	MavlinkShell		*get_shell();
diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp
index be50602b7d..87fcc3878b 100644
--- a/src/modules/mavlink/mavlink_stream.cpp
+++ b/src/modules/mavlink/mavlink_stream.cpp
@@ -47,7 +47,7 @@ MavlinkStream::MavlinkStream(Mavlink *mavlink) :
 	next(nullptr),
 	_mavlink(mavlink),
 	_interval(1000000),
-	_last_sent(0)
+	_last_sent(0 /* 0 means unlimited - updates on every iteration */)
 {
 }
 
@@ -70,25 +70,44 @@ MavlinkStream::set_interval(const unsigned int interval)
 int
 MavlinkStream::update(const hrt_abstime t)
 {
-	uint64_t dt = t - _last_sent;
-	unsigned int interval = _interval;
+	// If the message has never been sent before we want
+	// to send it immediately and can return right away
+	if (_last_sent == 0) {
+		// this will give different messages on the same run a different
+		// initial timestamp which will help spacing them out
+		// on the link scheduling
+		_last_sent = hrt_absolute_time();
+#ifndef __PX4_QURT
+		send(t);
+#endif
+		return 0;
+	}
+
+	int64_t dt = t - _last_sent;
+	int interval = _interval;
 
 	if (!const_rate()) {
 		interval /= _mavlink->get_rate_mult();
 	}
 
-	if (dt > 0 && dt >= interval) {
-		/* interval expired, send message */
+	// send the message if it is due or
+	// if it will overrun the next scheduled send interval
+	// by 40% of the interval time. This helps to avoid
+	// sending a scheduled message on average slower than
+	// scheduled. Doing this at 50% would risk sending
+	// the message too often as the loop runtime of the app
+	// needs to be accounted for as well.
+	// This method is not theoretically optimal but a suitable
+	// stopgap as it hits its deadlines well (0.5 Hz, 50 Hz and 250 Hz)
+
+	if (dt > (interval - (_mavlink->get_main_loop_delay() / 10) * 4)) {
+		// interval expired, send message
 #ifndef __PX4_QURT
 		send(t);
 #endif
-
-		if (const_rate()) {
-			_last_sent = (t / _interval) * _interval;
-
-		} else {
-			_last_sent = t;
-		}
+		// do not use the actual time but increment at a fixed rate
+		// so that processing delays do not distort the average rate
+		_last_sent = _last_sent + interval;
 
 		return 0;
 	}
diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h
index 67b8e359b1..028823f16f 100644
--- a/src/modules/mavlink/mavlink_stream.h
+++ b/src/modules/mavlink/mavlink_stream.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014-2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -98,7 +98,7 @@ public:
 
 protected:
 	Mavlink     *_mavlink;
-	unsigned int _interval;
+	unsigned int _interval;		//<<< if set to zero = unlimited rate
 
 #ifndef __PX4_QURT
 	virtual void send(const hrt_abstime t) = 0;