Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 09:43:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

VRBRAIN UARTDriver: Make use of ByteBuffer class

Browse Source 
This patch replaces the 'old style' ringbuffer by the ByteBuffer class.
An effort was made to keep the exchange as close as possible from a
drop-in replacement to minimize the risk of introducing bugs.

Although the exchange opens opportunities for improvement and
simplification of this class.

While at it, just like in the write case, explain why we are stopping.
This commit is contained in:
Murilo Belluzzo 2016-07-30 09:16:54 -03:00 committed by Andrew Tridgell
parent 4df627693d
commit e8bfcf02a0
2 changed files with 59 additions and 133 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

155
libraries/AP_HAL_VRBRAIN/UARTDriver.cpp
View File

@ -14,7 +14,6 @@
#include <termios.h> #include <termios.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <assert.h> #include <assert.h>
#include <AP_HAL/utility/RingBuffer.h>
#include "GPIO.h" #include "GPIO.h"
using namespace VRBRAIN; using namespace VRBRAIN;
@ -70,18 +69,13 @@ void VRBRAINUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
thrashing of the heap once we are up. The ttyACM0 driver may not thrashing of the heap once we are up. The ttyACM0 driver may not
connect for some time after boot connect for some time after boot
*/ */
if (rxS != 0 && rxS != _readbuf_size) { if (rxS != 0 && rxS != _readbuf.get_size()) {
_initialised = false; _initialised = false;
while (_in_timer) { while (_in_timer) {
hal.scheduler->delay(1); hal.scheduler->delay(1);
} }
_readbuf_size = rxS;
if (_readbuf != NULL) { _readbuf.set_size(rxS);
free(_readbuf);
}
_readbuf = (uint8_t *)malloc(_readbuf_size);
_readbuf_head = 0;
_readbuf_tail = 0;
} }
if (b != 0) { if (b != 0) {
@ -91,18 +85,12 @@ void VRBRAINUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
/* /*
allocate the write buffer allocate the write buffer
*/ */
if (txS != 0 && txS != _writebuf_size) { if (txS != 0 && txS != _writebuf.get_size()) {
_initialised = false; _initialised = false;
while (_in_timer) { while (_in_timer) {
hal.scheduler->delay(1); hal.scheduler->delay(1);
} }
_writebuf_size = txS; _writebuf.set_size(txS+16);
if (_writebuf != NULL) {
free(_writebuf);
}
_writebuf = (uint8_t *)malloc(_writebuf_size+16);
_writebuf_head = 0;
_writebuf_tail = 0;
} }
if (_fd == -1) { if (_fd == -1) {
@ -130,13 +118,13 @@ void VRBRAINUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
tcsetattr(_fd, TCSANOW, &t); tcsetattr(_fd, TCSANOW, &t);
} }
if (_writebuf_size != 0 && _readbuf_size != 0 && _fd != -1) { if (_writebuf.get_size() && _readbuf.get_size() && _fd != -1) {
if (!_initialised) { if (!_initialised) {
if (strcmp(_devpath, "/dev/ttyACM0") == 0) { if (strcmp(_devpath, "/dev/ttyACM0") == 0) {
((VRBRAINGPIO *)hal.gpio)->set_usb_connected(); ((VRBRAINGPIO *)hal.gpio)->set_usb_connected();
} }
::printf("initialised %s OK %u %u\n", _devpath, ::printf("initialised %s OK %u %u\n", _devpath,
(unsigned)_writebuf_size, (unsigned)_readbuf_size); (unsigned)_writebuf.get_size(), (unsigned)_readbuf.get_size());
} }
_initialised = true; _initialised = true;
} }
@ -201,19 +189,9 @@ void VRBRAINUARTDriver::end()
close(_fd); close(_fd);
_fd = -1; _fd = -1;
} }
if (_readbuf) {
free(_readbuf); _readbuf.set_size(0);
_readbuf = NULL; _writebuf.set_size(0);
}
if (_writebuf) {
free(_writebuf);
_writebuf = NULL;
}
_readbuf_size = _writebuf_size = 0;
_writebuf_head = 0;
_writebuf_tail = 0;
_readbuf_head = 0;
_readbuf_tail = 0;
} }
void VRBRAINUARTDriver::flush() {} void VRBRAINUARTDriver::flush() {}
@ -240,8 +218,8 @@ uint32_t VRBRAINUARTDriver::available()
try_initialise(); try_initialise();
return 0; return 0;
} }
uint16_t _tail;
return BUF_AVAILABLE(_readbuf); return _readbuf.available();
} }
/* /*
@ -253,8 +231,8 @@ uint32_t VRBRAINUARTDriver::txspace()
try_initialise(); try_initialise();
return 0; return 0;
} }
uint16_t _head;
return BUF_SPACE(_writebuf); return _writebuf.space();
} }
/* /*
@ -262,7 +240,6 @@ uint32_t VRBRAINUARTDriver::txspace()
*/ */
int16_t VRBRAINUARTDriver::read() int16_t VRBRAINUARTDriver::read()
{ {
uint8_t c;
if (_uart_owner_pid != getpid()){ if (_uart_owner_pid != getpid()){
return -1; return -1;
} }
@ -270,15 +247,12 @@ int16_t VRBRAINUARTDriver::read()
try_initialise(); try_initialise();
return -1; return -1;
} }
if (_readbuf == NULL) {
uint8_t byte;
if (!_readbuf.read_byte(&byte))
return -1; return -1;
}
if (BUF_EMPTY(_readbuf)) { return byte;
return -1;
}
c = _readbuf[_readbuf_head];
BUF_ADVANCEHEAD(_readbuf, 1);
return c;
} }
/* /*
@ -293,17 +267,14 @@ size_t VRBRAINUARTDriver::write(uint8_t c)
try_initialise(); try_initialise();
return 0; return 0;
} }
uint16_t _head;
while (BUF_SPACE(_writebuf) == 0) { while (_writebuf.space() == 0) {
if (_nonblocking_writes) { if (_nonblocking_writes) {
return 0; return 0;
} }
hal.scheduler->delay(1); hal.scheduler->delay(1);
} }
_writebuf[_writebuf_tail] = c; return _writebuf.write(&c, 1);
BUF_ADVANCETAIL(_writebuf, 1);
return 1;
} }
/* /*
@ -331,36 +302,7 @@ size_t VRBRAINUARTDriver::write(const uint8_t *buffer, size_t size)
return ret; return ret;
} }
uint16_t _head, space; return _writebuf.write(buffer, size);
space = BUF_SPACE(_writebuf);
if (space == 0) {
return 0;
}
if (size > space) {
size = space;
}
if (_writebuf_tail < _head) {
// perform as single memcpy
assert(_writebuf_tail+size <= _writebuf_size);
memcpy(&_writebuf[_writebuf_tail], buffer, size);
BUF_ADVANCETAIL(_writebuf, size);
return size;
}
// perform as two memcpy calls
uint16_t n = _writebuf_size - _writebuf_tail;
if (n > size) n = size;
assert(_writebuf_tail+n <= _writebuf_size);
memcpy(&_writebuf[_writebuf_tail], buffer, n);
BUF_ADVANCETAIL(_writebuf, n);
buffer += n;
n = size - n;
if (n > 0) {
assert(_writebuf_tail+n <= _writebuf_size);
memcpy(&_writebuf[_writebuf_tail], buffer, n);
BUF_ADVANCETAIL(_writebuf, n);
}
return size;
} }
/* /*
@ -409,7 +351,6 @@ int VRBRAINUARTDriver::_write_fd(const uint8_t *buf, uint16_t n)
} }
if (ret > 0) { if (ret > 0) {
BUF_ADVANCEHEAD(_writebuf, ret);
_last_write_time = AP_HAL::micros64(); _last_write_time = AP_HAL::micros64();
_total_written += ret; _total_written += ret;
if (! _first_write_time && _total_written > 5) { if (! _first_write_time && _total_written > 5) {
@ -447,7 +388,6 @@ int VRBRAINUARTDriver::_write_fd(const uint8_t *buf, uint16_t n)
// discarding bytes, even if this is a blocking port. This // discarding bytes, even if this is a blocking port. This
// prevents the ttyACM0 port blocking startup if the endpoint // prevents the ttyACM0 port blocking startup if the endpoint
// is not connected // is not connected
BUF_ADVANCEHEAD(_writebuf, n);
return n; return n;
} }
return ret; return ret;
@ -472,7 +412,6 @@ int VRBRAINUARTDriver::_read_fd(uint8_t *buf, uint16_t n)
} }
} }
if (ret > 0) { if (ret > 0) {
BUF_ADVANCETAIL(_readbuf, ret);
_total_read += ret; _total_read += ret;
} }
return ret; return ret;
@ -486,7 +425,8 @@ int VRBRAINUARTDriver::_read_fd(uint8_t *buf, uint16_t n)
*/ */
void VRBRAINUARTDriver::_timer_tick(void) void VRBRAINUARTDriver::_timer_tick(void)
{ {
uint16_t n; int ret;
uint32_t n;
if (!_initialised) return; if (!_initialised) return;
@ -498,44 +438,37 @@ void VRBRAINUARTDriver::_timer_tick(void)
_in_timer = true; _in_timer = true;
// write any pending bytes // write any pending bytes
uint16_t _tail; n = _writebuf.available();
n = BUF_AVAILABLE(_writebuf);
if (n > 0) { if (n > 0) {
uint16_t n1 = _writebuf_size - _writebuf_head; ByteBuffer::IoVec vec[2];
perf_begin(_perf_uart); perf_begin(_perf_uart);
if (n1 >= n) { const auto n_vec = _writebuf.peekiovec(vec, n);
// do as a single write for (int i = 0; i < n_vec; i++) {
_write_fd(&_writebuf[_writebuf_head], n); ret =_write_fd(vec[i].data, (uint16_t)vec[i].len);
} else { if (ret > 0)
// split into two writes _writebuf.advance(ret);
int ret = _write_fd(&_writebuf[_writebuf_head], n1);
if (ret == n1 && n > n1) {
_write_fd(&_writebuf[_writebuf_head], n - n1);
}
} }
perf_end(_perf_uart); perf_end(_perf_uart);
} }
// try to fill the read buffer // try to fill the read buffer
uint16_t _head; ByteBuffer::IoVec vec[2];
n = BUF_SPACE(_readbuf);
if (n > 0) {
uint16_t n1 = _readbuf_size - _readbuf_tail;
perf_begin(_perf_uart); perf_begin(_perf_uart);
if (n1 >= n) { const auto n_vec = _readbuf.reserve(vec, _readbuf.space());
// one read will do for (int i = 0; i < n_vec; i++) {
assert(_readbuf_tail+n <= _readbuf_size); ret = _read_fd(vec[i].data, vec[i].len);
_read_fd(&_readbuf[_readbuf_tail], n); if (ret < 0) {
} else { break;
assert(_readbuf_tail+n1 <= _readbuf_size); }
int ret = _read_fd(&_readbuf[_readbuf_tail], n1); _readbuf.commit((unsigned)ret);
if (ret == n1 && n > n1) {
assert(_readbuf_tail+(n-n1) <= _readbuf_size); /* stop reading as we read less than we asked for */
_read_fd(&_readbuf[_readbuf_tail], n - n1); if ((unsigned)ret < vec[i].len) {
break;
} }
} }
perf_end(_perf_uart); perf_end(_perf_uart);
}
_in_timer = false; _in_timer = false;
} }

