Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 12:14:10 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_Linux: 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.
This commit is contained in:
Murilo Belluzzo 2016-07-29 21:40:56 -03:00 committed by Andrew Tridgell
parent 8526b25654
commit ce81c45f80
4 changed files with 68 additions and 294 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
libraries/AP_HAL_Linux/RPIOUARTDriver.cpp
View File

@ -4,7 +4,6 @@
#include <cstdio> #include <cstdio>
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_HAL/utility/RingBuffer.h>
#include "px4io_protocol.h" #include "px4io_protocol.h"
@ -34,8 +33,6 @@ RPIOUARTDriver::RPIOUARTDriver() :
_need_set_baud(false), _need_set_baud(false),
_baudrate(0) _baudrate(0)
{ {
_readbuf = NULL;
_writebuf = NULL;
} }
bool RPIOUARTDriver::sem_take_nonblocking() bool RPIOUARTDriver::sem_take_nonblocking()
@ -75,31 +72,8 @@ void RPIOUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
_initialised = false; _initialised = false;
while (_in_timer) hal.scheduler->delay(1); while (_in_timer) hal.scheduler->delay(1);
/* _readbuf.set_size(rxS);
allocate the read buffer _writebuf.set_size(txS);
*/
if (rxS != 0 && rxS != _readbuf_size) {
_readbuf_size = rxS;
if (_readbuf != NULL) {
free(_readbuf);
}
_readbuf = (uint8_t *)malloc(_readbuf_size);
_readbuf_head = 0;
_readbuf_tail = 0;
}
/*
allocate the write buffer
*/
if (txS != 0 && txS != _writebuf_size) {
_writebuf_size = txS;
if (_writebuf != NULL) {
free(_writebuf);
}
_writebuf = (uint8_t *)malloc(_writebuf_size);
_writebuf_head = 0;
_writebuf_tail = 0;
}
_dev = hal.spi->get_device("raspio"); _dev = hal.spi->get_device("raspio");
@ -110,10 +84,9 @@ void RPIOUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
hal.scheduler->delay(1); hal.scheduler->delay(1);
} }
if (_writebuf_size != 0 && _readbuf_size != 0) { if (_writebuf.get_size() && _readbuf.get_size()) {
_initialised = true; _initialised = true;
} }
} }
int RPIOUARTDriver::_write_fd(const uint8_t *buf, uint16_t n) int RPIOUARTDriver::_write_fd(const uint8_t *buf, uint16_t n)
@ -150,7 +123,7 @@ void RPIOUARTDriver::_timer_tick(void)
return; return;
} }
struct IOPacket _dma_packet_tx, _dma_packet_rx; struct IOPacket _dma_packet_tx = {0}, _dma_packet_rx = {0};
_dma_packet_tx.count_code = 2 | PKT_CODE_WRITE; _dma_packet_tx.count_code = 2 | PKT_CODE_WRITE;
_dma_packet_tx.page = PX4IO_PAGE_UART_BUFFER; _dma_packet_tx.page = PX4IO_PAGE_UART_BUFFER;
@ -186,11 +159,10 @@ void RPIOUARTDriver::_timer_tick(void)
return; return;
} }
struct IOPacket _dma_packet_tx, _dma_packet_rx; struct IOPacket _dma_packet_tx = {0}, _dma_packet_rx = {0};
/* get write_buf bytes */ /* get write_buf bytes */
uint16_t _tail; uint32_t n = _writebuf.available();
uint16_t n = BUF_AVAILABLE(_writebuf);
if (n > PKT_MAX_REGS * 2) { if (n > PKT_MAX_REGS * 2) {
n = PKT_MAX_REGS * 2; n = PKT_MAX_REGS * 2;
@ -201,19 +173,7 @@ void RPIOUARTDriver::_timer_tick(void)
n = _max_size; n = _max_size;
} }
if (n > 0) { _writebuf.read(&((uint8_t *)_dma_packet_tx.regs)[0], n);
uint16_t n1 = _writebuf_size - _writebuf_head;
if (n1 >= n) {
// do as a single write
memcpy( &((uint8_t *)_dma_packet_tx.regs)[0], &_writebuf[_writebuf_head], n );
} else {
// split into two writes
memcpy( &((uint8_t *)_dma_packet_tx.regs)[0], &_writebuf[_writebuf_head], n1 );
memcpy( &((uint8_t *)_dma_packet_tx.regs)[n1], &_writebuf[0], n-n1 );
}
BUF_ADVANCEHEAD(_writebuf, n);
}
_dma_packet_tx.count_code = PKT_MAX_REGS | PKT_CODE_SPIUART; _dma_packet_tx.count_code = PKT_MAX_REGS | PKT_CODE_SPIUART;
_dma_packet_tx.page = PX4IO_PAGE_UART_BUFFER; _dma_packet_tx.page = PX4IO_PAGE_UART_BUFFER;
@ -244,8 +204,7 @@ void RPIOUARTDriver::_timer_tick(void)
_dev->get_semaphore()->give(); _dev->get_semaphore()->give();
/* add bytes to read buf */ /* add bytes to read buf */
uint16_t _head; n = _readbuf.space();
n = BUF_SPACE(_readbuf);
if (_dma_packet_rx.page == PX4IO_PAGE_UART_BUFFER) { if (_dma_packet_rx.page == PX4IO_PAGE_UART_BUFFER) {
@ -257,19 +216,7 @@ void RPIOUARTDriver::_timer_tick(void)
n = PKT_MAX_REGS * 2; n = PKT_MAX_REGS * 2;
} }
if (n > 0) { _readbuf.write(&((uint8_t *)_dma_packet_rx.regs)[0], n);
uint16_t n1 = _readbuf_size - _readbuf_tail;
if (n1 >= n) {
// one read will do
memcpy( &_readbuf[_readbuf_tail], &((uint8_t *)_dma_packet_rx.regs)[0], n );
} else {
memcpy( &_readbuf[_readbuf_tail], &((uint8_t *)_dma_packet_rx.regs)[0], n1 );
memcpy( &_readbuf[0], &((uint8_t *)_dma_packet_rx.regs)[n1], n-n1 );
}
BUF_ADVANCETAIL(_readbuf, n);
}
} }
_in_timer = false; _in_timer = false;

68
libraries/AP_HAL_Linux/SPIUARTDriver.cpp
View File

@ -5,7 +5,6 @@
#include <cstdio> #include <cstdio>
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_HAL/utility/RingBuffer.h>
extern const AP_HAL::HAL &hal; extern const AP_HAL::HAL &hal;
@ -49,31 +48,8 @@ void SPIUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
txS = 2048; txS = 2048;
} }
/* _readbuf.set_size(rxS);
allocate the read buffer _writebuf.set_size(txS);
*/
if (rxS != 0 && rxS != _readbuf_size) {
_readbuf_size = rxS;
if (_readbuf != NULL) {
free(_readbuf);
}
_readbuf = (uint8_t *)malloc(_readbuf_size);
_readbuf_head = 0;
_readbuf_tail = 0;
}
/*
allocate the write buffer
*/
if (txS != 0 && txS != _writebuf_size) {
_writebuf_size = txS;
if (_writebuf != NULL) {
free(_writebuf);
}
_writebuf = (uint8_t *)malloc(_writebuf_size);
_writebuf_head = 0;
_writebuf_tail = 0;
}
if (_buffer == NULL) { if (_buffer == NULL) {
/* Do not allocate new buffer, if we're just changing speed */ /* Do not allocate new buffer, if we're just changing speed */
@ -122,8 +98,6 @@ int SPIUARTDriver::_write_fd(const uint8_t *buf, uint16_t size)
_dev->get_semaphore()->give(); _dev->get_semaphore()->give();
BUF_ADVANCEHEAD(_writebuf, size);
uint16_t ret = size; uint16_t ret = size;
/* Since all SPI-transactions are transfers we need update /* Since all SPI-transactions are transfers we need update
@ -131,41 +105,7 @@ int SPIUARTDriver::_write_fd(const uint8_t *buf, uint16_t size)
* this operation since it's the same as in the * this operation since it's the same as in the
* UARTDriver::write(). * UARTDriver::write().
*/ */
_readbuf.write(_buffer, size);
uint8_t *buffer = _buffer;
uint16_t _head, space;
space = BUF_SPACE(_readbuf);
if (space == 0) {
return ret;
}
if (size > space) {
size = space;
}
if (_readbuf_tail < _head) {
// perform as single memcpy
assert(_readbuf_tail+size <= _readbuf_size);
memcpy(&_readbuf[_readbuf_tail], buffer, size);
BUF_ADVANCETAIL(_readbuf, size);
return ret;
}
// perform as two memcpy calls
uint16_t n = _readbuf_size - _readbuf_tail;
if (n > size) n = size;
assert(_readbuf_tail+n <= _readbuf_size);
memcpy(&_readbuf[_readbuf_tail], buffer, n);
BUF_ADVANCETAIL(_readbuf, n);
buffer += n;
n = size - n;
if (n > 0) {
assert(_readbuf_tail+n <= _readbuf_size);
memcpy(&_readbuf[_readbuf_tail], buffer, n);
BUF_ADVANCETAIL(_readbuf, n);
}
return ret; return ret;
} }
@ -194,8 +134,6 @@ int SPIUARTDriver::_read_fd(uint8_t *buf, uint16_t n)
_dev->transfer_fullduplex(ff_stub, buf, n); _dev->transfer_fullduplex(ff_stub, buf, n);
_dev->get_semaphore()->give(); _dev->get_semaphore()->give();
BUF_ADVANCETAIL(_readbuf, n);
return n; return n;
} }

