Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-06 19:00:27 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
d317bfaf2e3ed37e16bf2a3bcbf7c657e3cdcbd4
ardupilot/libraries/AP_HAL_SITL/UARTDriver.cpp
T
Tim TuxworthandClaude Sonnet 5 0d851d30fe AP_HAL_SITL: UARTDriver: simplify mcast interface pin, trim comment 
Two optional-polish points from Tridge's AI review of this PR: the
imr_interface ternary was redundant (htonl(INADDR_ANY) is 0, same as
mcast_if_addr when unset), and the routing-background comment mostly
duplicated SITL_Multicast.h's own. Simplify and trim.

The review's third suggestion (set IP_MULTICAST_IF before connect(),
matching Socket.cpp's ordering) is not applied here: Socket.cpp reaches
that ordering by creating and connecting the socket inline, with two
separate fds (fd/fd_in) for the send and receive sides.
_udp_start_multicast() instead delegates to the shared
_udp_start_client() for the send-side socket, which creates and
connects _fd as one atomic step with no exposed hook point between
the two -- reordering would mean restructuring that shared helper
(used by other UDP client callers too) for a change the review already
confirmed is behaviour-neutral on both Linux and macOS. Not worth the
risk for a defensive-only change.

Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>
2026-09-08 10:30:25 +10:00

1286 lines
41 KiB
C++
Raw Blame History

