2013-01-21 19:50:26 -04:00
|
|
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
|
2013-01-02 20:03:05 -04:00
|
|
|
#include <AP_HAL.h>
|
|
|
|
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
|
|
|
#include "UARTDriver.h"
|
2013-01-03 02:27:53 -04:00
|
|
|
|
2013-01-02 20:03:05 -04:00
|
|
|
#include <stdio.h>
|
|
|
|
#include <unistd.h>
|
2013-01-03 02:27:53 -04:00
|
|
|
#include <stdlib.h>
|
2013-01-03 06:12:10 -04:00
|
|
|
#include <errno.h>
|
|
|
|
#include <sys/ioctl.h>
|
|
|
|
#include <sys/types.h>
|
|
|
|
#include <sys/stat.h>
|
|
|
|
#include <fcntl.h>
|
2013-01-04 04:42:30 -04:00
|
|
|
#include <termios.h>
|
2013-01-02 20:03:05 -04:00
|
|
|
|
|
|
|
using namespace PX4;
|
|
|
|
|
2013-01-03 06:12:10 -04:00
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
|
2013-01-24 00:01:48 -04:00
|
|
|
PX4UARTDriver::PX4UARTDriver(const char *devpath, const char *perf_name) :
|
|
|
|
_devpath(devpath),
|
|
|
|
_perf_uart(perf_alloc(PC_ELAPSED, perf_name))
|
|
|
|
{}
|
|
|
|
|
2013-01-02 20:03:05 -04:00
|
|
|
|
2013-01-03 02:27:53 -04:00
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
|
2013-01-02 20:03:05 -04:00
|
|
|
/*
|
2013-01-04 04:42:30 -04:00
|
|
|
this UART driver maps to a serial device in /dev
|
2013-01-02 20:03:05 -04:00
|
|
|
*/
|
|
|
|
|
2013-01-04 04:42:30 -04:00
|
|
|
void PX4UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
|
|
|
|
{
|
2013-01-03 02:27:53 -04:00
|
|
|
if (!_initialised) {
|
2013-01-03 06:12:10 -04:00
|
|
|
_fd = open(_devpath, O_RDWR);
|
|
|
|
if (_fd == -1) {
|
|
|
|
fprintf(stdout, "Failed to open UART device %s - %s\n",
|
|
|
|
_devpath, strerror(errno));
|
|
|
|
return;
|
|
|
|
}
|
2013-01-22 06:52:21 -04:00
|
|
|
|
|
|
|
// always set it non-blocking for the low level IO
|
|
|
|
unsigned v;
|
|
|
|
v = fcntl(_fd, F_GETFL, 0);
|
|
|
|
fcntl(_fd, F_SETFL, v | O_NONBLOCK);
|
|
|
|
|
2013-01-03 02:27:53 -04:00
|
|
|
_initialised = true;
|
2013-01-22 05:11:12 -04:00
|
|
|
if (rxS == 0) {
|
|
|
|
rxS = 128;
|
|
|
|
}
|
|
|
|
if (txS == 0) {
|
|
|
|
txS = 128;
|
|
|
|
}
|
2013-01-03 02:27:53 -04:00
|
|
|
}
|
2013-01-04 04:42:30 -04:00
|
|
|
|
|
|
|
if (b != 0) {
|
|
|
|
// set the baud rate
|
|
|
|
struct termios t;
|
|
|
|
tcgetattr(_fd, &t);
|
|
|
|
cfsetspeed(&t, b);
|
2013-01-14 00:31:55 -04:00
|
|
|
// disable LF -> CR/LF
|
|
|
|
t.c_oflag &= ~ONLCR;
|
2013-01-04 04:42:30 -04:00
|
|
|
tcsetattr(_fd, TCSANOW, &t);
|
|
|
|
}
|
2013-01-22 05:11:12 -04:00
|
|
|
|
|
|
|
/*
|
|
|
|
allocate the read buffer
|
|
|
|
*/
|
2013-01-21 19:34:04 -04:00
|
|
|
if (rxS != 0 && rxS != _readbuf_size) {
|
2013-01-22 05:11:12 -04:00
|
|
|
_initialised = false;
|
|
|
|
while (_in_timer) hal.scheduler->delay(1);
|
2013-01-21 19:34:04 -04:00
|
|
|
_readbuf_size = rxS;
|
|
|
|
if (_readbuf != NULL) {
|
|
|
|
free(_readbuf);
|
|
|
|
}
|
|
|
|
_readbuf = (uint8_t *)malloc(_readbuf_size);
|
2013-01-22 05:11:12 -04:00
|
|
|
_readbuf_head = 0;
|
|
|
|
_readbuf_tail = 0;
|
|
|
|
_initialised = true;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
allocate the write buffer
|
|
|
|
*/
|
|
|
|
if (txS != 0 && txS != _writebuf_size) {
|
|
|
|
_initialised = false;
|
|
|
|
while (_in_timer) hal.scheduler->delay(1);
|
|
|
|
_writebuf_size = txS;
|
|
|
|
if (_writebuf != NULL) {
|
|
|
|
free(_writebuf);
|
|
|
|
}
|
|
|
|
_writebuf = (uint8_t *)malloc(_writebuf_size);
|
|
|
|
_writebuf_head = 0;
|
|
|
|
_writebuf_tail = 0;
|
|
|
|
_initialised = true;
|
2013-01-21 19:34:04 -04:00
|
|
|
}
|
2013-01-03 02:27:53 -04:00
|
|
|
}
|
|
|
|
|
2013-01-04 04:42:30 -04:00
|
|
|
void PX4UARTDriver::begin(uint32_t b)
|
|
|
|
{
|
2013-01-03 02:27:53 -04:00
|
|
|
begin(b, 0, 0);
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2013-01-02 20:03:05 -04:00
|
|
|
void PX4UARTDriver::end() {}
|
|
|
|
void PX4UARTDriver::flush() {}
|
|
|
|
bool PX4UARTDriver::is_initialized() { return true; }
|
2013-01-21 19:50:26 -04:00
|
|
|
void PX4UARTDriver::set_blocking_writes(bool blocking)
|
|
|
|
{
|
2013-01-22 06:52:21 -04:00
|
|
|
_nonblocking_writes = !blocking;
|
2013-01-03 06:12:10 -04:00
|
|
|
}
|
2013-01-02 20:03:05 -04:00
|
|
|
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);
|
2013-01-22 18:22:51 -04:00
|
|
|
_vprintf(fmt, ap);
|
2013-01-02 20:03:05 -04:00
|
|
|
va_end(ap);
|
|
|
|
}
|
|
|
|
|
|
|
|
void PX4UARTDriver::_printf_P(const prog_char *fmt, ...) {
|
|
|
|
va_list ap;
|
|
|
|
va_start(ap, fmt);
|
2013-01-22 18:22:51 -04:00
|
|
|
_vprintf(fmt, ap);
|
2013-01-02 20:03:05 -04:00
|
|
|
va_end(ap);
|
|
|
|
}
|
|
|
|
|
|
|
|
void PX4UARTDriver::vprintf(const char *fmt, va_list ap) {
|
2013-01-22 18:22:51 -04:00
|
|
|
_vprintf(fmt, ap);
|
2013-01-02 20:03:05 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
void PX4UARTDriver::vprintf_P(const prog_char *fmt, va_list ap) {
|
2013-01-22 18:22:51 -04:00
|
|
|
_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);
|
|
|
|
}
|
2013-01-02 20:03:05 -04:00
|
|
|
}
|
|
|
|
|
2013-01-22 05:11:12 -04:00
|
|
|
// handle %S -> %s
|
2013-01-22 18:22:51 -04:00
|
|
|
void PX4UARTDriver::_vprintf(const char *fmt, va_list ap)
|
2013-01-22 05:11:12 -04:00
|
|
|
{
|
2013-01-22 18:22:51 -04:00
|
|
|
if (hal.scheduler->in_timerprocess()) {
|
|
|
|
// not allowed from timers
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
// we don't use vdprintf() as it goes directly to the file descriptor
|
2013-01-22 05:11:12 -04:00
|
|
|
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';
|
|
|
|
}
|
|
|
|
}
|
2013-01-22 18:22:51 -04:00
|
|
|
_internal_vprintf(fmt2, ap);
|
2013-01-22 05:11:12 -04:00
|
|
|
free(fmt2);
|
|
|
|
}
|
|
|
|
} else {
|
2013-01-22 18:22:51 -04:00
|
|
|
_internal_vprintf(fmt, ap);
|
2013-01-22 05:11:12 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
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)?(buf##_tail - _head) - 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()
|
|
|
|
{
|
2013-01-03 06:12:10 -04:00
|
|
|
if (!_initialised) {
|
|
|
|
return 0;
|
2013-01-03 02:27:53 -04:00
|
|
|
}
|
2013-01-22 05:11:12 -04:00
|
|
|
uint16_t _tail;
|
|
|
|
return BUF_AVAILABLE(_readbuf);
|
2013-01-03 02:27:53 -04:00
|
|
|
}
|
|
|
|
|
2013-01-22 05:11:12 -04:00
|
|
|
/*
|
|
|
|
return number of bytes that can be added to the write buffer
|
|
|
|
*/
|
|
|
|
int16_t PX4UARTDriver::txspace()
|
|
|
|
{
|
2013-01-03 06:12:10 -04:00
|
|
|
if (!_initialised) {
|
|
|
|
return 0;
|
|
|
|
}
|
2013-01-22 05:11:12 -04:00
|
|
|
uint16_t _head;
|
|
|
|
return BUF_SPACE(_writebuf);
|
2013-01-03 02:27:53 -04:00
|
|
|
}
|
2013-01-02 20:03:05 -04:00
|
|
|
|
2013-01-22 05:11:12 -04:00
|
|
|
/*
|
|
|
|
read one byte from the read buffer
|
|
|
|
*/
|
|
|
|
int16_t PX4UARTDriver::read()
|
|
|
|
{
|
2013-01-21 19:34:04 -04:00
|
|
|
uint8_t c;
|
|
|
|
if (!_initialised || _readbuf == NULL) {
|
|
|
|
return -1;
|
|
|
|
}
|
2013-01-22 05:11:12 -04:00
|
|
|
if (BUF_EMPTY(_readbuf)) {
|
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
c = _readbuf[_readbuf_head];
|
|
|
|
BUF_ADVANCEHEAD(_readbuf, 1);
|
2013-01-21 19:34:04 -04:00
|
|
|
return c;
|
2013-01-02 20:03:05 -04:00
|
|
|
}
|
|
|
|
|
2013-01-22 05:11:12 -04:00
|
|
|
/*
|
|
|
|
write one byte to the buffer
|
|
|
|
*/
|
2013-01-21 19:50:26 -04:00
|
|
|
size_t PX4UARTDriver::write(uint8_t c)
|
|
|
|
{
|
2013-01-22 06:52:21 -04:00
|
|
|
if (!_initialised) {
|
2013-01-22 05:11:12 -04:00
|
|
|
return 0;
|
2013-01-21 19:50:26 -04:00
|
|
|
}
|
2013-01-22 18:22:51 -04:00
|
|
|
if (hal.scheduler->in_timerprocess()) {
|
|
|
|
// not allowed from timers
|
|
|
|
return 0;
|
|
|
|
}
|
2013-01-22 06:52:21 -04:00
|
|
|
uint16_t _head;
|
|
|
|
while (BUF_SPACE(_writebuf) == 0) {
|
|
|
|
if (_nonblocking_writes) {
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
hal.scheduler->delay(1);
|
|
|
|
}
|
2013-01-22 05:11:12 -04:00
|
|
|
_writebuf[_writebuf_tail] = c;
|
|
|
|
BUF_ADVANCETAIL(_writebuf, 1);
|
|
|
|
return 1;
|
2013-01-21 19:50:26 -04:00
|
|
|
}
|
|
|
|
|
2013-01-22 05:11:12 -04:00
|
|
|
/*
|
|
|
|
write size bytes to the write buffer
|
|
|
|
*/
|
2013-01-21 19:50:26 -04:00
|
|
|
size_t PX4UARTDriver::write(const uint8_t *buffer, size_t size)
|
|
|
|
{
|
|
|
|
if (!_initialised) {
|
2013-01-03 06:12:10 -04:00
|
|
|
return 0;
|
|
|
|
}
|
2013-01-22 18:22:51 -04:00
|
|
|
if (hal.scheduler->in_timerprocess()) {
|
|
|
|
// not allowed from timers
|
|
|
|
return 0;
|
|
|
|
}
|
2013-01-22 05:11:12 -04:00
|
|
|
uint16_t _head, space;
|
|
|
|
space = BUF_SPACE(_writebuf);
|
|
|
|
if (space == 0) {
|
|
|
|
return 0;
|
2013-01-21 19:50:26 -04:00
|
|
|
}
|
2013-01-22 05:11:12 -04:00
|
|
|
if (size > space) {
|
|
|
|
size = space;
|
|
|
|
}
|
|
|
|
if (_writebuf_tail < _head) {
|
|
|
|
// perform as single memcpy
|
|
|
|
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;
|
|
|
|
memcpy(&_writebuf[_writebuf_tail], buffer, n);
|
|
|
|
BUF_ADVANCETAIL(_writebuf, n);
|
|
|
|
buffer += n;
|
|
|
|
n = size - n;
|
|
|
|
if (n > 0) {
|
|
|
|
memcpy(&_writebuf[_writebuf_tail], buffer, n);
|
|
|
|
BUF_ADVANCETAIL(_writebuf, n);
|
|
|
|
}
|
|
|
|
return size;
|
2013-01-02 22:16:41 -04:00
|
|
|
}
|
|
|
|
|
2013-01-22 05:11:12 -04:00
|
|
|
/*
|
|
|
|
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)
|
2013-01-04 17:09:23 -04:00
|
|
|
{
|
2013-01-22 05:11:12 -04:00
|
|
|
uint16_t n;
|
|
|
|
|
|
|
|
if (!_initialised) return;
|
|
|
|
|
|
|
|
_in_timer = true;
|
|
|
|
|
|
|
|
// write any pending bytes
|
|
|
|
uint16_t _tail;
|
|
|
|
n = BUF_AVAILABLE(_writebuf);
|
|
|
|
if (n > 0) {
|
2013-01-24 00:01:48 -04:00
|
|
|
perf_begin(_perf_uart);
|
2013-01-22 05:11:12 -04:00
|
|
|
if (_tail > _writebuf_head) {
|
|
|
|
// do as a single write
|
|
|
|
int ret = ::write(_fd, &_writebuf[_writebuf_head], n);
|
|
|
|
if (ret > 0) {
|
|
|
|
BUF_ADVANCEHEAD(_writebuf, ret);
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
// split into two writes
|
|
|
|
uint16_t n1 = _writebuf_size - _writebuf_head;
|
|
|
|
int ret = ::write(_fd, &_writebuf[_writebuf_head], n1);
|
|
|
|
if (ret > 0) {
|
|
|
|
BUF_ADVANCEHEAD(_writebuf, ret);
|
|
|
|
}
|
|
|
|
if (ret == n1 && n != n1) {
|
|
|
|
ret = ::write(_fd, &_writebuf[_writebuf_head], n - n1);
|
|
|
|
if (ret > 0) {
|
|
|
|
BUF_ADVANCEHEAD(_writebuf, ret);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2013-01-24 00:01:48 -04:00
|
|
|
perf_end(_perf_uart);
|
2013-01-22 05:11:12 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// try to fill the read buffer
|
|
|
|
uint16_t _head;
|
|
|
|
n = BUF_SPACE(_readbuf);
|
|
|
|
if (n > 0) {
|
2013-01-24 00:01:48 -04:00
|
|
|
perf_begin(_perf_uart);
|
2013-01-22 05:11:12 -04:00
|
|
|
if (_readbuf_tail < _head) {
|
|
|
|
// one read will do
|
|
|
|
int ret = ::read(_fd, &_readbuf[_readbuf_tail], n);
|
|
|
|
if (ret > 0) {
|
|
|
|
BUF_ADVANCETAIL(_readbuf, ret);
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
uint16_t n1 = _readbuf_size - _readbuf_tail;
|
|
|
|
int ret = ::read(_fd, &_readbuf[_readbuf_tail], n1);
|
|
|
|
if (ret > 0) {
|
|
|
|
BUF_ADVANCETAIL(_readbuf, ret);
|
|
|
|
}
|
|
|
|
if (ret == n1 && n != n1) {
|
|
|
|
ret = ::read(_fd, &_readbuf[_readbuf_tail], n - n1);
|
|
|
|
if (ret > 0) {
|
|
|
|
BUF_ADVANCETAIL(_readbuf, ret);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2013-01-24 00:01:48 -04:00
|
|
|
perf_end(_perf_uart);
|
2013-01-22 05:11:12 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
_in_timer = false;
|
2013-01-04 17:09:23 -04:00
|
|
|
}
|
|
|
|
|
2013-01-04 17:01:10 -04:00
|
|
|
#endif // CONFIG_HAL_BOARD
|
2013-01-02 20:03:05 -04:00
|
|
|
|