[uavcan] Use CAN abstraction (#3440)

* [CAN] timestamp in us, and fix typo

* [uavcan] Use CAN abstraction

* [CAN] Add missing can2.

* [WIP] CAN error handling

---------

Co-authored-by: Fabien-B <Fabien-B@github.com>

conf/modules/can.xml

@@ -11,12 +11,11 @@
   </doc>
   <dep>
     <depends>mcu</depends>
-    <conflicts>uavcan</conflicts>
   </dep>
   <header>
     <file name="can.h" dir="mcu_periph"/>
   </header>
-  <event fun="can_event()" cond="ifeq ($(ARCH), stm32)"/>
+  <!-- <event fun="can_event()" cond="ifeq ($(ARCH), stm32)"/> -->
   <makefile>
     <configure name="USE_CANFD" default="FALSE"/>
     <define name="USE_CANFD" value="$(USE_CANFD)"/>

conf/modules/uavcan.xml

@@ -10,6 +10,9 @@
    <define name="UAVCAN_NODE_ID" value="100" description="The UAVCAN node ID of the AP"/>
    <define name="UAVCAN_BAUDRATE" value="1000000" description="The baudrate of the CAN interface"/>
   </doc>
+  <dep>
+    <depends>can</depends>
+  </dep>
   <header>
     <file name="uavcan.h" dir="modules/uavcan"/>
   </header>
@@ -37,4 +40,3 @@
     <file_arch name="uavcan.c" dir="modules/uavcan"/>
   </makefile>
 </module>
-

sw/airborne/arch/chibios/mcu_periph/can_arch.c

@@ -85,11 +85,23 @@ static void can_thd_rx(void* arg) {
   snprintf(thd_name, 10, "can%d_rx", cas->if_index);
   chRegSetThreadName(thd_name);
 
+  event_listener_t rxe;
+  chEvtRegister(&cas->cand->error_event, &rxe, EVENT_MASK(1));
+
+
   struct pprzaddr_can addr = {
     .can_ifindex = cas->if_index
   };
 
   while(!chThdShouldTerminateX()) {
+
+    eventmask_t evts = chEvtWaitAnyTimeout(ALL_EVENTS, TIME_IMMEDIATE);
+    // receive error
+    if (evts & EVENT_MASK(1)) {
+      chEvtGetAndClearFlags(&rxe);
+      canp->nb_errors++;
+    }
+
     CANRxFrame rx_frame;
     msg_t status = canReceiveTimeout(cas->cand, CAN_ANY_MAILBOX, &rx_frame, chTimeMS2I(50));
     if(status == MSG_OK) { 
@@ -110,7 +122,7 @@ static void can_thd_rx(void* arg) {
         .can_id = id,
         .len = can_dlc_to_len(rx_frame.DLC),
         .flags = 0,
-        .timestamp = get_sys_time_msec(),
+        .timestamp = TIME_I2US(chVTGetSystemTimeX())
       };
       
       if(rx_frame.FDF) {
@@ -142,9 +154,9 @@ int can_transmit_frame(struct pprzcan_frame* txframe, struct pprzaddr_can* addr)
   }
   if(txframe->can_id & CAN_FRAME_EFF) {
     frame.common.XTD = 1;
-    frame.ext.EID = txframe->can_id & CAN_EID_MASK
+    frame.ext.EID = txframe->can_id & CAN_EID_MASK;
   } else {
-    frame.std.SID = txframe->can_id & CAN_SID_MASK
+    frame.std.SID = txframe->can_id & CAN_SID_MASK;
   }
   memcpy(frame.data8, txframe->data, txframe->len);
 
@@ -308,4 +320,4 @@ static bool canConfigureIface(struct can_arch_periph* cas)
   cas->cfg.btr = CAN_BTR_SJW(0) | CAN_BTR_TS1(bs1 - 1) | CAN_BTR_TS2(bs2 - 1) | CAN_BTR_BRP(prescaler - 1);
 #endif
   return true;
-}
+}

sw/airborne/arch/chibios/modules/uavcan/uavcan.c

@@ -25,6 +25,8 @@
  */
 
 #include "uavcan.h"
+#include "mcu_periph/can.h"
+#include "modules/core/threads.h"
 
 #ifndef UAVCAN_NODE_ID
 #define UAVCAN_NODE_ID    100
@@ -45,17 +47,10 @@ static uavcan_event *uavcan_event_hd = NULL;
 #define UAVCAN_CAN1_BAUDRATE UAVCAN_BAUDRATE
 #endif
 
-static THD_WORKING_AREA(uavcan1_rx_wa, 1024 * 2);
-static THD_WORKING_AREA(uavcan1_tx_wa, 1024 * 2);
 
 struct uavcan_iface_t uavcan1 = {
-  .can_driver = &CAND1,
+  .can_net = {.can_ifindex = 1},
   .can_baudrate = UAVCAN_CAN1_BAUDRATE,
-  .can_cfg = {0},
-  .thread_rx_wa = uavcan1_rx_wa,
-  .thread_rx_wa_size = sizeof(uavcan1_rx_wa),
-  .thread_tx_wa = uavcan1_tx_wa,
-  .thread_tx_wa_size = sizeof(uavcan1_tx_wa),
   .node_id = UAVCAN_CAN1_NODE_ID,
   .transfer_id = 0,
   .initialized = false
@@ -71,117 +66,56 @@ struct uavcan_iface_t uavcan1 = {
 #define UAVCAN_CAN2_BAUDRATE UAVCAN_BAUDRATE
 #endif
 
-static THD_WORKING_AREA(uavcan2_rx_wa, 1024 * 2);
-static THD_WORKING_AREA(uavcan2_tx_wa, 1024 * 2);
 
 struct uavcan_iface_t uavcan2 = {
-  .can_driver = &CAND2,
+  .can_net = {.can_ifindex = 2},
   .can_baudrate = UAVCAN_CAN2_BAUDRATE,
-  .can_cfg = {0},
-  .thread_rx_wa = uavcan2_rx_wa,
-  .thread_rx_wa_size = sizeof(uavcan2_rx_wa),
-  .thread_tx_wa = uavcan2_tx_wa,
-  .thread_tx_wa_size = sizeof(uavcan2_tx_wa),
   .node_id = UAVCAN_CAN2_NODE_ID,
   .transfer_id = 0,
   .initialized = false
 };
 #endif
 
-/*
- * REMOVE ME - This is only here because the can driver is not yet used
- */
-void can_init(void) { }
 
-/*
- * Receiver thread.
- */
-static THD_FUNCTION(uavcan_rx, p)
-{
-  event_listener_t el;
-  CANRxFrame rx_msg;
+static void can_frame_cb(struct pprzcan_frame* rx_msg, UNUSED struct pprzaddr_can* src_addr, void* user_data) {
+  struct uavcan_iface_t *iface = (struct uavcan_iface_t *)user_data;
+
+  pprz_mtx_lock(&iface->mutex);
+
   CanardCANFrame rx_frame;
-  struct uavcan_iface_t *iface = (struct uavcan_iface_t *)p;
-
-  chRegSetThreadName("uavcan_rx");
-  chEvtRegister(&iface->can_driver->rxfull_event, &el, EVENT_MASK(0));
-  while (true) {
-    if (chEvtWaitAnyTimeout(ALL_EVENTS, TIME_MS2I(100)) == 0) {
-      continue;
-    }
-    chMtxLock(&iface->mutex);
-
-    // Wait until a CAN message is received
-    while (canReceive(iface->can_driver, CAN_ANY_MAILBOX, &rx_msg, TIME_IMMEDIATE) == MSG_OK) {
-      // Process message.
-      const uint32_t timestamp = TIME_I2US(chVTGetSystemTimeX());
-      memcpy(rx_frame.data, rx_msg.data8, 8);
-      rx_frame.data_len = rx_msg.DLC;
-#if defined(STM32_CAN_USE_FDCAN1) || defined(STM32_CAN_USE_FDCAN2)
-      if (rx_msg.common.XTD) {
-        rx_frame.id = CANARD_CAN_FRAME_EFF | rx_msg.ext.EID;
-      } else {
-        rx_frame.id = rx_msg.std.SID;
-      }
-#else
-      if (rx_msg.IDE) {
-        rx_frame.id = CANARD_CAN_FRAME_EFF | rx_msg.EID;
-      } else {
-        rx_frame.id = rx_msg.SID;
-      }
-#endif
-
-      // Let canard handle the frame
-      canardHandleRxFrame(&iface->canard, &rx_frame, timestamp);
-    }
-    chMtxUnlock(&iface->mutex);
+  memcpy(rx_frame.data, rx_msg->data, 8);
+  rx_frame.data_len = rx_msg->len;
+  if (rx_msg->can_id & CAN_FRAME_EFF) {
+    rx_frame.id = CANARD_CAN_FRAME_EFF | (rx_msg->can_id & CAN_EID_MASK);
+  } else {
+    rx_frame.id = rx_msg->can_id & CAN_SID_MASK;
   }
-  chEvtUnregister(&iface->can_driver->rxfull_event, &el);
+
+  // Let canard handle the frame
+  canardHandleRxFrame(&iface->canard, &rx_frame, rx_msg->timestamp);
+
+  pprz_mtx_unlock(&iface->mutex);
 }
 
 /*
  * Transmitter thread.
  */
-static THD_FUNCTION(uavcan_tx, p)
+static void uavcan_tx(void* p)
 {
-  event_listener_t txc, txe, txr;
   struct uavcan_iface_t *iface = (struct uavcan_iface_t *)p;
   uint8_t err_cnt = 0;
 
-  chRegSetThreadName("uavcan_tx");
-  chEvtRegister(&iface->can_driver->txempty_event, &txc, EVENT_MASK(0));
-  chEvtRegister(&iface->can_driver->error_event, &txe, EVENT_MASK(1));
-  chEvtRegister(&iface->tx_request, &txr, EVENT_MASK(2));
-
   while (true) {
-    eventmask_t evts = chEvtWaitAnyTimeout(ALL_EVENTS, TIME_MS2I(100));
-    // Succesfull transmit
-    if (evts == 0) {
-      continue;
-    }
+    pprz_bsem_wait(&iface->bsem);
 
-    // Transmit error
-    if (evts & EVENT_MASK(1)) {
-      chEvtGetAndClearFlags(&txe);
-      continue;
-    }
-
-    chMtxLock(&iface->mutex);
+    pprz_mtx_lock(&iface->mutex);
     for (const CanardCANFrame *txf = NULL; (txf = canardPeekTxQueue(&iface->canard)) != NULL;) {
-      CANTxFrame tx_msg;
-      memcpy(tx_msg.data8, txf->data, 8);
-#if defined(STM32_CAN_USE_FDCAN1) || defined(STM32_CAN_USE_FDCAN2)
-      tx_msg.DLC = txf->data_len; // TODO fixme for FDCAN (>8 bytes)
-      tx_msg.ext.EID = txf->id & CANARD_CAN_EXT_ID_MASK;
-      tx_msg.common.XTD = 1;
-      tx_msg.common.RTR = 0;
-#else
-      tx_msg.DLC = txf->data_len;
-      tx_msg.EID = txf->id & CANARD_CAN_EXT_ID_MASK;
-      tx_msg.IDE = CAN_IDE_EXT;
-      tx_msg.RTR = CAN_RTR_DATA;
-#endif
-      if (!canTryTransmitI(iface->can_driver, CAN_ANY_MAILBOX, &tx_msg)) {
+      struct pprzcan_frame tx_msg;
+      memcpy(tx_msg.data, txf->data, 8);
+      tx_msg.len = txf->data_len;
+      tx_msg.can_id = (txf->id & CANARD_CAN_EXT_ID_MASK) | CAN_FRAME_EFF;
+
+      if (!can_transmit_frame(&tx_msg, &iface->can_net)) {
         err_cnt = 0;
         canardPopTxQueue(&iface->canard);
       } else {
@@ -193,13 +127,13 @@ static THD_FUNCTION(uavcan_tx, p)
         }
 
         // Timeout - just exit and try again later
-        chMtxUnlock(&iface->mutex);
+        pprz_mtx_unlock(&iface->mutex);
         chThdSleepMilliseconds(++err_cnt);
-        chMtxLock(&iface->mutex);
+        pprz_mtx_lock(&iface->mutex);
         continue;
       }
     }
-    chMtxUnlock(&iface->mutex);
+    pprz_mtx_unlock(&iface->mutex);
   }
 }
 
@@ -239,133 +173,14 @@ static bool shouldAcceptTransfer(const CanardInstance *ins __attribute__((unused
   return false;
 }
 
-/**
- * Try to compute the timing registers for the can interface and set the configuration
- */
-static bool uavcanConfigureIface(struct uavcan_iface_t *iface)
-{
-  if (iface->can_baudrate < 1) {
-    return false;
-  }
-
-  // Hardware configurationn
-#if defined(STM32_CAN_USE_FDCAN1) || defined(STM32_CAN_USE_FDCAN2)
-  const uint32_t pclk = STM32_FDCANCLK;
-#else
-  const uint32_t pclk = STM32_PCLK1;
-#endif
-  static const int MaxBS1 = 16;
-  static const int MaxBS2 = 8;
-
-  /*
-    * Ref. "Automatic Baudrate Detection in CANopen Networks", U. Koppe, MicroControl GmbH & Co. KG
-    *      CAN in Automation, 2003
-    *
-    * According to the source, optimal quanta per bit are:
-    *   Bitrate        Optimal Maximum
-    *   1000 kbps      8       10
-    *   500  kbps      16      17
-    *   250  kbps      16      17
-    *   125  kbps      16      17
-    */
-  const int max_quanta_per_bit = (iface->can_baudrate >= 1000000) ? 10 : 17;
-  static const int MaxSamplePointLocation = 900;
-
-  /*
-    * Computing (prescaler * BS):
-    *   BITRATE = 1 / (PRESCALER * (1 / PCLK) * (1 + BS1 + BS2))       -- See the Reference Manual
-    *   BITRATE = PCLK / (PRESCALER * (1 + BS1 + BS2))                 -- Simplified
-    * let:
-    *   BS = 1 + BS1 + BS2                                             -- Number of time quanta per bit
-    *   PRESCALER_BS = PRESCALER * BS
-    * ==>
-    *   PRESCALER_BS = PCLK / BITRATE
-    */
-  const uint32_t prescaler_bs = pclk / iface->can_baudrate;
-
-// Searching for such prescaler value so that the number of quanta per bit is highest.
-  uint8_t bs1_bs2_sum = max_quanta_per_bit - 1;
-  while ((prescaler_bs % (1 + bs1_bs2_sum)) != 0) {
-    if (bs1_bs2_sum <= 2) {
-      return false;          // No solution
-    }
-    bs1_bs2_sum--;
-  }
-
-  const uint32_t prescaler = prescaler_bs / (1 + bs1_bs2_sum);
-  if ((prescaler < 1U) || (prescaler > 1024U)) {
-    return false;              // No solution
-  }
-
-  /*
-    * Now we have a constraint: (BS1 + BS2) == bs1_bs2_sum.
-    * We need to find the values so that the sample point is as close as possible to the optimal value.
-    *
-    *   Solve[(1 + bs1)/(1 + bs1 + bs2) == 7/8, bs2]  (* Where 7/8 is 0.875, the recommended sample point location *)
-    *   {{bs2 -> (1 + bs1)/7}}
-    *
-    * Hence:
-    *   bs2 = (1 + bs1) / 7
-    *   bs1 = (7 * bs1_bs2_sum - 1) / 8
-    *
-    * Sample point location can be computed as follows:
-    *   Sample point location = (1 + bs1) / (1 + bs1 + bs2)
-    *
-    * Since the optimal solution is so close to the maximum, we prepare two solutions, and then pick the best one:
-    *   - With rounding to nearest
-    *   - With rounding to zero
-    */
-// First attempt with rounding to nearest
-  uint8_t bs1 = ((7 * bs1_bs2_sum - 1) + 4) / 8;
-  uint8_t bs2 = bs1_bs2_sum - bs1;
-  uint16_t sample_point_permill = 1000 * (1 + bs1) / (1 + bs1 + bs2);
-
-// Second attempt with rounding to zero
-  if (sample_point_permill > MaxSamplePointLocation) {
-    bs1 = (7 * bs1_bs2_sum - 1) / 8;
-    bs2 = bs1_bs2_sum - bs1;
-    sample_point_permill = 1000 * (1 + bs1) / (1 + bs1 + bs2);
-  }
-
-  /*
-    * Final validation
-    * Helpful Python:
-    * def sample_point_from_btr(x):
-    *     assert 0b0011110010000000111111000000000 & x == 0
-    *     ts2,ts1,brp = (x>>20)&7, (x>>16)&15, x&511
-    *     return (1+ts1+1)/(1+ts1+1+ts2+1)
-    *
-    */
-  if ((iface->can_baudrate != (pclk / (prescaler * (1 + bs1 + bs2)))) || (bs1 < 1) || (bs1 > MaxBS1) || (bs2 < 1)
-      || (bs2 > MaxBS2)) {
-    return false;
-  }
-
-  // Configure the interface
-#if defined(STM32_CAN_USE_FDCAN1) || defined(STM32_CAN_USE_FDCAN2)
-  iface->can_cfg.NBTP = (0 << FDCAN_NBTP_NSJW_Pos) | ((bs1 - 1) << FDCAN_NBTP_NTSEG1_Pos) | ((
-                          bs2 - 1) << FDCAN_NBTP_NTSEG2_Pos) | ((prescaler - 1) << FDCAN_NBTP_NBRP_Pos);
-  iface->can_cfg.CCCR = FDCAN_CCCR_FDOE | FDCAN_CCCR_BRSE;
-#else
-  iface->can_cfg.mcr = CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_TXFP;
-  iface->can_cfg.btr = CAN_BTR_SJW(0) | CAN_BTR_TS1(bs1 - 1) | CAN_BTR_TS2(bs2 - 1) | CAN_BTR_BRP(prescaler - 1);
-#endif
-  return true;
-}
 
 /**
  * Initialize uavcan interface
  */
 static void uavcanInitIface(struct uavcan_iface_t *iface)
 {
-  // First try to configure abort if failed
-  if (!uavcanConfigureIface(iface)) {
-    return;
-  }
-
-  // Initialize mutexes/events for multithread locking
-  chMtxObjectInit(&iface->mutex);
-  chEvtObjectInit(&iface->tx_request);
+  pprz_mtx_init(&iface->mutex);
+  pprz_bsem_init(&iface->bsem, true);
 
   // Initialize canard
   canardInit(&iface->canard, iface->canard_memory_pool, sizeof(iface->canard_memory_pool),
@@ -373,13 +188,12 @@ static void uavcanInitIface(struct uavcan_iface_t *iface)
   // Update the uavcan node ID
   canardSetLocalNodeID(&iface->canard, iface->node_id);
 
-  // Start the can interface
-  canStart(iface->can_driver, &iface->can_cfg);
-
   // Start the receiver and transmitter thread
-  chThdCreateStatic(iface->thread_rx_wa, iface->thread_rx_wa_size, NORMALPRIO + 8, uavcan_rx, (void *)iface);
-  chThdCreateStatic(iface->thread_tx_wa, iface->thread_tx_wa_size, NORMALPRIO + 7, uavcan_tx, (void *)iface);
-  iface->initialized = true;
+  can_register_callback(can_frame_cb, &iface->can_net, (void *)iface);
+
+  if(!pprz_thread_create(&iface->thread_tx, 2048, "uavcan_tx", NORMALPRIO+7, uavcan_tx, (void *)iface)) {
+    iface->initialized = true;
+  }
 }
 
 /**
@@ -388,25 +202,9 @@ static void uavcanInitIface(struct uavcan_iface_t *iface)
 void uavcan_init(void)
 {
 #if UAVCAN_USE_CAN1
-#if defined(STM32_CAN_USE_FDCAN1) || defined(STM32_CAN_USE_FDCAN2)
-  // Configure the RAM
-  uavcan1.can_cfg.RXF0C = (32 << FDCAN_RXF0C_F0S_Pos) | (0 << FDCAN_RXF0C_F0SA_Pos);
-  uavcan1.can_cfg.RXF1C = (32 << FDCAN_RXF1C_F1S_Pos) | (128 << FDCAN_RXF1C_F1SA_Pos);
-  uavcan1.can_cfg.TXBC  = (32 << FDCAN_TXBC_TFQS_Pos) | (256 << FDCAN_TXBC_TBSA_Pos);
-  uavcan1.can_cfg.TXESC = 0x000; // 8 Byte mode only (4 words per message)
-  uavcan1.can_cfg.RXESC = 0x000; // 8 Byte mode only (4 words per message)
-#endif
   uavcanInitIface(&uavcan1);
 #endif
 #if UAVCAN_USE_CAN2
-#if defined(STM32_CAN_USE_FDCAN1) || defined(STM32_CAN_USE_FDCAN2)
-  // Configure the RAM
-  uavcan2.can_cfg.RXF0C = (32 << FDCAN_RXF0C_F0S_Pos) | (384 << FDCAN_RXF0C_F0SA_Pos);
-  uavcan2.can_cfg.RXF1C = (32 << FDCAN_RXF1C_F1S_Pos) | (512 << FDCAN_RXF1C_F1SA_Pos);
-  uavcan2.can_cfg.TXBC  = (32 << FDCAN_TXBC_TFQS_Pos) | (640 << FDCAN_TXBC_TBSA_Pos);
-  uavcan2.can_cfg.TXESC = 0x000; // 8 Byte mode only (4 words per message)
-  uavcan2.can_cfg.RXESC = 0x000; // 8 Byte mode only (4 words per message)
-#endif
   uavcanInitIface(&uavcan2);
 #endif
 }
@@ -434,12 +232,11 @@ void uavcan_broadcast(struct uavcan_iface_t *iface, uint64_t data_type_signature
                       uint16_t payload_len)
 {
   if (!iface->initialized) { return; }
-
-  chMtxLock(&iface->mutex);
+  pprz_mtx_lock(&iface->mutex);
   canardBroadcast(&iface->canard,
                   data_type_signature,
                   data_type_id, &iface->transfer_id,
                   priority, payload, payload_len);
-  chMtxUnlock(&iface->mutex);
-  chEvtBroadcast(&iface->tx_request);
+  pprz_mtx_unlock(&iface->mutex);
+  pprz_bsem_signal(&iface->bsem);
 }

sw/airborne/arch/chibios/modules/uavcan/uavcan.h

@@ -26,24 +26,25 @@
 #ifndef MODULES_UAVCAN_ARCH_H
 #define MODULES_UAVCAN_ARCH_H
 
-#include <hal.h>
 #include <canard.h>
 #include <string.h>
+#include "mcu_periph/can.h"
+#include "modules/core/threads.h"
 
 /** uavcan interface structure */
 struct uavcan_iface_t {
-  CANDriver *can_driver;
+  struct pprzaddr_can can_net;
   uint32_t can_baudrate;
-  CANConfig can_cfg;
 
   event_source_t tx_request;
-  mutex_t mutex;
-  void *thread_rx_wa;
-  void *thread_tx_wa;
-  void *thread_uavcan_wa;
-  size_t thread_rx_wa_size;
-  size_t thread_tx_wa_size;
-  size_t thread_uavcan_wa_size;
+
+  pprz_mutex_t mutex;
+  pprz_thread_t thread_tx;
+  // void *thread_tx_wa;
+  // void *thread_uavcan_wa;
+  // size_t thread_tx_wa_size;
+  // size_t thread_uavcan_wa_size;
+  pprz_bsem_t bsem;
 
   uint8_t node_id;
   CanardInstance canard;

sw/airborne/mcu_periph/can.c

@@ -29,6 +29,16 @@
 
 struct can_periph can1 = {
   .fd = 1,
+  .nb_errors = 0,
+  .callbacks = {0},
+  .callback_user_data = {0}
+};
+#endif
+
+#if USE_CAN2
+struct can_periph can2 = {
+  .fd = 2,
+  .nb_errors = 0,
   .callbacks = {0},
   .callback_user_data = {0}
 };

sw/airborne/mcu_periph/can.h

@@ -73,7 +73,7 @@ struct pprzcan_frame {
   socketcan_id_t can_id;
   uint8_t len;
   uint8_t flags;  // CAN FD specific flags
-  uint32_t timestamp;   // timestamp in ms.
+  uint32_t timestamp;   // timestamp in us.
   uint8_t data[SOCKETCAN_MAX_DLEN];
 };
 
@@ -89,6 +89,7 @@ typedef void(* can_rx_frame_callback_t)(struct pprzcan_frame* rxframe, struct pp
 struct can_periph {
   void* arch_struct;
   int fd;
+  uint32_t nb_errors;
   can_rx_frame_callback_t callbacks[CAN_NB_CALLBACKS_MAX];
   void* callback_user_data[CAN_NB_CALLBACKS_MAX];
 };

sw/airborne/modules/core/slcan.c

@@ -258,10 +258,11 @@ void slcan_can_rx_cb(struct pprzcan_frame* rxframe, struct pprzaddr_can* src_add
   }
 
   if(slcan->timestamp) {
-    slcan->tx_buf[idx++] = nibble2hex((rxframe->timestamp >> 12));
-    slcan->tx_buf[idx++] = nibble2hex((rxframe->timestamp >> 8));
-    slcan->tx_buf[idx++] = nibble2hex((rxframe->timestamp >> 4));
-    slcan->tx_buf[idx++] = nibble2hex((rxframe->timestamp >> 0));
+    uint16_t timestamp_ms = rxframe->timestamp / 1000;
+    slcan->tx_buf[idx++] = nibble2hex((timestamp_ms >> 12));
+    slcan->tx_buf[idx++] = nibble2hex((timestamp_ms >> 8));
+    slcan->tx_buf[idx++] = nibble2hex((timestamp_ms >> 4));
+    slcan->tx_buf[idx++] = nibble2hex((timestamp_ms >> 0));
   }
 
   slcan->tx_buf[idx++] = '\r';