Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

HAL_SITL: fixed receive of CANFD in SITL socketcan

Browse Source 
we need to use read() and look at the size to work out if each frame
is bxCAN of CANFD

See https://www.kernel.org/doc/Documentation/networking/can.txt
This commit is contained in:
Andrew Tridgell 2023-04-10 07:33:39 +10:00
parent a960e647b5
commit 81b1b69cd5
2 changed files with 10 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

79
libraries/AP_HAL_SITL/CANSocketIface.cpp
View File

@ -106,7 +106,7 @@ static AP_HAL::CANFrame makeCanFrame(const can_frame& sockcan_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);
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;
}
@ -341,16 +341,11 @@ bool CANIface::_pollRead()
uint8_t iterations_count = 0;
while (iterations_count < CAN_MAX_POLL_ITERATIONS_COUNT)
{
iterations_count++;
CanRxItem rx;
rx.timestamp_us = AP_HAL::native_micros64(); // Monotonic timestamp is not required to be precise (unlike UTC)
bool loopback = false;
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
@ -411,76 +406,20 @@ int CANIface::_read(AP_HAL::CANFrame& frame, uint64_t& timestamp_us, bool& loopb
if (_fd < 0) {
return -1;
}
auto iov = iovec();
auto sockcan_frame = can_frame();
iov.iov_base = &sockcan_frame;
iov.iov_len = sizeof(sockcan_frame);
union {
uint8_t data[CMSG_SPACE(sizeof(::timeval))];
struct cmsghdr align;
} control;
can_frame frame;
canfd_frame frame_fd;
} frames;
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);
const int res = read(_fd, &frames, sizeof(frames));
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;
if (res == sizeof(can_frame)) {
frame = makeCanFrame(frames.frame);
} else {
frame = makeCanFDFrame(frames.frame_fd);
}
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
*/

2
libraries/AP_HAL_SITL/CANSocketIface.h
View File

@ -144,8 +144,6 @@ 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();