aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

HAL_PX4: Rework support for FLOW_CONTROL_AUTO.

Browse Source 
Now instead of requiring the buffer to fill completely before we can
detect it is not draining, we use a time based mechanism to detect
when none of the first few bytes are transmitted after sitting in our
buffer a half second or more after flow control is enabled.  This
huristic is reliable only for the first several chracters because we
believe that the radio must still have plenty of room in it's own
buffers at that time even if it is not able to transmit them to the
other radio yet.  Note that the original algorithm made the same
assumption.

The new algorithm is especially helpful for cases where only keepalive
messages are transmitted before other packets can be requested by the
GCS.  In this situation, the original code required almost 2 minutes
to disable flow control and allow communication with the GCS.
This commit is contained in:
Brad Bosch 2015-07-13 18:52:48 -05:00 committed by Andrew Tridgell
parent c87a7c7df9
commit 6e9756ff79
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_PX4/UARTDriver.cpp
View File

@ -30,6 +30,7 @@ PX4UARTDriver::PX4UARTDriver(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)
{
}
@ -111,17 +112,6 @@ void PX4UARTDriver::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) {
@ -140,9 +130,6 @@ void PX4UARTDriver::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) {
@ -173,6 +160,11 @@ void PX4UARTDriver::set_flow_control(enum flow_control fcontrol)
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 = fcontrol;
}
@ -382,19 +374,33 @@ int PX4UARTDriver::_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;
@ -408,6 +414,9 @@ int PX4UARTDriver::_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_PX4/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;