Merge remote-tracking branch 'upstream/master' into obcfailsafe

NuttX

@@ -1 +1 @@
-Subproject commit 7e1b97bcf10d8495169eec355988ca5890bfd5df
+Subproject commit 088146b90eee5b614ea6386a64dae343a49a5172

makefiles/config_px4fmu-v2_default.mk

@@ -74,7 +74,7 @@ MODULES		+= modules/commander
 MODULES		+= modules/navigator
 MODULES		+= modules/mavlink
 MODULES		+= modules/gpio_led
-#MODULES		+= modules/uavcan
+MODULES		+= modules/uavcan
 
 #
 # Estimation modules (EKF/ SO3 / other filters)

src/drivers/airspeed/module.mk

@@ -36,3 +36,5 @@
 #
 
 SRCS			= airspeed.cpp
+
+MAXOPTIMIZATION	 = -Os

src/drivers/bma180/module.mk

@@ -38,3 +38,5 @@
 MODULE_COMMAND	= bma180
 
 SRCS		= bma180.cpp
+
+MAXOPTIMIZATION	 = -Os

src/drivers/ets_airspeed/module.mk

@@ -40,3 +40,5 @@ MODULE_COMMAND		= ets_airspeed
 SRCS			= ets_airspeed.cpp
 
 MODULE_STACKSIZE	= 1200
+
+MAXOPTIMIZATION	 = -Os

src/drivers/hil/module.mk

@@ -38,3 +38,5 @@
 MODULE_COMMAND	= hil
 
 SRCS		= hil.cpp
+
+MAXOPTIMIZATION	 = -Os

src/drivers/led/module.mk

@@ -36,3 +36,5 @@
 #
 
 SRCS		= led.cpp
+
+MAXOPTIMIZATION	 = -Os

src/drivers/md25/module.mk

@@ -40,3 +40,5 @@ MODULE_COMMAND		= md25
 
 SRCS			= md25.cpp \
 			  md25_main.cpp
+
+MAXOPTIMIZATION	 = -Os

src/drivers/meas_airspeed/module.mk

@@ -42,3 +42,5 @@ SRCS			= meas_airspeed.cpp
 MODULE_STACKSIZE	= 1200
 
 EXTRACXXFLAGS	= -Weffc++
+
+MAXOPTIMIZATION	 = -Os

src/drivers/ms5611/module.mk

@@ -38,3 +38,5 @@
 MODULE_COMMAND	= ms5611
 
 SRCS		= ms5611.cpp ms5611_spi.cpp ms5611_i2c.cpp
+
+MAXOPTIMIZATION	 = -Os

src/drivers/px4flow/module.mk

@@ -38,3 +38,5 @@
 MODULE_COMMAND	= px4flow
 
 SRCS		= px4flow.cpp
+
+MAXOPTIMIZATION	 = -Os

src/drivers/roboclaw/module.mk

@@ -39,3 +39,5 @@ MODULE_COMMAND		= roboclaw
 
 SRCS			= roboclaw_main.cpp \
 			  RoboClaw.cpp
+
+MAXOPTIMIZATION	 = -Os

src/modules/mavlink/mavlink_main.cpp

@@ -154,6 +154,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
 		instance = Mavlink::get_instance(6);
 		break;
 #endif
+
 	default:
 		return;
 	}
@@ -178,8 +179,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
 		if (last_write_try_times[(unsigned)channel] != 0 &&
 		    hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL &&
 		    last_write_success_times[(unsigned)channel] !=
-			last_write_try_times[(unsigned)channel])
-		{
+		    last_write_try_times[(unsigned)channel]) {
 			warnx("DISABLING HARDWARE FLOW CONTROL");
 			instance->enable_flow_control(false);
 		}
@@ -197,15 +197,20 @@ 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;
 			}
 		}
 
 		ssize_t ret = write(uart, ch, desired);
+
 		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);
 		}
 	}
 }
@@ -247,6 +252,14 @@ 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 {},
 	   _param_initialized(false),
@@ -424,6 +437,27 @@ Mavlink::destroy_all_instances()
 	return OK;
 }
 
+int
+Mavlink::get_status_all_instances()
+{
+	Mavlink *inst = ::_mavlink_instances;
+
+	unsigned iterations = 0;
+
+	while (inst != nullptr) {
+
+		printf("\ninstance #%u:\n", iterations);
+		inst->display_status();
+
+		/* move on */
+		inst = inst->next;
+		iterations++;
+	}
+
+	/* return an error if there are no instances */
+	return (iterations == 0);
+}
+
 bool
 Mavlink::instance_exists(const char *device_name, Mavlink *self)
 {
@@ -615,7 +649,8 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 	case 921600: speed = B921600; break;
 
 	default:
-		warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n", baud);
+		warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n",
+		      baud);
 		return -EINVAL;
 	}
 
