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_PX4: added automatic hardware flow control detection

Browse Source 
this allows us to detect if hardware flow control is not available and
automatically disable it
This commit is contained in:
Andrew Tridgell 2014-02-11 11:22:08 +11:00
parent a0369b85d1
commit c5c1d1358a
3 changed files with 44 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_HAL/UARTDriver.h
View File

@ -38,7 +38,11 @@ public:
virtual bool is_initialized() = 0;
virtual void set_blocking_writes(bool blocking) = 0;
virtual bool tx_pending() = 0;
virtual void enable_flow_control(bool enable) {};
enum flow_control {
FLOW_CONTROL_DISABLE=0, FLOW_CONTROL_ENABLE=1, FLOW_CONTROL_AUTO=2
};
virtual void set_flow_control(enum flow_control flow_control) {};
/* Implementations of BetterStream virtual methods. These are
* provided by AP_HAL to ensure consistency between ports to

38
libraries/AP_HAL_PX4/UARTDriver.cpp
View File

@ -27,7 +27,8 @@ PX4UARTDriver::PX4UARTDriver(const char *devpath, const char *perf_name) :
_baudrate(57600),
_perf_uart(perf_alloc(PC_ELAPSED, perf_name)),
_initialised(false),
_in_timer(false)
_in_timer(false),
_flow_control(FLOW_CONTROL_DISABLE)
{
}
@ -45,7 +46,7 @@ void PX4UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
return;
}
uint16_t min_tx_buffer = 512;
uint16_t min_tx_buffer = 1024;
uint16_t min_rx_buffer = 512;
if (strcmp(_devpath, "/dev/ttyACM0") == 0) {
min_tx_buffer = 16384;
@ -108,6 +109,17 @@ 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) {
@ -132,7 +144,7 @@ void PX4UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
}
}
void PX4UARTDriver::enable_flow_control(bool enable)
void PX4UARTDriver::set_flow_control(enum flow_control flow_control)
{
if (_fd == -1) {
return;
@ -140,12 +152,13 @@ void PX4UARTDriver::enable_flow_control(bool enable)
struct termios t;
tcgetattr(_fd, &t);
// we already enabled CRTS_IFLOW above, just enable output flow control
if (enable) {
if (flow_control != FLOW_CONTROL_DISABLE) {
t.c_cflag |= CRTSCTS;
} else {
t.c_cflag &= ~CRTSCTS;
}
tcsetattr(_fd, TCSANOW, &t);
_flow_control = flow_control;
}
void PX4UARTDriver::begin(uint32_t b)
@ -363,7 +376,19 @@ 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
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 &&
hrt_elapsed_time(&_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 (nwrite > n) {
nwrite = n;
}
@ -375,10 +400,12 @@ int PX4UARTDriver::_write_fd(const uint8_t *buf, uint16_t n)
if (ret > 0) {
BUF_ADVANCEHEAD(_writebuf, ret);
_last_write_time = hrt_absolute_time();
_total_written += ret;
return ret;
}
if (hrt_absolute_time() - _last_write_time > 2000) {
if (hrt_absolute_time() - _last_write_time > 2000 &&
_flow_control == FLOW_CONTROL_DISABLE) {
#if 0
// this trick is disabled for now, as it sometimes blocks on
// re-opening the ttyACM0 port, which would cause a crash
@ -432,6 +459,7 @@ int PX4UARTDriver::_read_fd(uint8_t *buf, uint16_t n)
}
if (ret > 0) {
BUF_ADVANCETAIL(_readbuf, ret);
_total_read += ret;
}
return ret;
}

7
libraries/AP_HAL_PX4/UARTDriver.h
View File

@ -36,7 +36,7 @@ public:
return _fd;
}
void enable_flow_control(bool enable);
void set_flow_control(enum flow_control flow_control);
private:
const char *_devpath;
@ -69,6 +69,11 @@ private:
void try_initialise(void);
uint32_t _last_initialise_attempt_ms;
uint32_t _os_write_buffer_size;
uint32_t _total_read;
uint32_t _total_written;
enum flow_control _flow_control;
};
#endif // __AP_HAL_PX4_UARTDRIVER_H__