/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
//
// Copyright (c) 2010 Michael Smith. All rights reserved.
//
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <limits.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <fcntl.h>
#include <stdarg.h>
#include <AP_Math/AP_Math.h>
#include <errno.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/un.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <sys/select.h>
#include <termios.h>
#include <sys/time.h>
#include <arpa/inet.h>
#include "UARTDriver.h"
#include "SITL_State.h"
#include "SITL_Multicast.h"
#if HAL_GCS_ENABLED
#include <AP_HAL/utility/packetise.h>
#endif
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include <AP_Filesystem/AP_Filesystem.h>
#include <AP_Common/ExpandingString.h>
#include <AP_HAL/utility/Socket_native.h>
extern const AP_HAL::HAL& hal;
using namespace HALSITL;
bool UARTDriver::_console;
/* UARTDriver method implementations */
void UARTDriver::_begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace)
{
if (baud == 0 && rxSpace == 0 && txSpace == 0) {
// this is a claim of the uart for the current thread, which
// is currently not implemented in SITL
return;
}
if (_portNumber >= ARRAY_SIZE(_sitlState->_serial_path)) {
AP_HAL::panic("port number out of range; you may need to extend _sitlState->_serial_path");
}
const char *path = _sitlState->_serial_path[_portNumber];
if (baud != 0) {
_uart_baudrate = baud;
}
if (strcmp(path, "GPS1") == 0) {
/* gps */
_connected = true;
_sim_serial_device = _sitlState->create_serial_sim("gps", "1", _portNumber);
} else if (strcmp(path, "GPS2") == 0) {
/* 2nd gps */
_connected = true;
_sim_serial_device = _sitlState->create_serial_sim("gps", "2", _portNumber);
} 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
uds:APM-UDS-serial0:wait
tcpclient:192.168.2.15:5762
udpclient:127.0.0.1
udpclient:127.0.0.1:14550
mcast:
mcast:239.255.145.50:14550
uart:/dev/ttyUSB0:57600
sim:ParticleSensor_SDS021:
file:/tmp/my-device-capture.BIN
logic_async_csv:/tmp/logic_async.csv:
*/
char *saveptr = nullptr;
char *s = strdup(path);
char *devtype = strtok_r(s, ":", &saveptr);
char *args1 = strtok_r(nullptr, ":", &saveptr);
char *args2 = strtok_r(nullptr, ":", &saveptr);
#if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane)
if (_portNumber == 1 && AP::sitl()->adsb_plane_count >= 0) {
// this is ordinarily port 5762. The ADSB simulation assumed
// this port, so if enabled we assume we'll be doing ADSB...
// add sanity check here that we're doing mavlink on this port?
::printf("SIM-ADSB connection on SERIAL%u\n", _portNumber);
_connected = true;
_sim_serial_device = _sitlState->create_serial_sim("adsb", nullptr, _portNumber);
} else
#endif
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, "uds") == 0) {
if (args1 == nullptr || args1[0] == '\0') {
AP_HAL::panic("Invalid Unix domain socket path: %s", path);
}
bool wait = (args2 && strcmp(args2, "wait") == 0);
_unix_start_connection(args1, wait);
} else if (strcmp(devtype, "tcpclient") == 0) {
if (args2 == nullptr) {
AP_HAL::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) : baud;
if (_uart_path == nullptr) {
::printf("UART connection %s:%u\n", args1, baudrate);
_uart_path = strdup(args1);
}
_uart_baudrate = baudrate;
_uart_start_connection();
} else if (strcmp(devtype, "sim") == 0) {
if (!_connected) {
::printf("SIM connection %s:%s on SERIAL%u\n", args1, args2, _portNumber);
_connected = true;
_sim_serial_device = _sitlState->create_serial_sim(args1, args2, _portNumber);
}
} else if (strcmp(devtype, "udpclient") == 0) {
// udp client connection
const char *ip = args1;
uint16_t port = args2?atoi(args2):14550;
if (!_connected) {
::printf("UDP connection %s:%u\n", ip, port);
_udp_start_client(ip, port);
}
} else if (strcmp(devtype, "mcast") == 0) {
// udp multicast connection
const char *ip = args1 && *args1?args1:mcast_ip_default;
uint16_t port = args2?atoi(args2):mcast_port_default;
if (!_connected) {
::printf("UDP multicast connection %s:%u\n", ip, port);
_udp_start_multicast(ip, port);
}
} else if (strcmp(devtype,"none") == 0) {
// skipping port
::printf("Skipping port %s\n", args1);
} else if (strcmp(devtype, "file") == 0) {
if (_connected) {
AP::FS().close(_fd);
}
::printf("FILE connection %s\n", args1);
_fd = AP::FS().open(args1, O_RDONLY);
if (_fd == -1) {
AP_HAL::panic("Failed to open (%s): %m", args1);
}
_connected = true;
} else if (strcmp(devtype, "outfile") == 0) {
if (_connected) {
AP::FS().close(_fd);
}
::printf("FILE output connection %s\n", args1);
_fd = AP::FS().open(args1, O_WRONLY|O_CREAT|O_TRUNC, 0644);
if (_fd == -1) {
AP_HAL::panic("Failed to open (%s): %m", args1);
}
_connected = true;
} else if (strcmp(devtype, "logic_async_csv") == 0) {
if (_connected) {
AP::FS().close(_fd);
}
::printf("logic_async_csv connection %s\n", args1);
_fd = AP::FS().open(args1, O_RDONLY);
if (_fd == -1) {
AP_HAL::panic("Failed to open (%s): %m", args1);
}
_connected = true;
logic_async_csv.active = true;
} else {
AP_HAL::panic("Invalid device path: %s", path);
}
free(s);
}
if (_sim_serial_device != nullptr) {
_sim_serial_device->set_autopilot_baud(baud);
}
if (hal.console != this) { // don't clear USB buffers (allows early startup messages to escape)
_readbuffer.clear();
_writebuffer.clear();
}
}
void UARTDriver::_end()
{
}
uint32_t UARTDriver::_available(void)
{
_check_connection();
if (!_connected) {
return 0;
}
return _readbuffer.available();
}
uint32_t UARTDriver::txspace(void)
{
_check_connection();
if (!_connected) {
return 0;
}
return _writebuffer.space();
}
void UARTDriver::set_flow_control(enum flow_control flow_control_setting)
{
_flow_control = flow_control_setting;
if (_flow_control == FLOW_CONTROL_AUTO) {
// reset auto-detection state
_auto_flow_start_ms = 0;
_auto_flow_detected = false;
}
// If already connected on a real UART path, apply immediately
if (_uart_path && _connected && _fd != -1) {
struct termios t {};
if (tcgetattr(_fd, &t) == 0) {
if (_flow_control != FLOW_CONTROL_DISABLE) {
t.c_cflag |= CRTSCTS;
} else {
t.c_cflag &= ~CRTSCTS;
}
tcsetattr(_fd, TCSANOW, &t);
}
}
}
enum AP_HAL::UARTDriver::flow_control UARTDriver::get_flow_control(void)
{
if (_uart_path) {
return _flow_control;
}
return FLOW_CONTROL_ENABLE;
}
ssize_t UARTDriver::_read(uint8_t *buffer, uint16_t count)
{
const ssize_t ret = _readbuffer.read(buffer, count);
_rx_stats_bytes += ret;
return ret;
}
bool UARTDriver::_discard_input(void)
{
_readbuffer.clear();
return true;
}
void UARTDriver::_flush(void)
{
// flush the write buffer - but don't fail and don't
// infinitely-loop. This is not a good definition of "flush", but
// it was judged that we had to return from this function even if
// we hadn't actually done our job.
uint32_t start_ms = AP_HAL::millis();
while (AP_HAL::millis() - start_ms < 1000) {
if (_writebuffer.available() == 0) {
break;
}
_timer_tick();
}
// ensure that the outbound TCP queue is also empty...
start_ms = AP_HAL::millis();
while (AP_HAL::millis() - start_ms < 1000) {
if (((HALSITL::UARTDriver*)hal.serial(0))->get_system_outqueue_length() == 0) {
break;
}
usleep(1000);
}
}
size_t UARTDriver::_write(const uint8_t *buffer, size_t size)
{
const auto _txspace = txspace();
if (_txspace < size) {
size = _txspace;
}
if (size <= 0) {
return 0;
}
/*
simulate byte loss at the link layer
*/
uint8_t lost_byte = 0;
#if !defined(HAL_BUILD_AP_PERIPH)
SITL::SIM *_sitl = AP::sitl();
if (_sitl && _sitl->uart_byte_loss_pct > 0) {
if (fabsf(rand_float()) < _sitl->uart_byte_loss_pct.get() * 0.01 * size) {
lost_byte = 1;
}
}
#endif // HAL_BUILD_AP_PERIPH
// Include lost byte in tx count, we think we sent it even though it was never added to the write buffer
_tx_stats_bytes += lost_byte;
const size_t ret = _writebuffer.write(buffer, size - lost_byte) + lost_byte;
if (_unbuffered_writes) {
handle_writing_from_writebuffer_to_device();
}
return ret;
}
/*
start a TCP connection for the serial port. If wait_for_connection
is true then block until a client connects
*/
void UARTDriver::_tcp_start_connection(uint16_t port, bool wait_for_connection)
{
int one=1;
int ret;
struct sockaddr_in _listen_sockaddr {};
if (_connected) {
return;
}
_use_send_recv = true;
if (_console) {
// hack for console access
_connected = true;
_use_send_recv = false;
_listen_fd = -1;
_fd = 1;
return;
}
if (_fd != -1) {
close(_fd);
}
if (_listen_fd == -1) {
memset(&_listen_sockaddr,0,sizeof(_listen_sockaddr));
#ifdef HAVE_SOCK_SIN_LEN
_listen_sockaddr.sin_len = sizeof(_listen_sockaddr);
#endif
if (port > 1000) {
_listen_sockaddr.sin_port = htons(port);
} else {
_listen_sockaddr.sin_port = htons(_sitlState->base_port() + port);
}
_listen_sockaddr.sin_family = AF_INET;
_listen_fd = socket(AF_INET, SOCK_STREAM, 0);
if (_listen_fd == -1) {
fprintf(stderr, "socket failed - %s\n", strerror(errno));
exit(1);
}
ret = fcntl(_listen_fd, F_SETFD, FD_CLOEXEC);
if (ret == -1) {
fprintf(stderr, "fcntl failed on setting FD_CLOEXEC - %s\n", strerror(errno));
exit(1);
}
/* we want to be able to re-use ports quickly */
if (setsockopt(_listen_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)) == -1) {
fprintf(stderr, "setsockopt failed: %s\n", strerror(errno));
exit(1);
}
fprintf(stderr, "bind port %u for SERIAL%u\n",
(unsigned)ntohs(_listen_sockaddr.sin_port),
(unsigned)_portNumber);
ret = bind(_listen_fd, (struct sockaddr *)&_listen_sockaddr, sizeof(_listen_sockaddr));
if (ret == -1) {
fprintf(stderr, "bind failed on port %u - %s\n",
(unsigned)ntohs(_listen_sockaddr.sin_port),
strerror(errno));
exit(1);
}
ret = listen(_listen_fd, 5);
if (ret == -1) {
fprintf(stderr, "listen failed - %s\n", strerror(errno));
exit(1);
}
fprintf(stderr, "SERIAL%u on TCP port %u\n", _portNumber,
(unsigned)ntohs(_listen_sockaddr.sin_port));
fflush(stdout);
}
if (wait_for_connection) {
fprintf(stdout, "Waiting for connection ....\n");
fflush(stdout);
_fd = accept(_listen_fd, nullptr, nullptr);
if (_fd == -1) {
fprintf(stderr, "accept() error - %s", strerror(errno));
exit(1);
}
setsockopt(_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
setsockopt(_fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one));
fcntl(_fd, F_SETFD, FD_CLOEXEC);
_connected = true;
fprintf(stdout, "Connection on serial port %u\n", (unsigned)ntohs(_listen_sockaddr.sin_port));
}
}
/*
start a Unix domain socket connection for the serial port. If
wait_for_connection is true then block until a client connects
*/
void UARTDriver::_unix_start_connection(const char *path, bool wait_for_connection)
{
struct sockaddr_un listen_sockaddr {};
if (_connected) {
return;
}
_use_send_recv = true;
_is_unix_socket = true;
if (_console) {
_connected = true;
_use_send_recv = false;
_listen_fd = -1;
_fd = 1;
return;
}
if (_fd != -1) {
close(_fd);
}
if (_listen_fd == -1) {
if (strlen(path) >= sizeof(listen_sockaddr.sun_path)) {
fprintf(stderr, "Unix domain socket path is too long: %s\n", path);
exit(1);
}
listen_sockaddr.sun_family = AF_UNIX;
strncpy(listen_sockaddr.sun_path, path, sizeof(listen_sockaddr.sun_path) - 1);
_listen_fd = socket(AF_UNIX, SOCK_STREAM, 0);
if (_listen_fd == -1) {
fprintf(stderr, "socket failed - %s\n", strerror(errno));
exit(1);
}
if (fcntl(_listen_fd, F_SETFD, FD_CLOEXEC) == -1) {
fprintf(stderr, "fcntl failed on setting FD_CLOEXEC - %s\n", strerror(errno));
exit(1);
}
int ret = bind(_listen_fd, (struct sockaddr *)&listen_sockaddr, sizeof(listen_sockaddr));
if (ret == -1 && errno == EADDRINUSE) {
struct stat path_stat;
if (lstat(path, &path_stat) == 0 && S_ISSOCK(path_stat.st_mode)) {
int probe_fd = socket(AF_UNIX, SOCK_STREAM, 0);
int connect_ret = probe_fd == -1 ? -1 : connect(
probe_fd,
(struct sockaddr *)&listen_sockaddr,
sizeof(listen_sockaddr));
int connect_errno = errno;
if (probe_fd != -1) {
close(probe_fd);
}
if (connect_ret == -1 && connect_errno == ECONNREFUSED) {
// A newly bound stream socket also refuses connections in
// the brief interval before listen(). Give it time to
// become live before treating the path as stale.
usleep(100000);
probe_fd = socket(AF_UNIX, SOCK_STREAM, 0);
connect_ret = probe_fd == -1 ? -1 : connect(
probe_fd,
(struct sockaddr *)&listen_sockaddr,
sizeof(listen_sockaddr));
connect_errno = errno;
if (probe_fd != -1) {
close(probe_fd);
}
struct stat current_path_stat;
if (connect_ret == -1 && connect_errno == ECONNREFUSED &&
lstat(path, &current_path_stat) == 0 &&
S_ISSOCK(current_path_stat.st_mode) &&
current_path_stat.st_dev == path_stat.st_dev &&
current_path_stat.st_ino == path_stat.st_ino &&
unlink(path) == 0) {
ret = bind(_listen_fd, (struct sockaddr *)&listen_sockaddr, sizeof(listen_sockaddr));
}
}
}
}
if (ret == -1) {
fprintf(stderr, "bind failed on Unix domain socket %s - %s\n", path, strerror(errno));
exit(1);
}
SocketAPM_native::register_unix_path(_listen_fd, path);
if (listen(_listen_fd, 5) == -1) {
fprintf(stderr, "listen failed - %s\n", strerror(errno));
exit(1);
}
fprintf(stderr, "SERIAL%u on Unix domain socket %s\n", _portNumber, path);
fflush(stdout);
}
if (wait_for_connection) {
fprintf(stdout, "Waiting for connection ....\n");
fflush(stdout);
_fd = accept(_listen_fd, nullptr, nullptr);
if (_fd == -1) {
fprintf(stderr, "accept() error - %s", strerror(errno));
exit(1);
}
if (fcntl(_fd, F_SETFD, FD_CLOEXEC) == -1) {
fprintf(stderr, "fcntl failed on setting FD_CLOEXEC - %s\n", strerror(errno));
exit(1);
}
_connected = true;
fprintf(stdout, "Connection on Unix domain socket for SERIAL%u\n", _portNumber);
}
}
/*
start a TCP client connection for the serial port.
*/
void UARTDriver::_tcp_start_client(const char *address, uint16_t port)
{
if (_connected) {
return;
}
_use_send_recv = true;
if (_fd != -1) {
close(_fd);
}
struct sockaddr_in sockaddr;
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);
constexpr auto one=1;
int ret;
// Retry for up to 30 seconds. Companion processes (e.g. AP_Periph
// connecting to an ArduPlane SITL TCP listener for a PPP link) can be
// launched ahead of the peer's socket, so a few seconds of grace is
// not enough when the peer is also rebuilding.
constexpr uint8_t max_attempts = 30;
for (uint8_t attempt = 0; attempt < max_attempts; ++attempt) {
_fd = socket(AF_INET, SOCK_STREAM, 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);
}
/* we want to be able to re-use ports quickly */
setsockopt(_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
ret = connect(_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
if (ret == 0) {
break;
}
fprintf(stderr, "connect failed on port %u - %s at %d retrying\n",
(unsigned) ntohs(sockaddr.sin_port), strerror(errno), AP_HAL::millis());
close(_fd);
// If connection failed, wait for a bit before retrying
sleep(1);
}
if (ret == -1) {
close(_fd);
fprintf(stderr, "connect failed on port %u - %s\n",
(unsigned)ntohs(sockaddr.sin_port),
strerror(errno));
exit(1);
}
setsockopt(_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
setsockopt(_fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one));
fcntl(_fd, F_SETFD, FD_CLOEXEC);
_connected = true;
fprintf(stdout, "New remote connection on serial port %u, p %u at %d\n", _portNumber, (unsigned) ntohs(sockaddr.sin_port), AP_HAL::millis());
}
/*
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);
}
fcntl(_fd, F_SETFL, fcntl(_fd, F_GETFL, 0) | O_NONBLOCK);
// 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;
#if HAL_GCS_ENABLED
_packetise = true;
#endif
_connected = true;
}
/*
start a UDP multicast connection
*/
void UARTDriver::_udp_start_multicast(const char *address, uint16_t port)
{
if (_connected) {
return;
}
// establish the listening port
struct sockaddr_in sockaddr;
int ret;
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);
_mc_fd = socket(AF_INET, SOCK_DGRAM, 0);
if (_mc_fd == -1) {
fprintf(stderr, "socket failed - %s\n", strerror(errno));
exit(1);
}
ret = fcntl(_mc_fd, F_SETFD, FD_CLOEXEC);
if (ret == -1) {
fprintf(stderr, "fcntl failed on setting FD_CLOEXEC - %s\n", strerror(errno));
exit(1);
}
fcntl(_mc_fd, F_SETFL, fcntl(_mc_fd, F_GETFL, 0) | O_NONBLOCK);
int one = 1;
if (setsockopt(_mc_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)) == -1) {
fprintf(stderr, "setsockopt failed: %s\n", strerror(errno));
exit(1);
}
// close on exec, to allow reboot
fcntl(_mc_fd, F_SETFD, FD_CLOEXEC);
#if defined(__CYGWIN__) || defined(__CYGWIN64__) || defined(CYGWIN_BUILD)
/*
on cygwin you need to bind to INADDR_ANY then use the multicast
IP_ADD_MEMBERSHIP to get on the right address
*/
sockaddr.sin_addr.s_addr = htonl(INADDR_ANY);
#endif
ret = bind(_mc_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
if (ret == -1) {
fprintf(stderr, "multicast bind failed on port %u - %s\n",
(unsigned)ntohs(sockaddr.sin_port),
strerror(errno));
exit(1);
}
// pin to one interface when SITL_MULTICAST_IF_ADDR is set (see
// SITL_Multicast.h). CAN_Multicast/SITL_Periph_State/the sim-state
// broadcast already do this; this UDP MAVLink mcast link
// (--serial5=mcast: etc) did not.
const uint32_t mcast_if_addr = sitl_multicast_interface_address();
struct ip_mreq mreq {};
mreq.imr_multiaddr.s_addr = inet_addr(address);
mreq.imr_interface.s_addr = mcast_if_addr;
ret = setsockopt(_mc_fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq));
if (ret == -1) {
fprintf(stderr, "multicast membership add failed on port %u - %s\n",
(unsigned)ntohs(sockaddr.sin_port),
strerror(errno));
exit(1);
}
// now start the outgoing connection as an ordinary UDP connection
_udp_start_client(address, port);
if (mcast_if_addr != 0) {
// also send on that interface; without this the routing table
// chooses for the outgoing side too
struct in_addr ifaddr {};
ifaddr.s_addr = mcast_if_addr;
if (setsockopt(_fd, IPPROTO_IP, IP_MULTICAST_IF, &ifaddr, sizeof(ifaddr)) == -1) {
fprintf(stderr, "multicast IP_MULTICAST_IF failed on port %u - %s\n",
(unsigned)port, strerror(errno));
exit(1);
}
}
}
/*
start a UART connection for the serial port
*/
void UARTDriver::_uart_start_connection(void)
{
struct termios t {};
if (!_connected) {
_fd = ::open(_uart_path, O_RDWR | O_CLOEXEC);
if (_fd == -1) {
static uint32_t last_error_print_ms;
if (AP_HAL::millis() - last_error_print_ms > 5000) {
::printf("Failed to open (%s): %s\n", _uart_path, strerror(errno));
last_error_print_ms = AP_HAL::millis();
}
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", _uart_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;
if (_flow_control != FLOW_CONTROL_DISABLE) {
t.c_cflag |= CRTSCTS;
}
tcsetattr(_fd, TCSANOW, &t);
// set baudrate
if (_uart_baudrate != 0) {
set_speed(_uart_baudrate);
}
_connected = true;
_use_send_recv = false;
}
/*
see if a new connection is coming in
*/
void UARTDriver::_check_connection(void)
{
if (_connected) {
// we only want 1 connection at a time
return;
}
if (_select_check(_listen_fd)) {
_fd = accept(_listen_fd, nullptr, nullptr);
if (_fd != -1) {
int one = 1;
if (!_is_unix_socket) {
setsockopt(_fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one));
}
setsockopt(_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
if (fcntl(_fd, F_SETFD, FD_CLOEXEC) == -1) {
fprintf(stderr, "fcntl failed on setting FD_CLOEXEC - %s\n", strerror(errno));
close(_fd);
_fd = -1;
return;
}
_connected = true;
fprintf(stdout, "New connection on SERIAL%u\n", _portNumber);
}
}
}
/*
use select() to see if something is pending
*/
bool UARTDriver::_select_check(int fd)
{
if (fd == -1) {
return false;
}
#if !APM_BUILD_TYPE(APM_BUILD_Replay)
fd_set fds;
struct timeval tv;
FD_ZERO(&fds);
FD_SET(fd, &fds);
// zero time means immediate return from select()
tv.tv_sec = 0;
tv.tv_usec = 0;
if (select(fd+1, &fds, nullptr, nullptr, &tv) == 1) {
return true;
}
#endif
return false;
}
bool UARTDriver::set_unbuffered_writes(bool on) {
if (_fd == -1) {
return false;
}
_unbuffered_writes = on;
// this has no effect
unsigned v = fcntl(_fd, F_GETFL, 0);
v &= ~O_NONBLOCK;
#if defined(__APPLE__) && defined(__MACH__)
fcntl(_fd, F_SETFL | F_NOCACHE, v | O_SYNC);
#elif defined(__OpenBSD__)
fcntl(_fd, F_SETFL, v | O_SYNC);
#else
fcntl(_fd, F_SETFL, v | O_DIRECT | O_SYNC);
#endif
return _unbuffered_writes;
}
void UARTDriver::_check_reconnect(void)
{
if (!_uart_path) {
return;
}
_uart_start_connection();
}
uint16_t UARTDriver::read_from_async_csv(uint8_t *buffer, uint16_t space)
{
if (_fd == -1) {
return 0;
}
const uint32_t micros = AP_HAL::micros();
if (micros < 5000000) {
// don't inject for the first several seconds
return 0;
}
uint8_t i;
for (i=0; i<space; i++) {
if (logic_async_csv.loaded) {
const uint32_t emit_timestamp_us = micros - logic_async_csv.first_emit_micros_us;
const uint32_t data_timestamp_us = logic_async_csv.loaded_data.timestamp_us - logic_async_csv.first_timestamp_us;
if (data_timestamp_us > emit_timestamp_us) {
return i;
}
buffer[i] = logic_async_csv.loaded_data.b;
logic_async_csv.loaded = false;
}
while (!logic_async_csv.loaded) {
uint8_t c;
const ssize_t nread = ::read(_fd, &c, 1);
if (nread == 0) {
// EOF
close(_fd);
_fd = -1;
return i;
}
// feed data into CSV Reader, handle new state:
auto retcode = logic_async_csv.csvreader.feed(c);
switch (retcode) {
case AP_CSVReader::RetCode::OK:
continue;
case AP_CSVReader::RetCode::ERROR:
AP_HAL::panic("Malformed CSV?");
case AP_CSVReader::RetCode::TERM_DONE:
case AP_CSVReader::RetCode::VECTOR_DONE:
switch (logic_async_csv.terms_seen) {
case 0: // start_time
if (!logic_async_csv.done_first_line) {
break;
}
logic_async_csv.loaded_data.timestamp_us = atof((char*)logic_async_csv.term) * 1000000; // seconds to microseconds
break;
case 1: // data
if (!logic_async_csv.done_first_line) {
break;
}
if (!hex_twochars_to_uint8((const char*)&logic_async_csv.term[2], logic_async_csv.loaded_data.b)) {
// invalid character; panic as we do for other
// malformed-CSV cases rather than silently stopping
AP_HAL::panic("Malformed CSV?");
}
break;
case 2: // error
case 3: // framing error
break;
case 4:
AP_HAL::panic("Too many terms in CSV, want (name,type,start_time,duration,data");
}
logic_async_csv.terms_seen++;
if (retcode != AP_CSVReader::RetCode::VECTOR_DONE) {
break;
}
// we've handled the last term, now handle the vector:
if (logic_async_csv.terms_seen != 4) {
AP_HAL::panic("Incorrect number off terms in CSV, want (Time [s],Value,Parity Error,Framing Error)");
}
logic_async_csv.terms_seen = 0;
if (!logic_async_csv.done_first_line) {
// skip the headers
logic_async_csv.done_first_line = true;
break;
}
if (logic_async_csv.first_timestamp_us == 0) {
logic_async_csv.first_timestamp_us = logic_async_csv.loaded_data.timestamp_us;
logic_async_csv.first_emit_micros_us = micros;
}
logic_async_csv.loaded = true;
}
}
}
return i;
}
void UARTDriver::handle_writing_from_writebuffer_to_device()
{
WITH_SEMAPHORE(write_mtx);
if (!_connected) {
_check_reconnect();
return;
}
ssize_t nwritten = 0;
uint32_t max_bytes = 10000;
#if !defined(HAL_BUILD_AP_PERIPH)
SITL::SIM *_sitl = AP::sitl();
if (_sitl && _sitl->telem_baudlimit_enable) {
// limit byte rate to configured baudrate
// Byte rate is bit rate divided by 10. 8 bits of data + start/stop bits
max_bytes = baud_limits.write.max_bytes(float(_uart_baudrate) * 0.1);
if (max_bytes == 0) {
return;
}
}
#endif
if (_packetise) {
uint16_t n = _writebuffer.available();
n = MIN(n, max_bytes);
#if HAL_GCS_ENABLED
if (n > 0) {
n = mavlink_packetise(_writebuffer, n);
}
#endif
if (n > 0) {
// keep as a single UDP packet
uint8_t tmpbuf[n];
_writebuffer.peekbytes(tmpbuf, n);
ssize_t ret = send(_fd, tmpbuf, n, MSG_DONTWAIT);
if (ret > 0) {
_writebuffer.advance(ret);
_tx_stats_bytes += ret;
}
}
} else {
uint32_t navail;
const uint8_t *readptr = _writebuffer.readptr(navail);
if (readptr && navail > 0) {
navail = MIN(navail, max_bytes);
if (_sim_serial_device != nullptr) {
nwritten = _sim_serial_device->write_to_device((const char*)readptr, navail);
} else if (!_use_send_recv) {
// For ENABLE/AUTO modes on a real UART, check CTS via ioctl
// before writing. This provides reliable flow control on
// platforms where kernel CRTSCTS may not work (e.g. macOS
// with USB-serial adapters).
bool cts_blocked = false;
if (_flow_control != FLOW_CONTROL_DISABLE) {
int modem_bits = 0;
if (ioctl(_fd, TIOCMGET, &modem_bits) == 0 &&
!(modem_bits & TIOCM_CTS)) {
// CTS deasserted - remote end asked us to stop sending
cts_blocked = true;
}
}
if (!cts_blocked) {
nwritten = ::write(_fd, readptr, navail);
if (nwritten == -1 && errno != EAGAIN && _uart_path) {
close(_fd);
_fd = -1;
_connected = false;
}
}
// AUTO flow control detection: mirror ChibiOS 500ms timeout logic
if (_flow_control == FLOW_CONTROL_AUTO && _uart_path) {
if (_auto_flow_start_ms == 0) {
_auto_flow_start_ms = AP_HAL::millis();
}
if (nwritten > 0 && !_auto_flow_detected) {
// first successful write - CTS active
::printf("SERIAL%u: hardware flow control enabled\n", _portNumber);
_auto_flow_detected = true;
} else if (!_auto_flow_detected && AP_HAL::millis() - _auto_flow_start_ms > 500U) {
// CTS never seen since AUTO was set - actually disable
::printf("SERIAL%u: hardware flow control not detected, disabled\n", _portNumber);
set_flow_control(FLOW_CONTROL_DISABLE);
}
}
} else {
nwritten = send(_fd, readptr, navail, MSG_DONTWAIT);
}
if (nwritten > 0) {
_writebuffer.advance(nwritten);
_tx_stats_bytes += nwritten;
}
}
}
}
void UARTDriver::handle_reading_from_device_to_readbuffer()
{
if (!_connected) {
_check_reconnect();
return;
}
uint32_t space = _readbuffer.space();
if (space == 0) {
return;
}
uint32_t max_bytes = 10000;
#if !defined(HAL_BUILD_AP_PERIPH)
SITL::SIM *_sitl = AP::sitl();
if (_sitl && _sitl->telem_baudlimit_enable) {
// limit byte rate to configured baudrate
// Byte rate is bit rate divided by 10. 8 bits of data + start/stop bits
max_bytes = baud_limits.read.max_bytes(float(_uart_baudrate) * 0.1);
if (max_bytes == 0) {
return;
}
}
#endif
space = MIN(space, max_bytes);
char buf[space];
ssize_t nread = 0;
if (_mc_fd >= 0) {
if (_select_check(_mc_fd)) {
struct sockaddr_in from;
socklen_t fromlen = sizeof(from);
nread = recvfrom(_mc_fd, buf, space, MSG_DONTWAIT, (struct sockaddr *)&from, &fromlen);
uint16_t port = ntohs(from.sin_port);
if (_mc_myport == 0) {
// get our own address, so we can recognise packets from ourself
struct sockaddr_in myaddr;
socklen_t myaddrlen;
if (getsockname(_fd, (struct sockaddr *)&myaddr, &myaddrlen) == 0) {
_mc_myport = ntohs(myaddr.sin_port);
}
}
if (_mc_myport == port) {
// assume this is a packet from ourselves. This is not
// entirely accurate, as it could be a packet from
// another machine that has assigned the same port,
// unfortunately we don't have a better way to detect
// packets from ourselves
nread = 0;
}
}
} else if (_sim_serial_device != nullptr) {
nread = _sim_serial_device->read_from_device(buf, space);
} else if (logic_async_csv.active) {
nread = read_from_async_csv((uint8_t*)buf, space);
} else if (!_use_send_recv) {
if (!_select_check(_fd)) {
return;
}
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);
if (nread <= 0 && !_is_udp) {
// the socket has reached EOF
close(_fd);
_fd = -1;
_connected = false;
fprintf(stdout, "Closed connection on SERIAL%u\n", _portNumber);
fflush(stdout);
#if defined(__CYGWIN__) || defined(__CYGWIN64__) || defined(CYGWIN_BUILD)
if (_portNumber == 0) {
// exit on cygwin port 0 is almost certainly closing the
// connection in MissionPlanner SITL. We want to exit or
// we leave a stray process which confuses restart
exit(0);
}
#endif
return;
}
}
if (nread > 0) {
_readbuffer.write((uint8_t *)buf, nread);
_receive_timestamp = AP_HAL::micros64();
}
}
void UARTDriver::_timer_tick(void)
{
handle_writing_from_writebuffer_to_device();
handle_reading_from_device_to_readbuffer();
}
/*
return timestamp estimate in microseconds for when the start of
a nbytes packet arrived on the uart. This should be treated as a
time constraint, not an exact time. It is guaranteed that the
packet did not start being received after this time, but it
could have been in a system buffer before the returned time.
This takes account of the baudrate of the link. For transports
that have no baudrate (such as USB) the time estimate may be
less accurate.
A return value of zero means the HAL does not support this API
*/
uint64_t UARTDriver::receive_time_constraint_us(uint16_t nbytes)
{
uint64_t last_receive_us = _receive_timestamp;
if (_uart_baudrate > 0) {
// assume 10 bits per byte.
uint32_t transport_time_us = (1000000UL * 10UL / _uart_baudrate) * (nbytes+available());
last_receive_us -= transport_time_us;
}
return last_receive_us;
}
ssize_t UARTDriver::get_system_outqueue_length() const
{
if (!_connected) {
return 0;
}
#if defined(__CYGWIN__) || defined(__CYGWIN64__) || defined(CYGWIN_BUILD)
return 0;
#elif defined(__APPLE__) && defined(__MACH__)
return 0;
#else
int size;
if (ioctl(_fd, TIOCOUTQ, &size) == -1) {
// ::fprintf(stderr, "ioctl TIOCOUTQ failed: %m\n");
return 0;
}
return size;
#endif
}
ssize_t UARTDriver::get_system_outqueue_limit() const
{
// AF_UNIX TIOCOUTQ includes per-write kernel accounting and data already
// delivered to the peer. Allow enough room for several full UART writes
// before applying the anti-lag throttle, while retaining the TCP limit.
return _is_unix_socket ? 65536 : 1024;
}
uint32_t UARTDriver::bw_in_bytes_per_second() const
{
// if connected, assume at least a 10/100Mbps connection if not limited
bool baud_limit = false;
#if !defined(HAL_BUILD_AP_PERIPH)
SITL::SIM *_sitl = AP::sitl();
baud_limit = (_sitl != nullptr) && _sitl->telem_baudlimit_enable;
#endif
const uint32_t bitrate = (_connected && !baud_limit) ? 10E6 : _uart_baudrate;
return bitrate/10; // convert bits to bytes minus overhead
};
#if HAL_UART_STATS_ENABLED
// request information on uart I/O for @SYS/uarts.txt for this uart
void UARTDriver::uart_info(ExpandingString &str, StatsTracker &stats, const uint32_t dt_ms)
{
const uint32_t tx_bytes = stats.tx.update(_tx_stats_bytes);
const uint32_t rx_bytes = stats.rx.update(_rx_stats_bytes);
str.printf("TX=%8u RX=%8u TXBD=%6u RXBD=%6u %s (%s)\n",
unsigned(tx_bytes),
unsigned(rx_bytes),
unsigned((tx_bytes * 10000) / dt_ms),
unsigned((rx_bytes * 10000) / dt_ms),
_connected ? "connected " : "not connected",
_sitlState->_serial_path[_portNumber]);
}
#endif
#endif // CONFIG_HAL_BOARD
Reference in New Issue View Git Blame Copy Permalink