Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

HAL_Linux: UART-like TCP sockets impl. on AP_HAL_Linux

This commit is contained in:
Víctor Mayoral Vilches 2014-06-11 08:34:07 +02:00 committed by Andrew Tridgell
parent b0742fbb5b
commit be68de20f1
3 changed files with 224 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_HAL_Linux/HAL_Linux_Class.cpp
View File

@ -46,6 +46,16 @@ HAL_Linux::HAL_Linux() :
&utilInstance)
{}
void _usage(void)
{
printf("Usage: -A uartAPath -B uartBPath -C uartCPath\n");
printf("Options:\n");
printf("\t-serial: -A /dev/ttyO4\n");
printf("\t -B /dev/ttyS1\n");
printf("\t-tcp: -C tcp:192.168.2.15:1243:wait\n");
printf("\t -A tcp:11.0.0.2:5678\n");
}
void HAL_Linux::init(int argc,char* const argv[]) const
{
int opt;
@ -64,7 +74,7 @@ void HAL_Linux::init(int argc,char* const argv[]) const
uartCDriver.set_device_path(optarg);
break;
case 'h':
printf("Usage: -A uartAPath -B uartBPath -C uartCPath\n");
_usage();
exit(0);
default:
printf("Unknown option '%c'\n", (char)opt);

171
libraries/AP_HAL_Linux/UARTDriver.cpp
View File

@ -15,11 +15,21 @@
#include <poll.h>
#include <assert.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <string.h>
#include <arpa/inet.h>
extern const AP_HAL::HAL& hal;
using namespace Linux;
#define DEVICE_TCP 0
#define DEVICE_SERIAL 1
#define DEVICE_UNKNOWN 99
LinuxUARTDriver::LinuxUARTDriver(bool default_console) :
device_path(NULL),
_rd_fd(-1),
@ -35,7 +45,7 @@ LinuxUARTDriver::LinuxUARTDriver(bool default_console) :
/*
set the tty device to use for this UART
*/
void LinuxUARTDriver::set_device_path(const char *path)
void LinuxUARTDriver::set_device_path(char *path)
{
device_path = path;
}
@ -61,6 +71,32 @@ void LinuxUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
if (device_path == NULL) {
return;
}
switch (_parseDevicePath(device_path)){
case DEVICE_TCP:
{
_connected = false;
if (_flag != NULL){
_tcp_start_connection(true);
} else {
_tcp_start_connection(false);
}
if (_connected) {
if (rxS < 1024) {
rxS = 1024;
}
if (txS < 1024) {
txS = 1024;
}
} else {
printf("LinuxUARTDriver TCP connection not stablished\n");
exit(1);
}
break;
}
case DEVICE_SERIAL:
{
uint8_t retries = 0;
while (retries < 5) {
_rd_fd = open(device_path, O_RDWR);
@ -99,6 +135,22 @@ void LinuxUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
if (txS < 1024) {
txS = 1024;
}
break;
}
default:
{
// Notify that the option is not valid and select standart input and output
printf("LinuxUARTDriver parsing failed, using default\n");
_rd_fd = 0;
_wr_fd = 1;
rxS = 512;
txS = 512;
fcntl(_rd_fd, F_SETFL, fcntl(_rd_fd, F_GETFL, 0) | O_NONBLOCK);
fcntl(_wr_fd, F_SETFL, fcntl(_wr_fd, F_GETFL, 0) | O_NONBLOCK);
break;
}
}
}
_initialised = false;
@ -145,12 +197,129 @@ void LinuxUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
}
}
/*
Device path accepts the following syntaxes:
- /dev/ttyO1
- tcp:192.168.2.15:1243:wait
*/
int LinuxUARTDriver::_parseDevicePath(char* arg)
{
const char *serial_string = "/dev/tty";
const char *tcp_string = "tcp";
_flag = NULL; // init flag
if(strstr(arg, tcp_string) != NULL){
// Parse the TCP string
char *protocol, *ip, *port, *flag;
protocol = strtok ( arg, ":" );
ip = strtok ( NULL, ":" );
port = strtok ( NULL, ":" );
flag = strtok ( NULL, ":" );
_base_port = (uint16_t) atoi(port);
_ip = ip;
_flag = flag;
return DEVICE_TCP;
} else if (strstr(arg, serial_string) != NULL){
return DEVICE_SERIAL;
} else {
return DEVICE_UNKNOWN;
}
}
/*
start a TCP connection for the serial port. If wait_for_connection
is true then block until a client connects
*/
void LinuxUARTDriver::_tcp_start_connection(bool wait_for_connection)
{
int one=1;
struct sockaddr_in sockaddr;
int ret;
int listen_fd = -1; // socket we are listening on
int net_fd = -1; // network file descriptor, will be linked to wr_fd and rd_fd
uint8_t portNumber = 1;
// if (_console) {
// // hack for console access
// connected = true;
// listen_fd = -1;
// fd = 1;
// return;
// }
if (net_fd != -1) {
close(net_fd);
}
if (listen_fd == -1) {
memset(&sockaddr,0,sizeof(sockaddr));
#ifdef HAVE_SOCK_SIN_LEN
sockaddr.sin_len = sizeof(sockaddr);
#endif
sockaddr.sin_port = htons(_base_port + portNumber);
sockaddr.sin_family = AF_INET;
// sockaddr.sin_addr.s_addr = inet_addr(_base_ip);
// Bind to all interfaces
sockaddr.sin_addr.s_addr = htonl(INADDR_ANY);
listen_fd = socket(AF_INET, SOCK_STREAM, 0);
if (listen_fd == -1) {
printf("socket failed - %s\n", strerror(errno));
exit(1);
}
/* we want to be able to re-use ports quickly */
setsockopt(listen_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
printf("bind port %u for %u\n",
(unsigned)ntohs(sockaddr.sin_port),
(unsigned)portNumber),
ret = bind(listen_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
if (ret == -1) {
printf("bind failed on port %u - %s\n",
(unsigned)ntohs(sockaddr.sin_port),
strerror(errno));
exit(1);
}
ret = listen(listen_fd, 5);
if (ret == -1) {
printf("listen failed - %s\n", strerror(errno));
exit(1);
}
printf("Serial port %u on TCP port %u\n", portNumber,
_base_port + portNumber);
// fflush(stdout);
}
if (wait_for_connection) {
printf("Waiting for connection ....\n");
// fflush(stdout);
net_fd = accept(listen_fd, NULL, NULL);
if (net_fd == -1) {
printf("accept() error - %s", strerror(errno));
exit(1);
}
setsockopt(net_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
setsockopt(net_fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one));
_connected = true;
_rd_fd = net_fd;
_wr_fd = net_fd;
}
}
/*
shutdown a UART
*/
void LinuxUARTDriver::end()
{
_initialised = false;
_connected = false;
while (_in_timer) hal.scheduler->delay(1);
if (_rd_fd == _wr_fd && _rd_fd != -1) {
close(_rd_fd);

10
libraries/AP_HAL_Linux/UARTDriver.h
View File

@ -25,18 +25,22 @@ public:
size_t write(uint8_t c);
size_t write(const uint8_t *buffer, size_t size);
void set_device_path(const char *path);
void set_device_path(char *path);
void _timer_tick(void);
private:
const char *device_path;
char *device_path;
int _rd_fd;
int _wr_fd;
bool _nonblocking_writes;
bool _console;
volatile bool _initialised;
volatile bool _in_timer;
uint16_t _base_port;
char *_ip;
char *_flag;
bool _connected; // true if a client has connected
// we use in-task ring buffers to reduce the system call cost
// of ::read() and ::write() in the main loop
@ -55,6 +59,8 @@ private:
int _write_fd(const uint8_t *buf, uint16_t n);
int _read_fd(uint8_t *buf, uint16_t n);
void _tcp_start_connection(bool wait_for_connection);
int _parseDevicePath(char* arg);
uint64_t _last_write_time;
};