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

460 lines
11 KiB
C++
Raw Blame History

/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
#include "UARTDriver.h"
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <errno.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <termios.h>
#include <drivers/drv_hrt.h>
#include <assert.h>
using namespace PX4;
extern const AP_HAL::HAL& hal;
PX4UARTDriver::PX4UARTDriver(const char *devpath, const char *perf_name) :
_devpath(devpath),
_perf_uart(perf_alloc(PC_ELAPSED, perf_name))
{}
extern const AP_HAL::HAL& hal;
/*
this UART driver maps to a serial device in /dev
*/
void PX4UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
{
if (!_initialised) {
_fd = open(_devpath, O_RDWR);
if (_fd == -1) {
fprintf(stdout, "Failed to open UART device %s - %s\n",
_devpath, strerror(errno));
return;
}
if (rxS == 0) {
rxS = 128;
}
if (txS == 0) {
txS = 128;
}
}
_initialised = false;
while (_in_timer) hal.scheduler->delay(1);
if (b != 0) {
// set the baud rate
struct termios t;
tcgetattr(_fd, &t);
cfsetspeed(&t, b);
// disable LF -> CR/LF
t.c_oflag &= ~ONLCR;
tcsetattr(_fd, TCSANOW, &t);
}
/*
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+16);
_writebuf_head = 0;
_writebuf_tail = 0;
}
if (_writebuf_size != 0 && _readbuf_size != 0) {
_initialised = true;
}
}
void PX4UARTDriver::begin(uint32_t b)
{
begin(b, 0, 0);
}
void PX4UARTDriver::end() {}
void PX4UARTDriver::flush() {}
bool PX4UARTDriver::is_initialized() { return true; }
void PX4UARTDriver::set_blocking_writes(bool blocking)
{
_nonblocking_writes = !blocking;
}
bool PX4UARTDriver::tx_pending() { return false; }
/* PX4 implementations of BetterStream virtual methods */
void PX4UARTDriver::print_P(const prog_char_t *pstr) {
print(pstr);
}
void PX4UARTDriver::println_P(const prog_char_t *pstr) {
println(pstr);
}
void PX4UARTDriver::printf(const char *fmt, ...) {
va_list ap;
va_start(ap, fmt);
_vprintf(fmt, ap);
va_end(ap);
}
void PX4UARTDriver::_printf_P(const prog_char *fmt, ...) {
va_list ap;
va_start(ap, fmt);
_vprintf(fmt, ap);
va_end(ap);
}
void PX4UARTDriver::vprintf(const char *fmt, va_list ap) {
_vprintf(fmt, ap);
}
void PX4UARTDriver::vprintf_P(const prog_char *fmt, va_list ap) {
_vprintf(fmt, ap);
}
void PX4UARTDriver::_internal_vprintf(const char *fmt, va_list ap)
{
if (hal.scheduler->in_timerprocess()) {
// not allowed from timers
return;
}
char *buf = NULL;
int n = avsprintf(&buf, fmt, ap);
if (n > 0) {
write((const uint8_t *)buf, n);
}
if (buf != NULL) {
free(buf);
}
}
// handle %S -> %s
void PX4UARTDriver::_vprintf(const char *fmt, va_list ap)
{
if (hal.scheduler->in_timerprocess()) {
// not allowed from timers
return;
}
// we don't use vdprintf() as it goes directly to the file descriptor
if (strstr(fmt, "%S")) {
char *fmt2 = strdup(fmt);
if (fmt2 != NULL) {
for (uint16_t i=0; fmt2[i]; i++) {
if (fmt2[i] == '%' && fmt2[i+1] == 'S') {
fmt2[i+1] = 's';
}
}
_internal_vprintf(fmt2, ap);
free(fmt2);
}
} else {
_internal_vprintf(fmt, ap);
}
}
/*
buffer handling macros
*/
#define BUF_AVAILABLE(buf) ((buf##_head > (_tail=buf##_tail))? (buf##_size - buf##_head) + _tail: _tail - buf##_head)
#define BUF_SPACE(buf) (((_head=buf##_head) > buf##_tail)?(_head - buf##_tail) - 1:((buf##_size - buf##_tail) + _head) - 1)
#define BUF_EMPTY(buf) (buf##_head == buf##_tail)
#define BUF_ADVANCETAIL(buf, n) buf##_tail = (buf##_tail + n) % buf##_size
#define BUF_ADVANCEHEAD(buf, n) buf##_head = (buf##_head + n) % buf##_size
/*
return number of bytes available to be read from the buffer
*/
int16_t PX4UARTDriver::available()
{
if (!_initialised) {
return 0;
}
uint16_t _tail;
return BUF_AVAILABLE(_readbuf);
}
/*
return number of bytes that can be added to the write buffer
*/
int16_t PX4UARTDriver::txspace()
{
if (!_initialised) {
return 0;
}
uint16_t _head;
return BUF_SPACE(_writebuf);
}
/*
read one byte from the read buffer
*/
int16_t PX4UARTDriver::read()
{
uint8_t c;
if (!_initialised || _readbuf == NULL) {
return -1;
}
if (BUF_EMPTY(_readbuf)) {
return -1;
}
c = _readbuf[_readbuf_head];
BUF_ADVANCEHEAD(_readbuf, 1);
return c;
}
/*
write one byte to the buffer
*/
size_t PX4UARTDriver::write(uint8_t c)
{
if (!_initialised) {
return 0;
}
if (hal.scheduler->in_timerprocess()) {
// not allowed from timers
return 0;
}
uint16_t _head;
while (BUF_SPACE(_writebuf) == 0) {
if (_nonblocking_writes) {
return 0;
}
hal.scheduler->delay(1);
}
_writebuf[_writebuf_tail] = c;
BUF_ADVANCETAIL(_writebuf, 1);
return 1;
}
/*
write size bytes to the write buffer
*/
size_t PX4UARTDriver::write(const uint8_t *buffer, size_t size)
{
if (!_initialised) {
return 0;
}
if (hal.scheduler->in_timerprocess()) {
// not allowed from timers
return 0;
}
if (!_nonblocking_writes) {
/*
use the per-byte delay loop in write() above for blocking writes
*/
size_t ret = 0;
while (size--) {
if (write(*buffer++) != 1) break;
ret++;
}
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;
}
/*
try writing n bytes, handling an unresponsive port
*/
int PX4UARTDriver::_write_fd(const uint8_t *buf, uint16_t n)
{
int ret = 0;
// the FIONWRITE check is to cope with broken O_NONBLOCK behaviour
// in NuttX on ttyACM0
int nwrite = 0;
if (ioctl(_fd, FIONWRITE, (unsigned long)&nwrite) == 0) {
if (nwrite > n) {
nwrite = n;
}
if (nwrite > 0) {
ret = ::write(_fd, buf, nwrite);
}
}
if (ret > 0) {
BUF_ADVANCEHEAD(_writebuf, ret);
_last_write_time = hrt_absolute_time();
return ret;
}
if (hrt_absolute_time() - _last_write_time > 2000) {
#if 0
// this trick is disabled for now, as it sometimes blocks on
// re-opening the ttyACM0 port, which would cause a crash
if (hrt_absolute_time() - _last_write_time > 2000000) {
// we haven't done a successful write for 2 seconds - try
// reopening the port
_initialised = false;
::close(_fd);
_fd = ::open(_devpath, O_RDWR);
if (_fd == -1) {
fprintf(stdout, "Failed to reopen UART device %s - %s\n",
_devpath, strerror(errno));
// leave it uninitialised
return n;
}
_last_write_time = hrt_absolute_time();
_initialised = true;
}
#else
_last_write_time = hrt_absolute_time();
#endif
// we haven't done a successful write for 2ms, which means the
// port is running at less than 500 bytes/sec. Start
// 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;
}
/*
try reading n bytes, handling an unresponsive port
*/
int PX4UARTDriver::_read_fd(uint8_t *buf, uint16_t n)
{
int ret = 0;
// the FIONREAD check is to cope with broken O_NONBLOCK behaviour
// in NuttX on ttyACM0
int nread = 0;
if (ioctl(_fd, FIONREAD, (unsigned long)&nread) == 0) {
if (nread > n) {
nread = n;
}
if (nread > 0) {
ret = ::read(_fd, buf, nread);
}
}
if (ret > 0) {
BUF_ADVANCETAIL(_readbuf, ret);
}
return ret;
}
/*
push any pending bytes to/from the serial port. This is called at
1kHz in the timer thread. Doing it this way reduces the system call
overhead in the main task enormously.
*/
void PX4UARTDriver::_timer_tick(void)
{
uint16_t n;
if (!_initialised) return;
_in_timer = true;
// write any pending bytes
uint16_t _tail;
n = BUF_AVAILABLE(_writebuf);
if (n > 0) {
perf_begin(_perf_uart);
if (_tail > _writebuf_head) {
// do as a single write
_write_fd(&_writebuf[_writebuf_head], n);
} else {
// split into two writes
uint16_t n1 = _writebuf_size - _writebuf_head;
int ret = _write_fd(&_writebuf[_writebuf_head], n1);
if (ret == n1 && n != n1) {
_write_fd(&_writebuf[_writebuf_head], n - n1);
}
}
perf_end(_perf_uart);
}
// try to fill the read buffer
uint16_t _head;
n = BUF_SPACE(_readbuf);
if (n > 0) {
perf_begin(_perf_uart);
if (_readbuf_tail < _head) {
// one read will do
assert(_readbuf_tail+n <= _readbuf_size);
_read_fd(&_readbuf[_readbuf_tail], n);
} else {
uint16_t n1 = _readbuf_size - _readbuf_tail;
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);
}
_in_timer = false;
}
#endif // CONFIG_HAL_BOARD
Reference in New Issue View Git Blame Copy Permalink