mirror of https://github.com/ArduPilot/ardupilot
HAL_SITL: support multicast UDP for CAN in SITL
this will work on windows and in WSL
This commit is contained in:
Andrew Tridgell
parent
624d8f3964
commit
b6e79d05fd
|
|
@ -32,21 +32,21 @@
|
|||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
|
||||
#include <sys/socket.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <net/if.h>
|
||||
#include <linux/can/raw.h>
|
||||
#include <cstring>
|
||||
#include "Scheduler.h"
|
||||
#include <AP_CANManager/AP_CANManager.h>
|
||||
#include <AP_Common/ExpandingString.h>
|
||||
#include "CAN_Multicast.h"
|
||||
#include "CAN_SocketCAN.h"
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
using namespace HALSITL;
|
||||
|
||||
#if HAL_CANMANAGER_ENABLED
|
||||
#define Debug(fmt, args...) do { AP::can().log_text(AP_CANManager::LOG_DEBUG, "CANLinuxIface", fmt, ##args); } while (0)
|
||||
#define Debug(fmt, args...) do { AP::can().log_text(AP_CANManager::LOG_DEBUG, "CANSITLIface", fmt, ##args); } while (0)
|
||||
#else
|
||||
#define Debug(fmt, args...)
|
||||
#endif
|
||||
|
|
@ -55,141 +55,9 @@ CANIface::CANSocketEventSource CANIface::evt_can_socket[HAL_NUM_CAN_IFACES];
|
|||
|
||||
uint8_t CANIface::_num_interfaces;
|
||||
|
||||
static can_frame makeSocketCanFrame(const AP_HAL::CANFrame& uavcan_frame)
|
||||
{
|
||||
can_frame sockcan_frame { uavcan_frame.id& AP_HAL::CANFrame::MaskExtID, uavcan_frame.dlc, { }, };
|
||||
memset(sockcan_frame.data, 0, sizeof(sockcan_frame.data));
|
||||
std::copy(uavcan_frame.data, uavcan_frame.data + uavcan_frame.dlc, sockcan_frame.data);
|
||||
if (uavcan_frame.isExtended()) {
|
||||
sockcan_frame.can_id |= CAN_EFF_FLAG;
|
||||
}
|
||||
if (uavcan_frame.isErrorFrame()) {
|
||||
sockcan_frame.can_id |= CAN_ERR_FLAG;
|
||||
}
|
||||
if (uavcan_frame.isRemoteTransmissionRequest()) {
|
||||
sockcan_frame.can_id |= CAN_RTR_FLAG;
|
||||
}
|
||||
return sockcan_frame;
|
||||
}
|
||||
|
||||
static canfd_frame makeSocketCanFDFrame(const AP_HAL::CANFrame& uavcan_frame)
|
||||
{
|
||||
canfd_frame sockcan_frame { uavcan_frame.id& AP_HAL::CANFrame::MaskExtID, AP_HAL::CANFrame::dlcToDataLength(uavcan_frame.dlc), CANFD_BRS, 0, 0, { }, };
|
||||
memset(sockcan_frame.data, 0, sizeof(sockcan_frame.data));
|
||||
std::copy(uavcan_frame.data, uavcan_frame.data + AP_HAL::CANFrame::dlcToDataLength(uavcan_frame.dlc), sockcan_frame.data);
|
||||
if (uavcan_frame.isExtended()) {
|
||||
sockcan_frame.can_id |= CAN_EFF_FLAG;
|
||||
}
|
||||
if (uavcan_frame.isErrorFrame()) {
|
||||
sockcan_frame.can_id |= CAN_ERR_FLAG;
|
||||
}
|
||||
if (uavcan_frame.isRemoteTransmissionRequest()) {
|
||||
sockcan_frame.can_id |= CAN_RTR_FLAG;
|
||||
}
|
||||
return sockcan_frame;
|
||||
}
|
||||
|
||||
static AP_HAL::CANFrame makeCanFrame(const can_frame& sockcan_frame)
|
||||
{
|
||||
AP_HAL::CANFrame can_frame(sockcan_frame.can_id & CAN_EFF_MASK, sockcan_frame.data, sockcan_frame.can_dlc);
|
||||
if (sockcan_frame.can_id & CAN_EFF_FLAG) {
|
||||
can_frame.id |= AP_HAL::CANFrame::FlagEFF;
|
||||
}
|
||||
if (sockcan_frame.can_id & CAN_ERR_FLAG) {
|
||||
can_frame.id |= AP_HAL::CANFrame::FlagERR;
|
||||
}
|
||||
if (sockcan_frame.can_id & CAN_RTR_FLAG) {
|
||||
can_frame.id |= AP_HAL::CANFrame::FlagRTR;
|
||||
}
|
||||
return can_frame;
|
||||
}
|
||||
|
||||
static AP_HAL::CANFrame makeCanFDFrame(const canfd_frame& sockcan_frame)
|
||||
{
|
||||
AP_HAL::CANFrame can_frame(sockcan_frame.can_id & CAN_EFF_MASK, sockcan_frame.data, sockcan_frame.len, true);
|
||||
if (sockcan_frame.can_id & CAN_EFF_FLAG) {
|
||||
can_frame.id |= AP_HAL::CANFrame::FlagEFF;
|
||||
}
|
||||
if (sockcan_frame.can_id & CAN_ERR_FLAG) {
|
||||
can_frame.id |= AP_HAL::CANFrame::FlagERR;
|
||||
}
|
||||
if (sockcan_frame.can_id & CAN_RTR_FLAG) {
|
||||
can_frame.id |= AP_HAL::CANFrame::FlagRTR;
|
||||
}
|
||||
return can_frame;
|
||||
}
|
||||
|
||||
bool CANIface::is_initialized() const
|
||||
{
|
||||
return _initialized;
|
||||
}
|
||||
|
||||
int CANIface::_openSocket(const std::string& iface_name)
|
||||
{
|
||||
errno = 0;
|
||||
|
||||
int s = socket(PF_CAN, SOCK_RAW, CAN_RAW);
|
||||
if (s < 0) {
|
||||
return s;
|
||||
}
|
||||
|
||||
std::shared_ptr<void> defer(&s, [](int* fd) { if (*fd >= 0) close(*fd); });
|
||||
const int ret = s;
|
||||
|
||||
// Detect the iface index
|
||||
auto ifr = ifreq();
|
||||
if (iface_name.length() >= IFNAMSIZ) {
|
||||
errno = ENAMETOOLONG;
|
||||
return -1;
|
||||
}
|
||||
std::strncpy(ifr.ifr_name, iface_name.c_str(), iface_name.length());
|
||||
if (ioctl(s, SIOCGIFINDEX, &ifr) < 0 || ifr.ifr_ifindex < 0) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Bind to the specified CAN iface
|
||||
{
|
||||
auto addr = sockaddr_can();
|
||||
addr.can_family = AF_CAN;
|
||||
addr.can_ifindex = ifr.ifr_ifindex;
|
||||
if (bind(s, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0) {
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
// Configure
|
||||
{
|
||||
const int on = 1;
|
||||
// Timestamping
|
||||
if (setsockopt(s, SOL_SOCKET, SO_TIMESTAMP, &on, sizeof(on)) < 0) {
|
||||
return -1;
|
||||
}
|
||||
// Socket loopback
|
||||
if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &on, sizeof(on)) < 0) {
|
||||
return -1;
|
||||
}
|
||||
// Allow CANFD
|
||||
if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &on, sizeof(on)) < 0) {
|
||||
return -1;
|
||||
}
|
||||
// Non-blocking
|
||||
if (fcntl(s, F_SETFL, O_NONBLOCK) < 0) {
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
// Validate the resulting socket
|
||||
{
|
||||
int socket_error = 0;
|
||||
socklen_t errlen = sizeof(socket_error);
|
||||
getsockopt(s, SOL_SOCKET, SO_ERROR, reinterpret_cast<void*>(&socket_error), &errlen);
|
||||
if (socket_error != 0) {
|
||||
errno = socket_error;
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
s = -1;
|
||||
return ret;
|
||||
return transport != nullptr;
|
||||
}
|
||||
|
||||
int16_t CANIface::send(const AP_HAL::CANFrame& frame, const uint64_t tx_deadline,
|
||||
|
|
@ -251,190 +119,68 @@ bool CANIface::_hasReadyRx()
|
|||
void CANIface::_poll(bool read, bool write)
|
||||
{
|
||||
if (read) {
|
||||
stats.num_poll_rx_events++;
|
||||
_pollRead(); // Read poll must be executed first because it may decrement _frames_in_socket_tx_queue
|
||||
}
|
||||
if (write) {
|
||||
stats.num_poll_tx_events++;
|
||||
_pollWrite();
|
||||
}
|
||||
}
|
||||
|
||||
bool CANIface::configureFilters(const CanFilterConfig* const filter_configs,
|
||||
const uint16_t num_configs)
|
||||
{
|
||||
#if 0
|
||||
if (filter_configs == nullptr || mode_ != FilteredMode) {
|
||||
return false;
|
||||
}
|
||||
_hw_filters_container.clear();
|
||||
_hw_filters_container.resize(num_configs);
|
||||
|
||||
for (unsigned i = 0; i < num_configs; i++) {
|
||||
const CanFilterConfig& fc = filter_configs[i];
|
||||
_hw_filters_container[i].can_id = fc.id & AP_HAL::CANFrame::MaskExtID;
|
||||
_hw_filters_container[i].can_mask = fc.mask & AP_HAL::CANFrame::MaskExtID;
|
||||
if (fc.id & AP_HAL::CANFrame::FlagEFF) {
|
||||
_hw_filters_container[i].can_id |= CAN_EFF_FLAG;
|
||||
}
|
||||
if (fc.id & AP_HAL::CANFrame::FlagRTR) {
|
||||
_hw_filters_container[i].can_id |= CAN_RTR_FLAG;
|
||||
}
|
||||
if (fc.mask & AP_HAL::CANFrame::FlagEFF) {
|
||||
_hw_filters_container[i].can_mask |= CAN_EFF_FLAG;
|
||||
}
|
||||
if (fc.mask & AP_HAL::CANFrame::FlagRTR) {
|
||||
_hw_filters_container[i].can_mask |= CAN_RTR_FLAG;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* SocketCAN emulates the CAN filters in software, so the number of filters is virtually unlimited.
|
||||
* This method returns a constant value.
|
||||
*/
|
||||
static constexpr unsigned NumFilters = CAN_FILTER_NUMBER;
|
||||
uint16_t CANIface::getNumFilters() const { return NumFilters; }
|
||||
|
||||
uint32_t CANIface::getErrorCount() const
|
||||
{
|
||||
uint32_t ec = 0;
|
||||
for (auto& kv : _errors) { ec += kv.second; }
|
||||
return ec;
|
||||
return 0;
|
||||
}
|
||||
|
||||
void CANIface::_pollWrite()
|
||||
{
|
||||
if (transport == nullptr) {
|
||||
return;
|
||||
}
|
||||
while (_hasReadyTx()) {
|
||||
WITH_SEMAPHORE(sem);
|
||||
const CanTxItem tx = _tx_queue.top();
|
||||
uint64_t curr_time = AP_HAL::micros64();
|
||||
const uint64_t curr_time = AP_HAL::micros64();
|
||||
if (tx.deadline >= curr_time) {
|
||||
// hal.console->printf("%x TDEAD: %lu CURRT: %lu DEL: %lu\n",tx.frame.id, tx.deadline, curr_time, tx.deadline-curr_time);
|
||||
const int res = _write(tx.frame);
|
||||
if (res == 1) { // Transmitted successfully
|
||||
_incrementNumFramesInSocketTxQueue();
|
||||
if (tx.loopback) {
|
||||
_pending_loopback_ids.insert(tx.frame.id);
|
||||
}
|
||||
bool ok = transport->send(tx.frame);
|
||||
if (ok) {
|
||||
stats.tx_success++;
|
||||
} else if (res == 0) { // Not transmitted, nor is it an error
|
||||
stats.tx_full++;
|
||||
break; // Leaving the loop, the frame remains enqueued for the next retry
|
||||
} else { // Transmission error
|
||||
stats.tx_rejected++;
|
||||
} else {
|
||||
break;
|
||||
}
|
||||
} else {
|
||||
// hal.console->printf("TDEAD: %lu CURRT: %lu DEL: %lu\n", tx.deadline, curr_time, curr_time-tx.deadline);
|
||||
stats.tx_timedout++;
|
||||
}
|
||||
|
||||
// Removing the frame from the queue even if transmission failed
|
||||
// Removing the frame from the queue
|
||||
(void)_tx_queue.pop();
|
||||
}
|
||||
}
|
||||
|
||||
bool CANIface::_pollRead()
|
||||
{
|
||||
uint8_t iterations_count = 0;
|
||||
while (iterations_count < CAN_MAX_POLL_ITERATIONS_COUNT)
|
||||
{
|
||||
CanRxItem rx;
|
||||
rx.timestamp_us = AP_HAL::micros64(); // Monotonic timestamp is not required to be precise (unlike UTC)
|
||||
bool loopback = false;
|
||||
int res;
|
||||
res = _read(rx.frame, rx.timestamp_us, loopback);
|
||||
if (res == 1) {
|
||||
bool accept = true;
|
||||
if (loopback) { // We receive loopback for all CAN frames
|
||||
_confirmSentFrame();
|
||||
rx.flags |= Loopback;
|
||||
accept = _wasInPendingLoopbackSet(rx.frame);
|
||||
stats.tx_confirmed++;
|
||||
if (transport == nullptr) {
|
||||
return false;
|
||||
}
|
||||
if (accept) {
|
||||
CanRxItem rx {};
|
||||
bool ok = transport->receive(rx.frame);
|
||||
if (!ok) {
|
||||
return false;
|
||||
}
|
||||
rx.timestamp_us = AP_HAL::micros64();
|
||||
WITH_SEMAPHORE(sem);
|
||||
add_to_rx_queue(rx);
|
||||
stats.rx_received++;
|
||||
return true;
|
||||
}
|
||||
} else if (res == 0) {
|
||||
break;
|
||||
} else {
|
||||
stats.rx_errors++;
|
||||
break;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
int CANIface::_write(const AP_HAL::CANFrame& frame) const
|
||||
{
|
||||
if (_fd < 0) {
|
||||
return -1;
|
||||
}
|
||||
errno = 0;
|
||||
int res = 0;
|
||||
|
||||
if (frame.isCanFDFrame()) {
|
||||
const canfd_frame sockcan_frame = makeSocketCanFDFrame(frame);
|
||||
res = write(_fd, &sockcan_frame, sizeof(sockcan_frame));
|
||||
if (res > 0 && res != sizeof(sockcan_frame)) {
|
||||
return -1;
|
||||
}
|
||||
} else {
|
||||
const can_frame sockcan_frame = makeSocketCanFrame(frame);
|
||||
res = write(_fd, &sockcan_frame, sizeof(sockcan_frame));
|
||||
if (res > 0 && res != sizeof(sockcan_frame)) {
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
if (res <= 0) {
|
||||
if (errno == ENOBUFS || errno == EAGAIN) { // Writing is not possible atm, not an error
|
||||
return 0;
|
||||
}
|
||||
return res;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
int CANIface::_read(AP_HAL::CANFrame& frame, uint64_t& timestamp_us, bool& loopback) const
|
||||
{
|
||||
if (_fd < 0) {
|
||||
return -1;
|
||||
}
|
||||
union {
|
||||
can_frame frame;
|
||||
canfd_frame frame_fd;
|
||||
} frames;
|
||||
|
||||
const int res = read(_fd, &frames, sizeof(frames));
|
||||
if (res <= 0) {
|
||||
return (res < 0 && errno == EWOULDBLOCK) ? 0 : res;
|
||||
}
|
||||
if (res == sizeof(can_frame)) {
|
||||
frame = makeCanFrame(frames.frame);
|
||||
} else {
|
||||
frame = makeCanFDFrame(frames.frame_fd);
|
||||
}
|
||||
/*
|
||||
* Timestamp
|
||||
*/
|
||||
timestamp_us = AP_HAL::micros64();
|
||||
return 1;
|
||||
}
|
||||
|
||||
// Might block forever, only to be used for testing
|
||||
void CANIface::flush_tx()
|
||||
{
|
||||
WITH_SEMAPHORE(sem);
|
||||
do {
|
||||
_updateDownStatusFromPollResult(_pollfd);
|
||||
_poll(true, true);
|
||||
} while(!_tx_queue.empty() && !_down);
|
||||
} while(!_tx_queue.empty());
|
||||
}
|
||||
|
||||
void CANIface::clear_rx()
|
||||
|
|
@ -445,11 +191,6 @@ void CANIface::clear_rx()
|
|||
std::swap( _rx_queue, empty );
|
||||
}
|
||||
|
||||
void CANIface::_incrementNumFramesInSocketTxQueue()
|
||||
{
|
||||
_frames_in_socket_tx_queue++;
|
||||
}
|
||||
|
||||
void CANIface::_confirmSentFrame()
|
||||
{
|
||||
if (_frames_in_socket_tx_queue > 0) {
|
||||
|
|
@ -457,88 +198,51 @@ void CANIface::_confirmSentFrame()
|
|||
}
|
||||
}
|
||||
|
||||
bool CANIface::_wasInPendingLoopbackSet(const AP_HAL::CANFrame& frame)
|
||||
{
|
||||
if (_pending_loopback_ids.count(frame.id) > 0) {
|
||||
_pending_loopback_ids.erase(frame.id);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool CANIface::_checkHWFilters(const can_frame& frame) const
|
||||
{
|
||||
if (!_hw_filters_container.empty()) {
|
||||
for (auto& f : _hw_filters_container) {
|
||||
if (((frame.can_id & f.can_mask) ^ f.can_id) == 0) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
} else {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
bool CANIface::_checkHWFilters(const canfd_frame& frame) const
|
||||
{
|
||||
if (!_hw_filters_container.empty()) {
|
||||
for (auto& f : _hw_filters_container) {
|
||||
if (((frame.can_id & f.can_mask) ^ f.can_id) == 0) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
} else {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
void CANIface::_updateDownStatusFromPollResult(const pollfd& pfd)
|
||||
{
|
||||
if (!_down && (pfd.revents & POLLERR)) {
|
||||
int error = 0;
|
||||
socklen_t errlen = sizeof(error);
|
||||
getsockopt(pfd.fd, SOL_SOCKET, SO_ERROR, reinterpret_cast<void*>(&error), &errlen);
|
||||
|
||||
_down= error == ENETDOWN || error == ENODEV;
|
||||
stats.num_downs++;
|
||||
Debug("Iface %d is dead; error %d", _fd, error);
|
||||
}
|
||||
}
|
||||
|
||||
bool CANIface::init(const uint32_t bitrate, const uint32_t fdbitrate, const OperatingMode mode)
|
||||
{
|
||||
// we are using vcan, so bitrate is irrelevant
|
||||
return init(bitrate, mode);
|
||||
}
|
||||
|
||||
bool CANIface::init(const uint32_t bitrate, const OperatingMode mode)
|
||||
{
|
||||
char iface_name[16];
|
||||
sprintf(iface_name, "vcan%u", _self_index);
|
||||
bitrate_ = bitrate;
|
||||
mode_ = mode;
|
||||
if (_initialized) {
|
||||
return _initialized;
|
||||
const auto *_sitl = AP::sitl();
|
||||
if (_sitl == nullptr) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// TODO: Add possibility change bitrate
|
||||
_fd = _openSocket(iface_name);
|
||||
if (_fd > 0) {
|
||||
_bitrate = bitrate;
|
||||
_initialized = true;
|
||||
} else {
|
||||
_initialized = false;
|
||||
if (_self_index >= HAL_NUM_CAN_IFACES) {
|
||||
return false;
|
||||
}
|
||||
return _initialized;
|
||||
const SITL::SIM::CANTransport can_type = _sitl->can_transport[_self_index];
|
||||
switch (can_type) {
|
||||
case SITL::SIM::CANTransport::MulticastUDP:
|
||||
transport = new CAN_Multicast();
|
||||
break;
|
||||
case SITL::SIM::CANTransport::SocketCAN:
|
||||
#if HAL_CAN_WITH_SOCKETCAN
|
||||
transport = new CAN_SocketCAN();
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
if (transport == nullptr) {
|
||||
return false;
|
||||
}
|
||||
if (!transport->init(_self_index)) {
|
||||
delete transport;
|
||||
transport = nullptr;
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool CANIface::select(bool &read_select, bool &write_select,
|
||||
const AP_HAL::CANFrame* const pending_tx, uint64_t blocking_deadline)
|
||||
{
|
||||
if (transport == nullptr) {
|
||||
return false;
|
||||
}
|
||||
// Detecting whether we need to block at all
|
||||
bool need_block = !write_select; // Write queue is infinite
|
||||
|
||||
// call poll here to flush some tx
|
||||
_poll(true, true);
|
||||
|
||||
|
|
@ -547,39 +251,22 @@ bool CANIface::select(bool &read_select, bool &write_select,
|
|||
}
|
||||
|
||||
if (need_block) {
|
||||
if (_down) {
|
||||
return false;
|
||||
} else {
|
||||
_pollfd.fd = _fd;
|
||||
_pollfd.fd = transport->get_read_fd();
|
||||
_pollfd.events |= POLLIN;
|
||||
stats.num_rx_poll_req++;
|
||||
if (_hasReadyTx() && write_select) {
|
||||
_pollfd.events |= POLLOUT;
|
||||
stats.num_tx_poll_req++;
|
||||
}
|
||||
}
|
||||
if (_evt_handle != nullptr && blocking_deadline > AP_HAL::micros64()) {
|
||||
_evt_handle->wait(blocking_deadline - AP_HAL::micros64());
|
||||
}
|
||||
}
|
||||
|
||||
// Writing the output masks
|
||||
if (!_down) {
|
||||
write_select = true; // Always ready to write if not down
|
||||
} else {
|
||||
write_select = false;
|
||||
}
|
||||
if (_hasReadyRx()) {
|
||||
read_select = true; // Readability depends only on RX buf, even if down
|
||||
} else {
|
||||
read_select = false;
|
||||
}
|
||||
write_select = true;
|
||||
read_select = _hasReadyRx();
|
||||
|
||||
// Return value is irrelevant as long as it's non-negative
|
||||
return true;
|
||||
}
|
||||
|
||||
bool CANIface::set_event_handle(AP_HAL::EventHandle* handle) {
|
||||
bool CANIface::set_event_handle(AP_HAL::EventHandle* handle)
|
||||
{
|
||||
_evt_handle = handle;
|
||||
evt_can_socket[_self_index]._ifaces[_self_index] = this;
|
||||
_evt_handle->set_source(&evt_can_socket[_self_index]);
|
||||
|
|
@ -601,13 +288,9 @@ bool CANIface::CANSocketEventSource::wait(uint16_t duration_us, AP_HAL::EventHan
|
|||
if (_ifaces[i] == nullptr) {
|
||||
continue;
|
||||
}
|
||||
if (_ifaces[i]->_down) {
|
||||
continue;
|
||||
}
|
||||
pollfds[num_pollfds] = _ifaces[i]->_pollfd;
|
||||
pollfd_iface_map[num_pollfds] = i;
|
||||
num_pollfds++;
|
||||
_ifaces[i]->stats.num_poll_waits++;
|
||||
}
|
||||
|
||||
if (num_pollfds == 0) {
|
||||
|
|
@ -617,13 +300,9 @@ bool CANIface::CANSocketEventSource::wait(uint16_t duration_us, AP_HAL::EventHan
|
|||
const uint32_t start_us = AP_HAL::micros();
|
||||
do {
|
||||
uint16_t wait_us = MIN(100, duration_us);
|
||||
// Timeout conversion
|
||||
auto ts = timespec();
|
||||
ts.tv_sec = 0;
|
||||
ts.tv_nsec = wait_us * 1000UL;
|
||||
|
||||
// check FD for input
|
||||
const int res = ppoll(pollfds, num_pollfds, &ts, nullptr);
|
||||
const int res = poll(pollfds, num_pollfds, wait_us/1000U);
|
||||
|
||||
if (res < 0) {
|
||||
return false;
|
||||
|
|
@ -642,7 +321,6 @@ bool CANIface::CANSocketEventSource::wait(uint16_t duration_us, AP_HAL::EventHan
|
|||
if (_ifaces[pollfd_iface_map[i]] == nullptr) {
|
||||
continue;
|
||||
}
|
||||
_ifaces[pollfd_iface_map[i]]->_updateDownStatusFromPollResult(pollfds[i]);
|
||||
|
||||
const bool poll_read = pollfds[i].revents & POLLIN;
|
||||
const bool poll_write = pollfds[i].revents & POLLOUT;
|
||||
|
|
@ -655,32 +333,16 @@ void CANIface::get_stats(ExpandingString &str)
|
|||
{
|
||||
str.printf("tx_requests: %u\n"
|
||||
"tx_rejected: %u\n"
|
||||
"tx_full: %u\n"
|
||||
"tx_confirmed: %u\n"
|
||||
"tx_success: %u\n"
|
||||
"tx_timedout: %u\n"
|
||||
"rx_received: %u\n"
|
||||
"rx_errors: %u\n"
|
||||
"num_downs: %u\n"
|
||||
"num_rx_poll_req: %u\n"
|
||||
"num_tx_poll_req: %u\n"
|
||||
"num_poll_waits: %u\n"
|
||||
"num_poll_tx_events: %u\n"
|
||||
"num_poll_rx_events: %u\n",
|
||||
"rx_errors: %u\n",
|
||||
stats.tx_requests,
|
||||
stats.tx_rejected,
|
||||
stats.tx_full,
|
||||
stats.tx_confirmed,
|
||||
stats.tx_success,
|
||||
stats.tx_timedout,
|
||||
stats.rx_received,
|
||||
stats.rx_errors,
|
||||
stats.num_downs,
|
||||
stats.num_rx_poll_req,
|
||||
stats.num_tx_poll_req,
|
||||
stats.num_poll_waits,
|
||||
stats.num_poll_tx_events,
|
||||
stats.num_poll_rx_events);
|
||||
stats.rx_errors);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -13,17 +13,6 @@
|
|||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Many thanks to members of the UAVCAN project:
|
||||
* Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
* Ilia Sheremet <illia.sheremet@gmail.com>
|
||||
*
|
||||
* license info can be found in the uavcan submodule located:
|
||||
* modules/uavcan/LICENSE
|
||||
* modules/uavcan/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp
|
||||
*/
|
||||
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "AP_HAL_SITL.h"
|
||||
|
|
@ -32,28 +21,16 @@
|
|||
|
||||
#include <AP_HAL/CANIface.h>
|
||||
|
||||
#include <linux/can.h>
|
||||
|
||||
#include <string>
|
||||
#include <queue>
|
||||
#include <memory>
|
||||
#include <map>
|
||||
#include <unordered_set>
|
||||
#include <poll.h>
|
||||
#include "CAN_Transport.h"
|
||||
|
||||
namespace HALSITL {
|
||||
|
||||
enum class SocketCanError
|
||||
{
|
||||
SocketReadFailure,
|
||||
SocketWriteFailure,
|
||||
TxTimeout
|
||||
};
|
||||
|
||||
#define CAN_MAX_POLL_ITERATIONS_COUNT 100
|
||||
#define CAN_MAX_INIT_TRIES_COUNT 100
|
||||
#define CAN_FILTER_NUMBER 8
|
||||
|
||||
class CANIface: public AP_HAL::CANIface {
|
||||
public:
|
||||
CANIface(int index)
|
||||
|
|
@ -86,10 +63,6 @@ public:
|
|||
int16_t receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_us,
|
||||
CanIOFlags& out_flags) override;
|
||||
|
||||
// Set Filters to ignore frames not to be handled by us
|
||||
bool configureFilters(const CanFilterConfig* filter_configs,
|
||||
uint16_t num_configs) override;
|
||||
|
||||
// Always return false, there's no busoff condition in virtual CAN
|
||||
bool is_busoff() const override
|
||||
{
|
||||
|
|
@ -100,9 +73,6 @@ public:
|
|||
|
||||
void clear_rx() override;
|
||||
|
||||
// Get number of Filter configurations
|
||||
uint16_t getNumFilters() const override;
|
||||
|
||||
// Get total number of Errors discovered
|
||||
uint32_t getErrorCount() const override;
|
||||
|
||||
|
|
@ -146,19 +116,8 @@ private:
|
|||
|
||||
bool _pollRead();
|
||||
|
||||
int _write(const AP_HAL::CANFrame& frame) const;
|
||||
|
||||
int _read(AP_HAL::CANFrame& frame, uint64_t& ts_usec, bool& loopback) const;
|
||||
|
||||
void _incrementNumFramesInSocketTxQueue();
|
||||
|
||||
void _confirmSentFrame();
|
||||
|
||||
bool _wasInPendingLoopbackSet(const AP_HAL::CANFrame& frame);
|
||||
|
||||
bool _checkHWFilters(const can_frame& frame) const;
|
||||
bool _checkHWFilters(const canfd_frame& frame) const;
|
||||
|
||||
bool _hasReadyTx();
|
||||
|
||||
bool _hasReadyRx();
|
||||
|
|
@ -169,12 +128,7 @@ private:
|
|||
|
||||
void _updateDownStatusFromPollResult(const pollfd& pfd);
|
||||
|
||||
uint32_t _bitrate;
|
||||
|
||||
bool _down;
|
||||
bool _initialized;
|
||||
|
||||
int _fd;
|
||||
CAN_Transport *transport;
|
||||
|
||||
const uint8_t _self_index;
|
||||
|
||||
|
|
@ -184,29 +138,16 @@ private:
|
|||
static CANSocketEventSource evt_can_socket[HAL_NUM_CAN_IFACES];
|
||||
|
||||
pollfd _pollfd;
|
||||
std::map<SocketCanError, uint64_t> _errors;
|
||||
std::priority_queue<CanTxItem> _tx_queue;
|
||||
std::queue<CanRxItem> _rx_queue;
|
||||
std::unordered_multiset<uint32_t> _pending_loopback_ids;
|
||||
std::vector<can_filter> _hw_filters_container;
|
||||
|
||||
/*
|
||||
additional statistics
|
||||
bus statistics
|
||||
*/
|
||||
struct bus_stats : public AP_HAL::CANIface::bus_stats_t {
|
||||
uint32_t tx_full;
|
||||
uint32_t tx_confirmed;
|
||||
uint32_t num_downs;
|
||||
uint32_t num_rx_poll_req;
|
||||
uint32_t num_tx_poll_req;
|
||||
uint32_t num_poll_waits;
|
||||
uint32_t num_poll_tx_events;
|
||||
uint32_t num_poll_rx_events;
|
||||
} stats;
|
||||
AP_HAL::CANIface::bus_stats_t stats;
|
||||
|
||||
HAL_Semaphore sem;
|
||||
|
||||
protected:
|
||||
bool add_to_rx_queue(const CanRxItem &rx_item) override {
|
||||
_rx_queue.push(rx_item);
|
||||
return true;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,178 @@
|
|||
/*
|
||||
multicast UDP transport for SITL CAN
|
||||
*/
|
||||
#include "CAN_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;
|
||||
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;
|
||||
}
|
||||
|
||||
/*
|
||||
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 ::send(fd_out, (void*)&pkt, data_length+10, 0) == data_length+10;
|
||||
}
|
||||
|
||||
/*
|
||||
receive a CAN 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);
|
||||
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;
|
||||
}
|
||||
|
||||
// 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);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
#endif // HAL_NUM_CAN_IFACES
|
||||
|
|
@ -0,0 +1,24 @@
|
|||
/*
|
||||
multicast UDP transport for SITL CAN
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include "CAN_Transport.h"
|
||||
|
||||
#if HAL_NUM_CAN_IFACES
|
||||
|
||||
class CAN_Multicast : public CAN_Transport {
|
||||
public:
|
||||
bool init(uint8_t instance) override;
|
||||
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;
|
||||
}
|
||||
|
||||
private:
|
||||
int fd_in = -1;
|
||||
int fd_out = -1;
|
||||
};
|
||||
|
||||
#endif // HAL_NUM_CAN_IFACES
|
||||
|
|
@ -0,0 +1,91 @@
|
|||
/*
|
||||
socketcan transport for SITL CAN
|
||||
*/
|
||||
#include "CAN_SocketCAN.h"
|
||||
|
||||
#if HAL_NUM_CAN_IFACES && HAL_CAN_WITH_SOCKETCAN
|
||||
|
||||
#include <net/if.h>
|
||||
#include <poll.h>
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <linux/can.h>
|
||||
#include <errno.h>
|
||||
#include <stdlib.h>
|
||||
#include "CAN_SocketCAN.h"
|
||||
|
||||
/*
|
||||
initialise socketcan transport
|
||||
*/
|
||||
bool CAN_SocketCAN::init(uint8_t instance)
|
||||
{
|
||||
struct sockaddr_can addr {};
|
||||
struct ifreq ifr {};
|
||||
int ret;
|
||||
|
||||
fd = socket(PF_CAN, SOCK_RAW | SOCK_NONBLOCK, CAN_RAW);
|
||||
if (fd < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
snprintf(ifr.ifr_name, sizeof(ifr.ifr_name), "vcan%u", instance);
|
||||
ret = ioctl(fd, SIOCGIFINDEX, &ifr);
|
||||
if (ret == -1) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
addr.can_family = AF_CAN;
|
||||
addr.can_ifindex = ifr.ifr_ifindex;
|
||||
|
||||
ret = bind(fd, (struct sockaddr*)&addr, sizeof(addr));
|
||||
if (ret == -1) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
fail:
|
||||
if (fd != -1) {
|
||||
close(fd);
|
||||
fd = -1;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/*
|
||||
send a CAN frame
|
||||
*/
|
||||
bool CAN_SocketCAN::send(const AP_HAL::CANFrame &frame)
|
||||
{
|
||||
if (frame.canfd) {
|
||||
// not supported on socketcan
|
||||
return false;
|
||||
}
|
||||
struct can_frame transmit_frame {};
|
||||
transmit_frame.can_id = frame.id;
|
||||
transmit_frame.can_dlc = frame.dlc;
|
||||
|
||||
const uint8_t data_length = AP_HAL::CANFrame::dlcToDataLength(frame.dlc);
|
||||
memcpy(transmit_frame.data, frame.data, data_length);
|
||||
|
||||
return ::write(fd, &transmit_frame, sizeof(transmit_frame)) == sizeof(transmit_frame);
|
||||
}
|
||||
|
||||
/*
|
||||
receive a CAN frame
|
||||
*/
|
||||
bool CAN_SocketCAN::receive(AP_HAL::CANFrame &frame)
|
||||
{
|
||||
struct can_frame receive_frame;
|
||||
const ssize_t ret = ::read(fd, &receive_frame, sizeof(receive_frame));
|
||||
if (ret != sizeof(receive_frame)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// run constructor to initialise
|
||||
new(&frame) AP_HAL::CANFrame(receive_frame.can_id, receive_frame.data, receive_frame.can_dlc, false);
|
||||
return true;
|
||||
}
|
||||
|
||||
#endif // HAL_NUM_CAN_IFACES
|
||||
|
|
@ -0,0 +1,23 @@
|
|||
/*
|
||||
socketcan transport for SITL CAN
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include "CAN_Transport.h"
|
||||
|
||||
#if HAL_NUM_CAN_IFACES && HAL_CAN_WITH_SOCKETCAN
|
||||
|
||||
class CAN_SocketCAN : public CAN_Transport {
|
||||
public:
|
||||
bool init(uint8_t instance) override;
|
||||
bool send(const AP_HAL::CANFrame &frame) override;
|
||||
bool receive(AP_HAL::CANFrame &frame) override;
|
||||
int get_read_fd(void) const override {
|
||||
return fd;
|
||||
}
|
||||
|
||||
private:
|
||||
int fd = -1;
|
||||
};
|
||||
|
||||
#endif // HAL_NUM_CAN_IFACES
|
||||
|
|
@ -0,0 +1,21 @@
|
|||
/*
|
||||
parent class for CAN transports in ArduPilot SITL
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include "AP_HAL_SITL.h"
|
||||
|
||||
#if HAL_NUM_CAN_IFACES
|
||||
|
||||
#include <AP_HAL/CANIface.h>
|
||||
|
||||
class CAN_Transport {
|
||||
public:
|
||||
virtual ~CAN_Transport() {}
|
||||
virtual bool init(uint8_t instance) = 0;
|
||||
virtual bool send(const AP_HAL::CANFrame &frame) = 0;
|
||||
virtual bool receive(AP_HAL::CANFrame &frame) = 0;
|
||||
virtual int get_read_fd(void) const = 0;
|
||||
};
|
||||
|
||||
#endif // HAL_NUM_CAN_IFACES
|
||||
Loading…
Reference in New Issue