Browse Source

Merge branch 'linux' of http://github.com/mcharleb/Firmware into linux

sbg
Mark Charlebois 10 years ago
parent
commit
41a2d30cfe
  1. 20
      src/modules/mavlink/mavlink_main.cpp
  2. 6
      src/modules/mavlink/mavlink_receiver.cpp

20
src/modules/mavlink/mavlink_main.cpp

@ -51,7 +51,7 @@
#include <assert.h> #include <assert.h>
#include <math.h> #include <math.h>
#include <poll.h> #include <poll.h>
#ifndef __PX4_QURT #ifndef __PX4_POSIX
#include <termios.h> #include <termios.h>
#endif #endif
#include <time.h> #include <time.h>
@ -149,7 +149,7 @@ Mavlink::Mavlink() :
_forwarding_on(false), _forwarding_on(false),
_passing_on(false), _passing_on(false),
_ftp_on(false), _ftp_on(false),
#ifndef __PX4_QURT #ifndef __PX4_POSIX
_uart_fd(-1), _uart_fd(-1),
#endif #endif
_baudrate(57600), _baudrate(57600),
@ -412,7 +412,7 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self)
} }
} }
#ifndef __PX4_QURT #ifndef __PX4_POSIX
int int
Mavlink::get_uart_fd(unsigned index) Mavlink::get_uart_fd(unsigned index)
{ {
@ -430,7 +430,7 @@ Mavlink::get_uart_fd()
{ {
return _uart_fd; return _uart_fd;
} }
#endif // __PX4_QURT #endif // __PX4_POSIX
int int
Mavlink::get_instance_id() Mavlink::get_instance_id()
@ -572,7 +572,7 @@ int Mavlink::get_component_id()
return mavlink_system.compid; return mavlink_system.compid;
} }
#ifndef __PX4_QURT #ifndef __PX4_POSIX
int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
{ {
/* process baud rate */ /* process baud rate */
@ -758,7 +758,7 @@ Mavlink::get_free_tx_buf()
*/ */
int buf_free = 0; int buf_free = 0;
#ifndef __PX4_QURT #ifndef __PX4_POSIX
// No FIONWRITE on Linux // No FIONWRITE on Linux
#if !defined(__PX4_LINUX) #if !defined(__PX4_LINUX)
@ -832,7 +832,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF);
buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8);
#ifndef __PX4_QURT #ifndef __PX4_POSIX
/* send message to UART */ /* send message to UART */
ssize_t ret = ::write(_uart_fd, buf, packet_len); ssize_t ret = ::write(_uart_fd, buf, packet_len);
@ -884,7 +884,7 @@ Mavlink::resend_message(mavlink_message_t *msg)
buf[MAVLINK_NUM_HEADER_BYTES + msg->len] = (uint8_t)(msg->checksum & 0xFF); buf[MAVLINK_NUM_HEADER_BYTES + msg->len] = (uint8_t)(msg->checksum & 0xFF);
buf[MAVLINK_NUM_HEADER_BYTES + msg->len + 1] = (uint8_t)(msg->checksum >> 8); buf[MAVLINK_NUM_HEADER_BYTES + msg->len + 1] = (uint8_t)(msg->checksum >> 8);
#ifndef __PX4_QURT #ifndef __PX4_POSIX
/* send message to UART */ /* send message to UART */
ssize_t ret = ::write(_uart_fd, buf, packet_len); ssize_t ret = ::write(_uart_fd, buf, packet_len);
@ -1344,7 +1344,7 @@ Mavlink::task_main(int argc, char *argv[])
/* flush stdout in case MAVLink is about to take it over */ /* flush stdout in case MAVLink is about to take it over */
fflush(stdout); fflush(stdout);
#ifndef __PX4_QURT #ifndef __PX4_POSIX
struct termios uart_config_original; struct termios uart_config_original;
/* default values for arguments */ /* default values for arguments */
@ -1616,7 +1616,7 @@ Mavlink::task_main(int argc, char *argv[])
/* wait for threads to complete */ /* wait for threads to complete */
pthread_join(_receive_thread, NULL); pthread_join(_receive_thread, NULL);
#ifndef __PX4_QURT #ifndef __PX4_POSIX
/* reset the UART flags to original state */ /* reset the UART flags to original state */
tcsetattr(_uart_fd, TCSANOW, &uart_config_original); tcsetattr(_uart_fd, TCSANOW, &uart_config_original);

6
src/modules/mavlink/mavlink_receiver.cpp

@ -60,7 +60,7 @@
#include <time.h> #include <time.h>
#include <float.h> #include <float.h>
#include <unistd.h> #include <unistd.h>
#ifndef __PX4_QURT #ifndef __PX4_POSIX
#include <sys/prctl.h> #include <sys/prctl.h>
#include <termios.h> #include <termios.h>
#endif #endif
@ -1480,7 +1480,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
void * void *
MavlinkReceiver::receive_thread(void *arg) MavlinkReceiver::receive_thread(void *arg)
{ {
#ifndef __PX4_QURT #ifndef __PX4_POSIX
int uart_fd = _mavlink->get_uart_fd(); int uart_fd = _mavlink->get_uart_fd();
const int timeout = 500; const int timeout = 500;
@ -1570,7 +1570,7 @@ MavlinkReceiver::receive_start(Mavlink *parent)
pthread_attr_t receiveloop_attr; pthread_attr_t receiveloop_attr;
pthread_attr_init(&receiveloop_attr); pthread_attr_init(&receiveloop_attr);
#ifndef __PX4_QURT #ifndef __PX4_POSIX
// set to non-blocking read // set to non-blocking read
int flags = fcntl(parent->get_uart_fd(), F_GETFL, 0); int flags = fcntl(parent->get_uart_fd(), F_GETFL, 0);
fcntl(parent->get_uart_fd(), F_SETFL, flags | O_NONBLOCK); fcntl(parent->get_uart_fd(), F_SETFL, flags | O_NONBLOCK);

Loading…
Cancel
Save