17
libraries/AP_HAL_VRBRAIN/UARTDriver.h
View File

@ -1,8 +1,10 @@
#pragma once #pragma once
#include "AP_HAL_VRBRAIN.h" #include <AP_HAL/utility/RingBuffer.h>
#include <systemlib/perf_counter.h> #include <systemlib/perf_counter.h>
#include "AP_HAL_VRBRAIN.h"
class VRBRAIN::VRBRAINUARTDriver : public AP_HAL::UARTDriver { class VRBRAIN::VRBRAINUARTDriver : public AP_HAL::UARTDriver {
public: public:
VRBRAINUARTDriver(const char *devpath, const char *perf_name); VRBRAINUARTDriver(const char *devpath, const char *perf_name);
@ -48,18 +50,9 @@ private:
// we use in-task ring buffers to reduce the system call cost // we use in-task ring buffers to reduce the system call cost
// of ::read() and ::write() in the main loop // of ::read() and ::write() in the main loop
uint8_t *_readbuf; ByteBuffer _readbuf;
uint16_t _readbuf_size; ByteBuffer _writebuf;
// _head is where the next available data is. _tail is where new
// data is put
volatile uint16_t _readbuf_head;
volatile uint16_t _readbuf_tail;
uint8_t *_writebuf;
uint16_t _writebuf_size;
volatile uint16_t _writebuf_head;
volatile uint16_t _writebuf_tail;
perf_counter_t _perf_uart; perf_counter_t _perf_uart;
int _write_fd(const uint8_t *buf, uint16_t n); int _write_fd(const uint8_t *buf, uint16_t n);