Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 01:33:56 -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

175
libraries/AP_HAL_VRBRAIN/UARTDriver.cpp
View File

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

17
libraries/AP_HAL_VRBRAIN/UARTDriver.h
View File

@ -1,8 +1,10 @@
#pragma once
#include "AP_HAL_VRBRAIN.h"
#include <AP_HAL/utility/RingBuffer.h>
#include <systemlib/perf_counter.h>
#include "AP_HAL_VRBRAIN.h"
class VRBRAIN::VRBRAINUARTDriver : public AP_HAL::UARTDriver {
public:
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
// of ::read() and ::write() in the main loop
uint8_t *_readbuf;
uint16_t _readbuf_size;
ByteBuffer _readbuf;
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;
int _write_fd(const uint8_t *buf, uint16_t n);