@@ -845,6 +880,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
 	case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
 			mavlink_param_request_list_t req;
 			mavlink_msg_param_request_list_decode(msg, &req);
+
 			if (req.target_system == mavlink_system.sysid &&
 			    (req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) {
 				/* Start sending parameters */
@@ -861,7 +897,9 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
 				mavlink_param_set_t mavlink_param_set;
 				mavlink_msg_param_set_decode(msg, &mavlink_param_set);
 
-				if (mavlink_param_set.target_system == mavlink_system.sysid && ((mavlink_param_set.target_component == mavlink_system.compid) || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) {
+				if (mavlink_param_set.target_system == mavlink_system.sysid
+				    && ((mavlink_param_set.target_component == mavlink_system.compid)
+					|| (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) {
 					/* local name buffer to enforce null-terminated string */
 					char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
 					strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
@@ -888,7 +926,9 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
 			mavlink_param_request_read_t mavlink_param_request_read;
 			mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read);
 
-			if (mavlink_param_request_read.target_system == mavlink_system.sysid && ((mavlink_param_request_read.target_component == mavlink_system.compid) || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) {
+			if (mavlink_param_request_read.target_system == mavlink_system.sysid
+			    && ((mavlink_param_request_read.target_component == mavlink_system.compid)
+				|| (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) {
 				/* when no index is given, loop through string ids and compare them */
 				if (mavlink_param_request_read.param_index == -1) {
 					/* local name buffer to enforce null-terminated string */
@@ -954,9 +994,11 @@ Mavlink::send_statustext(unsigned severity, const char *string)
 		case MAVLINK_IOC_SEND_TEXT_INFO:
 			statustext.severity = MAV_SEVERITY_INFO;
 			break;
+
 		case MAVLINK_IOC_SEND_TEXT_CRITICAL:
 			statustext.severity = MAV_SEVERITY_CRITICAL;
 			break;
+
 		case MAVLINK_IOC_SEND_TEXT_EMERGENCY:
 			statustext.severity = MAV_SEVERITY_EMERGENCY;
 			break;
@@ -1079,9 +1121,11 @@ Mavlink::message_buffer_init(int size)
 	_message_buffer.data = (char *)malloc(_message_buffer.size);
 
 	int ret;
+
 	if (_message_buffer.data == 0) {
 		ret = ERROR;
 		_message_buffer.size = 0;
+
 	} else {
 		ret = OK;
 	}
@@ -1395,7 +1439,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
 		configure_stream("HIGHRES_IMU", 1.0f * rate_mult);
 		configure_stream("ATTITUDE", 10.0f * rate_mult);
-		configure_stream("VFR_HUD", 10.0f * rate_mult);
+		configure_stream("VFR_HUD", 8.0f * rate_mult);
 		configure_stream("GPS_RAW_INT", 1.0f * rate_mult);
 		configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult);
 		configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult);
@@ -1453,7 +1497,8 @@ Mavlink::task_main(int argc, char *argv[])
 		if (_subscribe_to_stream != nullptr) {
 			if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {
 				if (_subscribe_to_stream_rate > 0.0f) {
-					warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, (double)_subscribe_to_stream_rate);
+					warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name,
+					      (double)_subscribe_to_stream_rate);
 
 				} else {
 					warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name);
@@ -1506,6 +1551,7 @@ Mavlink::task_main(int argc, char *argv[])
 
 				// Pull a single message from the buffer
 				size_t read_count = available;
+
 				if (read_count > sizeof(mavlink_message_t)) {
 					read_count = sizeof(mavlink_message_t);
 				}
@@ -1534,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);
 	}
 
@@ -1584,6 +1644,7 @@ Mavlink::task_main(int argc, char *argv[])
 		message_buffer_destroy();
 		pthread_mutex_destroy(&_message_buffer_mutex);
 	}
+
 	/* destroy log buffer */
 	mavlink_logbuffer_destroy(&_logbuffer);
 
@@ -1605,6 +1666,7 @@ int Mavlink::start_helper(int argc, char *argv[])
 		/* out of memory */
 		res = -ENOMEM;
 		warnx("OUT OF MEM");
+
 	} else {
 		/* this will actually only return once MAVLink exits */
 		res = instance->task_main(argc, argv);
@@ -1664,7 +1726,40 @@ Mavlink::start(int argc, char *argv[])
 void
 Mavlink::display_status()
 {
-	warnx("running");
+
+	if (_rstatus.heartbeat_time > 0) {
+		printf("\tGCS heartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time));
+	}
+
+	if (_rstatus.timestamp > 0) {
+
+		printf("\ttype:\t\t");
+
+		switch (_rstatus.type) {
+		case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO:
+			printf("3DR RADIO\n");
+			break;
+
+		default:
+			printf("UNKNOWN RADIO\n");
+			break;
+		}
+
+		printf("\trssi:\t\t%d\n", _rstatus.rssi);
+		printf("\tremote rssi:\t%u\n", _rstatus.remote_rssi);
+		printf("\ttxbuf:\t\t%u\n", _rstatus.txbuf);
+		printf("\tnoise:\t\t%d\n", _rstatus.noise);
+		printf("\tremote noise:\t%u\n", _rstatus.remote_noise);
+		printf("\trx errors:\t%u\n", _rstatus.rxerrors);
+		printf("\tfixed:\t\t%u\n", _rstatus.fixed);
+
+	} 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
@@ -1752,8 +1847,8 @@ int mavlink_main(int argc, char *argv[])
 	} else if (!strcmp(argv[1], "stop-all")) {
 		return Mavlink::destroy_all_instances();
 
-		// } else if (!strcmp(argv[1], "status")) {
-		// 	mavlink::g_mavlink->status();
+	} else if (!strcmp(argv[1], "status")) {
+		return Mavlink::get_status_all_instances();
 
 	} else if (!strcmp(argv[1], "stream")) {
 		return Mavlink::stream_command(argc, argv);

src/modules/mavlink/mavlink_main.h

@@ -51,6 +51,7 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
+#include <uORB/topics/telemetry_status.h>
 
 #include "mavlink_bridge_header.h"
 #include "mavlink_orb_subscription.h"
@@ -97,6 +98,8 @@ public:
 
 	static int		destroy_all_instances();
 
+	static int		get_status_all_instances();
+
 	static bool		instance_exists(const char *device_name, Mavlink *self);
 
 	static void		forward_message(const mavlink_message_t *msg, Mavlink *self);
@@ -229,6 +232,26 @@ 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
+	 */
+	struct telemetry_status_s&	get_rx_status() { return _rstatus; }
+
 protected:
 	Mavlink			*next;
 
@@ -285,6 +308,16 @@ 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 {
 		int write_ptr;
 		int read_ptr;

src/modules/mavlink/mavlink_receiver.cpp

@@ -111,7 +111,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_telemetry_status_pub(-1),
 	_rc_pub(-1),
 	_manual_pub(-1),
-	_telemetry_heartbeat_time(0),
 	_radio_status_available(false),
 	_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
 	_hil_frames(0),
@@ -400,11 +399,11 @@ 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;
-		memset(&tstatus, 0, sizeof(tstatus));
+		struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
 
 		tstatus.timestamp = hrt_absolute_time();
-		tstatus.heartbeat_time = _telemetry_heartbeat_time;
+		tstatus.telem_time = tstatus.timestamp;
+		/* tstatus.heartbeat_time is set by system heartbeats */
 		tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
 		tstatus.rssi = rstatus.rssi;
 		tstatus.remote_rssi = rstatus.remrssi;
@@ -461,16 +460,20 @@ 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) {
-			_telemetry_heartbeat_time = hrt_absolute_time();
+
+			struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
+
+			hrt_abstime tnow = hrt_absolute_time();
+
+			/* always set heartbeat, publish only if telemetry link not up */
+			tstatus.heartbeat_time = tnow;
 
 			/* if no radio status messages arrive, lets at least publish that heartbeats were received */
 			if (!_radio_status_available) {
 
-				struct telemetry_status_s tstatus;
-				memset(&tstatus, 0, sizeof(tstatus));
-
-				tstatus.timestamp = _telemetry_heartbeat_time;
-				tstatus.heartbeat_time = _telemetry_heartbeat_time;
+				tstatus.timestamp = tnow;
+				/* telem_time indicates the timestamp of a telemetry status packet and we got none */
+				tstatus.telem_time = 0;
 				tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
 
 				if (_telemetry_status_pub < 0) {
@@ -955,6 +958,9 @@ MavlinkReceiver::receive_thread(void *arg)
 					_mavlink->handle_message(&msg);
 				}
 			}
+
+			/* count received bytes */
+			_mavlink->count_rxbytes(nread);
 		}
 	}
 

src/modules/mavlink/mavlink_receiver.h

@@ -148,7 +148,6 @@ private:
 	orb_advert_t _telemetry_status_pub;
 	orb_advert_t _rc_pub;
 	orb_advert_t _manual_pub;
-	hrt_abstime _telemetry_heartbeat_time;
 	bool _radio_status_available;
 	int _control_mode_sub;
 	int _hil_frames;

src/modules/uORB/topics/telemetry_status.h

@@ -58,6 +58,7 @@ enum TELEMETRY_STATUS_RADIO_TYPE {
 struct telemetry_status_s {
 	uint64_t timestamp;
 	uint64_t heartbeat_time;		/**< Time of last received heartbeat from remote system */
+	uint64_t telem_time;			/**< Time of last received telemetry status packet, 0 for none */
 	enum TELEMETRY_STATUS_RADIO_TYPE type;	/**< type of the radio hardware     */
 	uint8_t rssi;				/**< local signal strength                      */
 	uint8_t remote_rssi;			/**< remote signal strength                     */