206
libraries/AP_HAL_Linux/UARTDriver.cpp
View File

@ -18,7 +18,6 @@
#include <unistd.h> #include <unistd.h>
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_HAL/utility/RingBuffer.h>
#include "ConsoleDevice.h" #include "ConsoleDevice.h"
#include "TCPServerDevice.h" #include "TCPServerDevice.h"
@ -105,54 +104,15 @@ void UARTDriver::_allocate_buffers(uint16_t rxS, uint16_t txS)
txS = 32000; txS = 32000;
} }
/* if (_writebuf.set_size(txS) && _readbuf.set_size(rxS)) {
allocate the read buffer
*/
if (rxS != 0 && rxS != _readbuf_size) {
_readbuf_size = rxS;
if (_readbuf != NULL) {
free(_readbuf);
}
_readbuf = (uint8_t *)malloc(_readbuf_size);
_readbuf_head = 0;
_readbuf_tail = 0;
}
/*
allocate the write buffer
*/
if (txS != 0 && txS != _writebuf_size) {
_writebuf_size = txS;
if (_writebuf != NULL) {
free(_writebuf);
}
_writebuf = (uint8_t *)malloc(_writebuf_size);
_writebuf_head = 0;
_writebuf_tail = 0;
}
if (_writebuf_size != 0 && _readbuf_size != 0) {
_initialised = true; _initialised = true;
} }
} }
void UARTDriver::_deallocate_buffers() void UARTDriver::_deallocate_buffers()
{ {
if (_readbuf) { _readbuf.set_size(0);
free(_readbuf); _writebuf.set_size(0);
_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;
} }
/* /*
@ -283,7 +243,7 @@ void UARTDriver::set_blocking_writes(bool blocking)
*/ */
bool UARTDriver::tx_pending() bool UARTDriver::tx_pending()
{ {
return !BUF_EMPTY(_writebuf); return (_writebuf.available() > 0);
} }
/* /*
@ -294,8 +254,7 @@ uint32_t UARTDriver::available()
if (!_initialised) { if (!_initialised) {
return 0; return 0;
} }
uint16_t _tail; return _readbuf.available();
return BUF_AVAILABLE(_readbuf);
} }
/* /*
@ -306,22 +265,21 @@ uint32_t UARTDriver::txspace()
if (!_initialised) { if (!_initialised) {
return 0; return 0;
} }
uint16_t _head; return _writebuf.space();
return BUF_SPACE(_writebuf);
} }
int16_t UARTDriver::read() int16_t UARTDriver::read()
{ {
uint8_t c; if (!_initialised) {
if (!_initialised || _readbuf == NULL) {
return -1; return -1;
} }
if (BUF_EMPTY(_readbuf)) {
uint8_t byte;
if (!_readbuf.read_byte(&byte)) {
return -1; return -1;
} }
c = _readbuf[_readbuf_head];
BUF_ADVANCEHEAD(_readbuf, 1); return byte;
return c;
} }
/* Linux implementations of Print virtual methods */ /* Linux implementations of Print virtual methods */
@ -330,17 +288,14 @@ size_t UARTDriver::write(uint8_t c)
if (!_initialised) { if (!_initialised) {
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;
} }
/* /*
@ -363,36 +318,7 @@ size_t UARTDriver::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;
} }
/* /*
@ -400,8 +326,6 @@ size_t UARTDriver::write(const uint8_t *buffer, size_t size)
*/ */
int UARTDriver::_write_fd(const uint8_t *buf, uint16_t n) int UARTDriver::_write_fd(const uint8_t *buf, uint16_t n)
{ {
int ret = 0;
/* /*
allow for delayed connection. This allows ArduPilot to start allow for delayed connection. This allows ArduPilot to start
before a network interface is available. before a network interface is available.
@ -413,14 +337,7 @@ int UARTDriver::_write_fd(const uint8_t *buf, uint16_t n)
return 0; return 0;
} }
ret = _device->write(buf, n); return _device->write(buf, n);
if (ret > 0) {
BUF_ADVANCEHEAD(_writebuf, ret);
return ret;
}
return ret;
} }
/* /*
@ -428,15 +345,7 @@ int UARTDriver::_write_fd(const uint8_t *buf, uint16_t n)
*/ */
int UARTDriver::_read_fd(uint8_t *buf, uint16_t n) int UARTDriver::_read_fd(uint8_t *buf, uint16_t n)
{ {
int ret; return _device->read(buf, n);
ret = _device->read(buf, n);
if (ret > 0) {
BUF_ADVANCETAIL(_readbuf, ret);
}
return ret;
} }
@ -449,12 +358,11 @@ bool UARTDriver::_write_pending_bytes(void)
uint16_t n; uint16_t n;
// write any pending bytes // write any pending bytes
uint16_t _tail; uint32_t available_bytes = _writebuf.available();
uint16_t available_bytes = BUF_AVAILABLE(_writebuf);
n = available_bytes; n = available_bytes;
if (_packetise && n > 0 && if (_packetise && n > 0 &&
(_writebuf[_writebuf_head] != MAVLINK_STX_MAVLINK1 && (_writebuf.peek(0) != MAVLINK_STX_MAVLINK1 &&
_writebuf[_writebuf_head] != MAVLINK_STX)) { _writebuf.peek(0) != MAVLINK_STX)) {
/* /*
we have a non-mavlink packet at the start of the we have a non-mavlink packet at the start of the
buffer. Look ahead for a MAVLink start byte, up to 256 bytes buffer. Look ahead for a MAVLink start byte, up to 256 bytes
@ -463,7 +371,7 @@ bool UARTDriver::_write_pending_bytes(void)
uint16_t limit = n>256?256:n; uint16_t limit = n>256?256:n;
uint16_t i; uint16_t i;
for (i=0; i<limit; i++) { for (i=0; i<limit; i++) {
uint8_t b = _writebuf[(_writebuf_head + i) % _writebuf_size]; int16_t b = _writebuf.peek(i);
if (b == MAVLINK_STX_MAVLINK1 || b == MAVLINK_STX) { if (b == MAVLINK_STX_MAVLINK1 || b == MAVLINK_STX) {
n = i; n = i;
break; break;
@ -474,8 +382,8 @@ bool UARTDriver::_write_pending_bytes(void)
n = limit; n = limit;
} }
} }
const uint8_t b = _writebuf[_writebuf_head]; const int16_t b = _writebuf.peek(0);
if (_packetise && n > 0 && if (_packetise && n > 0 && b > 0 &&
(b == MAVLINK_STX_MAVLINK1 || b == MAVLINK_STX)) { (b == MAVLINK_STX_MAVLINK1 || b == MAVLINK_STX)) {
uint8_t min_length = (b == MAVLINK_STX_MAVLINK1)?8:12; uint8_t min_length = (b == MAVLINK_STX_MAVLINK1)?8:12;
// this looks like a MAVLink packet - try to write on // this looks like a MAVLink packet - try to write on
@ -487,10 +395,10 @@ bool UARTDriver::_write_pending_bytes(void)
// the length of the packet is the 2nd byte, and mavlink // the length of the packet is the 2nd byte, and mavlink
// packets have a 6 byte header plus 2 byte checksum, // packets have a 6 byte header plus 2 byte checksum,
// giving len+8 bytes // giving len+8 bytes
uint8_t len = _writebuf[(_writebuf_head + 1) % _writebuf_size]; int16_t len = _writebuf.peek(1);
if (b == MAVLINK_STX) { if (b == MAVLINK_STX) {
// check for signed packet with extra 13 bytes // check for signed packet with extra 13 bytes
uint8_t incompat_flags = _writebuf[(_writebuf_head + 2) % _writebuf_size]; int16_t incompat_flags = _writebuf.peek(2);
if (incompat_flags & MAVLINK_IFLAG_SIGNED) { if (incompat_flags & MAVLINK_IFLAG_SIGNED) {
min_length += MAVLINK_SIGNATURE_BLOCK_LEN; min_length += MAVLINK_SIGNATURE_BLOCK_LEN;
} }
@ -507,30 +415,27 @@ bool UARTDriver::_write_pending_bytes(void)
} }
if (n > 0) { if (n > 0) {
uint16_t n1 = _writebuf_size - _writebuf_head; int ret;
if (n1 >= n) {
// do as a single write if (_packetise) {
_write_fd(&_writebuf[_writebuf_head], n); // keep as a single UDP packet
uint8_t tmpbuf[n];
_writebuf.peekbytes(tmpbuf, n);
ret = _write_fd(tmpbuf, n);
if (ret > 0)
_writebuf.advance(ret);
} else { } else {
// split into two writes ByteBuffer::IoVec vec[2];
if (_packetise) { const auto n_vec = _writebuf.peekiovec(vec, n);
// keep as a single UDP packet for (int i = 0; i < n_vec; i++) {
uint8_t tmpbuf[n]; ret = _write_fd(vec[i].data, (uint16_t)vec[i].len);
memcpy(tmpbuf, &_writebuf[_writebuf_head], n1); if (ret > 0)
if (n > n1) { _writebuf.advance(ret);
memcpy(&tmpbuf[n1], &_writebuf[0], n-n1);
}
_write_fd(tmpbuf, n);
} else {
int ret = _write_fd(&_writebuf[_writebuf_head], n1);
if (ret == n1 && n > n1) {
_write_fd(&_writebuf[_writebuf_head], n - n1);
}
} }
} }
} }
return BUF_AVAILABLE(_writebuf) != available_bytes; return _writebuf.available() != available_bytes;
} }
/* /*
@ -540,8 +445,6 @@ bool UARTDriver::_write_pending_bytes(void)
*/ */
void UARTDriver::_timer_tick(void) void UARTDriver::_timer_tick(void)
{ {
uint16_t n;
if (!_initialised) return; if (!_initialised) return;
_in_timer = true; _in_timer = true;
@ -552,22 +455,17 @@ void UARTDriver::_timer_tick(void)
} }
// try to fill the read buffer // try to fill the read buffer
uint16_t _head; int ret;
n = BUF_SPACE(_readbuf); ByteBuffer::IoVec vec[2];
if (n > 0) {
uint16_t n1 = _readbuf_size - _readbuf_tail; const auto n_vec = _readbuf.reserve(vec, _readbuf.space());
if (n1 >= n) { for (int i = 0; i < n_vec; i++) {
// one read will do ret = _read_fd(vec[i].data, vec[i].len);
assert(_readbuf_tail+n <= _readbuf_size); if (ret < 0)
_read_fd(&_readbuf[_readbuf_tail], n); break;
} else { _readbuf.commit((unsigned)ret);
assert(_readbuf_tail+n1 <= _readbuf_size); if ((unsigned)ret < vec[i].len)
int ret = _read_fd(&_readbuf[_readbuf_tail], n1); break;
if (ret == n1 && n > n1) {
assert(_readbuf_tail+(n-n1) <= _readbuf_size);
_read_fd(&_readbuf[_readbuf_tail], n - n1);
}
}
} }
_in_timer = false; _in_timer = false;

17
libraries/AP_HAL_Linux/UARTDriver.h
View File

@ -1,6 +1,7 @@
#pragma once #pragma once
#include <AP_HAL/utility/OwnPtr.h> #include <AP_HAL/utility/OwnPtr.h>
#include <AP_HAL/utility/RingBuffer.h>
#include "AP_HAL_Linux.h" #include "AP_HAL_Linux.h"
#include "SerialDevice.h" #include "SerialDevice.h"
@ -68,24 +69,14 @@ private:
protected: protected:
const char *device_path; const char *device_path;
volatile bool _initialised; volatile bool _initialised;
// 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{0};
uint16_t _readbuf_size; ByteBuffer _writebuf{0};
// _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;
virtual int _write_fd(const uint8_t *buf, uint16_t n); virtual int _write_fd(const uint8_t *buf, uint16_t n);
virtual int _read_fd(uint8_t *buf, uint16_t n); virtual int _read_fd(uint8_t *buf, uint16_t n);
}; };
} }