Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-07 00:13:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

HAL_Linux: improved UDP packetisation and add flow control reporting

Browse Source 
report we have flow control on UDP and TCP
This commit is contained in:
Andrew Tridgell 2014-10-09 12:21:50 +11:00
parent 08f8fdfabd
commit 076bb1294e
2 changed files with 52 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
libraries/AP_HAL_Linux/UARTDriver.cpp
View File

@ -31,7 +31,8 @@ LinuxUARTDriver::LinuxUARTDriver(bool default_console) :
device_path(NULL), device_path(NULL),
_rd_fd(-1), _rd_fd(-1),
_wr_fd(-1), _wr_fd(-1),
_packetise(false) _packetise(false),
_flow_control(FLOW_CONTROL_DISABLE)
{ {
if (default_console) { if (default_console) {
_rd_fd = 0; _rd_fd = 0;
@ -83,15 +84,17 @@ void LinuxUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
} }
if (!_connected) { if (!_connected) {
printf("LinuxUARTDriver TCP connection not stablished\n"); ::printf("LinuxUARTDriver TCP connection not stablished\n");
exit(1); exit(1);
} }
_flow_control = FLOW_CONTROL_ENABLE;
break; break;
} }
case DEVICE_UDP: case DEVICE_UDP:
{ {
_udp_start_connection(); _udp_start_connection();
_flow_control = FLOW_CONTROL_ENABLE;
break; break;
} }
@ -100,7 +103,7 @@ void LinuxUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
_rd_fd = open(device_path, O_RDWR); _rd_fd = open(device_path, O_RDWR);
_wr_fd = _rd_fd; _wr_fd = _rd_fd;
if (_rd_fd == -1) { if (_rd_fd == -1) {
fprintf(stdout, "Failed to open UART device %s - %s\n", ::fprintf(stdout, "Failed to open UART device %s - %s\n",
device_path, strerror(errno)); device_path, strerror(errno));
return; return;
} }
@ -108,12 +111,15 @@ void LinuxUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
// always run the file descriptor non-blocking, and deal with // always run the file descriptor non-blocking, and deal with
// blocking IO in the higher level calls // blocking IO in the higher level calls
fcntl(_rd_fd, F_SETFL, fcntl(_rd_fd, F_GETFL, 0) | O_NONBLOCK); fcntl(_rd_fd, F_SETFL, fcntl(_rd_fd, F_GETFL, 0) | O_NONBLOCK);
// TODO: add proper flow control support
_flow_control = FLOW_CONTROL_DISABLE;
break; break;
} }
default: default:
{ {
// Notify that the option is not valid and select standart input and output // Notify that the option is not valid and select standart input and output
printf("LinuxUARTDriver parsing failed, using default\n"); ::printf("LinuxUARTDriver parsing failed, using default\n");
_rd_fd = 0; _rd_fd = 0;
_wr_fd = 1; _wr_fd = 1;
@ -267,20 +273,20 @@ void LinuxUARTDriver::_tcp_start_connection(bool wait_for_connection)
listen_fd = socket(AF_INET, SOCK_STREAM, 0); listen_fd = socket(AF_INET, SOCK_STREAM, 0);
if (listen_fd == -1) { if (listen_fd == -1) {
printf("socket failed - %s\n", strerror(errno)); ::printf("socket failed - %s\n", strerror(errno));
exit(1); exit(1);
} }
/* we want to be able to re-use ports quickly */ /* we want to be able to re-use ports quickly */
setsockopt(listen_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); setsockopt(listen_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
printf("bind port %u for %u\n", ::printf("bind port %u for %u\n",
(unsigned)ntohs(sockaddr.sin_port), (unsigned)ntohs(sockaddr.sin_port),
(unsigned)portNumber); (unsigned)portNumber);
ret = bind(listen_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)); ret = bind(listen_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
if (ret == -1) { if (ret == -1) {
printf("bind failed on port %u - %s\n", ::printf("bind failed on port %u - %s\n",
(unsigned)ntohs(sockaddr.sin_port), (unsigned)ntohs(sockaddr.sin_port),
strerror(errno)); strerror(errno));
exit(1); exit(1);
@ -288,21 +294,21 @@ void LinuxUARTDriver::_tcp_start_connection(bool wait_for_connection)
ret = listen(listen_fd, 5); ret = listen(listen_fd, 5);
if (ret == -1) { if (ret == -1) {
printf("listen failed - %s\n", strerror(errno)); ::printf("listen failed - %s\n", strerror(errno));
exit(1); exit(1);
} }
printf("Serial port %u on TCP port %u\n", portNumber, ::printf("Serial port %u on TCP port %u\n", portNumber,
_base_port + portNumber); _base_port + portNumber);
fflush(stdout); fflush(stdout);
} }
if (wait_for_connection) { if (wait_for_connection) {
printf("Waiting for connection ....\n"); ::printf("Waiting for connection ....\n");
fflush(stdout); ::fflush(stdout);
net_fd = accept(listen_fd, NULL, NULL); net_fd = accept(listen_fd, NULL, NULL);
if (net_fd == -1) { if (net_fd == -1) {
printf("accept() error - %s", strerror(errno)); ::printf("accept() error - %s", strerror(errno));
exit(1); exit(1);
} }
setsockopt(net_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); setsockopt(net_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
@ -338,13 +344,13 @@ void LinuxUARTDriver::_udp_start_connection(void)
_rd_fd = socket(AF_INET, SOCK_DGRAM, 0); _rd_fd = socket(AF_INET, SOCK_DGRAM, 0);
if (_rd_fd == -1) { if (_rd_fd == -1) {
printf("socket failed - %s\n", strerror(errno)); ::printf("socket failed - %s\n", strerror(errno));
exit(1); exit(1);
} }
ret = connect(_rd_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)); ret = connect(_rd_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
if (ret == -1) { if (ret == -1) {
printf("connect failed to %s:%u - %s\n", ::printf("connect failed to %s:%u - %s\n",
_ip, (unsigned)_base_port, _ip, (unsigned)_base_port,
strerror(errno)); strerror(errno));
exit(1); exit(1);
@ -598,29 +604,45 @@ void LinuxUARTDriver::_timer_tick(void)
if (n < 8) { if (n < 8) {
n = 0; n = 0;
} else { } else {
// the length of the packet is the 2nd byte, and mavlink
// packets have a 6 byte header plus 2 byte checksum,
// giving len+8 bytes
uint16_t ofs = (_writebuf_head + 1) % _writebuf_size; uint16_t ofs = (_writebuf_head + 1) % _writebuf_size;
uint8_t len = _writebuf[ofs]; uint8_t len = _writebuf[ofs];
if (n < len+8) { if (n < len+8) {
// we don't have a full packet yet
n = 0; n = 0;
} else if (n > len+8) { } else if (n > len+8) {
// send just 1 packet at a time (so MAVLink packets
// are aligned on UDP boundaries)
n = len+8; n = len+8;
} }
} }
} }
if (n > 0) { if (n > 0) {
if (_tail > _writebuf_head) { uint16_t n1 = _writebuf_size - _writebuf_head;
if (n1 >= n) {
// do as a single write // do as a single write
_write_fd(&_writebuf[_writebuf_head], n); _write_fd(&_writebuf[_writebuf_head], n);
} else { } else {
// split into two writes // split into two writes
uint16_t n1 = _writebuf_size - _writebuf_head; if (_packetise) {
// keep as a single UDP packet
uint8_t tmpbuf[n];
memcpy(tmpbuf, &_writebuf[_writebuf_head], n1);
if (n > n1) {
memcpy(&tmpbuf[n1], &_writebuf[0], n-n1);
}
_write_fd(tmpbuf, n);
} else {
int ret = _write_fd(&_writebuf[_writebuf_head], n1); int ret = _write_fd(&_writebuf[_writebuf_head], n1);
if (ret == n1 && n > n1) { if (ret == n1 && n > n1) {
_write_fd(&_writebuf[_writebuf_head], n - n1); _write_fd(&_writebuf[_writebuf_head], n - n1);
} }
} }
} }
}
// try to fill the read buffer // try to fill the read buffer
uint16_t _head; uint16_t _head;

3
libraries/AP_HAL_Linux/UARTDriver.h
View File

@ -29,6 +29,8 @@ public:
void _timer_tick(void); void _timer_tick(void);
enum flow_control get_flow_control(void) { return _flow_control; }
private: private:
char *device_path; char *device_path;
int _rd_fd; int _rd_fd;
@ -47,6 +49,7 @@ private:
// of ::read() and ::write() in the main loop // of ::read() and ::write() in the main loop
uint8_t *_readbuf; uint8_t *_readbuf;
uint16_t _readbuf_size; uint16_t _readbuf_size;
enum flow_control _flow_control;
// _head is where the next available data is. _tail is where new // _head is where the next available data is. _tail is where new
// data is put // data is put