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

HAL_PX4: fixed blocking bulk writes to UARTs 

this fixes a problem with uBlox initialisation on PX4
This commit is contained in:
Andrew Tridgell 2013-03-20 15:41:52 +11:00
parent 89c85f9c5a
commit 8766e86091
2 changed files with 44 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
libraries/AP_HAL_PX4/UARTDriver.cpp
View File

@ -36,16 +36,13 @@ extern const AP_HAL::HAL& hal;
void PX4UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
{
if (!_initialised) {
_fd = open(_devpath, O_RDWR | O_NONBLOCK);
_fd = open(_devpath, O_RDWR);
if (_fd == -1) {
fprintf(stdout, "Failed to open UART device %s - %s\n",
_devpath, strerror(errno));
return;
}
// always set it non-blocking for the low level IO
fcntl(_fd, F_SETFL, fcntl(_fd, F_GETFL, 0) | O_NONBLOCK);
if (rxS == 0) {
rxS = 128;
}
@ -273,6 +270,19 @@ size_t PX4UARTDriver::write(const uint8_t *buffer, size_t size)
// 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) {
@ -339,7 +349,7 @@ int PX4UARTDriver::_write_fd(const uint8_t *buf, uint16_t n)
// reopening the port
_initialised = false;
::close(_fd);
_fd = ::open(_devpath, O_RDWR | O_NONBLOCK);
_fd = ::open(_devpath, O_RDWR);
if (_fd == -1) {
fprintf(stdout, "Failed to reopen UART device %s - %s\n",
_devpath, strerror(errno));
@ -347,8 +357,6 @@ int PX4UARTDriver::_write_fd(const uint8_t *buf, uint16_t n)
return n;
}
// always set it non-blocking for the low level IO
fcntl(_fd, F_SETFL, fcntl(_fd, F_GETFL, 0) | O_NONBLOCK);
_last_write_time = hrt_absolute_time();
_initialised = true;
}
@ -366,6 +374,31 @@ int PX4UARTDriver::_write_fd(const uint8_t *buf, uint16_t 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
@ -406,23 +439,14 @@ void PX4UARTDriver::_timer_tick(void)
if (_readbuf_tail < _head) {
// one read will do
assert(_readbuf_tail+n <= _readbuf_size);
int ret = ::read(_fd, &_readbuf[_readbuf_tail], n);
if (ret > 0) {
BUF_ADVANCETAIL(_readbuf, ret);
}
_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 > 0) {
BUF_ADVANCETAIL(_readbuf, ret);
}
int ret = _read_fd(&_readbuf[_readbuf_tail], n1);
if (ret == n1 && n != n1) {
assert(_readbuf_tail+(n-n1) <= _readbuf_size);
ret = ::read(_fd, &_readbuf[_readbuf_tail], n - n1);
if (ret > 0) {
BUF_ADVANCETAIL(_readbuf, ret);
}
_read_fd(&_readbuf[_readbuf_tail], n - n1);
}
}
perf_end(_perf_uart);

1
libraries/AP_HAL_PX4/UARTDriver.h
View File

@ -73,6 +73,7 @@ private:
perf_counter_t _perf_uart;
int _write_fd(const uint8_t *buf, uint16_t n);
int _read_fd(uint8_t *buf, uint16_t n);
uint64_t _last_write_time;
};