From 8c4b900f9ac6aa48c60def3d1cda04ee22e59777 Mon Sep 17 00:00:00 2001 From: Peter van der Perk <57130844+PetervdPerk-NXP@users.noreply.github.com> Date: Thu, 10 Jun 2021 04:26:28 +0200 Subject: [PATCH] uavcan_v1: Correct transmission deadline check & fix SocketCAN tx deadline --- src/drivers/uavcan_v1/CanardSocketCAN.cpp | 18 +++++++++++++++--- src/drivers/uavcan_v1/Uavcan.cpp | 2 +- 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/src/drivers/uavcan_v1/CanardSocketCAN.cpp b/src/drivers/uavcan_v1/CanardSocketCAN.cpp index 85e711fd74..81e78735c4 100644 --- a/src/drivers/uavcan_v1/CanardSocketCAN.cpp +++ b/src/drivers/uavcan_v1/CanardSocketCAN.cpp @@ -39,6 +39,15 @@ #include +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); } diff --git a/src/drivers/uavcan_v1/Uavcan.cpp b/src/drivers/uavcan_v1/Uavcan.cpp index 06233784ae..f1a01db5c0 100644 --- a/src/drivers/uavcan_v1/Uavcan.cpp +++ b/src/drivers/uavcan_v1/Uavcan.cpp @@ -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);