Mavlink expand telemetry_status and split radio_status

msg/CMakeLists.txt

@@ -86,6 +86,7 @@ set(msg_files
 	pwm_input.msg
 	qshell_req.msg
 	qshell_retval.msg
+	radio_status.msg
 	rate_ctrl_status.msg
 	rc_channels.msg
 	rc_parameter_map.msg
@@ -120,6 +121,7 @@ set(msg_files
 	vehicle_attitude_setpoint.msg
 	vehicle_command.msg
 	vehicle_command_ack.msg
+	vehicle_constraints.msg
 	vehicle_control_mode.msg
 	vehicle_global_position.msg
 	vehicle_gps_position.msg
@@ -134,7 +136,6 @@ set(msg_files
 	vehicle_trajectory_waypoint.msg
 	vtol_vehicle_status.msg
 	wind_estimate.msg
-	vehicle_constraints.msg
 	)
 
 if(NOT EXTERNAL_MODULES_LOCATION STREQUAL "")

msg/radio_status.msg

@@ -0,0 +1,22 @@
+
+uint64 timestamp	# time since system start (microseconds)
+
+uint8 RADIO_TYPE_GENERIC = 0
+uint8 RADIO_TYPE_3DR_RADIO = 1
+uint8 RADIO_TYPE_UBIQUITY_BULLET = 2
+uint8 RADIO_TYPE_WIRE = 3
+uint8 RADIO_TYPE_USB = 4
+uint8 RADIO_TYPE_IRIDIUM = 5
+
+uint8 type				# type of the radio hardware
+
+uint8 rssi				# local signal strength
+uint8 remote_rssi			# remote signal strength
+
+uint8 txbuf				# how full the tx buffer is as a percentage
+uint8 noise				# background noise level
+
+uint8 remote_noise			# remote background noise level
+uint16 rxerrors				# receive errors
+
+uint16 fixed				# count of error corrected packets

msg/telemetry_status.msg

@@ -8,14 +8,26 @@ uint8 TELEMETRY_STATUS_RADIO_TYPE_USB = 4
 uint8 TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM = 5
 
 uint64 heartbeat_time			# Time of last received heartbeat from remote system
-uint64 telem_time			# Time of last received telemetry status packet, 0 for none
+
 uint8 type				# type of the radio hardware
-uint8 rssi				# local signal strength
+
-uint8 remote_rssi			# remote signal strength
-uint16 rxerrors				# receive errors
-uint16 fixed				# count of error corrected packets
-uint8 noise				# background noise level
-uint8 remote_noise			# remote background noise level
-uint8 txbuf				# how full the tx buffer is as a percentage
 uint8 system_id				# system id of the remote system
 uint8 component_id			# component id of the remote system
+
+uint8 mode
+
+bool flow_control
+bool forwarding
+bool mavlink_v2
+bool ftp
+
+uint8 streams
+
+float32 data_rate
+
+float32 rate_multiplier
+
+float32 rate_rx
+
+float32 rate_tx
+float32 rate_txerr

src/modules/logger/logger.cpp

@@ -624,11 +624,13 @@ void Logger::add_default_topics()
 	add_topic("mission_result");
 	add_topic("optical_flow", 50);
 	add_topic("position_setpoint_triplet", 200);
+	add_topic("radio_status");
 	add_topic("rate_ctrl_status", 30);
 	add_topic("sensor_combined", 100);
 	add_topic("sensor_preflight", 200);
 	add_topic("system_power", 500);
 	add_topic("tecs_status", 200);
+	add_topic("telemetry_status");
 	add_topic("vehicle_attitude", 30);
 	add_topic("vehicle_attitude_setpoint", 100);
 	add_topic("vehicle_command");

src/modules/mavlink/mavlink_main.cpp

@@ -72,6 +72,7 @@
 #include <lib/ecl/geo/geo.h>
 #include <dataman/dataman.h>
 #include <version/version.h>
+#include <mathlib/mathlib.h>
 
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_command_ack.h>
@@ -186,7 +187,6 @@ mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel)
 static void usage();
 
 bool Mavlink::_boot_complete = false;
-bool Mavlink::_config_link_on = false;
 
 Mavlink::Mavlink() :
 	_device_name("/dev/ttyS1"),
@@ -222,9 +222,7 @@ Mavlink::Mavlink() :
 	_uart_fd(-1),
 	_baudrate(57600),
 	_datarate(1000),
-	_datarate_events(500),
 	_rate_mult(1.0f),
-	_last_hw_rate_timestamp(0),
 	_mavlink_param_queue_index(0),
 	mavlink_link_termination_allowed(false),
 	_subscribe_to_stream(nullptr),
@@ -240,9 +238,6 @@ Mavlink::Mavlink() :
 	_bytes_txerr(0),
 	_bytes_rx(0),
 	_bytes_timestamp(0),
-	_rate_tx(0.0f),
-	_rate_txerr(0.0f),
-	_rate_rx(0.0f),
 #ifdef __PX4_POSIX
 	_myaddr {},
 	_src_addr{},
@@ -258,8 +253,6 @@ Mavlink::Mavlink() :
 	_protocol(SERIAL),
 	_network_port(14556),
 	_remote_port(DEFAULT_REMOTE_PORT_UDP),
-	_rstatus {},
-	_ping_stats{},
 	_message_buffer {},
 	_message_buffer_mutex {},
 	_send_mutex {},
@@ -275,8 +268,7 @@ Mavlink::Mavlink() :
 	_system_type(0),
 
 	/* performance counters */
-	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
+	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el"))
-	_txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
 {
 	_instance_id = Mavlink::instance_count();
 
@@ -322,13 +314,12 @@ Mavlink::Mavlink() :
 		break;
 	}
 
-	_rstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
+	_tstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
 }
 
 Mavlink::~Mavlink()
 {
 	perf_free(_loop_perf);
-	perf_free(_txerr_perf);
 
 	if (_task_running) {
 		/* task wakes up every 10ms or so at the longest */
@@ -366,12 +357,6 @@ Mavlink::set_proto_version(unsigned version)
 	}
 }
 
