mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2026-10-06 19:00:27 +08:00
Peter BarkerandClaude Fable 5
06790021b3
The CAN multicast transport's port is a fixed constant. The group already varies with the CAN bus number, keeping one simulation's buses apart from each other, but every simulation on the machine shared the same set of buses: each peripheral heard, and answered, every vehicle. Follow SITL_MCAST_STATE_PORT's pattern: take an override from a SITL_CAN_MCAST_PORT environment variable, which reaches the vehicle and its peripherals through inheritance. A framework running simulations concurrently exports a per-instance port; the default behaviour is unchanged. Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
97 lines
2.2 KiB
C++
97 lines
2.2 KiB
C++
/*
|
|
multicast UDP transport for SITL CAN
|
|
*/
|
|
#include "CAN_Multicast.h"
|
|
#include "SITL_Multicast.h"
|
|
|
|
#if HAL_NUM_CAN_IFACES
|
|
|
|
#include <net/if.h>
|
|
#include <unistd.h>
|
|
#include <sys/ioctl.h>
|
|
#include <errno.h>
|
|
#include <stdlib.h>
|
|
#include <sys/socket.h>
|
|
#include <netinet/in.h>
|
|
#include <netinet/udp.h>
|
|
#include <arpa/inet.h>
|
|
#include <fcntl.h>
|
|
#include <AP_Math/crc.h>
|
|
|
|
#define MCAST_ADDRESS_BASE "239.65.82.0"
|
|
#define MCAST_PORT 57732U
|
|
#define MCAST_MAGIC 0x2934U
|
|
#define MCAST_FLAG_CANFD 0x0001
|
|
#define MCAST_MAX_PKT_LEN 74 // 64 byte data + 10 byte header
|
|
|
|
struct PACKED mcast_pkt {
|
|
uint16_t magic;
|
|
uint16_t crc;
|
|
uint16_t flags;
|
|
uint32_t message_id;
|
|
uint8_t data[MCAST_MAX_PKT_LEN-10];
|
|
};
|
|
|
|
/*
|
|
initialise multicast transport
|
|
*/
|
|
bool CAN_Multicast::init(uint8_t instance)
|
|
{
|
|
// setup incoming multicast socket
|
|
char address[] = MCAST_ADDRESS_BASE;
|
|
|
|
address[strlen(address)-1] = '0' + instance;
|
|
sock.set_multicast_interface_address(sitl_multicast_interface_address());
|
|
return sock.connect(address, sitl_can_multicast_port(MCAST_PORT));
|
|
}
|
|
|
|
/*
|
|
send a CAN frame
|
|
*/
|
|
bool CAN_Multicast::send(const AP_HAL::CANFrame &frame)
|
|
{
|
|
struct mcast_pkt pkt {};
|
|
pkt.magic = MCAST_MAGIC;
|
|
pkt.flags = 0;
|
|
#if HAL_CANFD_SUPPORTED
|
|
if (frame.canfd) {
|
|
pkt.flags |= MCAST_FLAG_CANFD;
|
|
}
|
|
#endif
|
|
pkt.message_id = frame.id;
|
|
const uint8_t data_length = AP_HAL::CANFrame::dlcToDataLength(frame.dlc);
|
|
memcpy(pkt.data, frame.data, data_length);
|
|
pkt.crc = crc16_ccitt((uint8_t*)&pkt.flags, data_length+6, 0xFFFFU);
|
|
|
|
return sock.send((void*)&pkt, data_length+10) == data_length+10;
|
|
}
|
|
|
|
/*
|
|
receive a CAN frame
|
|
*/
|
|
bool CAN_Multicast::receive(AP_HAL::CANFrame &frame)
|
|
{
|
|
struct mcast_pkt pkt;
|
|
ssize_t ret = sock.recv((void*)&pkt, sizeof(pkt), 0);
|
|
if (ret < 10) {
|
|
return false;
|
|
}
|
|
if (pkt.magic != MCAST_MAGIC) {
|
|
return false;
|
|
}
|
|
if (pkt.crc != crc16_ccitt((uint8_t*)&pkt.flags, ret-4, 0xFFFFU)) {
|
|
return false;
|
|
}
|
|
|
|
// run constructor to initialise
|
|
new(&frame) AP_HAL::CANFrame(pkt.message_id, pkt.data, ret-10, (pkt.flags & MCAST_FLAG_CANFD) != 0);
|
|
|
|
if (sem_handle != nullptr) {
|
|
sem_handle->signal();
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
#endif // HAL_NUM_CAN_IFACES
|