Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

HAL_SITL: use SocketAPM for CAN multicast and SITL_Periph state

This commit is contained in:
Andrew Tridgell 2023-11-23 16:56:12 +11:00 committed by Tom Pittenger
parent ea237724c7
commit 58cf4f65a9
4 changed files with 22 additions and 173 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

94
libraries/AP_HAL_SITL/CAN_Multicast.cpp
View File

@ -38,83 +38,9 @@ bool CAN_Multicast::init(uint8_t instance)
{
// setup incoming multicast socket
char address[] = MCAST_ADDRESS_BASE;
struct sockaddr_in sockaddr {};
struct ip_mreq mreq {};
int one = 1;
int ret;
#ifdef HAVE_SOCK_SIN_LEN
sockaddr.sin_len = sizeof(sockaddr);
#endif
address[strlen(address)-1] = '0' + instance;
sockaddr.sin_port = htons(MCAST_PORT);
sockaddr.sin_family = AF_INET;
sockaddr.sin_addr.s_addr = inet_addr(address);
fd_in = socket(AF_INET, SOCK_DGRAM, 0);
if (fd_in == -1) {
goto fail;
}
ret = fcntl(fd_in, F_SETFD, FD_CLOEXEC);
if (ret == -1) {
goto fail;
}
if (setsockopt(fd_in, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)) == -1) {
goto fail;
}
// close on exec, to allow reboot
fcntl(fd_in, 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(fd_in, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
if (ret == -1) {
goto fail;
}
mreq.imr_multiaddr.s_addr = inet_addr(address);
mreq.imr_interface.s_addr = htonl(INADDR_ANY);
ret = setsockopt(fd_in, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq));
if (ret == -1) {
goto fail;
}
// setup outgoing socket
fd_out = socket(AF_INET, SOCK_DGRAM, 0);
if (fd_out == -1) {
goto fail;
}
ret = fcntl(fd_out, F_SETFD, FD_CLOEXEC);
if (ret == -1) {
goto fail;
}
ret = connect(fd_out, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
if (ret == -1) {
goto fail;
}
return true;
fail:
if (fd_in != -1) {
(void)close(fd_in);
fd_in = -1;
}
if (fd_out != -1) {
(void)close(fd_out);
fd_out = -1;
}
return false;
return sock.connect(address, MCAST_PORT);
}
/*
@ -135,7 +61,7 @@ bool CAN_Multicast::send(const AP_HAL::CANFrame &frame)
memcpy(pkt.data, frame.data, data_length);
pkt.crc = crc16_ccitt((uint8_t*)&pkt.flags, data_length+6, 0xFFFFU);
return ::send(fd_out, (void*)&pkt, data_length+10, 0) == data_length+10;
return sock.send((void*)&pkt, data_length+10) == data_length+10;
}
/*
@ -144,9 +70,7 @@ bool CAN_Multicast::send(const AP_HAL::CANFrame &frame)
bool CAN_Multicast::receive(AP_HAL::CANFrame &frame)
{
struct mcast_pkt pkt;
struct sockaddr_in src_addr;
socklen_t src_len = sizeof(src_addr);
ssize_t ret = ::recvfrom(fd_in, (void*)&pkt, sizeof(pkt), MSG_DONTWAIT, (struct sockaddr *)&src_addr, &src_len);
ssize_t ret = sock.recv((void*)&pkt, sizeof(pkt), 0);
if (ret < 10) {
return false;
}
@ -157,18 +81,6 @@ bool CAN_Multicast::receive(AP_HAL::CANFrame &frame)
return false;
}
// ensure it isn't a packet we sent
struct sockaddr_in send_addr;
socklen_t send_len = sizeof(send_addr);
if (getsockname(fd_out, (struct sockaddr *)&send_addr, &send_len) != 0) {
return false;
}
if (src_addr.sin_port == send_addr.sin_port &&
src_addr.sin_family == send_addr.sin_family &&
src_addr.sin_addr.s_addr == send_addr.sin_addr.s_addr) {
return false;
}
// run constructor to initialise
new(&frame) AP_HAL::CANFrame(pkt.message_id, pkt.data, ret-10, (pkt.flags & MCAST_FLAG_CANFD) != 0);

5
libraries/AP_HAL_SITL/CAN_Multicast.h
View File

@ -13,12 +13,11 @@ public:
bool send(const AP_HAL::CANFrame &frame) override;
bool receive(AP_HAL::CANFrame &frame) override;
int get_read_fd(void) const override {
return fd_in;
return sock.get_read_fd();
}
private:
int fd_in = -1;
int fd_out = -1;
SocketAPM sock{true};
};
#endif // HAL_NUM_CAN_IFACES

91
libraries/AP_HAL_SITL/SITL_Periph_State.cpp
View File

@ -138,59 +138,11 @@ void SITL_State::wait_clock(uint64_t wait_time_usec)
*/
void SimMCast::multicast_open(void)
{
struct sockaddr_in sockaddr {};
int ret;
#ifdef HAVE_SOCK_SIN_LEN
sockaddr.sin_len = sizeof(sockaddr);
#endif
sockaddr.sin_port = htons(SITL_MCAST_PORT);
sockaddr.sin_family = AF_INET;
sockaddr.sin_addr.s_addr = inet_addr(SITL_MCAST_IP);
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);
}
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);
}
#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);
}
struct ip_mreq mreq {};
mreq.imr_multiaddr.s_addr = inet_addr(SITL_MCAST_IP);
mreq.imr_interface.s_addr = htonl(INADDR_ANY);
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));
if (!sock.connect(SITL_MCAST_IP, SITL_MCAST_PORT)) {
fprintf(stderr, "multicast socket failed - %s\n", strerror(errno));
exit(1);
}
servo_sock.set_blocking(false);
::printf("multicast receiver initialised\n");
}
@ -199,29 +151,17 @@ void SimMCast::multicast_open(void)
*/
void SimMCast::servo_fd_open(void)
{
servo_fd = socket(AF_INET, SOCK_DGRAM, 0);
if (servo_fd == -1) {
fprintf(stderr, "socket failed - %s\n", strerror(errno));
exit(1);
}
int ret = fcntl(servo_fd, F_SETFD, FD_CLOEXEC);
if (ret == -1) {
fprintf(stderr, "fcntl failed on setting FD_CLOEXEC - %s\n", strerror(errno));
exit(1);
}
int one = 1;
if (setsockopt(servo_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)) == -1) {
fprintf(stderr, "setsockopt failed: %s\n", strerror(errno));
exit(1);
}
in_addr.sin_port = htons(SITL_SERVO_PORT);
ret = connect(servo_fd, (struct sockaddr *)&in_addr, sizeof(in_addr));
if (ret == -1) {
fprintf(stderr, "multicast servo connect failed\n");
const char *in_addr = nullptr;
uint16_t port;
sock.last_recv_address(in_addr, port);
if (in_addr == nullptr) {
return;
}
if (!servo_sock.connect(in_addr, SITL_SERVO_PORT)) {
fprintf(stderr, "servo socket failed - %s\n", strerror(errno));
exit(1);
}
servo_sock.set_blocking(false);
}
/*
@ -241,7 +181,7 @@ void SimMCast::servo_send(void)
for (uint8_t i=0; i<SITL_NUM_CHANNELS; i++) {
out_float[i] = (mask & (1U<<i)) ? out[i] : nanf("");
}
send(servo_fd, (void*)out_float, sizeof(out_float), 0);
servo_sock.send((void*)out_float, sizeof(out_float));
}
/*
@ -257,8 +197,7 @@ void SimMCast::multicast_read(void)
printf("Waiting for multicast state\n");
}
struct SITL::sitl_fdm state;
socklen_t len = sizeof(in_addr);
while (recvfrom(mc_fd, (void*)&state, sizeof(state), MSG_WAITALL, (sockaddr *)&in_addr, &len) != sizeof(state)) {
while (sock.recv((void*)&state, sizeof(state), 0) != sizeof(state)) {
// nop
}
if (_sitl->state.timestamp_us == 0) {
@ -278,7 +217,7 @@ void SimMCast::multicast_read(void)
}
hal.scheduler->stop_clock(_sitl->state.timestamp_us + base_time_us);
HALSITL::Scheduler::timer_event();
if (servo_fd == -1) {
if (!servo_sock.is_connected()) {
servo_fd_open();
} else {
servo_send();

5
libraries/AP_HAL_SITL/SITL_Periph_State.h
View File

@ -30,9 +30,8 @@ public:
void update(const struct sitl_input &input) override;
private:
int mc_fd = -1;
int servo_fd = -1;
struct sockaddr_in in_addr;
SocketAPM sock{true};
SocketAPM servo_sock{true};
// offset between multicast timestamp and local timestamp
uint64_t base_time_us;