Mavlink check transmit buffer before send

msg/telemetry_status.msg

@@ -24,8 +24,10 @@ bool ftp
 uint8 streams
 
 float32 data_rate
+float32 current_data_rate
 
-float32 rate_multiplier
+float32 data_rate_multiplier
+float32 error_rate_multiplier
 
 float32 rate_rx
 

src/modules/logger/logger.cpp

@@ -642,7 +642,7 @@ void Logger::add_default_topics()
 	add_topic("system_power", 500);
 	add_topic("tecs_status", 200);
 	add_topic("trajectory_setpoint");
-	add_topic("telemetry_status");
+	add_topic("telemetry_status", 1000);
 	add_topic("vehicle_air_data", 200);
 	add_topic("vehicle_attitude", 30);
 	add_topic("vehicle_attitude_setpoint", 100);

src/modules/mavlink/mavlink_main.cpp

@@ -233,9 +233,7 @@ Mavlink::Mavlink() :
 	_uart_fd(-1),
 	_baudrate(57600),
 	_datarate(1000),
-	_rate_mult(1.0f),
 	_mavlink_param_queue_index(0),
-	mavlink_link_termination_allowed(false),
 	_subscribe_to_stream(nullptr),
 	_subscribe_to_stream_rate(0.0f),
 	_udp_initialised(false),
@@ -245,10 +243,6 @@ Mavlink::Mavlink() :
 	_mavlink_start_time(0),
 	_protocol_version_switch(-1),
 	_protocol_version(0),
-	_bytes_tx(0),
-	_bytes_txerr(0),
-	_bytes_rx(0),
-	_bytes_timestamp(0),
 #if defined(CONFIG_NET) || defined(__PX4_POSIX)
 	_myaddr {},
 	_src_addr{},
@@ -964,6 +958,31 @@ Mavlink::send_packet()
 	return ret;
 }
 
+bool
+Mavlink::should_send_bytes(unsigned packet_len)
+{
+	if (get_protocol() == SERIAL) {
+		/* check if there is space in the buffer, let it overflow else */
+		unsigned buf_free = get_free_tx_buf();
+
+		if (buf_free < packet_len) {
+			/* not enough space in buffer to send */
+			count_txerrbytes(packet_len);
+			return false;
+		}
+	}
+
+	const float dt = hrt_elapsed_time(&_bytes_timestamp) / 1e6f;
+	const float rate = (_bytes_tx + packet_len) / dt;
+
+	if (rate > (get_allowable_data_rate())) {
+		count_txerrbytes(packet_len);
+		return false;
+	}
+
+	return true;
+}
+
 void
 Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
 {
@@ -979,17 +998,6 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
 		_mavlink_start_time = _last_write_try_time;
 	}
 
-	if (get_protocol() == SERIAL) {
-		/* check if there is space in the buffer, let it overflow else */
-		unsigned buf_free = get_free_tx_buf();
-
-		if (buf_free < packet_len) {
-			/* not enough space in buffer to send */
-			count_txerrbytes(packet_len);
-			return;
-		}
-	}
-
 	size_t ret = -1;
 
 	/* send message to UART */
@@ -1663,63 +1671,46 @@ Mavlink::update_rate_mult()
 	}
 
 	/* scale up and down as the link permits */
-	float bandwidth_mult = (float)(_datarate * mavlink_ulog_streaming_rate_inv - const_rate) / rate;
+	float bandwidth_mult = (float)(get_allowable_data_rate() * mavlink_ulog_streaming_rate_inv - const_rate) / rate;
 
 	/* if we do not have flow control, limit to the set data rate */
 	if (!get_flow_control_enabled()) {
-		bandwidth_mult = fminf(1.0f, bandwidth_mult);
-	}
-
-	float hardware_mult = 1.0f;
-
-	/* scale down if we have a TX err rate suggesting link congestion */
-	if (_tstatus.rate_txerr > 0.0f && !_radio_status_critical) {
-		hardware_mult = (_tstatus.rate_tx) / (_tstatus.rate_tx + _tstatus.rate_txerr);
-
-	} else if (_radio_status_available) {
-
-		// check for RADIO_STATUS timeout and reset
-		if (hrt_elapsed_time(&_rstatus.timestamp) > 5_s) {
-			PX4_ERR("instance %d: RADIO_STATUS timeout", _instance_id);
-			set_telemetry_status_type(telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_GENERIC);
-
-			_radio_status_available = false;
-			_radio_status_critical = false;
-			_radio_status_mult = 1.0f;
-		}
-
-		hardware_mult *= _radio_status_mult;
+		bandwidth_mult = math::constrain(bandwidth_mult, 0.0f, 1.0f);
 	}
 
-	/* pick the minimum from bandwidth mult and hardware mult as limit */
-	_rate_mult = fminf(bandwidth_mult, hardware_mult);
-
 	/* ensure the rate multiplier never drops below 5% so that something is always sent */
-	_rate_mult = math::constrain(_rate_mult, 0.05f, 1.0f);
+	_rate_mult = math::constrain(bandwidth_mult * get_error_rate_mult(), 0.05f, 1.0f);
 }
 
 void
 Mavlink::update_radio_status(const radio_status_s &radio_status)
 {
-	_rstatus = radio_status;
 	set_telemetry_status_type(telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO);
 
 	/* check hardware limits */
 	_radio_status_available = true;
-	_radio_status_critical = (radio_status.txbuf < RADIO_BUFFER_LOW_PERCENTAGE);
+
+	// radio rx errors indicate a significant problem, drop rate as a precaution
+	if (radio_status.rxerrors > _rstatus.rxerrors) {
+		_error_rate_mult *= 0.50f;
+	}
 
 	if (radio_status.txbuf < RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE) {
 		/* this indicates link congestion, reduce rate by 20% */
-		_radio_status_mult *= 0.80f;
+		_error_rate_mult *= 0.80f;
 
 	} else if (radio_status.txbuf < RADIO_BUFFER_LOW_PERCENTAGE) {
 		/* this indicates link congestion, reduce rate by 2.5% */
-		_radio_status_mult *= 0.975f;
+		_error_rate_mult *= 0.975f;
 
 	} else if (radio_status.txbuf > RADIO_BUFFER_HALF_PERCENTAGE) {
 		/* this indicates spare bandwidth, increase by 2.5% */
-		_radio_status_mult *= 1.025f;
+		_error_rate_mult *= 1.025f;
 	}
+
+	_error_rate_mult = math::constrain(_error_rate_mult, 0.01f, 1.0f);
+
+	_rstatus = radio_status;
 }
 
 int
@@ -2057,10 +2048,6 @@ Mavlink::task_main(int argc, char *argv[])
 			break;
 #endif
 