-void
-Mavlink::count_txerr()
-{
-	perf_count(_txerr_perf);
-}
-
 int
 Mavlink::instance_count()
 {
@@ -814,7 +799,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, bool force_flow_
 		_is_usb_uart = true;
 		/* USB has no baudrate, but use a magic number for 'fast' */
 		_baudrate = 2000000;
-		_rstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB;
+		_tstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB;
 	}
 
 #if defined(__PX4_LINUX) || defined(__PX4_DARWIN) || defined(__PX4_CYGWIN)
@@ -919,8 +904,9 @@ Mavlink::get_free_tx_buf()
 			 * and if the last try was not the last successful write
 			 */
 			if (_last_write_try_time != 0 &&
-			    hrt_elapsed_time(&_last_write_success_time) > 500 * 1000UL &&
+			    hrt_elapsed_time(&_last_write_success_time) > 500_ms &&
 			    _last_write_success_time != _last_write_try_time) {
+
 				enable_flow_control(FLOW_CONTROL_OFF);
 			}
 		}
@@ -952,16 +938,13 @@ Mavlink::send_packet()
 
 	if (get_protocol() == UDP) {
 
-
 		ret = sendto(_socket_fd, _network_buf, _network_buf_len, 0,
 			     (struct sockaddr *)&_src_addr, sizeof(_src_addr));
 
-		struct telemetry_status_s &tstatus = get_rx_status();
-
 		/* resend message via broadcast if no valid connection exists */
 		if ((_mode != MAVLINK_MODE_ONBOARD) && broadcast_enabled() &&
 		    (!get_client_source_initialized()
-		     || (hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000))) {
+		     || (hrt_elapsed_time(&_tstatus.heartbeat_time) > 3_s))) {
 
 			if (!_broadcast_address_found) {
 				find_broadcast_address();
@@ -1017,7 +1000,6 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
 
 		if (buf_free < packet_len) {
 			/* not enough space in buffer to send */
-			count_txerr();
 			count_txerrbytes(packet_len);
 			return;
 		}
@@ -1044,7 +1026,6 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
 #endif
 
 	if (ret != (size_t) packet_len) {
-		count_txerr();
 		count_txerrbytes(packet_len);
 
 	} else {
@@ -1247,10 +1228,6 @@ Mavlink::init_udp()
 void
 Mavlink::handle_message(const mavlink_message_t *msg)
 {
-	if (!accepting_commands()) {
-		return;
-	}
-
 	/*
 	 *  NOTE: this is called from the receiver thread
 	 */
@@ -1378,27 +1355,20 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, int
 	return sub_new;
 }
 
-int
-Mavlink::interval_from_rate(float rate)
-{
-	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)
 {
 	PX4_DEBUG("configure_stream(%s, %.3f)", stream_name, (double)rate);
 
 	/* calculate interval in us, -1 means unlimited stream, 0 means disabled */
-	int interval = interval_from_rate(rate);
+	int interval = 0;
+
+	if (rate > 0.000001f) {
+		interval = (1000000.0f / rate);
+
+	} else if (rate < 0.0f) {
+		interval = -1;
+	}
 
 	/* search if stream exists */
 	MavlinkStream *stream;
@@ -1440,38 +1410,6 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
 	return PX4_ERROR;
 }
 
-void
-Mavlink::adjust_stream_rates(const float multiplier)
-{
-	/* do not allow to push us to zero */
-	if (multiplier < MAVLINK_MIN_MULTIPLIER) {
-		return;
-	}
-
-	/* search if stream exists */
-	MavlinkStream *stream;
-	LL_FOREACH(_streams, stream) {
-		/* set new interval */
-		int interval = stream->get_interval();
-
-		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);
-		}
-	}
-}
-
 void
 Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
 {
@@ -1631,12 +1569,6 @@ Mavlink::pass_message(const mavlink_message_t *msg)
 	}
 }
 
-float
-Mavlink::get_rate_mult()
-{
-	return _rate_mult;
-}
-
 MavlinkShell *
 Mavlink::get_shell()
 {
@@ -1700,57 +1632,52 @@ Mavlink::update_rate_mult()
 		bandwidth_mult = fminf(1.0f, bandwidth_mult);
 	}
 
-	/* check if we have radio feedback */
+	float hardware_mult = 1.0f;
-	struct telemetry_status_s &tstatus = get_rx_status();
-
-	bool radio_critical = false;
-	bool radio_found = false;
-
-	/* 2nd pass: Now check hardware limits */
-	if (tstatus.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) {
-
-		radio_found = true;
-
-		if (tstatus.txbuf < RADIO_BUFFER_LOW_PERCENTAGE) {
-			radio_critical = true;
-		}
-	}
-
-	float hardware_mult = _rate_mult;
 
 	/* scale down if we have a TX err rate suggesting link congestion */
-	if (_rate_txerr > 0.0f && !radio_critical) {
+	if (_tstatus.rate_txerr > 0.0f && !_radio_status_critical) {
-		hardware_mult = (_rate_tx) / (_rate_tx + _rate_txerr);
+		hardware_mult = (_tstatus.rate_tx) / (_tstatus.rate_tx + _tstatus.rate_txerr);
 
-	} else if (radio_found && tstatus.telem_time != _last_hw_rate_timestamp) {
+	} else if (_radio_status_available) {
-
+		hardware_mult *= _radio_status_mult;
-		if (tstatus.txbuf < RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE) {
-			/* this indicates link congestion, reduce rate by 20% */
-			hardware_mult *= 0.80f;
-
-		} else if (tstatus.txbuf < RADIO_BUFFER_LOW_PERCENTAGE) {
-			/* this indicates link congestion, reduce rate by 2.5% */
-			hardware_mult *= 0.975f;
-
-		} else if (tstatus.txbuf > RADIO_BUFFER_HALF_PERCENTAGE) {
-			/* this indicates spare bandwidth, increase by 2.5% */
-			hardware_mult *= 1.025f;
-			/* limit to a max multiplier of 1 */
-			hardware_mult = fminf(1.0f, hardware_mult);
-		}
-
-	} else if (!radio_found) {
-		/* no limitation, set hardware to 1 */
-		hardware_mult = 1.0f;
 	}
 
-	_last_hw_rate_timestamp = tstatus.telem_time;
-
 	/* 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 = fmaxf(0.05f, _rate_mult);
+	_rate_mult = math::constrain(_rate_mult, 0.05f, 1.0f);
+}
+
+void
+Mavlink::update_radio_status(const radio_status_s &radio_status)
+{
+	_rstatus = radio_status;
+
+	/* check hardware limits */
+	if (radio_status.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) {
+
+		_radio_status_available = true;
+		_radio_status_critical = (radio_status.txbuf < RADIO_BUFFER_LOW_PERCENTAGE);
+
+		if (radio_status.txbuf < RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE) {
+			/* this indicates link congestion, reduce rate by 20% */
+			_radio_status_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;
+
+		} else if (radio_status.txbuf > RADIO_BUFFER_HALF_PERCENTAGE) {
+			/* this indicates spare bandwidth, increase by 2.5% */
+			_radio_status_mult *= 1.025f;
+		}
+
+	} else {
+		_radio_status_available = false;
+		_radio_status_critical = false;
+		_radio_status_mult = 1.0f;
+	}
 }
 
 int
@@ -2079,7 +2006,7 @@ Mavlink::task_main(int argc, char *argv[])
 
 			} else if (strcmp(myoptarg, "iridium") == 0) {
 				_mode = MAVLINK_MODE_IRIDIUM;
-				_rstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM;
+				_tstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM;
 
 			} else if (strcmp(myoptarg, "minimal") == 0) {
 				_mode = MAVLINK_MODE_MINIMAL;
@@ -2517,10 +2444,12 @@ 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;
+				const float dt = (t - _bytes_timestamp) / 1000.0f;
-				_rate_tx = _bytes_tx / dt;
+
-				_rate_txerr = _bytes_txerr / dt;
+				_tstatus.rate_tx = _bytes_tx / dt;
-				_rate_rx = _bytes_rx / dt;
+				_tstatus.rate_txerr = _bytes_txerr / dt;
+				_tstatus.rate_rx = _bytes_rx / dt;
+
 				_bytes_tx = 0;
 				_bytes_txerr = 0;
 				_bytes_rx = 0;
@@ -2529,6 +2458,11 @@ Mavlink::task_main(int argc, char *argv[])
 			_bytes_timestamp = t;
 		}
 
+		// publish status at 1 Hz, or sooner if HEARTBEAT has updated
+		if ((hrt_elapsed_time(&_tstatus.timestamp) >= 1_s) || (_tstatus.timestamp < _tstatus.heartbeat_time)) {
+			publish_telemetry_status();
+		}
+
 		perf_end(_loop_perf);
 
 		/* confirm task running only once fully initialized */
@@ -2590,10 +2524,37 @@ Mavlink::task_main(int argc, char *argv[])
 	return OK;
 }
 
+void Mavlink::publish_telemetry_status()
+{
+	// many fields are populated in place
+
+	_tstatus.mode = _mode;
+	_tstatus.data_rate = _datarate;
+	_tstatus.rate_multiplier = _rate_mult;
+	_tstatus.flow_control = get_flow_control_enabled();
+	_tstatus.ftp = ftp_enabled();
+	_tstatus.forwarding = get_forwarding_on();
+	_tstatus.mavlink_v2 = (_protocol_version == 2);
+
+	int num_streams = 0;
+
+	MavlinkStream *stream;
+	LL_FOREACH(_streams, stream) {
+		// count
+		num_streams++;
+	}
+
+	_tstatus.streams = num_streams;
+
+	_tstatus.timestamp = hrt_absolute_time();
+	int instance;
+	orb_publish_auto(ORB_ID(telemetry_status), &_telem_status_pub, &_tstatus, &instance, ORB_PRIO_DEFAULT);
+}
+
 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) {
+	if (_uart_fd >= 0 && _radio_id != 0 && _tstatus.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) {
 		/* request to configure radio and radio is present */
 		FILE *fs = fdopen(_uart_fd, "w");
 
@@ -2727,17 +2688,17 @@ void
 Mavlink::display_status()
 {
 
-	if (_rstatus.heartbeat_time > 0) {
+	if (_tstatus.heartbeat_time > 0) {
-		printf("\tGCS heartbeat:\t%llu us ago\n", (unsigned long long)hrt_elapsed_time(&_rstatus.heartbeat_time));
+		printf("\tGCS heartbeat:\t%llu us ago\n", (unsigned long long)hrt_elapsed_time(&_tstatus.heartbeat_time));
 	}
 
 	printf("\tmavlink chan: #%u\n", _channel);
 
-	if (_rstatus.timestamp > 0) {
+	if (_tstatus.timestamp > 0) {
 
 		printf("\ttype:\t\t");
 
-		switch (_rstatus.type) {
+		switch (_tstatus.type) {
 		case telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO:
 			printf("3DR RADIO\n");
 			printf("\trssi:\t\t%d\n", _rstatus.rssi);
@@ -2759,24 +2720,23 @@ Mavlink::display_status()
 		}
 
 	} else {
-		printf("\tno telem status.\n");
+		printf("\tno radio status.\n");
 	}
 
 	printf("\tflow control: %s\n", _flow_control_mode ? "ON" : "OFF");
 	printf("\trates:\n");
-	printf("\t  tx: %.3f kB/s\n", (double)_rate_tx);
+	printf("\t  tx: %.3f kB/s\n", (double)_tstatus.rate_tx);
-	printf("\t  txerr: %.3f kB/s\n", (double)_rate_txerr);
+	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  rx: %.3f kB/s\n", (double)_rate_rx);
+	printf("\t  rx: %.3f kB/s\n", (double)_tstatus.rate_rx);
 
 	if (_mavlink_ulog) {
 		printf("\tULog rate: %.1f%% of max %.1f%%\n", (double)_mavlink_ulog->current_data_rate() * 100.,
 		       (double)_mavlink_ulog->maximum_data_rate() * 100.);
 	}
 
-	printf("\taccepting commands: %s, FTP enabled: %s, TX enabled: %s\n",
+	printf("\tFTP enabled: %s, TX enabled: %s\n",
-	       accepting_commands() ? "YES" : "NO",
 	       _ftp_on ? "YES" : "NO",
 	       _transmitting_enabled ? "YES" : "NO");
 	printf("\tmode: %s\n", mavlink_mode_str(_mode));

src/modules/mavlink/mavlink_main.h

@@ -66,6 +66,7 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
+#include <uORB/topics/radio_status.h>
 #include <uORB/topics/telemetry_status.h>
 
 #include "mavlink_bridge_header.h"
@@ -81,6 +82,8 @@ enum Protocol {
 	TCP,
 };
 
+using namespace time_literals;
+
 #define HASH_PARAM "_HASH_CHECK"
 
 class Mavlink
@@ -233,13 +236,9 @@ public:
 
 	bool			get_forwarding_on() { return _forwarding_on; }
 
-	bool			get_config_link_on() { return _config_link_on; }
+	bool			is_connected() { return ((_tstatus.heartbeat_time > 0) && (hrt_absolute_time() - _tstatus.heartbeat_time < 3_s)); }
 
-	void			set_config_link_on(bool on) { _config_link_on = on; }
+	bool			broadcast_enabled() { return _broadcast_mode == BROADCAST_MODE_ON; }
-
-	bool			is_connected() { return ((_rstatus.heartbeat_time > 0) && (hrt_absolute_time() - _rstatus.heartbeat_time < 3000000)); }
-
-	bool			broadcast_enabled() { return _broadcast_mode > BROADCAST_MODE_OFF; }
 
 	/**
 	 * Set the boot complete flag on all instances
@@ -387,7 +386,7 @@ public:
 
 	MavlinkStream 		*get_streams() const { return _streams; }
 
-	float			get_rate_mult();
+	float			get_rate_mult() const { return _rate_mult; }
 
 	float			get_baudrate() { return _baudrate; }
 
@@ -403,11 +402,6 @@ public:
 	void			lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
 	void			unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); }
 
-	/**
-	 * Count a transmission error
-	 */
-	void			count_txerr();
-
 	/**
 	 * Count transmitted bytes
 	 */
@@ -426,7 +420,9 @@ public:
 	/**
 	 * Get the receive status of this MAVLink link
 	 */
-	struct telemetry_status_s	&get_rx_status() { return _rstatus; }
+	telemetry_status_s	&get_telemetry_status() { return _tstatus; }
+
+	void update_radio_status(const radio_status_s &radio_status);
 
 	ringbuffer::RingBuffer	*get_logbuffer() { return &_logbuffer; }
 
@@ -459,8 +455,6 @@ public:
 
 	bool			is_usb_uart() { return _is_usb_uart; }
 
-	bool			accepting_commands() { return true; /* non-trivial side effects ((!_config_link_on) || (_mode == MAVLINK_MODE_CONFIG));*/ }
-
 	int			get_data_rate()		{ return _datarate; }
 	void			set_data_rate(int rate) { if (rate > 0) { _datarate = rate; } }
 
@@ -514,7 +508,9 @@ private:
 	bool			_transmitting_enabled_commanded;
 	bool			_first_heartbeat_sent{false};
 
-	orb_advert_t		_mavlink_log_pub;
+	orb_advert_t		_mavlink_log_pub{nullptr};
+	orb_advert_t		_telem_status_pub{nullptr};
+
 	bool			_task_running;
 	static bool		_boot_complete;
 	static constexpr int MAVLINK_MAX_INSTANCES = 4;
@@ -558,9 +554,11 @@ private:
 
 	int			_baudrate;
 	int			_datarate;		///< data rate for normal streams (attitude, position, etc.)
-	int			_datarate_events;	///< data rate for params, waypoints, text messages
 	float			_rate_mult;
-	hrt_abstime		_last_hw_rate_timestamp;
+
+	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
@@ -586,9 +584,6 @@ private:
 	unsigned		_bytes_txerr;
 	unsigned		_bytes_rx;
 	uint64_t		_bytes_timestamp;
-	float			_rate_tx;
-	float			_rate_txerr;
-	float			_rate_rx;
 
 #ifdef __PX4_POSIX
 	struct sockaddr_in _myaddr;
@@ -609,9 +604,10 @@ private:
 	unsigned short _network_port;
 	unsigned short _remote_port;
 
-	struct telemetry_status_s	_rstatus;			///< receive status
+	radio_status_s		_rstatus{};
+	telemetry_status_s	_tstatus{};
 
-	struct ping_statistics_s	_ping_stats;		///< ping statistics
+	ping_statistics_s	_ping_stats{};
 
 	struct mavlink_message_buffer {
 		int write_ptr;
@@ -638,17 +634,13 @@ private:
 	param_t			_param_broadcast;
 
 	unsigned		_system_type;
-	static bool		_config_link_on;
 
 	perf_counter_t		_loop_perf;			/**< loop performance counter */
-	perf_counter_t		_txerr_perf;			/**< TX error counter */
 
 	void			mavlink_update_system();
 
 	int			mavlink_open_uart(int baudrate, const char *uart_name, bool force_flow_control);
 
-	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;
@@ -669,13 +661,6 @@ private:
 	 */
 	int configure_streams_to_default(const char *configure_single_stream = nullptr);
 
-	/**
-	 * Adjust the stream rates based on the current rate
-	 *
-	 * @param multiplier if greater than 1, the transmission rate will increase, if smaller than one decrease
-	 */
-	void adjust_stream_rates(const float multiplier);
-
 	int message_buffer_init(int size);
 
 	void message_buffer_destroy();
@@ -690,6 +675,8 @@ private:
 
 	void pass_message(const mavlink_message_t *msg);
 
+	void publish_telemetry_status();
+
 	/**
 	 * Check the configuration of a connected radio
 	 *

src/modules/mavlink/mavlink_receiver.cpp

@@ -89,6 +89,7 @@
 
 #include <commander/px4_custom_mode.h>
 
+#include <uORB/topics/radio_status.h>
 #include <uORB/topics/vehicle_command_ack.h>
 
 #include "mavlink_bridge_header.h"
@@ -132,7 +133,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_att_pos_mocap_pub(nullptr),
 	_vision_position_pub(nullptr),
 	_vision_attitude_pub(nullptr),
-	_telemetry_status_pub(nullptr),
+	_radio_status_pub(nullptr),
 	_ping_pub(nullptr),
 	_rc_pub(nullptr),
 	_manual_pub(nullptr),
@@ -193,25 +194,13 @@ void MavlinkReceiver::acknowledge(uint8_t sysid, uint8_t compid, uint16_t comman
 void
 MavlinkReceiver::handle_message(mavlink_message_t *msg)
 {
-	if (!_mavlink->get_config_link_on()) {
-		if (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_CONFIG) {
-			_mavlink->set_config_link_on(true);
-		}
-	}
-
 	switch (msg->msgid) {
 	case MAVLINK_MSG_ID_COMMAND_LONG:
-		if (_mavlink->accepting_commands()) {
+		handle_message_command_long(msg);
-			handle_message_command_long(msg);
-		}
-
 		break;
 
 	case MAVLINK_MSG_ID_COMMAND_INT:
-		if (_mavlink->accepting_commands()) {
+		handle_message_command_int(msg);
-			handle_message_command_int(msg);
-		}
-
 		break;
 
 	case MAVLINK_MSG_ID_COMMAND_ACK:
@@ -227,10 +216,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		break;
 
 	case MAVLINK_MSG_ID_SET_MODE:
-		if (_mavlink->accepting_commands()) {
+		handle_message_set_mode(msg);
-			handle_message_set_mode(msg);
-		}
-
 		break;
 
 	case MAVLINK_MSG_ID_ATT_POS_MOCAP:
@@ -1426,29 +1412,21 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 		mavlink_radio_status_t rstatus;
 		mavlink_msg_radio_status_decode(msg, &rstatus);
 
-		struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
+		radio_status_s status = {};
+		status.timestamp = hrt_absolute_time();
+		status.type = radio_status_s::RADIO_TYPE_3DR_RADIO;
+		status.rssi = rstatus.rssi;
+		status.remote_rssi = rstatus.remrssi;
+		status.txbuf = rstatus.txbuf;
+		status.noise = rstatus.noise;
+		status.remote_noise = rstatus.remnoise;
+		status.rxerrors = rstatus.rxerrors;
+		status.fixed = rstatus.fixed;
 
-		tstatus.timestamp = hrt_absolute_time();
+		_mavlink->update_radio_status(status);
-		tstatus.telem_time = tstatus.timestamp;
-		/* tstatus.heartbeat_time is set by system heartbeats */
-		tstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
-		tstatus.rssi = rstatus.rssi;
-		tstatus.remote_rssi = rstatus.remrssi;
-		tstatus.txbuf = rstatus.txbuf;
-		tstatus.noise = rstatus.noise;
-		tstatus.remote_noise = rstatus.remnoise;
-		tstatus.rxerrors = rstatus.rxerrors;
-		tstatus.fixed = rstatus.fixed;
-		tstatus.system_id = msg->sysid;
-		tstatus.component_id = msg->compid;
 
-		if (_telemetry_status_pub == nullptr) {
+		int multi_instance;
-			int multi_instance;
+		orb_publish_auto(ORB_ID(radio_status), &_radio_status_pub, &status, &multi_instance, ORB_PRIO_HIGH);
-			_telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, &multi_instance, ORB_PRIO_HIGH);
-
-		} else {
-			orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
-		}
 	}
 }
 
@@ -1895,21 +1873,14 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
 		/* ignore own heartbeats, accept only heartbeats from GCS */
 		if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) {
 
-			struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
+			telemetry_status_s &tstatus = _mavlink->get_telemetry_status();
 
 			/* set heartbeat time and topic time and publish -
 			 * the telem status also gets updated on telemetry events
 			 */
-			tstatus.timestamp = hrt_absolute_time();
 			tstatus.heartbeat_time = tstatus.timestamp;
-
+			tstatus.system_id = msg->sysid;
-			if (_telemetry_status_pub == nullptr) {
+			tstatus.component_id = msg->compid;
-				int multi_instance;
-				_telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, &multi_instance, ORB_PRIO_HIGH);
-
-			} else {
-				orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
-			}
 		}
 	}
 }
@@ -2538,22 +2509,6 @@ MavlinkReceiver::receive_thread(void *arg)
 	// poll timeout in ms. Also defines the max update frequency of the mission & param manager, etc.
 	const int timeout = 10;
 
-	// publish the telemetry status once for the iridium telemetry
-	if (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_IRIDIUM) {
-		struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
-
-		tstatus.timestamp = hrt_absolute_time();
-		tstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM;
-
-		if (_telemetry_status_pub == nullptr) {
-			int multi_instance;
-			_telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, &multi_instance, ORB_PRIO_HIGH);
-
-		} else {
-			orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
-		}
-	}
-
 #ifdef __PX4_POSIX
 	/* 1500 is the Wifi MTU, so we make sure to fit a full packet */
 	uint8_t buf[1600 * 5];

src/modules/mavlink/mavlink_receiver.h

@@ -229,7 +229,7 @@ private:
 	orb_advert_t _att_pos_mocap_pub;
 	orb_advert_t _vision_position_pub;
 	orb_advert_t _vision_attitude_pub;
-	orb_advert_t _telemetry_status_pub;
+	orb_advert_t _radio_status_pub;
 	orb_advert_t _ping_pub;
 	orb_advert_t _rc_pub;
 	orb_advert_t _manual_pub;