Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

HAL_SITL: added support for real UART devices

Browse Source 
This allows use of real UART devices in the simulator. Useful for GPS
driver work or other MAVLink peripherals
This commit is contained in:
Andrew Tridgell 2015-11-04 07:59:43 +11:00
parent 6984168a45
commit a5a1680fb3
4 changed files with 132 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_HAL_SITL/SITL_State.h
View File

@ -58,6 +58,15 @@ public:
// return TCP client address for uartC
const char *get_client_address(void) const { return _client_address; }
// paths for UART devices
const char *_uart_path[5] {
"tcp:0:wait",
"GPS1",
"tcp:2",
"tcp:3",
"GPS2"
};
private:
void _parse_command_line(int argc, char * const argv[]);
void _set_param_default(const char *parm);

21
libraries/AP_HAL_SITL/SITL_cmdline.cpp
View File

@ -102,7 +102,12 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
enum long_options {
CMDLINE_CLIENT=0,
CMDLINE_GIMBAL,
CMDLINE_AUTOTESTDIR
CMDLINE_AUTOTESTDIR,
CMDLINE_UARTA,
CMDLINE_UARTB,
CMDLINE_UARTC,
CMDLINE_UARTD,
CMDLINE_UARTE
};
const struct GetOptLong::option options[] = {
@ -116,6 +121,11 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
{"synthetic-clock", false, 0, 'S'},
{"home", true, 0, 'O'},
{"model", true, 0, 'M'},
{"uartA", true, 0, CMDLINE_UARTA},
{"uartB", true, 0, CMDLINE_UARTB},
{"uartC", true, 0, CMDLINE_UARTC},
{"uartD", true, 0, CMDLINE_UARTD},
{"uartE", true, 0, CMDLINE_UARTE},
{"client", true, 0, CMDLINE_CLIENT},
{"gimbal", false, 0, CMDLINE_GIMBAL},
{"autotest-dir", true, 0, CMDLINE_AUTOTESTDIR},
@ -171,6 +181,15 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
case CMDLINE_AUTOTESTDIR:
autotest_dir = strdup(gopt.optarg);
break;
case CMDLINE_UARTA:
case CMDLINE_UARTB:
case CMDLINE_UARTC:
case CMDLINE_UARTD:
case CMDLINE_UARTE:
_uart_path[opt - CMDLINE_UARTA] = gopt.optarg;
break;
default:
_usage();
exit(1);

137
libraries/AP_HAL_SITL/UARTDriver.cpp
View File

@ -35,10 +35,13 @@
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <sys/select.h>
#include <termios.h>
#include "UARTDriver.h"
#include "SITL_State.h"
extern const AP_HAL::HAL& hal;
using namespace HALSITL;
bool SITLUARTDriver::_console;
@ -53,34 +56,48 @@ void SITLUARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace)
if (rxSpace != 0) {
_rxSpace = rxSpace;
}
switch (_portNumber) {
case 0:
_tcp_start_connection(true);
break;
case 1:
const char *path = _sitlState->_uart_path[_portNumber];
if (strcmp(path, "GPS1") == 0) {
/* gps */
_connected = true;
_fd = _sitlState->gps_pipe();
break;
case 2:
if (_sitlState->get_client_address() != NULL) {
_tcp_start_client(_sitlState->get_client_address());
} else {
_tcp_start_connection(false);
}
break;
case 4:
/* gps2 */
} else if (strcmp(path, "GPS2") == 0) {
/* 2nd gps */
_connected = true;
_fd = _sitlState->gps2_pipe();
break;
default:
_tcp_start_connection(false);
break;
} else {
/* parse type:args:flags string for path.
For example:
tcp:5760:wait // tcp listen on port 5760
tcp:0:wait // tcp listen on use base_port + 0
tcpclient:192.168.2.15:5762
uart:/dev/ttyUSB0:57600
*/
char *saveptr = NULL;
char *s = strdup(path);
char *devtype = strtok_r(s, ":", &saveptr);
char *args1 = strtok_r(NULL, ":", &saveptr);
char *args2 = strtok_r(NULL, ":", &saveptr);
if (strcmp(devtype, "tcp") == 0) {
uint16_t port = atoi(args1);
bool wait = (args2 && strcmp(args2, "wait") == 0);
_tcp_start_connection(port, wait);
} else if (strcmp(devtype, "tcpclient") == 0) {
if (args2 == NULL) {
hal.scheduler->panic("Invalid tcp client path: %s", path);
}
uint16_t port = atoi(args2);
_tcp_start_client(args1, port);
} else if (strcmp(devtype, "uart") == 0) {
uint32_t baudrate = args2? atoi(args2) : 57600;
::printf("UART connection %s:%u\n", args1, baudrate);
_uart_start_connection(args1, baudrate);
} else {
hal.scheduler->panic("Invalid device path: %s", path);
}
free(s);
}
}
@ -142,8 +159,12 @@ int16_t SITLUARTDriver::read(void)
return -1;
}
if (_console) {
return ::read(0, &c, 1);
if (!_use_send_recv) {
int fd = _console?0:_fd;
if (::read(fd, &c, 1) != 1) {
return -1;
}
return (uint8_t)c;
}
int n = recv(_fd, &c, 1, MSG_DONTWAIT);
@ -175,7 +196,7 @@ size_t SITLUARTDriver::write(uint8_t c)
if (_nonblocking_writes) {
flags |= MSG_DONTWAIT;
}
if (_console) {
if (!_use_send_recv) {
return ::write(_fd, &c, 1);
}
return send(_fd, &c, 1, flags);
@ -194,7 +215,7 @@ size_t SITLUARTDriver::write(const uint8_t *buffer, size_t size)
start a TCP connection for the serial port. If wait_for_connection
is true then block until a client connects
*/
void SITLUARTDriver::_tcp_start_connection(bool wait_for_connection)
void SITLUARTDriver::_tcp_start_connection(uint16_t port, bool wait_for_connection)
{
int one=1;
struct sockaddr_in sockaddr;
@ -204,9 +225,12 @@ void SITLUARTDriver::_tcp_start_connection(bool wait_for_connection)
return;
}
_use_send_recv = true;
if (_console) {
// hack for console access
_connected = true;
_use_send_recv = false;
_listen_fd = -1;
_fd = 1;
return;
@ -222,7 +246,11 @@ void SITLUARTDriver::_tcp_start_connection(bool wait_for_connection)
#ifdef HAVE_SOCK_SIN_LEN
sockaddr.sin_len = sizeof(sockaddr);
#endif
sockaddr.sin_port = htons(_sitlState->base_port() + _portNumber);
if (port > 1000) {
sockaddr.sin_port = htons(port);
} else {
sockaddr.sin_port = htons(_sitlState->base_port() + port);
}
sockaddr.sin_family = AF_INET;
_listen_fd = socket(AF_INET, SOCK_STREAM, 0);
@ -275,7 +303,7 @@ void SITLUARTDriver::_tcp_start_connection(bool wait_for_connection)
/*
start a TCP client connection for the serial port.
*/
void SITLUARTDriver::_tcp_start_client(const char *address)
void SITLUARTDriver::_tcp_start_client(const char *address, uint16_t port)
{
int one=1;
struct sockaddr_in sockaddr;
@ -285,19 +313,12 @@ void SITLUARTDriver::_tcp_start_client(const char *address)
return;
}
_use_send_recv = true;
if (_fd != -1) {
close(_fd);
}
char *addr2 = strdup(address);
char *p = strchr(addr2, ':');
if (p == NULL) {
fprintf(stderr, "need IP:port\n");
exit(1);
}
*p = 0;
uint16_t port = htons(atoi(p+1));
memset(&sockaddr,0,sizeof(sockaddr));
#ifdef HAVE_SOCK_SIN_LEN
@ -305,9 +326,7 @@ void SITLUARTDriver::_tcp_start_client(const char *address)
#endif
sockaddr.sin_port = port;
sockaddr.sin_family = AF_INET;
sockaddr.sin_addr.s_addr = inet_addr(addr2);
free(addr2);
sockaddr.sin_addr.s_addr = inet_addr(address);
_fd = socket(AF_INET, SOCK_STREAM, 0);
if (_fd == -1) {
@ -331,6 +350,44 @@ void SITLUARTDriver::_tcp_start_client(const char *address)
_connected = true;
}
/*
start a UART connection for the serial port
*/
void SITLUARTDriver::_uart_start_connection(const char *path, uint32_t baudrate)
{
struct termios t {};
if (!_connected) {
::printf("Opening %s\n", path);
_fd = ::open(path, O_RDWR | O_CLOEXEC);
}
if (_fd == -1) {
hal.scheduler->panic("Unable to open UART %s", path);
}
// set non-blocking
int flags = fcntl(_fd, F_GETFL, 0);
flags = flags | O_NONBLOCK;
fcntl(_fd, F_SETFL, flags);
// disable LF -> CR/LF
tcgetattr(_fd, &t);
t.c_iflag &= ~(BRKINT | ICRNL | IMAXBEL | IXON | IXOFF);
t.c_oflag &= ~(OPOST | ONLCR);
t.c_lflag &= ~(ISIG | ICANON | IEXTEN | ECHO | ECHOE | ECHOK | ECHOCTL | ECHOKE);
t.c_cc[VMIN] = 0;
tcsetattr(_fd, TCSANOW, &t);
// set baudrate
tcgetattr(_fd, &t);
cfsetspeed(&t, baudrate);
tcsetattr(_fd, TCSANOW, &t);
_connected = true;
_use_send_recv = false;
}
/*
see if a new connection is coming in
*/

9
libraries/AP_HAL_SITL/UARTDriver.h
View File

@ -8,6 +8,7 @@
#include <stdint.h>
#include <stdarg.h>
#include "AP_HAL_SITL_Namespace.h"
#include <AP_HAL/utility/Socket.h>
class HALSITL::SITLUARTDriver : public AP_HAL::UARTDriver {
public:
@ -57,7 +58,8 @@ public:
private:
uint8_t _portNumber;
bool _connected; // true if a client has connected
bool _connected = false; // true if a client has connected
bool _use_send_recv = false;
int _listen_fd; // socket we are listening on
int _serial_port;
static bool _console;
@ -68,8 +70,9 @@ private:
// IPv4 address of target for uartC
const char *_tcp_client_addr;
void _tcp_start_connection(bool wait_for_connection);
void _tcp_start_client(const char *address);
void _tcp_start_connection(uint16_t port, bool wait_for_connection);
void _uart_start_connection(const char *path, uint32_t baudrate);
void _tcp_start_client(const char *address, uint16_t port);
void _check_connection(void);
static bool _select_check(int );
static void _set_nonblocking(int );