-//		case 'e':
-//			mavlink_link_termination_allowed = true;
-//			break;
-
 		case 'm': {
 
 				int mode;
@@ -2280,8 +2267,6 @@ Mavlink::task_main(int argc, char *argv[])
 
 		hrt_abstime t = hrt_absolute_time();
 
-		update_rate_mult();
-
 		if (param_sub->update(&param_time, nullptr)) {
 			mavlink_update_parameters();
 
@@ -2474,6 +2459,8 @@ Mavlink::task_main(int argc, char *argv[])
 			_subscribe_to_stream = nullptr;
 		}
 
+		update_rate_mult();
+
 		/* update streams */
 		MavlinkStream *stream;
 		LL_FOREACH(_streams, stream) {
@@ -2542,20 +2529,33 @@ Mavlink::task_main(int argc, char *argv[])
 		}
 
 		/* update TX/RX rates*/
-		if (t > _bytes_timestamp + 1000000) {
-			if (_bytes_timestamp != 0) {
-				const float dt = (t - _bytes_timestamp) / 1000.0f;
+		const uint64_t elapsed = hrt_elapsed_time(&_bytes_timestamp);
+
+		if (elapsed >= 1_s) {
+			const float dt = elapsed / 1000.0f;
 
-				_tstatus.rate_tx = _bytes_tx / dt;
-				_tstatus.rate_txerr = _bytes_txerr / dt;
-				_tstatus.rate_rx = _bytes_rx / dt;
+			_tstatus.rate_tx = _bytes_tx / dt;
+			_tstatus.rate_txerr = _bytes_txerr / dt;
+			_tstatus.rate_rx = _bytes_rx / dt;
 
-				_bytes_tx = 0;
-				_bytes_txerr = 0;
-				_bytes_rx = 0;
+			// gradually increase data rate if there were no errors
+			if (_bytes_txerr == 0) {
+				_error_rate_mult += 0.05f;
+
+			} else {
+				const float rate = static_cast<float>(_bytes_tx) / static_cast<float>(_bytes_tx + _bytes_txerr);
+
+				if (PX4_ISFINITE(rate)) {
+					_error_rate_mult *= rate;
+				}
 			}
 
-			_bytes_timestamp = t;
+			_error_rate_mult = math::constrain(_error_rate_mult, 0.05f, 1.0f);
+
+			_bytes_tx = 0;
+			_bytes_txerr = 0;
+			_bytes_rx = 0;
+			_bytes_timestamp = hrt_absolute_time();
 		}
 
 		// publish status at 1 Hz, or sooner if HEARTBEAT has updated
@@ -2629,8 +2629,10 @@ void Mavlink::publish_telemetry_status()
 	// many fields are populated in place
 
 	_tstatus.mode = _mode;
-	_tstatus.data_rate = _datarate;
-	_tstatus.rate_multiplier = _rate_mult;
+	_tstatus.data_rate = get_data_rate();
+	_tstatus.current_data_rate = get_allowable_data_rate();
+	_tstatus.data_rate_multiplier = get_rate_mult();
+	_tstatus.error_rate_multiplier = get_error_rate_mult();
 	_tstatus.flow_control = get_flow_control_enabled();
 	_tstatus.ftp = ftp_enabled();
 	_tstatus.forwarding = get_forwarding_on();
@@ -2827,8 +2829,8 @@ Mavlink::display_status()
 	printf("\trates:\n");
 	printf("\t  tx: %.3f kB/s\n", (double)_tstatus.rate_tx);
 	printf("\t  txerr: %.3f kB/s\n", (double)_tstatus.rate_txerr);
-	printf("\t  tx rate mult: %.3f\n", (double)_rate_mult);
-	printf("\t  tx rate max: %i B/s\n", _datarate);
+	printf("\t  tx rate mult: %.3f\n", (double)get_rate_mult());
+	printf("\t  tx rate max: %i B/s\n", get_data_rate());
 	printf("\t  rx: %.3f kB/s\n", (double)_tstatus.rate_rx);
 
 	if (_mavlink_ulog) {

src/modules/mavlink/mavlink_main.h

@@ -59,11 +59,11 @@
 #include <net/if.h>
 #endif
 
-#include <parameters/param.h>
+#include <lib/parameters/param.h>
 #include <perf/perf_counter.h>
-#include <pthread.h>
 #include <systemlib/mavlink_log.h>
 #include <drivers/device/ringbuffer.h>
+#include <lib/mathlib/mathlib.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/mission.h>
@@ -307,6 +307,8 @@ class Mavlink : public ModuleParams
 	 */
 	void			send_bytes(const uint8_t *buf, unsigned packet_len);
 
+	bool			should_send_bytes(unsigned packet_len);
+
 	/**
 	 * Flush the transmit buffer and send one MAVLink packet
 	 *
@@ -392,6 +394,7 @@ class Mavlink : public ModuleParams
 	MavlinkStream 		*get_streams() const { return _streams; }
 
 	float			get_rate_mult() const { return _rate_mult; }
+	float			get_error_rate_mult() const { return _error_rate_mult; }
 
 	float			get_baudrate() { return _baudrate; }
 
@@ -400,6 +403,7 @@ class Mavlink : public ModuleParams
 	bool			get_has_received_messages() { return _received_messages; }
 	void			set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; }
 	bool			get_wait_to_transmit() { return _wait_to_transmit; }
+
 	bool			should_transmit() { return (_transmitting_enabled && _boot_complete && (!_wait_to_transmit || (_wait_to_transmit && _received_messages))); }
 
 	bool			message_buffer_write(const void *ptr, int size);
@@ -465,7 +469,9 @@ class Mavlink : public ModuleParams
 
 	bool			is_usb_uart() { return _is_usb_uart; }
 
-	int			get_data_rate()		{ return _datarate; }
+	int			get_data_rate() const { return _datarate; }
+	int			get_allowable_data_rate() const { return _datarate * _error_rate_mult; }
+
 	void			set_data_rate(int rate) { if (rate > 0) { _datarate = rate; } }
 
 	unsigned		get_main_loop_delay() const { return _main_loop_delay; }
@@ -565,11 +571,11 @@ class Mavlink : public ModuleParams
 
 	int			_baudrate;
 	int			_datarate;		///< data rate for normal streams (attitude, position, etc.)
-	float			_rate_mult;
+
+	float			_rate_mult{1.0f};
+	float			_error_rate_mult{1.0f};
 
 	bool			_radio_status_available{false};
-	bool			_radio_status_critical{false};
-	float			_radio_status_mult{1.0f};
 
 	/**
 	 * If the queue index is not at 0, the queue sending
@@ -578,8 +584,6 @@ class Mavlink : public ModuleParams
 	 */
 	unsigned int		_mavlink_param_queue_index;
 
-	bool			mavlink_link_termination_allowed;
-
 	char 			*_subscribe_to_stream;
 	float			_subscribe_to_stream_rate;  ///< rate of stream to subscribe to (0=disable, -1=unlimited, -2=default)
 	bool 			_udp_initialised;
@@ -591,10 +595,10 @@ class Mavlink : public ModuleParams
 	int32_t			_protocol_version_switch;
 	int32_t			_protocol_version;
 
-	unsigned		_bytes_tx;
-	unsigned		_bytes_txerr;
-	unsigned		_bytes_rx;
-	uint64_t		_bytes_timestamp;
+	unsigned		_bytes_tx{0};
+	unsigned		_bytes_txerr{0};
+	unsigned		_bytes_rx{0};
+	uint64_t		_bytes_timestamp{0};
 
 #if defined(CONFIG_NET) || defined(__PX4_POSIX)
 	struct sockaddr_in _myaddr;

src/modules/mavlink/mavlink_parameters.cpp

@@ -320,7 +320,7 @@ MavlinkParametersManager::send(const hrt_abstime t)
 	int i = 0;
 
 	// Send while burst is not exceeded, we still have buffer space and still something to send
-	while ((i++ < max_num_to_send) && (_mavlink->get_free_tx_buf() >= get_size()) && send_params());
+	while ((i++ < max_num_to_send) && _mavlink->should_send_bytes(get_size()) && send_params());
 }
 
 bool

src/modules/mavlink/mavlink_receiver.cpp

@@ -2741,7 +2741,7 @@ MavlinkReceiver::receive_thread(void *arg)
 			}
 		}
 
-		hrt_abstime t = hrt_absolute_time();
+		const hrt_abstime t = hrt_absolute_time();
 
 		if (t - last_send_update > timeout * 1000) {
 			_mission_manager.check_active_mission();

src/modules/mavlink/mavlink_stream.cpp

@@ -105,7 +105,9 @@ MavlinkStream::update(const hrt_abstime &t)
 		// do not use the actual time but increment at a fixed rate, so that processing delays do not
 		// distort the average rate. The check of the maximum interval is done to ensure that after a
 		// long time not sending anything, sending multiple messages in a short time is avoided.
-		if (send(t)) {
+
+		// check TX buffer before doing anything
+		if (_mavlink->should_send_bytes(get_size()) && send(t)) {
 			_last_sent = ((interval > 0) && ((int64_t)(1.5f * interval) > dt)) ? _last_sent + interval : t;
 
 			if (!_first_message_sent) {