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

HAL_SITL: allowed SITL to reopen uart connections

Browse Source
This commit is contained in:
Andrew Tridgell 2016-08-22 17:11:48 +10:00
parent 49b82b767f
commit 25f1dbd8c2
2 changed files with 35 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
libraries/AP_HAL_SITL/UARTDriver.cpp
View File

@ -86,7 +86,9 @@ void UARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace)
} else if (strcmp(devtype, "uart") == 0) {
uint32_t baudrate = args2? atoi(args2) : baud;
::printf("UART connection %s:%u\n", args1, baudrate);
_uart_start_connection(args1, baudrate);
_uart_path = strdup(args1);
_uart_baudrate = baudrate;
_uart_start_connection();
} else {
AP_HAL::panic("Invalid device path: %s", path);
}
@ -299,19 +301,22 @@ void UARTDriver::_tcp_start_client(const char *address, uint16_t port)
/*
start a UART connection for the serial port
*/
void UARTDriver::_uart_start_connection(const char *path, uint32_t baudrate)
void UARTDriver::_uart_start_connection(void)
{
struct termios t {};
if (!_connected) {
::printf("Opening %s\n", path);
_fd = ::open(path, O_RDWR | O_CLOEXEC);
_fd = ::open(_uart_path, O_RDWR | O_CLOEXEC);
if (_fd == -1) {
return;
}
// use much smaller buffer sizes on real UARTs
_writebuffer.set_size(1024);
_readbuffer.set_size(512);
::printf("Opened %s\n", _uart_path);
}
if (_fd == -1) {
AP_HAL::panic("Unable to open UART %s", path);
AP_HAL::panic("Unable to open UART %s", _uart_path);
}
// set non-blocking
@ -332,7 +337,7 @@ void UARTDriver::_uart_start_connection(const char *path, uint32_t baudrate)
// set baudrate
tcgetattr(_fd, &t);
cfsetspeed(&t, baudrate);
cfsetspeed(&t, _uart_baudrate);
tcsetattr(_fd, TCSANOW, &t);
_connected = true;
@ -390,9 +395,18 @@ void UARTDriver::_set_nonblocking(int fd)
fcntl(fd, F_SETFL, v | O_NONBLOCK);
}
void UARTDriver::_check_reconnect(void)
{
if (!_uart_path) {
return;
}
_uart_start_connection();
}
void UARTDriver::_timer_tick(void)
{
if (!_connected) {
_check_reconnect();
return;
}
uint32_t navail;
@ -402,6 +416,11 @@ void UARTDriver::_timer_tick(void)
if (readptr && navail > 0) {
if (!_use_send_recv) {
nwritten = ::write(_fd, readptr, navail);
if (nwritten == -1 && errno != EAGAIN && _uart_path) {
close(_fd);
_fd = -1;
_connected = false;
}
} else {
nwritten = send(_fd, readptr, navail, MSG_DONTWAIT);
}
@ -420,6 +439,11 @@ void UARTDriver::_timer_tick(void)
if (!_use_send_recv) {
int fd = _console?0:_fd;
nread = ::read(fd, buf, space);
if (nread == -1 && errno != EAGAIN && _uart_path) {
close(_fd);
_fd = -1;
_connected = false;
}
} else {
if (_select_check(_fd)) {
nread = recv(_fd, buf, space, MSG_DONTWAIT);

6
libraries/AP_HAL_SITL/UARTDriver.h
View File

@ -73,11 +73,15 @@ private:
ByteBuffer _readbuffer{16384};
ByteBuffer _writebuffer{16384};
const char *_uart_path;
uint32_t _uart_baudrate;
// IPv4 address of target for uartC
const char *_tcp_client_addr;
void _tcp_start_connection(uint16_t port, bool wait_for_connection);
void _uart_start_connection(const char *path, uint32_t baudrate);
void _uart_start_connection(void);
void _check_reconnect();
void _tcp_start_client(const char *address, uint16_t port);
void _check_connection(void);
static bool _select_check(int );