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

AP_HAL_PX4: UARTDriver: Make use of ByteBuffer class 

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-30 08:54:37 -03:00 committed by Andrew Tridgell
parent 26650049c0
commit c7a65a026c
3 changed files with 65 additions and 146 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_HAL_Linux/UARTDriver.cpp
View File

@ -355,14 +355,12 @@ int UARTDriver::_read_fd(uint8_t *buf, uint16_t n)
*/
bool UARTDriver::_write_pending_bytes(void)
{
uint16_t n;
// write any pending bytes
uint32_t available_bytes = _writebuf.available();
n = available_bytes;
uint16_t n = available_bytes;
int16_t b = _writebuf.peek(0);
if (_packetise && n > 0 &&
(_writebuf.peek(0) != MAVLINK_STX_MAVLINK1 &&
_writebuf.peek(0) != MAVLINK_STX)) {
b != MAVLINK_STX_MAVLINK1 && b != MAVLINK_STX) {
/*
we have a non-mavlink packet at the start of the
buffer. Look ahead for a MAVLink start byte, up to 256 bytes
@ -371,7 +369,7 @@ bool UARTDriver::_write_pending_bytes(void)
uint16_t limit = n>256?256:n;
uint16_t i;
for (i=0; i<limit; i++) {
int16_t b = _writebuf.peek(i);
b = _writebuf.peek(i);
if (b == MAVLINK_STX_MAVLINK1 || b == MAVLINK_STX) {
n = i;
break;
@ -382,8 +380,8 @@ bool UARTDriver::_write_pending_bytes(void)
n = limit;
}
}
const int16_t b = _writebuf.peek(0);
if (_packetise && n > 0 && b > 0 &&
b = _writebuf.peek(0);
if (_packetise && n > 0 &&
(b == MAVLINK_STX_MAVLINK1 || b == MAVLINK_STX)) {
uint8_t min_length = (b == MAVLINK_STX_MAVLINK1)?8:12;
// this looks like a MAVLink packet - try to write on

181
libraries/AP_HAL_PX4/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 PX4;
@ -70,19 +69,14 @@ void PX4UARTDriver::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 PX4UARTDriver::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,18 +118,17 @@ void PX4UARTDriver::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) {
((PX4GPIO *)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;
}
_uart_owner_pid = getpid();
}
void PX4UARTDriver::set_flow_control(enum flow_control fcontrol)
@ -201,19 +188,9 @@ void PX4UARTDriver::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 PX4UARTDriver::flush() {}
@ -235,34 +212,33 @@ bool PX4UARTDriver::tx_pending() { return false; }
return number of bytes available to be read from the buffer
*/
uint32_t PX4UARTDriver::available()
{
if (!_initialised) {
{
if (!_initialised) {
try_initialise();
return 0;
}
uint16_t _tail;
return BUF_AVAILABLE(_readbuf);
return 0;
}
return _readbuf.available();
}
/*
return number of bytes that can be added to the write buffer
*/
uint32_t PX4UARTDriver::txspace()
{
if (!_initialised) {
{
if (!_initialised) {
try_initialise();
return 0;
}
uint16_t _head;
return BUF_SPACE(_writebuf);
return 0;
}
return _writebuf.space();
}
/*
read one byte from the read buffer
*/
int16_t PX4UARTDriver::read()
{
uint8_t c;
int16_t PX4UARTDriver::read()
{
if (_uart_owner_pid != getpid()){
return -1;
}
@ -270,15 +246,13 @@ int16_t PX4UARTDriver::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 PX4UARTDriver::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 PX4UARTDriver::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 PX4UARTDriver::_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 PX4UARTDriver::_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 PX4UARTDriver::_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 PX4UARTDriver::_read_fd(uint8_t *buf, uint16_t n)
*/
void PX4UARTDriver::_timer_tick(void)
{
uint16_t n;
int ret;
uint32_t n;
if (!_initialised) return;
@ -498,44 +438,33 @@ void PX4UARTDriver::_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);
}
}
perf_end(_perf_uart);
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);
if ((unsigned)ret < vec[i].len)
break;
}
perf_end(_perf_uart);
_in_timer = false;
}

16
libraries/AP_HAL_PX4/UARTDriver.h
View File

@ -1,5 +1,7 @@
#pragma once
#include <AP_HAL/utility/RingBuffer.h>
#include "AP_HAL_PX4.h"
#include <systemlib/perf_counter.h>
@ -48,18 +50,8 @@ 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;
// _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;
ByteBuffer _readbuf{0};
ByteBuffer _writebuf{0};
perf_counter_t _perf_uart;
int _write_fd(const uint8_t *buf, uint16_t n);