Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-06 07:53:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

HAL_SITL: added udpclient support

Browse Source 
this allows direct UDP output from SITL
This commit is contained in:
Andrew Tridgell 2018-12-05 16:32:49 +11:00
parent 4ae8595da3
commit 9f8f36aad8
2 changed files with 63 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
libraries/AP_HAL_SITL/UARTDriver.cpp
View File

@ -103,6 +103,11 @@ void UARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace)
_connected = true;
_fd = _sitlState->sim_fd(args1, args2);
}
} else if (strcmp(devtype, "udpclient") == 0) {
const char *ip = args1;
uint16_t port = args2?atoi(args2):14550;
::printf("UDP connection %s:%u\n", ip, port);
_udp_start_client(ip, port);
} else {
AP_HAL::panic("Invalid device path: %s", path);
}
@ -343,6 +348,61 @@ void UARTDriver::_tcp_start_client(const char *address, uint16_t port)
}
/*
start a UDP client connection for the serial port.
*/
void UARTDriver::_udp_start_client(const char *address, uint16_t port)
{
struct sockaddr_in sockaddr;
int ret;
if (_connected) {
return;
}
_use_send_recv = true;
if (_fd != -1) {
close(_fd);
}
memset(&sockaddr,0,sizeof(sockaddr));
#ifdef HAVE_SOCK_SIN_LEN
sockaddr.sin_len = sizeof(sockaddr);
#endif
sockaddr.sin_port = htons(port);
sockaddr.sin_family = AF_INET;
sockaddr.sin_addr.s_addr = inet_addr(address);
_fd = socket(AF_INET, SOCK_DGRAM, 0);
if (_fd == -1) {
fprintf(stderr, "socket failed - %s\n", strerror(errno));
exit(1);
}
ret = fcntl(_fd, F_SETFD, FD_CLOEXEC);
if (ret == -1) {
fprintf(stderr, "fcntl failed on setting FD_CLOEXEC - %s\n", strerror(errno));
exit(1);
}
// try to setup for broadcast, this may fail if insufficient privileges
int one = 1;
setsockopt(_fd,SOL_SOCKET,SO_BROADCAST,(char *)&one,sizeof(one));
ret = connect(_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
if (ret == -1) {
fprintf(stderr, "udp connect failed on port %u - %s\n",
(unsigned)ntohs(sockaddr.sin_port),
strerror(errno));
exit(1);
}
_is_udp = true;
_connected = true;
}
/*
start a UART connection for the serial port
*/
@ -507,7 +567,7 @@ void UARTDriver::_timer_tick(void)
} else {
if (_select_check(_fd)) {
nread = recv(_fd, buf, space, MSG_DONTWAIT);
if (nread <= 0) {
if (nread <= 0 && !_is_udp) {
// the socket has reached EOF
close(_fd);
_connected = false;

2
libraries/AP_HAL_SITL/UARTDriver.h
View File

@ -103,6 +103,7 @@ private:
void _uart_start_connection(void);
void _check_reconnect();
void _tcp_start_client(const char *address, uint16_t port);
void _udp_start_client(const char *address, uint16_t port);
void _check_connection(void);
static bool _select_check(int );
static void _set_nonblocking(int );
@ -110,6 +111,7 @@ private:
SITL_State *_sitlState;
uint64_t _receive_timestamp;
bool _is_udp;
};
#endif