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

HAL_VRBrain: Rework of support for FLOW_CONTROL_AUTO from PX4 HAL

Browse Source
This commit is contained in:
Brad Bosch 2015-07-13 19:08:58 -05:00 committed by Andrew Tridgell
parent 6e9756ff79
commit 3ad0188488
2 changed files with 35 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
libraries/AP_HAL_VRBRAIN/UARTDriver.cpp
View File

@ -29,6 +29,7 @@ VRBRAINUARTDriver::VRBRAINUARTDriver(const char *devpath, const char *perf_name)
_initialised(false),
_in_timer(false),
_perf_uart(perf_alloc(PC_ELAPSED, perf_name)),
_os_start_auto_space(-1),
_flow_control(FLOW_CONTROL_DISABLE)
{
}
@ -110,17 +111,6 @@ void VRBRAINUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
if (_fd == -1) {
return;
}
// work out the OS write buffer size by looking at how many
// bytes could be written when we first open the port
int nwrite = 0;
if (ioctl(_fd, FIONWRITE, (unsigned long)&nwrite) == 0) {
_os_write_buffer_size = nwrite;
if (_os_write_buffer_size & 1) {
// it is reporting one short
_os_write_buffer_size += 1;
}
}
}
if (_baudrate != 0) {
@ -139,9 +129,6 @@ void VRBRAINUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
tcgetattr(_fd, &t);
t.c_cflag |= CRTS_IFLOW;
tcsetattr(_fd, TCSANOW, &t);
// reset _total_written to reset flow control auto check
_total_written = 0;
}
if (_writebuf_size != 0 && _readbuf_size != 0 && _fd != -1) {
@ -167,6 +154,11 @@ void VRBRAINUARTDriver::set_flow_control(enum flow_control flow_control)
t.c_cflag &= ~CRTSCTS;
}
tcsetattr(_fd, TCSANOW, &t);
if (fcontrol == FLOW_CONTROL_AUTO) {
// reset flow control auto state machine
_total_written = 0;
_first_write_time = 0;
}
_flow_control = flow_control;
}
@ -375,19 +367,33 @@ int VRBRAINUARTDriver::_write_fd(const uint8_t *buf, uint16_t n)
// the FIONWRITE check is to cope with broken O_NONBLOCK behaviour
// in NuttX on ttyACM0
// FIONWRITE is also used for auto flow control detection
// Assume output flow control is not working if:
// port is configured for auto flow control
// and this is not the first write since flow control turned on
// and no data has been removed from the buffer since flow control turned on
// and more than .5 seconds elapsed after writing a total of > 5 characters
//
int nwrite = 0;
if (ioctl(_fd, FIONWRITE, (unsigned long)&nwrite) == 0) {
if (nwrite == 0 &&
_flow_control == FLOW_CONTROL_AUTO &&
_last_write_time != 0 &&
_total_written != 0 &&
_os_write_buffer_size == _total_written &&
(hal.scheduler->micros64() - _last_write_time) > 500*1000UL) {
// it doesn't look like hw flow control is working
::printf("disabling flow control on %s _total_written=%u\n",
_devpath, (unsigned)_total_written);
set_flow_control(FLOW_CONTROL_DISABLE);
if (_flow_control == FLOW_CONTROL_AUTO) {
if (_first_write_time == 0) {
if (_total_written == 0) {
// save the remaining buffer bytes for comparison next write
_os_start_auto_space = nwrite;
}
} else {
if (_os_start_auto_space - nwrite + 1 >= _total_written &&
(hal.scheduler->micros64() - _first_write_time) > 500*1000UL) {
// it doesn't look like hw flow control is working
::printf("disabling flow control on %s _total_written=%u\n",
_devpath, (unsigned)_total_written);
set_flow_control(FLOW_CONTROL_DISABLE);
}
}
}
if (nwrite > n) {
nwrite = n;
@ -401,6 +407,9 @@ int VRBRAINUARTDriver::_write_fd(const uint8_t *buf, uint16_t n)
BUF_ADVANCEHEAD(_writebuf, ret);
_last_write_time = hal.scheduler->micros64();
_total_written += ret;
if (! _first_write_time && _total_written > 5) {
_first_write_time = _last_write_time;
}
return ret;
}

3
libraries/AP_HAL_VRBRAIN/UARTDriver.h
View File

@ -66,12 +66,13 @@ private:
int _write_fd(const uint8_t *buf, uint16_t n);
int _read_fd(uint8_t *buf, uint16_t n);
uint64_t _first_write_time;
uint64_t _last_write_time;
void try_initialise(void);
uint32_t _last_initialise_attempt_ms;
uint32_t _os_write_buffer_size;
uint32_t _os_start_auto_space;
uint32_t _total_read;
uint32_t _total_written;
enum flow_control _flow_control;