Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-07 16: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 Randy Mackay
parent d4c370ce6e
commit 4af7e454df
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), _initialised(false),
_in_timer(false), _in_timer(false),
_perf_uart(perf_alloc(PC_ELAPSED, perf_name)), _perf_uart(perf_alloc(PC_ELAPSED, perf_name)),
_os_start_auto_space(-1),
_flow_control(FLOW_CONTROL_DISABLE) _flow_control(FLOW_CONTROL_DISABLE)
{ {
} }
@ -110,17 +111,6 @@ void VRBRAINUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
if (_fd == -1) { if (_fd == -1) {
return; 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) { if (_baudrate != 0) {
@ -139,9 +129,6 @@ void VRBRAINUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
tcgetattr(_fd, &t); tcgetattr(_fd, &t);
t.c_cflag |= CRTS_IFLOW; t.c_cflag |= CRTS_IFLOW;
tcsetattr(_fd, TCSANOW, &t); 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) { 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; t.c_cflag &= ~CRTSCTS;
} }
tcsetattr(_fd, TCSANOW, &t); 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; _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 // the FIONWRITE check is to cope with broken O_NONBLOCK behaviour
// in NuttX on ttyACM0 // 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; int nwrite = 0;
if (ioctl(_fd, FIONWRITE, (unsigned long)&nwrite) == 0) { if (ioctl(_fd, FIONWRITE, (unsigned long)&nwrite) == 0) {
if (nwrite == 0 && if (_flow_control == FLOW_CONTROL_AUTO) {
_flow_control == FLOW_CONTROL_AUTO && if (_first_write_time == 0) {
_last_write_time != 0 && if (_total_written == 0) {
_total_written != 0 && // save the remaining buffer bytes for comparison next write
_os_write_buffer_size == _total_written && _os_start_auto_space = nwrite;
(hal.scheduler->micros64() - _last_write_time) > 500*1000UL) { }
// it doesn't look like hw flow control is working } else {
::printf("disabling flow control on %s _total_written=%u\n", if (_os_start_auto_space - nwrite + 1 >= _total_written &&
_devpath, (unsigned)_total_written); (hal.scheduler->micros64() - _first_write_time) > 500*1000UL) {
set_flow_control(FLOW_CONTROL_DISABLE); // 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) { if (nwrite > n) {
nwrite = n; nwrite = n;
@ -401,6 +407,9 @@ int VRBRAINUARTDriver::_write_fd(const uint8_t *buf, uint16_t n)
BUF_ADVANCEHEAD(_writebuf, ret); BUF_ADVANCEHEAD(_writebuf, ret);
_last_write_time = hal.scheduler->micros64(); _last_write_time = hal.scheduler->micros64();
_total_written += ret; _total_written += ret;
if (! _first_write_time && _total_written > 5) {
_first_write_time = _last_write_time;
}
return ret; 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 _write_fd(const uint8_t *buf, uint16_t n);
int _read_fd(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; uint64_t _last_write_time;
void try_initialise(void); void try_initialise(void);
uint32_t _last_initialise_attempt_ms; 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_read;
uint32_t _total_written; uint32_t _total_written;
enum flow_control _flow_control; enum flow_control _flow_control;