yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

uavcan_v1: Correct transmission deadline check & fix SocketCAN tx deadline

Browse Source
This commit is contained in:
Peter van der Perk
2021-06-10 04:26:28 +02:00
committed by GitHub
parent ca86416ce6
commit 8c4b900f9a
2 changed files with 16 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
src/drivers/uavcan_v1/CanardSocketCAN.cpp
View File

@@ -39,6 +39,15 @@
#include <px4_platform_common/log.h>
uint64_t getMonotonicTimestampUSec(void)
{
struct timespec ts {};
clock_gettime(CLOCK_MONOTONIC, &ts);
return ts.tv_sec * 1000000ULL + ts.tv_nsec / 1000ULL;
}
int CanardSocketCAN::init()
{
const char *const can_iface_name = "can0";
@@ -120,7 +129,7 @@ int CanardSocketCAN::init()
_send_cmsg->cmsg_level = SOL_CAN_RAW;
_send_cmsg->cmsg_type = CAN_RAW_TX_DEADLINE;
_send_cmsg->cmsg_len = sizeof(struct timeval);
_send_tv = (struct timeval *)CMSG_DATA(&_send_cmsg);
_send_tv = (struct timeval *)CMSG_DATA(_send_cmsg);
// Setup RX msg
_recv_iov.iov_base = &_recv_frame;
@@ -158,9 +167,12 @@ int16_t CanardSocketCAN::transmit(const CanardFrame &txf, int timeout_ms)
memcpy(&frame->data, txf.payload, txf.payload_size);
}
uint64_t deadline_systick = getMonotonicTimestampUSec() + (txf.timestamp_usec - hrt_absolute_time()) +
CONFIG_USEC_PER_TICK; // Compensate for precision loss when converting hrt to systick
/* Set CAN_RAW_TX_DEADLINE timestamp */
_send_tv->tv_usec = txf.timestamp_usec % 1000000ULL;
_send_tv->tv_sec = (txf.timestamp_usec - _send_tv->tv_usec) / 1000000ULL;
_send_tv->tv_usec = deadline_systick % 1000000ULL;
_send_tv->tv_sec = (deadline_systick - _send_tv->tv_usec) / 1000000ULL;
return sendmsg(_fd, &_send_msg, 0);
}

2
src/drivers/uavcan_v1/Uavcan.cpp
View File

@@ -271,7 +271,7 @@ void UavcanNode::transmit()
for (const CanardFrame *txf = nullptr; (txf = canardTxPeek(&_canard_instance)) != nullptr;) {
// Attempt transmission only if the frame is not yet timed out while waiting in the TX queue.
// Otherwise just drop it and move on to the next one.
if (txf->timestamp_usec == 0 || hrt_absolute_time() > txf->timestamp_usec) {
if (txf->timestamp_usec == 0 || txf->timestamp_usec > hrt_absolute_time()) {
// Send the frame. Redundant interfaces may be used here.
const int tx_res = _can_interface->transmit(*txf);