AP_HAL_ChibiOS: add implementation for tracking transmit timestamp on…

… CAN
bugobliterator · Oct 18, 2023 · 77f131e · 77f131e
1 parent 7c4090f
commit 77f131e
Show file tree

Hide file tree

Showing 4 changed files with 38 additions and 0 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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;
+        }
     }
 }