dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

enable receiving data over network port

This commit is contained in:
tumbili 2015-06-19 14:28:27 +02:00
parent a652642286
commit c4d92ff05b
2 changed files with 39 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/mavlink/mavlink_main.h
View File

@ -45,9 +45,9 @@
#ifdef __PX4_NUTTX
#include <nuttx/fs/fs.h>
#else
#include <drivers/device/device.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <drivers/device/device.h>
#endif
#include <systemlib/param/param.h>
#include <systemlib/perf_counter.h>

55
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -1624,34 +1624,56 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
void *
MavlinkReceiver::receive_thread(void *arg)
{
#ifndef __PX4_POSIX
int uart_fd = _mavlink->get_uart_fd();
const int timeout = 500;
uint8_t buf[32];
mavlink_message_t msg;
/* set thread name */
char thread_name[24];
sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id());
prctl(PR_SET_NAME, thread_name, getpid());
struct pollfd fds[1];
fds[0].fd = uart_fd;
fds[0].events = POLLIN;
int uart_fd = -1;
if (_mavlink->get_protocol() == SERIAL) {
uart_fd = _mavlink->get_uart_fd();
#ifndef __PX4_POSIX
/* set thread name */
char thread_name[24];
sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id());
prctl(PR_SET_NAME, thread_name, getpid());
#endif
fds[0].fd = uart_fd;
fds[0].events = POLLIN;
}
#ifdef __PX4_POSIX
struct sockaddr_in srcaddr;
socklen_t addrlen = sizeof(srcaddr);
if (_mavlink->get_protocol() == UDP || _mavlink->get_protocol() == TCP) {
fds[0].fd = _mavlink->get_socket_fd();
fds[0].events = POLLIN;
}
#endif
ssize_t nread = 0;
while (!_mavlink->_task_should_exit) {
if (poll(fds, 1, timeout) > 0) {
/* non-blocking read. read may return negative values */
if ((nread = ::read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) {
/* to avoid reading very small chunks wait for data before reading */
usleep(1000);
if (poll(&fds[0], 1, timeout) > 0) {
if (_mavlink->get_protocol() == SERIAL) {
/* non-blocking read. read may return negative values */
if ((nread = ::read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) {
/* to avoid reading very small chunks wait for data before reading */
usleep(1000);
}
}
#ifdef __PX4_POSIX
if (_mavlink->get_protocol() == UDP) {
if (fds[0].revents & POLLIN) {
nread = recvfrom(_mavlink->get_socket_fd(), buf, sizeof(buf), 0, (struct sockaddr *)&srcaddr, &addrlen);
}
} else {
// could be TCP or other protocol
}
#endif
/* if read failed, this loop won't execute */
for (ssize_t i = 0; i < nread; i++) {
if (mavlink_parse_char(_mavlink->get_channel(), buf[i], &msg, &status)) {
@ -1667,7 +1689,6 @@ MavlinkReceiver::receive_thread(void *arg)
_mavlink->count_rxbytes(nread);
}
}
#endif
return NULL;
}