fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-15 19:31:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
commit
This commit is contained in:
Lorenz Meier
2016-08-28 10:08:15 +02:00
parent 6b08e8ce1f
commit 6f19531ce4
4 changed files with 27 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+13 -18
View File
@@ -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;