From 77f131e09ad04bbd86fd91119d0b67e3c3dd601c Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Wed, 18 Oct 2023 18:06:54 +1100 Subject: [PATCH] AP_HAL_ChibiOS: add implementation for tracking transmit timestamp on CAN --- libraries/AP_HAL_ChibiOS/CANFDIface.cpp | 3 +++ libraries/AP_HAL_ChibiOS/CANFDIface.h | 16 ++++++++++++++++ libraries/AP_HAL_ChibiOS/CANIface.h | 16 ++++++++++++++++ libraries/AP_HAL_ChibiOS/CanIface.cpp | 3 +++ 4 files changed, 38 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp index 2d261f7242084..b2bc46b6b012e 100644 --- a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp +++ b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp @@ -801,6 +801,9 @@ void CANIface::handleTxCompleteInterrupt(const uint64_t timestamp_us) stats.fdf_tx_success++; } pending_tx_[i].pushed = true; + if ((pending_tx_[i].frame.id & tracked_tx_ts_mask) == tracked_tx_ts_value) { + tracked_tx_timestamp_us = timestamp_us; + } } else { continue; } diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.h b/libraries/AP_HAL_ChibiOS/CANFDIface.h index 9b21444161720..d54b1f139fa37 100644 --- a/libraries/AP_HAL_ChibiOS/CANFDIface.h +++ b/libraries/AP_HAL_ChibiOS/CANFDIface.h @@ -126,6 +126,11 @@ class ChibiOS::CANIface : public AP_HAL::CANIface #endif const uint8_t self_index_; + // track tx timestamps + uint32_t tracked_tx_ts_mask; + uint32_t tracked_tx_ts_value; + uint64_t tracked_tx_timestamp_us; + bool computeTimings(uint32_t target_bitrate, Timings& out_timings); void setupMessageRam(void); @@ -257,6 +262,17 @@ class ChibiOS::CANIface : public AP_HAL::CANIface // register. Indexed by locical interface number static constexpr CanType* const Can[HAL_NUM_CAN_IFACES] = { HAL_CAN_BASE_LIST }; + // set mask and value on packet id to track tx timestamp for + void set_track_tx_timestamp(uint32_t mask, uint32_t value) override { + tracked_tx_ts_mask = mask; + tracked_tx_ts_value = value; + } + + // get timestamp of last packet sent with matching mask and value + uint64_t get_tracked_tx_timestamp() override { + return tracked_tx_timestamp_us; + } + protected: bool add_to_rx_queue(const CanRxItem &rx_item) override { return rx_queue_.push(rx_item); diff --git a/libraries/AP_HAL_ChibiOS/CANIface.h b/libraries/AP_HAL_ChibiOS/CANIface.h index 47809fa79b35a..f63c058d8dc63 100644 --- a/libraries/AP_HAL_ChibiOS/CANIface.h +++ b/libraries/AP_HAL_ChibiOS/CANIface.h @@ -115,6 +115,11 @@ class ChibiOS::CANIface : public AP_HAL::CANIface #endif const uint8_t self_index_; + // track tx timestamps + uint32_t tracked_tx_ts_mask; + uint32_t tracked_tx_ts_value; + uint64_t tracked_tx_timestamp_us; + bool computeTimings(uint32_t target_bitrate, Timings& out_timings); void setupMessageRam(void); @@ -250,6 +255,17 @@ class ChibiOS::CANIface : public AP_HAL::CANIface int8_t get_iface_num(void) const override { return self_index_; } + + // set mask and value on packet id to track tx timestamp for + void set_track_tx_timestamp(uint32_t mask, uint32_t value) override { + tracked_tx_ts_mask = mask; + tracked_tx_ts_value = value; + } + + // get timestamp of last packet sent with matching mask and value + uint64_t get_tracked_tx_timestamp() override { + return tracked_tx_timestamp_us; + } }; #endif //HAL_NUM_CAN_IFACES #endif //# if defined(STM32H7XX) || defined(STM32G4) diff --git a/libraries/AP_HAL_ChibiOS/CanIface.cpp b/libraries/AP_HAL_ChibiOS/CanIface.cpp index 677f484804943..ebc1d9bbc5e0d 100644 --- a/libraries/AP_HAL_ChibiOS/CanIface.cpp +++ b/libraries/AP_HAL_ChibiOS/CanIface.cpp @@ -500,6 +500,9 @@ void CANIface::handleTxMailboxInterrupt(uint8_t mailbox_index, bool txok, const #if !defined(HAL_BOOTLOADER_BUILD) stats.last_transmit_us = timestamp_us; #endif + if ((txi.frame.id & tracked_tx_ts_mask) == tracked_tx_ts_value) { + tracked_tx_timestamp_us = timestamp_us; + } } }