From 6f19531ce447369abad465694c988610292bfa02 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Aug 2016 11:31:05 +0200 Subject: [PATCH] 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 | 17 +++++++++---- src/modules/mavlink/mavlink_main.h | 1 + src/modules/mavlink/mavlink_params.c | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 31 ++++++++++-------------- 4 files changed, 27 insertions(+), 24 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index d074423193..a7ccd4d255 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/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); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index cf4bb46765..c69286ac9c 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/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; diff --git a/src/modules/mavlink/mavlink_params.c b/src/modules/mavlink/mavlink_params.c index c988b9aa0f..cd02a967aa 100644 --- a/src/modules/mavlink/mavlink_params.c +++ b/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 diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 8de279da74..bed54c56da 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/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, ¶m); param.sched_priority = SCHED_PRIORITY_MAX - 80;