MAVLink: Cleanup of port handling, switch to MAVLink 2 if receiving MAVLink 2. Announce MAVLink 2 capability in autopilot_version message

commit

src/modules/mavlink/mavlink_main.cpp

@@ -216,6 +216,7 @@ Mavlink::Mavlink() :
 	_last_write_success_time(0),
 	_last_write_try_time(0),
 	_mavlink_start_time(0),
+	_protocol_version_switch(0),
 	_protocol_version(0),
 	_bytes_tx(0),
 	_bytes_txerr(0),
@@ -335,13 +336,14 @@ Mavlink::~Mavlink()
 void
 Mavlink::set_proto_version(unsigned version)
 {
-	_protocol_version = version;
-
 	if (version == 1 || ((version == 0) && !_received_messages)) {
 		get_status()->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
+		_protocol_version = 1;
 
-	} else if (version == 2) {
+	} else if (version == 2 &&
+		   ((_protocol_version_switch == 0) || (_protocol_version_switch == 2))) {
 		get_status()->flags &= ~(MAVLINK_STATUS_FLAG_OUT_MAVLINK1);
+		_protocol_version = 2;
 	}
 }
 
@@ -564,9 +566,13 @@ void Mavlink::mavlink_update_system(void)
 	int32_t component_id;
 	param_get(_param_component_id, &component_id);
 
-	int32_t proto;
+	int32_t proto = 0;
 	param_get(_param_proto_ver, &proto);
-	set_proto_version(proto);
+
+	if (_protocol_version_switch != proto) {
+		_protocol_version_switch = proto;
+		set_proto_version(proto);
+	}
 
 	param_get(_param_radio_id, &_radio_id);
 
@@ -1249,6 +1255,7 @@ void Mavlink::send_autopilot_capabilites()
 		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET;
 		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED;
 		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_ACTUATOR_TARGET;
+		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_MAVLINK2;
 		msg.flight_sw_version = version_tag_to_number(px4_git_tag);
 		msg.middleware_sw_version = version_tag_to_number(px4_git_tag);
 		msg.os_sw_version = version_tag_to_number(os_git_tag);

src/modules/mavlink/mavlink_main.h

@@ -507,6 +507,7 @@ private:
 	uint64_t		_last_write_success_time;
 	uint64_t		_last_write_try_time;
 	uint64_t		_mavlink_start_time;
+	int32_t			_protocol_version_switch;
 	int32_t			_protocol_version;
 
 	unsigned		_bytes_tx;

src/modules/mavlink/mavlink_params.c

@@ -54,7 +54,7 @@ PARAM_DEFINE_INT32(MAV_COMP_ID, 1);
  * @value 1 Always use version 1
  * @value 2 Always use version 2
  */
-PARAM_DEFINE_INT32(MAV_PROTO_VER, 1);
+PARAM_DEFINE_INT32(MAV_PROTO_VER, 0);
 
 /**
  * MAVLink Radio ID

src/modules/mavlink/mavlink_receiver.cpp

@@ -2074,9 +2074,11 @@ MavlinkReceiver::receive_thread(void *arg)
 {
 
 	/* set thread name */
-	char thread_name[24];
-	sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id());
-	px4_prctl(PR_SET_NAME, thread_name, getpid());
+	{
+		char thread_name[24];
+		sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id());
+		px4_prctl(PR_SET_NAME, thread_name, getpid());
+	}
 
 	const int timeout = 500;
 #ifdef __PX4_POSIX
@@ -2088,14 +2090,10 @@ MavlinkReceiver::receive_thread(void *arg)
 #endif
 	mavlink_message_t msg;
 
-	struct pollfd fds[1];
-
-	int uart_fd = -1;
+	struct pollfd fds[1] = {};
 
 	if (_mavlink->get_protocol() == SERIAL) {
-		uart_fd = _mavlink->get_uart_fd();
-
-		fds[0].fd = uart_fd;
+		fds[0].fd = _mavlink->get_uart_fd();
 		fds[0].events = POLLIN;
 	}
 
@@ -2127,7 +2125,7 @@ MavlinkReceiver::receive_thread(void *arg)
 				const unsigned character_count = 20;
 
 				/* non-blocking read. read may return negative values */
-				if ((nread = ::read(uart_fd, buf, sizeof(buf))) < (ssize_t)character_count) {
+				if ((nread = ::read(fds[0].fd, buf, sizeof(buf))) < (ssize_t)character_count) {
 					unsigned sleeptime = (1.0f / (_mavlink->get_baudrate() / 10)) * character_count * 1000000;
 					usleep(sleeptime);
 				}
@@ -2172,8 +2170,11 @@ MavlinkReceiver::receive_thread(void *arg)
 				for (ssize_t i = 0; i < nread; i++) {
 					if (mavlink_parse_char(_mavlink->get_channel(), buf[i], &msg, &status)) {
 
-						/* check if we received version 2 */
-						// XXX todo _mavlink->set_proto_version(2);
+						/* check if we received version 2 and request a switch. */
+						if (!(_mavlink->get_status()->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1)) {
+							/* this will only switch to proto version 2 if allowed in settings */
+							_mavlink->set_proto_version(2);
+						}
 
 						/* handle generic messages and commands */
 						handle_message(&msg);
@@ -2240,12 +2241,6 @@ MavlinkReceiver::receive_start(pthread_t *thread, Mavlink *parent)
 	pthread_attr_t receiveloop_attr;
 	pthread_attr_init(&receiveloop_attr);
 
-#ifndef __PX4_POSIX
-	// set to non-blocking read
-	int flags = fcntl(parent->get_uart_fd(), F_GETFL, 0);
-	fcntl(parent->get_uart_fd(), F_SETFL, flags | O_NONBLOCK);
-#endif
-
 	struct sched_param param;
 	(void)pthread_attr_getschedparam(&receiveloop_attr, &param);
 	param.sched_priority = SCHED_PRIORITY_MAX - 80;