Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_SITL: add support for CANFD in SITL

Browse Source
This commit is contained in:
bugobliterator 2022-03-27 01:11:40 +05:30 committed by Tom Pittenger
parent 7d2ded8de1
commit bd2bf7fedc
2 changed files with 127 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

137
libraries/AP_HAL_SITL/CANSocketIface.cpp
View File

@ -71,19 +71,50 @@ static can_frame makeSocketCanFrame(const AP_HAL::CANFrame& uavcan_frame)
return sockcan_frame;
}
static AP_HAL::CANFrame makeUavcanFrame(const can_frame& sockcan_frame)
static canfd_frame makeSocketCanFDFrame(const AP_HAL::CANFrame& uavcan_frame)
{
AP_HAL::CANFrame uavcan_frame(sockcan_frame.can_id & CAN_EFF_MASK, sockcan_frame.data, sockcan_frame.can_dlc);
canfd_frame sockcan_frame { uavcan_frame.id& AP_HAL::CANFrame::MaskExtID, AP_HAL::CANFrame::dlcToDataLength(uavcan_frame.dlc), CANFD_BRS, 0, 0, { } };
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) {
uavcan_frame.id |= AP_HAL::CANFrame::FlagEFF;
can_frame.id |= AP_HAL::CANFrame::FlagEFF;
}
if (sockcan_frame.can_id & CAN_ERR_FLAG) {
uavcan_frame.id |= AP_HAL::CANFrame::FlagERR;
can_frame.id |= AP_HAL::CANFrame::FlagERR;
}
if (sockcan_frame.can_id & CAN_RTR_FLAG) {
uavcan_frame.id |= AP_HAL::CANFrame::FlagRTR;
can_frame.id |= AP_HAL::CANFrame::FlagRTR;
}
return uavcan_frame;
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);
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
@ -135,6 +166,10 @@ int CANIface::_openSocket(const std::string& iface_name)
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;
@ -308,7 +343,12 @@ bool CANIface::_pollRead()
CanRxItem rx;
rx.timestamp_us = AP_HAL::native_micros64(); // Monotonic timestamp is not required to be precise (unlike UTC)
bool loopback = false;
const int res = _read(rx.frame, rx.timestamp_us, loopback);
int res;
if (iterations_count % 2 == 0) {
res = _read(rx.frame, rx.timestamp_us, loopback);
} else {
res = _readfd(rx.frame, rx.timestamp_us, loopback);
}
if (res == 1) {
bool accept = true;
if (loopback) { // We receive loopback for all CAN frames
@ -339,19 +379,27 @@ int CANIface::_write(const AP_HAL::CANFrame& frame) const
return -1;
}
errno = 0;
int res = 0;
const can_frame sockcan_frame = makeSocketCanFrame(frame);
const int res = write(_fd, &sockcan_frame, sizeof(sockcan_frame));
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;
}
if (res != sizeof(sockcan_frame)) {
return -1;
}
return 1;
}
@ -389,7 +437,48 @@ int CANIface::_read(AP_HAL::CANFrame& frame, uint64_t& timestamp_us, bool& loopb
return 0;
}
frame = makeUavcanFrame(sockcan_frame);
frame = makeCanFrame(sockcan_frame);
/*
* Timestamp
*/
timestamp_us = AP_HAL::native_micros64();
return 1;
}
int CANIface::_readfd(AP_HAL::CANFrame& frame, uint64_t& timestamp_us, bool& loopback) const
{
if (_fd < 0) {
return -1;
}
auto iov = iovec();
auto sockcan_frame = canfd_frame();
iov.iov_base = &sockcan_frame;
iov.iov_len = sizeof(sockcan_frame);
union {
uint8_t data[CMSG_SPACE(sizeof(::timeval))];
struct cmsghdr align;
} control;
auto msg = msghdr();
msg.msg_iov = &iov;
msg.msg_iovlen = 1;
msg.msg_control = control.data;
msg.msg_controllen = sizeof(control.data);
const int res = recvmsg(_fd, &msg, MSG_DONTWAIT);
if (res <= 0) {
return (res < 0 && errno == EWOULDBLOCK) ? 0 : res;
}
/*
* Flags
*/
loopback = (msg.msg_flags & static_cast<int>(MSG_CONFIRM)) != 0;
if (!loopback && !_checkHWFilters(sockcan_frame)) {
return 0;
}
frame = makeCanFDFrame(sockcan_frame);
/*
* Timestamp
*/
@ -450,6 +539,20 @@ bool CANIface::_checkHWFilters(const can_frame& frame) const
}
}
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)) {
@ -463,6 +566,12 @@ void CANIface::_updateDownStatusFromPollResult(const pollfd& pfd)
}
}
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];

4
libraries/AP_HAL_SITL/CANSocketIface.h
View File

@ -67,6 +67,7 @@ public:
~CANIface() { }
// Initialise CAN Peripheral
bool init(const uint32_t bitrate, const uint32_t fdbitrate, const OperatingMode mode) override;
bool init(const uint32_t bitrate, const OperatingMode mode) override;
// Put frame into Tx FIFO returns negative on error, 0 on buffer full,
@ -136,6 +137,8 @@ private:
int _read(AP_HAL::CANFrame& frame, uint64_t& ts_usec, bool& loopback) const;
int _readfd(AP_HAL::CANFrame& frame, uint64_t& ts_usec, bool& loopback) const;
void _incrementNumFramesInSocketTxQueue();
void _confirmSentFrame();
@ -143,6 +146,7 @@ private:
bool _wasInPendingLoopbackSet(const AP_HAL::CANFrame& frame);
bool _checkHWFilters(const can_frame& frame) const;
bool _checkHWFilters(const canfd_frame& frame) const;
bool _hasReadyTx();