Nuttx: mavlink fixes

Needed to ifdef SITL functionality not supoprted in NuttX build.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois 2015-06-09 16:32:22 -07:00
parent 980061e508
commit 13dd993e01
2 changed files with 6 additions and 2 deletions

View File

@ -690,8 +690,10 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
_is_usb_uart = true; _is_usb_uart = true;
} }
#ifdef __PX4_LINUX
/* Put in raw mode */ /* Put in raw mode */
cfmakeraw(&uart_config); cfmakeraw(&uart_config);
#endif
if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) {
warnx("ERR SET CONF %s\n", uart_name); warnx("ERR SET CONF %s\n", uart_name);
@ -942,6 +944,7 @@ Mavlink::resend_message(mavlink_message_t *msg)
void void
Mavlink::init_udp() Mavlink::init_udp()
{ {
#ifdef __PX4_LINUX
PX4_INFO("Setting up UDP w/port %d\n",_network_port); PX4_INFO("Setting up UDP w/port %d\n",_network_port);
memset((char *)&_myaddr, 0, sizeof(_myaddr)); memset((char *)&_myaddr, 0, sizeof(_myaddr));
@ -964,6 +967,7 @@ Mavlink::init_udp()
// wait for client to connect to socket // wait for client to connect to socket
recvfrom(_socket_fd,inbuf,sizeof(inbuf),0,(struct sockaddr *)&_src_addr,&addrlen); recvfrom(_socket_fd,inbuf,sizeof(inbuf),0,(struct sockaddr *)&_src_addr,&addrlen);
#endif
} }
void void

View File

@ -398,13 +398,13 @@ private:
float _rate_rx; float _rate_rx;
#ifdef __PX4_POSIX #ifdef __PX4_POSIX
int _socket_fd;
struct sockaddr_in _myaddr; struct sockaddr_in _myaddr;
struct sockaddr_in _src_addr; struct sockaddr_in _src_addr;
#endif
int _socket_fd;
Protocol _protocol; Protocol _protocol;
unsigned short _network_port; unsigned short _network_port;
#endif
struct telemetry_status_s _rstatus; ///< receive status struct telemetry_status_s _rstatus; ///< receive status