MAVLink: Improve message handling / tracking

The message handling was not obeying action focused messages and high-rate messages properly before. With this change update rates track the desired rates closely. Critical high-rate messages such as ADS-B are queued additionally to guarantee that all received packets are being correctly forwarded.

msg/transponder_report.msg

@@ -12,4 +12,4 @@ uint8 tslc 		# Time since last communication in seconds
 uint16 flags 		# Flags to indicate various statuses including valid data fields
 uint16 squawk 		# Squawk code
 
-uint32 ORB_QUEUE_LENGTH = 3
+uint32 ORB_QUEUE_LENGTH = 10

src/modules/mavlink/mavlink_main.cpp

@@ -1355,23 +1355,31 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, int
 	return sub_new;
 }
 
-unsigned int
+int
 Mavlink::interval_from_rate(float rate)
 {
-	return (rate > 0.0f) ? (1000000.0f / rate) : 0;
+	if (rate > 0.000001f) {
+		return (1000000.0f / rate);
+
+	} else if (rate < 0.0f) {
+		return -1;
+
+	} else {
+		return 0;
+	}
 }
 
 int
 Mavlink::configure_stream(const char *stream_name, const float rate)
 {
-	/* calculate interval in us, 0 means disabled stream */
-	unsigned int interval = interval_from_rate(rate);
+	/* calculate interval in us, -1 means unlimited stream, 0 means disabled */
+	int interval = interval_from_rate(rate);
 
 	/* search if stream exists */
 	MavlinkStream *stream;
 	LL_FOREACH(_streams, stream) {
 		if (strcmp(stream_name, stream->get_name()) == 0) {
-			if (interval > 0) {
+			if (interval != 0) {
 				/* set new interval */
 				stream->set_interval(interval);
 
@@ -1385,7 +1393,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
 		}
 	}
 
-	if (interval <= 0) {
+	if (interval == 0) {
 		/* stream was not active and is requested to be disabled, do nothing */
 		return OK;
 	}
@@ -1404,7 +1412,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
 	}
 
 	/* if we reach here, the stream list does not contain the stream */
-	warnx("stream %s not found", stream_name);
+	PX4_WARN("stream %s not found", stream_name);
 
 	return PX4_ERROR;
 }
@@ -1413,7 +1421,7 @@ void
 Mavlink::adjust_stream_rates(const float multiplier)
 {
 	/* do not allow to push us to zero */
-	if (multiplier < 0.0005f) {
+	if (multiplier < MAVLINK_MIN_MULTIPLIER) {
 		return;
 	}
 
@@ -1421,16 +1429,23 @@ Mavlink::adjust_stream_rates(const float multiplier)
 	MavlinkStream *stream;
 	LL_FOREACH(_streams, stream) {
 		/* set new interval */
-		unsigned interval = stream->get_interval();
-		interval /= multiplier;
+		int interval = stream->get_interval();
 
-		/* allow max ~2000 Hz */
-		if (interval < 1600) {
-			interval = 500;
+		if (interval > 0) {
+			interval /= multiplier;
+
+			/* limit min / max interval */
+			if (interval < MAVLINK_MIN_INTERVAL) {
+				interval = MAVLINK_MIN_INTERVAL;
+			}
+
+			if (interval > MAVLINK_MAX_INTERVAL) {
+				interval = MAVLINK_MAX_INTERVAL;
+			}
+
+			/* set new interval */
+			stream->set_interval(interval);
 		}
-
-		/* set new interval */
-		stream->set_interval(interval * multiplier);
 	}
 }
 
@@ -1975,8 +1990,8 @@ Mavlink::task_main(int argc, char *argv[])
 		/* STATUSTEXT stream is like normal stream but gets messages from logbuffer instead of uORB */
 		configure_stream("STATUSTEXT", 20.0f);
 
-		/* COMMAND_LONG stream: use high rate to avoid commands skipping */
-		configure_stream("COMMAND_LONG", 100.0f);
+		/* COMMAND_LONG stream: use unlimited rate to send all commands */
+		configure_stream("COMMAND_LONG");
 
 	}
 
@@ -1990,7 +2005,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("SERVO_OUTPUT_RAW_0", 1.0f);
 		configure_stream("ALTITUDE", 1.0f);
 		configure_stream("GPS_RAW_INT", 1.0f);
-		configure_stream("ADSB_VEHICLE", 2.0f);
+		configure_stream("ADSB_VEHICLE");
 		configure_stream("COLLISION", 2.0f);
 		configure_stream("DISTANCE_SENSOR", 0.5f);
 		configure_stream("OPTICAL_FLOW_RAD", 1.0f);
@@ -2006,20 +2021,20 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("NAMED_VALUE_FLOAT", 1.0f);
 		configure_stream("VFR_HUD", 4.0f);
 		configure_stream("WIND_COV", 1.0f);
-		configure_stream("CAMERA_IMAGE_CAPTURED", 500.0f);
+		configure_stream("CAMERA_IMAGE_CAPTURED");
 		break;
 
 	case MAVLINK_MODE_ONBOARD:
 		configure_stream("SYS_STATUS", 5.0f);
 		configure_stream("EXTENDED_SYS_STATE", 5.0f);
 		configure_stream("HIGHRES_IMU", 50.0f);
-		configure_stream("ATTITUDE", 250.0f);
+		configure_stream("ATTITUDE", 100.0f);
 		configure_stream("ATTITUDE_QUATERNION", 50.0f);
 		configure_stream("RC_CHANNELS", 20.0f);
 		configure_stream("SERVO_OUTPUT_RAW_0", 10.0f);
 		configure_stream("ALTITUDE", 10.0f);
 		configure_stream("GPS_RAW_INT", 5.0f);
-		configure_stream("ADSB_VEHICLE", 10.0f);
+		configure_stream("ADSB_VEHICLE");
 		configure_stream("COLLISION", 10.0f);
 		configure_stream("DISTANCE_SENSOR", 10.0f);
 		configure_stream("OPTICAL_FLOW_RAD", 10.0f);
@@ -2038,9 +2053,8 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("SYSTEM_TIME", 1.0f);
 		configure_stream("TIMESYNC", 10.0f);
 		configure_stream("CAMERA_CAPTURE", 2.0f);
-		//camera trigger is rate limited at the source, do not limit here
-		configure_stream("CAMERA_TRIGGER", 500.0f);
-		configure_stream("CAMERA_IMAGE_CAPTURED", 5.0f);
+		configure_stream("CAMERA_TRIGGER");
+		configure_stream("CAMERA_IMAGE_CAPTURED");
 		configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f);
 		break;
 
@@ -2077,7 +2091,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("SERVO_OUTPUT_RAW_1", 20.0f);
 		configure_stream("ALTITUDE", 10.0f);
 		configure_stream("GPS_RAW_INT", 10.0f);
-		configure_stream("ADSB_VEHICLE", 20.0f);
+		configure_stream("ADSB_VEHICLE");
 		configure_stream("COLLISION", 20.0f);
 		configure_stream("DISTANCE_SENSOR", 10.0f);
 		configure_stream("OPTICAL_FLOW_RAD", 10.0f);
@@ -2092,8 +2106,8 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("NAMED_VALUE_FLOAT", 50.0f);
 		configure_stream("VFR_HUD", 20.0f);
 		configure_stream("WIND_COV", 10.0f);
-		configure_stream("CAMERA_TRIGGER", 500.0f);
-		configure_stream("CAMERA_IMAGE_CAPTURED", 5.0f);
+		configure_stream("CAMERA_TRIGGER");
+		configure_stream("CAMERA_IMAGE_CAPTURED");
 		configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f);
 		configure_stream("MANUAL_CONTROL", 5.0f);
 		break;
@@ -2109,14 +2123,14 @@ Mavlink::task_main(int argc, char *argv[])
 	/* set main loop delay depending on data rate to minimize CPU overhead */
 	_main_loop_delay = (MAIN_LOOP_DELAY * 1000) / _datarate;
 
-	/* hard limit to 500 Hz at max */
-	if (_main_loop_delay < 2000) {
-		_main_loop_delay = 2000;
+	/* hard limit to 1000 Hz at max */
+	if (_main_loop_delay < MAVLINK_MIN_INTERVAL) {
+		_main_loop_delay = MAVLINK_MIN_INTERVAL;
 	}
 
 	/* hard limit to 100 Hz at least */
-	if (_main_loop_delay > 10000) {
-		_main_loop_delay = 10000;
+	if (_main_loop_delay > MAVLINK_MAX_INTERVAL) {
+		_main_loop_delay = MAVLINK_MAX_INTERVAL;
 	}
 
 	/* now the instance is fully initialized and we can bump the instance count */
@@ -2147,53 +2161,7 @@ Mavlink::task_main(int argc, char *argv[])
 			mavlink_update_system();
 		}
 
-		/* radio config check */
-		if (_uart_fd >= 0 && _radio_id != 0 && _rstatus.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) {
-			/* request to configure radio and radio is present */
-			FILE *fs = fdopen(_uart_fd, "w");
-
-			if (fs) {
-				/* switch to AT command mode */
-				usleep(1200000);
-				fprintf(fs, "+++\n");
-				usleep(1200000);
-
-				if (_radio_id > 0) {
-					/* set channel */
-					fprintf(fs, "ATS3=%u\n", _radio_id);
-					usleep(200000);
-
-				} else {
-					/* reset to factory defaults */
-					fprintf(fs, "AT&F\n");
-					usleep(200000);
-				}
-
-				/* write config */
-				fprintf(fs, "AT&W");
-				usleep(200000);
-
-				/* reboot */
-				fprintf(fs, "ATZ");
-				usleep(200000);
-
-				// XXX NuttX suffers from a bug where
-				// fclose() also closes the fd, not just
-				// the file stream. Since this is a one-time
-				// config thing, we leave the file struct
-				// allocated.
-#ifndef __PX4_NUTTX
-				fclose(fs);
-#endif
-
-			} else {
-				PX4_WARN("open fd %d failed", _uart_fd);
-			}
-
-			/* reset param and save */
-			_radio_id = 0;
-			param_set(_param_radio_id, &_radio_id);
-		}
+		check_radio_config();
 
 		if (status_sub->update(&status_time, &status)) {
 			/* switch HIL mode if required */
@@ -2261,7 +2229,7 @@ Mavlink::task_main(int argc, char *argv[])
 		/* check for requested subscriptions */
 		if (_subscribe_to_stream != nullptr) {
 			if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {
-				if (_subscribe_to_stream_rate > 0.0f) {
+				if (fabsf(_subscribe_to_stream_rate) > 0.00001f) {
 					if (get_protocol() == SERIAL) {
 						PX4_DEBUG("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name,
 							  (double)_subscribe_to_stream_rate);
@@ -2422,6 +2390,57 @@ Mavlink::task_main(int argc, char *argv[])
 	return OK;
 }
 
+void Mavlink::check_radio_config()
+{
+	/* radio config check */
+	if (_uart_fd >= 0 && _radio_id != 0 && _rstatus.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) {
+		/* request to configure radio and radio is present */
+		FILE *fs = fdopen(_uart_fd, "w");
+
+		if (fs) {
+			/* switch to AT command mode */
+			usleep(1200000);
+			fprintf(fs, "+++\n");
+			usleep(1200000);
+
+			if (_radio_id > 0) {
+				/* set channel */
+				fprintf(fs, "ATS3=%u\n", _radio_id);
+				usleep(200000);
+
+			} else {
+				/* reset to factory defaults */
+				fprintf(fs, "AT&F\n");
+				usleep(200000);
+			}
+
+			/* write config */
+			fprintf(fs, "AT&W");
+			usleep(200000);
+
+			/* reboot */
+			fprintf(fs, "ATZ");
+			usleep(200000);
+
+			// XXX NuttX suffers from a bug where
+			// fclose() also closes the fd, not just
+			// the file stream. Since this is a one-time
+			// config thing, we leave the file struct
+			// allocated.
+#ifndef __PX4_NUTTX
+			fclose(fs);
+#endif
+
+		} else {
+			PX4_WARN("open fd %d failed", _uart_fd);
+		}
+
+		/* reset param and save */
+		_radio_id = 0;
+		param_set(_param_radio_id, &_radio_id);
+	}
+}
+
 int Mavlink::start_helper(int argc, char *argv[])
 {
 	/* create the instance in task context */

src/modules/mavlink/mavlink_main.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2017 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
@@ -33,14 +33,17 @@
 
 /**
  * @file mavlink_main.h
- * MAVLink 1.0 protocol interface definition.
  *
- * @author Lorenz Meier <lm@inf.ethz.ch>
+ * MAVLink 2.0 protocol interface definition.
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #pragma once
 
+#include <px4_posix.h>
+
 #include <stdbool.h>
 #ifdef __PX4_NUTTX
 #include <nuttx/fs/fs.h>
@@ -309,7 +312,7 @@ public:
 
 	mavlink_channel_t	get_channel();
 
-	void			configure_stream_threadsafe(const char *stream_name, float rate);
+	void			configure_stream_threadsafe(const char *stream_name, float rate = -1.0f);
 
 	bool			_task_should_exit;	/**< if true, mavlink task should exit */
 
@@ -431,7 +434,7 @@ public:
 
 	bool			verbose() { return _verbose; }
 
-	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; }
@@ -455,6 +458,9 @@ public:
 		if (_mavlink_ulog) { _mavlink_ulog_stop_requested = true; }
 	}
 
+
+	void set_uorb_main_fd(int fd, unsigned int interval);
+
 protected:
 	Mavlink			*next;
 
@@ -464,7 +470,10 @@ private:
 	orb_advert_t		_mavlink_log_pub;
 	bool			_task_running;
 	static bool		_boot_complete;
-	static const unsigned MAVLINK_MAX_INSTANCES = 4;
+	static constexpr unsigned MAVLINK_MAX_INSTANCES = 4;
+	static constexpr unsigned MAVLINK_MIN_INTERVAL = 1500;
+	static constexpr unsigned MAVLINK_MAX_INTERVAL = 10000;
+	static constexpr float MAVLINK_MIN_MULTIPLIER = 0.0005f;
 	mavlink_message_t _mavlink_buffer;
 	mavlink_status_t _mavlink_status;
 
@@ -590,13 +599,13 @@ private:
 	int			mavlink_open_uart(int baudrate, const char *uart_name);
 #endif
 
-	static unsigned int	interval_from_rate(float rate);
+	static int		interval_from_rate(float rate);
 
 	static constexpr unsigned RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE = 25;
 	static constexpr unsigned RADIO_BUFFER_LOW_PERCENTAGE = 35;
 	static constexpr unsigned RADIO_BUFFER_HALF_PERCENTAGE = 50;
 
-	int configure_stream(const char *stream_name, const float rate);
+	int configure_stream(const char *stream_name, const float rate = -1.0f);
 
 	/**
 	 * Adjust the stream rates based on the current rate
@@ -619,6 +628,14 @@ private:
 
 	void pass_message(const mavlink_message_t *msg);
 
+	/**
+	 * Check the configuration of a connected radio
+	 *
+	 * This convenience function allows to re-configure a connected
+	 * radio without removing it from the main system harness.
+	 */
+	void check_radio_config();
+
 	/**
 	 * Update rate mult so total bitrate will be equal to _datarate.
 	 */

src/modules/mavlink/mavlink_orb_subscription.h

@@ -93,6 +93,8 @@ public:
 	orb_id_t get_topic() const;
 	int get_instance() const;
 
+	int get_fd() { return _fd; }
+
 private:
 	const orb_id_t _topic;		///< topic metadata
 	int _fd;			///< subscription handle

src/modules/mavlink/mavlink_receiver.cpp

@@ -2084,7 +2084,7 @@ void MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg)
 	t.flags = adsb.flags;
 	t.squawk = adsb.squawk;
 
-	//warnx("code: %d callsign: %s, vel: %8.4f", (int)t.ICAO_address, t.callsign, (double)t.hor_velocity);
+	//PX4_INFO("code: %d callsign: %s, vel: %8.4f, tslc: %d", (int)t.ICAO_address, t.callsign, (double)t.hor_velocity, (int)t.tslc);
 
 	if (_transponder_report_pub == nullptr) {
 		_transponder_report_pub = orb_advertise_queue(ORB_ID(transponder_report), &t, transponder_report_s::ORB_QUEUE_LENGTH);

src/modules/mavlink/mavlink_stream.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014-2017 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
@@ -36,6 +36,7 @@
  * Mavlink messages stream implementation.
  *
  * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Lorenz Meier <lorenz@px4.io>
  */
 
 #include <stdlib.h>
@@ -59,7 +60,7 @@ MavlinkStream::~MavlinkStream()
  * Set messages interval in ms
  */
 void
-MavlinkStream::set_interval(const unsigned int interval)
+MavlinkStream::set_interval(const int interval)
 {
 	_interval = interval;
 }
@@ -78,7 +79,7 @@ MavlinkStream::update(const hrt_abstime t)
 		// on the link scheduling
 		_last_sent = hrt_absolute_time();
 #ifndef __PX4_QURT
-		send(t);
+		(void)send(t);
 #endif
 		return 0;
 	}
@@ -90,13 +91,13 @@ MavlinkStream::update(const hrt_abstime t)
 	}
 
 	int64_t dt = t - _last_sent;
-	int interval = _interval;
+	int interval = (_interval > 0) ? _interval : 0;
 
 	if (!const_rate()) {
 		interval /= _mavlink->get_rate_mult();
 	}
 
-	// send the message if it is due or
+	// 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
@@ -106,17 +107,23 @@ MavlinkStream::update(const hrt_abstime t)
 	// 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)) {
+	if (interval == 0 || (dt > (interval - (_mavlink->get_main_loop_delay() / 10) * 4))) {
 		// interval expired, send message
+		bool sent = true;
 #ifndef __PX4_QURT
-		send(t);
+		sent = send(t);
 #endif
-		// if the interval is non-zero do not use the actual time but
+
+		// If the interval is non-zero do not use the actual time but
 		// increment at a fixed rate, so that processing delays do not
 		// distort the average rate
-		_last_sent = (interval > 0) ? _last_sent + interval : t;
+		if (sent) {
+			_last_sent = (interval > 0) ? _last_sent + interval : t;
+			return 0;
 
-		return 0;
+		} else {
+			return -1;
+		}
 	}
 
 	return -1;

src/modules/mavlink/mavlink_stream.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014-2016 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014-2017 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
@@ -59,14 +59,14 @@ public:
 	 *
 	 * @param interval the inveral in microseconds (us) between messages
 	 */
-	void set_interval(const unsigned int interval);
+	void set_interval(const int interval);
 
 	/**
 	 * Get the interval
 	 *
 	 * @return the inveral in microseconds (us) between messages
 	 */
-	unsigned get_interval() { return _interval; }
+	int get_interval() { return _interval; }
 
 	/**
 	 * @return 0 if updated / sent, -1 if unchanged
@@ -97,10 +97,10 @@ public:
 
 protected:
 	Mavlink     *_mavlink;
-	unsigned int _interval;		///< if set to zero = unlimited rate
+	int _interval;		///< if set to negative value = unlimited rate
 
 #ifndef __PX4_QURT
-	virtual void send(const hrt_abstime t) = 0;
+	virtual bool send(const hrt_abstime t) = 0;
 #endif
 
 private: