Uavcan update (#3579)

* Make UAVCAN arch independant
* Comply with uavcan info requests
* Comply with uavcan transfer_ids specifications
* Add uavcan dynamic node id allocation server
* Add uavcan tunnel device
* Add uavcan RC input
* Use uavcan semaphores to achieve thread safety
* Add key-value store
* Various tweaks to make everything work...

conf/modules/actuators_uavcan.xml

@@ -26,5 +26,7 @@
     <file name="uavcan.equipment.actuator.ArrayCommand.c" dir="$(PAPARAZZI_HOME)/var/include/DSDLcode/src"/>
     <file name="uavcan.equipment.device.Temperature.c" dir="$(PAPARAZZI_HOME)/var/include/DSDLcode/src"/>
   </makefile>
+  <makefile target="sim|nps">
+    <file_arch name="actuators_uavcan_arch.c"/>
+  </makefile>
 </module>
-

conf/modules/gps_uavcan.xml

@@ -12,7 +12,7 @@
     <file name="gps_uavcan.h"/>
   </header>
   <init fun="gps_uavcan_init()"/>
-  <makefile>
+  <makefile target="ap|fbw">
     <file name="gps_uavcan.c"/>
     <!-- Load DSDL generated files-->
     <include name="$(PAPARAZZI_HOME)/var/include/DSDLcode/include"/>

conf/modules/radio_control_uavcan.xml

@@ -0,0 +1,28 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+<module name="radio_control_uavcan" dir="radio_control" task="radio_control">
+  <doc>
+    <description>Radio control from DroneCAN message `dronecan.sensors.rc.RCInput`.</description>
+  </doc>
+  <dep>
+    <depends>uavcan,radio_control_common</depends>
+    <provides>radio_control</provides>
+  </dep>
+  <header>
+    <file name="radio_control_uavcan.h"/>
+  </header>
+  <init fun="rc_uavcan_init()"/>
+  <event fun="rc_uavcan_event()"/>
+  <makefile>
+    <configure name="RC_UAVCAN_TYPE" default="RC_UAVCAN_SBUS"/>
+    <define name="RC_UAVCAN_TYPE" value="$(RC_UAVCAN_SBUS)" />
+    <file name="radio_control_uavcan.c"/>
+
+    <!-- Load DSDL generated files-->
+    <include name="$(PAPARAZZI_HOME)/var/include/DSDLcode/include"/>
+    <file name="dronecan.sensors.rc.RCInput.c" dir="$(PAPARAZZI_HOME)/var/include/DSDLcode/src"/>
+    <test>
+      <include name="$(PAPARAZZI_HOME)/sw/ext/dronecan/libcanard"/>
+    </test>
+    <test/>
+  </makefile>
+</module>

conf/modules/tunnel_uavcan.xml

@@ -0,0 +1,33 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+<module name="tunnel_uavcan" dir="datalink" task="radio_control">
+  <doc>
+    <description>UAVCAN device over `uavcan.tunnel.Broadcast` message.</description>
+    <configure name="TUNNEL_UAVCAN" value="uavcan1" default="uavcan1" description="uavcan interface"/>
+    <define name="TUNNEL_UAVCAN_PROTOCOL" description="uavcan.tunnel.Protocol enum value." value="3"/>
+    <define name="TUNNEL_UAVCAN_CHANNEL_ID" description="uavcan.tunnel.Broadcast.channel_id" value="0"/>
+  </doc>
+  <dep>
+    <depends>uavcan</depends>
+    <provides></provides>
+  </dep>
+  <header>
+    <file name="tunnel_uavcan.h"/>
+  </header>
+  <init fun="tunnel_uavcan_init()"/>
+  <makefile>
+    <configure name="TUNNEL_UAVCAN" default="uavcan1" case="upper|lower"/>
+    <define name="TUNNEL_UAVCAN" value="$(TUNNEL_UAVCAN_LOWER)" />
+    <define name="USE_TUNNEL_UAVCAN"/>
+    <file name="tunnel_uavcan.c" dir="modules/datalink"/>
+    <file name="ring_buffer.c" dir="utils" />
+
+    <!-- Load DSDL generated files-->
+    <include name="$(PAPARAZZI_HOME)/var/include/DSDLcode/include"/>
+    <file name="uavcan.tunnel.Broadcast.c" dir="$(PAPARAZZI_HOME)/var/include/DSDLcode/src"/>
+    <test>
+      <include name="$(PAPARAZZI_HOME)/sw/ext/dronecan/libcanard"/>
+      <define name="USE_CAN1" />
+      <define name="UAVCAN_USE_CAN1" />
+    </test>
+  </makefile>
+</module>

conf/modules/uavcan.xml

@@ -1,6 +1,6 @@
 <!DOCTYPE module SYSTEM "module.dtd">
 
-<module name="uavcan" dir="uavcan">
+<module name="uavcan" dir="uavcan" task="core">
   <doc>
     <description>
       UAVCan interface for transmitting/receiving uavcan messages on CAN busses
@@ -17,7 +17,8 @@
     <file name="uavcan.h" dir="modules/uavcan"/>
   </header>
   <init fun="uavcan_init()"/>
-  <makefile target="ap">
+  <periodic fun="uavcan_reporting()" freq="2"/>
+  <makefile>
     <!-- Enable the CAN busses if needed -->
     <configure name="UAVCAN_USE_CAN1" default="FALSE"/>
     <configure name="UAVCAN_USE_CAN2" default="FALSE"/>
@@ -25,6 +26,7 @@
     <define name="USE_CAN2" cond="ifeq ($(UAVCAN_USE_CAN2), TRUE)"/>
     <define name="UAVCAN_USE_CAN1" value="$(UAVCAN_USE_CAN1)"/>
     <define name="UAVCAN_USE_CAN2" value="$(UAVCAN_USE_CAN2)"/>
+    <define name="CANARD_ALLOCATE_SEM"/>
 
     <!-- Load canard -->
     <include name="$(PAPARAZZI_SRC)/sw/ext/dronecan/libcanard"/>
@@ -35,9 +37,13 @@
     <file name="uavcan.protocol.NodeStatus.c" dir="$(PAPARAZZI_HOME)/var/include/DSDLcode/src"/>
     <file name="uavcan.protocol.GetNodeInfo_req.c" dir="$(PAPARAZZI_HOME)/var/include/DSDLcode/src"/>
     <file name="uavcan.protocol.GetNodeInfo_res.c" dir="$(PAPARAZZI_HOME)/var/include/DSDLcode/src"/>
+    <file name="uavcan.protocol.dynamic_node_id.Allocation.c" dir="$(PAPARAZZI_HOME)/var/include/DSDLcode/src"/>
 
     <!-- Load uavcan itself -->
-    <file_arch name="uavcan.c" dir="modules/uavcan"/>
+    <file name="uavcan.c" dir="modules/uavcan"/>
+    <file name="uavcan_reporting.c" dir="modules/uavcan"/>
+    <file name="uavcan_allocator.c" dir="modules/uavcan"/>
     <file name="framed_ring_buffer.c" dir="utils"/>
+    <file name="kv_store.c" dir="utils"/>
   </makefile>
 </module>

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

@@ -125,7 +125,7 @@ void sys_time_usleep(uint32_t us)
   }
 }
 
-void sys_time_msleep(uint16_t ms)
+void sys_time_msleep(uint32_t ms)
 {
   chThdSleepMilliseconds(ms);
 }

sw/airborne/arch/chibios/mcu_periph/sys_time_arch.h

@@ -44,8 +44,6 @@
 extern uint32_t get_sys_time_usec(void);
 extern uint32_t get_sys_time_usec100(void);
 extern uint32_t get_sys_time_msec(void);
-extern void sys_time_usleep(uint32_t us);
-extern void sys_time_msleep(uint16_t ms);
 extern void sys_time_ssleep(uint8_t s);
 
 #endif /* SYS_TIME_ARCH_H */

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

@@ -0,0 +1,24 @@
+
+
+#include "mcu_periph/can_arch.h"
+#include "mcu_periph/can.h"
+#include "mcu_periph/sys_time_arch.h"
+#include "stdio.h"
+#include "string.h"
+
+
+
+struct can_arch_periph {
+
+};
+
+
+
+void can_hw_init(void) {
+  
+}
+
+int can_transmit_frame(struct pprzcan_frame* txframe __attribute__((__unused__)), struct pprzaddr_can* addr __attribute__((__unused__))) {
+  // TODO
+  return 0;
+}

sw/airborne/arch/linux/mcu_periph/can_arch.h

@@ -0,0 +1,5 @@
+
+
+#pragma once
+
+void can_hw_init(void);

sw/airborne/arch/linux/mcu_periph/sys_time_arch.c

@@ -25,6 +25,7 @@
  */
 
 #include "mcu_periph/sys_time.h"
+#include "mcu_periph/sys_time_arch.h"
 #include <stdio.h>
 #include <pthread.h>
 #include <sys/timerfd.h>
@@ -215,3 +216,12 @@ uint32_t get_sys_time_msec(void)
   }
   return d_sec * 1000 + d_nsec / 1000000;
 }
+
+void sys_time_usleep(uint32_t us)
+{
+  usleep(us);
+}
+
+void sys_time_msleep(uint32_t ms) {
+  sys_time_usleep(ms*1000);
+}

sw/airborne/arch/linux/mcu_periph/sys_time_arch.h

@@ -30,6 +30,7 @@
 
 #include "std.h"
 #include <unistd.h>
+#include "mcu_periph/sys_time.h"
 
 /**
  * Get the time in microseconds since startup.
@@ -51,10 +52,6 @@ extern uint32_t get_sys_time_usec100(void);
  */
 extern uint32_t get_sys_time_msec(void);
 
-static inline void sys_time_usleep(uint32_t us)
-{
-  usleep(us);
-}
 
 /** elapsed time in microsecs between two timespecs */
 static inline unsigned int sys_time_elapsed_us(struct timespec *prev, struct timespec *now)

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

@@ -0,0 +1 @@
+../../linux/mcu_periph/can_arch.c

sw/airborne/arch/sim/mcu_periph/can_arch.h

@@ -0,0 +1 @@
+../../linux/mcu_periph/can_arch.h

sw/airborne/arch/sim/mcu_periph/sys_time_arch.c

@@ -26,6 +26,7 @@
  */
 
 #include "mcu_periph/sys_time.h"
+#include "mcu_periph/sys_time_arch.h"
 
 
 void sys_time_arch_init(void)
@@ -54,3 +55,9 @@ void sys_tick_handler(void)
     }
   }
 }
+
+void sys_time_usleep(uint32_t us __attribute__((unused))) {}
+
+void sys_time_msleep(uint32_t ms) {
+  sys_time_usleep(ms*1000);
+}

sw/airborne/arch/sim/mcu_periph/sys_time_arch.h

@@ -29,6 +29,7 @@
 #define SYS_TIME_ARCH_H
 
 #include "std.h"
+#include "mcu_periph/sys_time.h"
 
 extern void sys_tick_handler(void);
 
@@ -62,7 +63,4 @@ static inline uint32_t get_sys_time_msec(void)
          msec_of_cpu_ticks(sys_time.nb_sec_rem);
 }
 
-
-static inline void sys_time_usleep(uint32_t us __attribute__((unused))) {}
-
 #endif /* SYS_TIME_ARCH_H */

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

@@ -1,30 +0,0 @@
-/*
- * Copyright (C) 2021 Freek van Tienen <freek.v.tienen@gmail.com>
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- */
-/**
- * @file arch/chibios/modules/uavcan/uavcan.h
- * Stub needed for compiling modules requiring uavcan
- */
-#ifndef MODULES_UAVCAN_ARCH_H
-#define MODULES_UAVCAN_ARCH_H
-
-struct uavcan_iface_t {};
-
-#endif /* MODULES_UAVCAN_ARCH_H */

sw/airborne/arch/stm32/mcu_periph/sys_time_arch.c

@@ -28,6 +28,7 @@
  */
 
 #include "mcu_periph/sys_time.h"
+#include "mcu_periph/sys_time_arch.h"
 
 #include "libopencm3/cm3/systick.h"
 
@@ -96,3 +97,40 @@ void sys_tick_handler(void)
     }
   }
 }
+
+/** Busy wait in microseconds.
+ *
+ *  max value is limited by the max number of cycle
+ *  i.e 2^32 * usec_of_cpu_ticks(systick_get_reload())
+ */
+void sys_time_usleep(uint32_t us)
+{
+  // start time
+  uint32_t start = systick_get_value();
+  // max time of one full counter cycle (n + 1 ticks)
+  uint32_t DT = usec_of_cpu_ticks(systick_get_reload() + 1);
+  // number of cycles
+  uint32_t n = us / DT;
+  // remaining number of cpu ticks
+  uint32_t rem = cpu_ticks_of_usec(us % DT);
+  // end time depend on the current value of the counter
+  uint32_t end;
+  if (rem < start) {
+    end = start - rem;
+  } else {
+    // one more count flag is required
+    n++;
+    end = systick_get_reload() - rem + start;
+  }
+  // count number of cycles (when counter reachs 0)
+  while (n) {
+    while (!systick_get_countflag());
+    n--;
+  }
+  // wait remaining ticks
+  while (systick_get_value() > end);
+}
+
+void sys_time_msleep(uint32_t ms) {
+  sys_time_usleep(ms*1000);
+}

sw/airborne/arch/stm32/mcu_periph/sys_time_arch.h

@@ -74,38 +74,4 @@ static inline uint32_t get_sys_time_msec(void)
          msec_of_cpu_ticks(systick_get_reload() - systick_get_value());
 }
 
-
-/** Busy wait in microseconds.
- *
- *  max value is limited by the max number of cycle
- *  i.e 2^32 * usec_of_cpu_ticks(systick_get_reload())
- */
-static inline void sys_time_usleep(uint32_t us)
-{
-  // start time
-  uint32_t start = systick_get_value();
-  // max time of one full counter cycle (n + 1 ticks)
-  uint32_t DT = usec_of_cpu_ticks(systick_get_reload() + 1);
-  // number of cycles
-  uint32_t n = us / DT;
-  // remaining number of cpu ticks
-  uint32_t rem = cpu_ticks_of_usec(us % DT);
-  // end time depend on the current value of the counter
-  uint32_t end;
-  if (rem < start) {
-    end = start - rem;
-  } else {
-    // one more count flag is required
-    n++;
-    end = systick_get_reload() - rem + start;
-  }
-  // count number of cycles (when counter reachs 0)
-  while (n) {
-    while (!systick_get_countflag());
-    n--;
-  }
-  // wait remaining ticks
-  while (systick_get_value() > end);
-}
-
 #endif /* SYS_TIME_ARCH_H */

sw/airborne/mcu_periph/sys_time.h

@@ -82,6 +82,16 @@ struct sys_time {
 
 extern struct sys_time sys_time;
 
+/**
+ * Sleep @param us microseconds
+ */
+void sys_time_usleep(uint32_t us);
+
+/**
+ * Sleep @param ms milliseconds
+ */
+void sys_time_msleep(uint32_t ms);
+
 
 extern void sys_time_init(void);
 

sw/airborne/modules/core/abi_sender_ids.h

@@ -664,6 +664,10 @@
 #define RADIO_CONTROL_INTERMCU_ID 9
 #endif
 
+#ifndef RADIO_CONTROL_UAVCAN_ID
+#define RADIO_CONTROL_UAVCAN_ID 10
+#endif
+
 /*
  * IDs of VEL_SP senders
  */

sw/airborne/modules/datalink/pprz_dl.h

@@ -41,7 +41,9 @@
 #if USE_SYSLINK
 #include "modules/datalink/bitcraze/syslink_dl.h"
 #endif
-
+#if USE_TUNNEL_UAVCAN
+#include "modules/datalink/tunnel_uavcan.h"
+#endif
 /** PPRZ transport structure */
 extern struct pprz_transport pprz_tp;
 

sw/airborne/modules/datalink/tunnel_uavcan.c

@@ -0,0 +1,174 @@
+/*
+ * device over uavcan tunnel.
+ * Copyright (C) 2026 Fabien-B <fabien-b@github.com> 
+ * This file is part of paparazzi. See LICENCE file.
+ */
+
+#include "tunnel_uavcan.h"
+#include "uavcan/uavcan.h"
+#include <uavcan.tunnel.Protocol.h>
+
+#if !defined(TUNNEL_UAVCAN_IFACE) && USE_CAN1
+#define TUNNEL_UAVCAN_IFACE uavcan1
+#endif
+
+#if !defined(TUNNEL_UAVCAN_IFACE) && USE_CAN2
+#define TUNNEL_UAVCAN_IFACE uavcan2
+#endif
+
+#if !defined(TUNNEL_UAVCAN_PROTOCOL)
+#define TUNNEL_UAVCAN_PROTOCOL 3
+#endif
+
+#if !defined(TUNNEL_UAVCAN_CHANNEL_ID)
+#define TUNNEL_UAVCAN_CHANNEL_ID 0
+#endif
+
+struct tunnel_uavcan_periph tunnel_uavcan0;
+
+
+// Serial device functions
+static int      tunnel_uavcan_check_free_space(struct tunnel_uavcan_periph* up, long *fd, uint16_t len);
+static void     tunnel_uavcan_put_byte(struct tunnel_uavcan_periph* up, long fd, uint8_t c);
+static void     tunnel_uavcan_put_buffer(struct tunnel_uavcan_periph* up, long fd, const uint8_t *data, uint16_t len);
+static void     tunnel_uavcan_send_message(struct tunnel_uavcan_periph* up, long fd);
+static int      tunnel_uavcan_char_available(struct tunnel_uavcan_periph* up);
+static uint8_t  tunnel_uavcan_get_byte(struct tunnel_uavcan_periph* up);
+void tunnel_uavcan_set_baudrate(struct tunnel_uavcan_periph* up, uint32_t baudrate);
+
+static void tunnel_uavcan_cb(struct uavcan_iface_t *iface, CanardRxTransfer *transfer);
+
+void tunnel_uavcan_init_periph(struct tunnel_uavcan_periph* ucdl, uint8_t channel_id);
+
+void tunnel_uavcan_init(void) {
+  tunnel_uavcan_init_periph(&tunnel_uavcan0, TUNNEL_UAVCAN_CHANNEL_ID);
+}
+
+void tunnel_uavcan_init_periph(struct tunnel_uavcan_periph* ucdl, uint8_t channel_id) {
+  ucdl->channel_id = channel_id;
+  ucdl->iface = &TUNNEL_UAVCAN_IFACE;
+  ring_buffer_init(&ucdl->tx_rb, ucdl->_tbuf, TX_BUFFER_SIZE);
+  ring_buffer_init(&ucdl->rx_rb, ucdl->_rbuf, RX_BUFFER_SIZE);
+  pprz_mtx_init(&ucdl->tx_mtx);
+  pprz_mtx_init(&ucdl->rx_mtx);
+
+  uavcan_bind(UAVCAN_TUNNEL_BROADCAST_ID, UAVCAN_TUNNEL_BROADCAST_SIGNATURE, &ucdl->tunnel_brd_ev, &tunnel_uavcan_cb);
+
+  // setup device
+  ucdl->device.check_free_space = (check_free_space_t)tunnel_uavcan_check_free_space;
+  ucdl->device.put_byte = (put_byte_t)tunnel_uavcan_put_byte;
+  ucdl->device.put_buffer = (put_buffer_t)tunnel_uavcan_put_buffer;
+  ucdl->device.send_message = (send_message_t)tunnel_uavcan_send_message;
+  ucdl->device.char_available = (char_available_t)tunnel_uavcan_char_available;
+  ucdl->device.get_byte = (get_byte_t)tunnel_uavcan_get_byte;
+  ucdl->device.set_baudrate = (set_baudrate_t)tunnel_uavcan_set_baudrate;
+  ucdl->device.periph = ucdl;
+}
+
+static void tunnel_uavcan_cb(struct uavcan_iface_t *iface, CanardRxTransfer *transfer) {
+  (void)iface;
+  struct uavcan_tunnel_Broadcast utb_msg;
+  if(uavcan_tunnel_Broadcast_decode(transfer, &utb_msg)) {
+    // decode error
+    return;
+  }
+
+  if(utb_msg.protocol.protocol != TUNNEL_UAVCAN_PROTOCOL) {
+    return;
+  }
+  
+  if(tunnel_uavcan0.channel_id == utb_msg.channel_id) {
+    if(pprz_mtx_trylock(&tunnel_uavcan0.rx_mtx)==0) {
+      ring_buffer_write(&tunnel_uavcan0.rx_rb, utb_msg.buffer.data, utb_msg.buffer.len);
+      pprz_mtx_unlock(&tunnel_uavcan0.rx_mtx);
+    }
+  }
+}
+
+/**
+ * @return free_space if free_space > len, else 0.
+ * That's not logic...
+ */
+static int tunnel_uavcan_check_free_space(struct tunnel_uavcan_periph* up, long *fd, uint16_t len) {
+  int free_space = 0;
+  if(pprz_mtx_lock(&up->tx_mtx) == 0) {
+    free_space = (int)ring_buffer_free_space(&up->tx_rb);
+    if(len < free_space) {
+      // keep the device locked and set the flag in fd.
+      *fd = 1;
+    } else {
+      // no space available, unlock.
+      free_space = 0;
+      pprz_mtx_unlock(&up->tx_mtx);
+    }
+    
+  }
+  return free_space;
+}
+
+static void tunnel_uavcan_put_byte(struct tunnel_uavcan_periph* up, long fd, uint8_t c) {
+  tunnel_uavcan_put_buffer(up, fd, (const uint8_t*)&c, 1);
+}
+
+static void tunnel_uavcan_put_buffer(struct tunnel_uavcan_periph* up, long fd, const uint8_t *data, uint16_t len) {
+  if(fd == 0) {
+    // if fd is zero, assume the driver is not already locked
+    pprz_mtx_lock(&up->tx_mtx);
+  }
+
+  ring_buffer_write(&up->tx_rb, data, len);
+
+  if(fd == 0) {
+    // send immediately. mutex is already locked, so set flag.
+    tunnel_uavcan_send_message(up, 1);
+  }
+}
+
+static void tunnel_uavcan_send_message(struct tunnel_uavcan_periph* up, long fd) {
+  struct uavcan_tunnel_Broadcast msg;
+  uint8_t buffer[UAVCAN_TUNNEL_BROADCAST_MAX_SIZE];
+  msg.protocol.protocol = TUNNEL_UAVCAN_PROTOCOL;
+
+  if(fd == 0) {
+    // flag not set, lock and unlock
+    pprz_mtx_lock(&up->tx_mtx);
+  }
+
+  // Empty the buffer
+  while(ring_buffer_available(&up->tx_rb)) {
+    size_t len = ring_buffer_read(&up->tx_rb, msg.buffer.data, sizeof(msg.buffer.data));
+    msg.buffer.len = len;
+    msg.channel_id = up->channel_id;
+
+    // Encode the message
+    uint32_t total_size = uavcan_tunnel_Broadcast_encode(&msg, buffer);
+    // Then send it
+    uavcan_broadcast(up->iface, UAVCAN_TUNNEL_BROADCAST_SIGNATURE, UAVCAN_TUNNEL_BROADCAST_ID, CANARD_TRANSFER_PRIORITY_MEDIUM, buffer, total_size);
+  }
+
+  pprz_mtx_unlock(&up->tx_mtx);
+}
+
+
+static int tunnel_uavcan_char_available(struct tunnel_uavcan_periph* up) {
+  int available = 0;
+  if(pprz_mtx_lock(&up->rx_mtx) == 0) {
+    available = (int)ring_buffer_available(&up->rx_rb);
+    pprz_mtx_unlock(&up->rx_mtx);
+  }
+  return available;
+}
+
+static uint8_t tunnel_uavcan_get_byte(struct tunnel_uavcan_periph* up) {
+  uint8_t c;
+  if(pprz_mtx_lock(&up->rx_mtx) == 0) {
+    ring_buffer_read(&up->rx_rb, &c, 1);
+    pprz_mtx_unlock(&up->rx_mtx);
+  }
+  return c;
+}
+
+
+void tunnel_uavcan_set_baudrate(struct tunnel_uavcan_periph* up __attribute__((unused)), uint32_t baudrate __attribute__((unused))) {
+
+}

sw/airborne/modules/datalink/tunnel_uavcan.h

@@ -0,0 +1,37 @@
+/*
+ * device over uavcan tunnel.
+ * Copyright (C) 2026 Fabien-B <fabien-b@github.com> 
+ * This file is part of paparazzi. See LICENCE file.
+ */
+
+#pragma once
+
+#include "pprzlink/pprzlink_device.h"
+#include "uavcan.tunnel.Broadcast.h"
+#include "utils/ring_buffer.h"
+#include "core/threads.h"
+#include "uavcan/uavcan.h"
+
+#define TX_BUFFER_SIZE 512
+#define RX_BUFFER_SIZE 512
+
+struct tunnel_uavcan_periph {
+  uint8_t channel_id;
+  uint8_t _tbuf[TX_BUFFER_SIZE];
+  ring_buffer_t tx_rb;
+  uint8_t _rbuf[RX_BUFFER_SIZE];
+  ring_buffer_t rx_rb;
+
+  pprz_mutex_t tx_mtx;
+  pprz_mutex_t rx_mtx;
+  uavcan_event tunnel_brd_ev;
+
+  struct uavcan_iface_t *iface;
+
+  /** Generic device interface */
+  struct link_device device;
+};
+
+extern struct tunnel_uavcan_periph tunnel_uavcan0;
+
+void tunnel_uavcan_init(void);

sw/airborne/modules/datalink/xbee_dl.c

@@ -30,6 +30,10 @@
 #include "mcu_periph/uart.h"
 #include "generated/airframe.h"
 
+#if USE_TUNNEL_UAVCAN
+#include "modules/datalink/tunnel_uavcan.h"
+#endif
+
 #define STR_(s) #s
 #define STR(s) STR_(s)
 

sw/airborne/modules/radio_control/radio_control_uavcan.c

@@ -0,0 +1,76 @@
+ /*
+ * Radio control over uavcan
+ * Copyright (C) 2026 Fabien-B <fabien-b@github.com> 
+ * This file is part of paparazzi. See LICENCE file.
+ */
+
+/** @file "modules/radio_control/radio_control_uavcan.c"
+ * @author Fabien-B <Fabien-B@github.com>
+ * Radio control from DroneCAN message `dronecan.sensors.rc.RCInput`.
+ */
+
+#include "modules/radio_control/radio_control_uavcan.h"
+#include "modules/radio_control/radio_control.h"
+#include "uavcan/uavcan.h"
+#include "modules/core/abi.h"
+#include "modules/core/threads.h"
+#include "dronecan.sensors.rc.RCInput.h"
+#include "generated/radio.h"
+
+static uavcan_event rc_uavcan_ev;
+
+struct dronecan_sensors_rc_RCInput rc_dronecan_msg;
+static bool rc_frame_available;
+static pprz_mutex_t rc_mtx;
+
+
+static void rc_uavcan_cb(struct uavcan_iface_t *iface __attribute__((unused)), CanardRxTransfer *transfer) {
+  if(pprz_mtx_trylock(&rc_mtx) == 0) {
+    if(dronecan_sensors_rc_RCInput_decode(transfer, &rc_dronecan_msg)) {
+      return; // decode error
+    }
+    rc_frame_available = true;
+    pprz_mtx_unlock(&rc_mtx);
+  }
+}
+
+void rc_uavcan_init(void)
+{
+  pprz_mtx_init(&rc_mtx);
+  rc_frame_available = false;
+  radio_control.nb_channel = 32;  // max value
+  uavcan_bind(DRONECAN_SENSORS_RC_RCINPUT_ID, DRONECAN_SENSORS_RC_RCINPUT_SIGNATURE, &rc_uavcan_ev, &rc_uavcan_cb);
+}
+
+
+void rc_uavcan_event(void)
+{
+  bool send_abi = false;
+  if(rc_frame_available) {
+    if(pprz_mtx_trylock(&rc_mtx) == 0) {
+      if((rc_dronecan_msg.status & DRONECAN_SENSORS_RC_RCINPUT_STATUS_FAILSAFE) ||
+         (rc_dronecan_msg.status & DRONECAN_SENSORS_RC_RCINPUT_STATUS_QUALITY_VALID && rc_dronecan_msg.quality == 0)) {
+        // RC LOST
+      } else {
+        radio_control.nb_channel = rc_dronecan_msg.rcin.len;
+        radio_control.frame_cpt++;
+        radio_control.time_since_last_frame = 0;
+        radio_control.radio_ok_cpt = 0;
+        radio_control.status = RC_OK;
+
+        NormalizePpmIIR(rc_dronecan_msg.rcin.data, radio_control);
+        
+        send_abi = true;
+      }
+      
+      rc_frame_available = false;
+      pprz_mtx_unlock(&rc_mtx);
+    }
+
+    if(send_abi) {
+      AbiSendMsgRADIO_CONTROL(RADIO_CONTROL_UAVCAN_ID, &radio_control);
+    }
+  }
+}
+
+

sw/airborne/modules/radio_control/radio_control_uavcan.h

@@ -0,0 +1,22 @@
+ /*
+ * Radio control over uavcan
+ * Copyright (C) 2026 Fabien-B <fabien-b@github.com> 
+ * This file is part of paparazzi. See LICENCE file.
+ */
+
+/** @file "modules/radio_control/radio_control_uavcan.h"
+ * @author Fabien-B <Fabien-B@github.com>
+ * Radio control from DroneCAN message `dronecan.sensors.rc.RCInput`.
+ */
+
+#pragma once
+
+// Generated code holding the description of a given transmitter
+#include "generated/radio.h"
+
+#define RC_PPM_TICKS_OF_USEC(_x) (_x)
+#define RC_PPM_SIGNED_TICKS_OF_USEC(_x) (_x)
+#define USEC_OF_RC_PPM_TICKS(_x) (_x)
+
+extern void rc_uavcan_init(void);
+extern void rc_uavcan_event(void);

sw/airborne/modules/uavcan/uavcan.c

@@ -27,6 +27,10 @@
 #include "uavcan.h"
 #include "mcu_periph/can.h"
 #include "modules/core/threads.h"
+#include "uavcan.protocol.NodeStatus.h"
+#include "uavcan_reporting.h"
+#include "mcu_periph/sys_time.h"
+#include "modules/uavcan/uavcan_allocator.h"
 
 #ifndef UAVCAN_NODE_ID
 #define UAVCAN_NODE_ID    100
@@ -47,20 +51,10 @@ static uavcan_event *uavcan_event_hd = NULL;
 #define UAVCAN_CAN1_BAUDRATE UAVCAN_BAUDRATE
 #endif
 
-struct uavcan_msg_header_t {
-  uint64_t data_type_signature;
-  uint16_t data_type_id;
-  uint8_t priority;
-  uint16_t payload_len;
-  uint8_t *payload;
-};
-
-
 struct uavcan_iface_t uavcan1 = {
   .can_net = {.can_ifindex = 1},
   .can_baudrate = UAVCAN_CAN1_BAUDRATE,
   .node_id = UAVCAN_CAN1_NODE_ID,
-  .transfer_id = 0,
   .initialized = false
 };
 #endif
@@ -79,7 +73,6 @@ struct uavcan_iface_t uavcan2 = {
   .can_net = {.can_ifindex = 2},
   .can_baudrate = UAVCAN_CAN2_BAUDRATE,
   .node_id = UAVCAN_CAN2_NODE_ID,
-  .transfer_id = 0,
   .initialized = false
 };
 #endif
@@ -88,8 +81,6 @@ struct uavcan_iface_t uavcan2 = {
 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 = {0};
   memcpy(rx_frame.data, rx_msg->data, 8);
   rx_frame.data_len = rx_msg->len;
@@ -101,13 +92,8 @@ static void can_frame_cb(struct pprzcan_frame* rx_msg, UNUSED struct pprzaddr_ca
 
   // Let canard handle the frame
   canardHandleRxFrame(&iface->canard, &rx_frame, rx_msg->timestamp);
-
-  pprz_mtx_unlock(&iface->mutex);
 }
 
-
-uint8_t msg_payload[UAVCAN_MSG_MAX_SIZE];
-
 /*
  * Transmitter thread.
  */
@@ -117,29 +103,10 @@ static void uavcan_tx(void* p)
   uint8_t err_cnt = 0;
 
   while (true) {
+    // wait to be awaken
     pprz_bsem_wait(&iface->bsem);
 
-    // read the Tx FIFO to canard
-    pprz_mtx_lock(&iface->tx_fifo_mutex);
-    while(true) {
-      struct uavcan_msg_header_t header;
-      int ret = framed_ring_buffer_get(&iface->_tx_fifo, (uint8_t*)&header, sizeof(struct uavcan_msg_header_t));
-      if(ret < 0) {break;}
-      if(header.payload_len >= UAVCAN_MSG_MAX_SIZE) {
-        chSysHalt("UAVCAN_MSG_MAX_SIZE too small");
-      }
-      ret = framed_ring_buffer_get(&iface->_tx_fifo, msg_payload, UAVCAN_MSG_MAX_SIZE);
-      if(ret < 0) {break;}
-      pprz_mtx_lock(&iface->mutex);
-      canardBroadcast(&iface->canard,
-                    header.data_type_signature,
-                    header.data_type_id, &iface->transfer_id,
-                    header.priority, msg_payload, header.payload_len);
-      pprz_mtx_unlock(&iface->mutex);
-    }
-    pprz_mtx_unlock(&iface->tx_fifo_mutex);
-
-    pprz_mtx_lock(&iface->mutex);
+    pprz_mtx_lock(&iface->tx_mutex);
     for (const CanardCANFrame *txf = NULL; (txf = canardPeekTxQueue(&iface->canard)) != NULL;) {
       struct pprzcan_frame tx_msg;
       memcpy(tx_msg.data, txf->data, 8);
@@ -158,13 +125,13 @@ static void uavcan_tx(void* p)
         }
 
         // Timeout - just exit and try again later
-        pprz_mtx_unlock(&iface->mutex);
-        chThdSleepMilliseconds(++err_cnt);
-        pprz_mtx_lock(&iface->mutex);
+        pprz_mtx_unlock(&iface->tx_mutex);
+        sys_time_msleep(++err_cnt);
+        pprz_mtx_lock(&iface->tx_mutex);
         continue;
       }
     }
-    pprz_mtx_unlock(&iface->mutex);
+    pprz_mtx_unlock(&iface->tx_mutex);
   }
 }
 
@@ -174,7 +141,6 @@ static void uavcan_tx(void* p)
 static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer)
 {
   struct uavcan_iface_t *iface = (struct uavcan_iface_t *)ins->user_reference;
-
   // Go through all registered callbacks and call function callback if found
   for (uavcan_event *ev = uavcan_event_hd; ev; ev = ev->next) {
     if (transfer->data_type_id == ev->data_type_id) {
@@ -200,33 +166,55 @@ static bool shouldAcceptTransfer(const CanardInstance *ins __attribute__((unused
       return true;
     }
   }
-  // No callback found return
   return false;
 }
 
 
+
+#if CANARD_ALLOCATE_SEM
+
+void canard_allocate_sem_take(CanardPoolAllocator *allocator) {
+  pprz_bsem_t* bsem = (pprz_bsem_t*)allocator->semaphore;
+  pprz_bsem_wait(bsem);
+}
+
+void canard_allocate_sem_give(CanardPoolAllocator *allocator){
+  pprz_bsem_t* bsem = (pprz_bsem_t*)allocator->semaphore;
+  pprz_bsem_signal(bsem);
+}
+
+#endif
+
+
 /**
  * Initialize uavcan interface
  */
 static void uavcanInitIface(struct uavcan_iface_t *iface)
 {
-  pprz_mtx_init(&iface->mutex);
   pprz_bsem_init(&iface->bsem, true);
-  pprz_mtx_init(&iface->tx_fifo_mutex);
+  pprz_mtx_init(&iface->tx_mutex);
 
-  // Initialize tx fifo
-  framed_ring_buffer_init(&iface->_tx_fifo, iface->_tx_fifo_buffer, UAVCAN_TX_FIFO_SIZE);
+  kv_init(&iface->transfer_ids_store, UAVCAN_TID_STORE_CAPACITY, sizeof(uint8_t),
+          iface->transfer_ids_keys, iface->transfer_ids_values, iface->transfer_ids_used);
 
   // Initialize canard
   canardInit(&iface->canard, iface->canard_memory_pool, sizeof(iface->canard_memory_pool),
              onTransferReceived, shouldAcceptTransfer, iface);
+  
+
+#if CANARD_ALLOCATE_SEM
+  // must be initialized after canardInit
+  pprz_bsem_init(&iface->allocator_bsem, false);
+  iface->canard.allocator.semaphore = &iface->allocator_bsem;
+#endif
+
   // Update the uavcan node ID
   canardSetLocalNodeID(&iface->canard, iface->node_id);
 
   // Start the receiver and transmitter thread
   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)) {
+  if(!pprz_thread_create(&iface->thread_tx, 2048, "uavcan_tx", PPRZ_NORMAL_PRIO +7, uavcan_tx, (void *)iface)) {
     iface->initialized = true;
   }
 }
@@ -242,6 +230,8 @@ void uavcan_init(void)
 #if UAVCAN_USE_CAN2
   uavcanInitIface(&uavcan2);
 #endif
+  uavcan_init_reporting();
+  uavcan_allocator_init();
 }
 
 /**
@@ -259,36 +249,95 @@ void uavcan_bind(uint16_t data_type_id, uint64_t data_type_signature, uavcan_eve
   uavcan_event_hd = ev;
 }
 
+static uint8_t* get_transfer_id(struct uavcan_iface_t *iface, uint8_t destination_node_id, CanardTxTransfer* transfer) {
+  uint32_t key = (destination_node_id << 24) | (iface->node_id <<16) | transfer->data_type_id;
+  uint8_t* transfer_id = (uint8_t*)kv_get(&iface->transfer_ids_store, key);
+
+  if (transfer_id == NULL) {
+    // key does not exists yet
+    uint8_t zero = 0;
+    kv_set(&iface->transfer_ids_store, key, &zero);
+  }
+
+  return transfer_id;
+}
+
 /**
+ * @param transfer should be initialized with canardInitTxTransfer.
+ */
+void uavcan_transfer(struct uavcan_iface_t *iface, CanardTxTransfer* transfer)
+{
+  if (!iface->initialized) { return; }
+  pprz_mtx_lock(&iface->tx_mutex);
+
+  transfer->transfer_type = CanardTransferTypeBroadcast;
+  transfer->inout_transfer_id = get_transfer_id(iface, 0, transfer);
+  if(canardBroadcastObj(&iface->canard, transfer) < 0) {
+    iface->nb_errors++;
+  }
+
+  pprz_mtx_unlock(&iface->tx_mutex);
+  
+  // Wake Tx thread
+  pprz_bsem_signal(&iface->bsem);
+}
+
+
+/**
+ * @param transfer should be initialized with canardInitTxTransfer.
+ */
+void uavcan_request(struct uavcan_iface_t *iface, uint8_t destination_node_id, CanardTxTransfer* transfer)
+{
+  if (!iface->initialized) { return; }
+  pprz_mtx_lock(&iface->tx_mutex);
+
+  transfer->transfer_type = CanardTransferTypeRequest;
+  transfer->inout_transfer_id = get_transfer_id(iface, destination_node_id, transfer);
+  if(canardRequestOrRespondObj(&iface->canard, destination_node_id, transfer) < 0) {
+    iface->nb_errors++;
+  }
+
+  pprz_mtx_unlock(&iface->tx_mutex);
+  
+  // Wake Tx thread
+  pprz_bsem_signal(&iface->bsem);
+}
+
+
+
+
+/**
+ * Legacy function
  * Broadcast an uavcan message to a specific interface
  */
 void uavcan_broadcast(struct uavcan_iface_t *iface, uint64_t data_type_signature, uint16_t data_type_id,
                       uint8_t priority, const void *payload,
                       uint16_t payload_len)
+{
+  CanardTxTransfer transfer;
+  canardInitTxTransfer(&transfer);
+  transfer.data_type_signature = data_type_signature;
+  transfer.data_type_id = data_type_id;
+  transfer.priority = priority;
+  transfer.payload = payload;
+  transfer.payload_len = payload_len;
+
+  uavcan_transfer(iface, &transfer);
+}
+
+
+/**
+ * @param inout_transfer_id should be set to the request transfer id before calling this function
+ */
+void uavcan_response(struct uavcan_iface_t *iface, uint8_t destination_node_id, CanardTxTransfer* transfer)
 {
   if (!iface->initialized) { return; }
-  pprz_mtx_lock(&iface->tx_fifo_mutex);
+  pprz_mtx_lock(&iface->tx_mutex);
 
-  struct uavcan_msg_header_t header = {
-    .data_type_signature = data_type_signature,
-    .data_type_id = data_type_id,
-    .priority = priority,
-    .payload_len = payload_len
-  };
+  transfer->transfer_type = CanardTransferTypeResponse;
+  canardRequestOrRespondObj(&iface->canard, destination_node_id,  transfer);
 
-  if(framed_ring_buffer_put(&iface->_tx_fifo, (uint8_t*)&header, sizeof(struct uavcan_msg_header_t)) < 0) {
-    // fail to post header
-    pprz_mtx_unlock(&iface->tx_fifo_mutex);
-    return;
-  }
-
-  if(framed_ring_buffer_put(&iface->_tx_fifo, payload, payload_len) < 0) {
-    // fail to post payload. Remove the header from the fifo
-    framed_ring_buffer_drop_last(&iface->_tx_fifo);
-    pprz_mtx_unlock(&iface->tx_fifo_mutex);
-    return;
-  }
-  pprz_mtx_unlock(&iface->tx_fifo_mutex);
+  pprz_mtx_unlock(&iface->tx_mutex);
   
   // Wake Tx thread
   pprz_bsem_signal(&iface->bsem);

sw/airborne/modules/uavcan/uavcan.h

@@ -31,41 +31,39 @@
 #include "mcu_periph/can.h"
 #include "modules/core/threads.h"
 #include "utils/framed_ring_buffer.h"
+#include "utils/kv_store.h"
 
 
 #ifndef UAVCAN_TX_FIFO_SIZE
 #define UAVCAN_TX_FIFO_SIZE 1024
 #endif
 
-#ifndef UAVCAN_MSG_MAX_SIZE
-#define UAVCAN_MSG_MAX_SIZE 256
-#endif
-
+#define UAVCAN_TID_STORE_CAPACITY   30
 
 /** uavcan interface structure */
 struct uavcan_iface_t {
   struct pprzaddr_can can_net;
   uint32_t can_baudrate;
 
-  event_source_t tx_request;
-
-  pprz_mutex_t mutex;
+  pprz_mutex_t rx_mutex;
+  pprz_mutex_t tx_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;
 
+  pprz_bsem_t bsem;
+  
   uint8_t node_id;
   CanardInstance canard;
   uint8_t canard_memory_pool[1024 * 2];
+  #if CANARD_ALLOCATE_SEM
+  pprz_bsem_t allocator_bsem;
+  #endif
+  uint16_t nb_errors;
 
-  uint8_t _tx_fifo_buffer[UAVCAN_TX_FIFO_SIZE];
-  struct framed_ring_buffer _tx_fifo;
-  pprz_mutex_t tx_fifo_mutex;
-
-  uint8_t transfer_id;
+  //uint8_t transfer_id;  // TODO see comment: The Transfer ID value cannot be shared between requests that have different descriptors!
+  uint32_t transfer_ids_keys[UAVCAN_TID_STORE_CAPACITY];
+  uint8_t transfer_ids_values[UAVCAN_TID_STORE_CAPACITY];
+  uint8_t transfer_ids_used[UAVCAN_TID_STORE_CAPACITY];
+  kv_store_t transfer_ids_store;
   bool initialized;
 };
 
@@ -91,8 +89,12 @@ extern struct uavcan_iface_t uavcan2;
 
 /** uavcan external functions */
 void uavcan_init(void);
+void uavcan_reporting(void);
 void uavcan_bind(uint16_t data_type_id, uint64_t data_type_signature, uavcan_event *ev, uavcan_callback cb);
+void uavcan_transfer(struct uavcan_iface_t *iface, CanardTxTransfer* transfer);
+void uavcan_request(struct uavcan_iface_t *iface, uint8_t destination_node_id, CanardTxTransfer* transfer);
 void uavcan_broadcast(struct uavcan_iface_t *iface, uint64_t data_type_signature, uint16_t data_type_id,
                       uint8_t priority, const void *payload, uint16_t payload_len);
+void uavcan_response(struct uavcan_iface_t *iface, uint8_t destination_node_id, CanardTxTransfer* transfer);
 
 #endif /* MODULES_UAVCAN_ARCH_H */

sw/airborne/modules/uavcan/uavcan_allocator.c

@@ -0,0 +1,263 @@
+ /*
+ * Copyright (C) 2025 Fabien-B <fabien-b@github.com> 
+ * This file is part of paparazzi. See LICENCE file.
+ */
+
+ /**
+ *  Dynamic node ID allocation.
+ *  See https://dronecan.github.io/Specification/6._Application_level_functions/#dynamic-node-id-allocation
+ */
+
+#include "uavcan/uavcan.h"
+#include "uavcan/uavcan_allocator.h"
+#include "uavcan.protocol.dynamic_node_id.Allocation.h"
+#include "uavcan.protocol.GetNodeInfo.h"
+
+#ifndef UAVCAN_MAX_NODES
+#define UAVCAN_MAX_NODES 50
+#endif
+
+#define INVALID_STAGE -1
+
+static int detectRequestStage(struct uavcan_protocol_dynamic_node_id_Allocation* msg);
+static int getExpectedStage(void);
+static int findFreeNodeID(const uint8_t preferred);
+static bool unique_id_identical(int index);
+static void handleAllocationRequest(struct uavcan_iface_t *iface, uint8_t preferred_node_id);
+static void id_alloc_uavcan_cb(struct uavcan_iface_t *iface __attribute__((unused)), CanardRxTransfer *transfer);
+
+
+static uavcan_event id_alloc_ev;
+static uavcan_event node_info_ev;
+
+
+// keep the correspondance between node id and unique IDs. (even or the fixed ids)
+static struct uavcan_node_mapping_t uavcan_node_ids[UAVCAN_MAX_NODES] = {0};
+
+
+struct uavcan_unique_id_t current_unique_id = {0};
+uint32_t last_message_timestamp = 0;
+
+
+struct uavcan_node_mapping_t* uavcan_get_node_id_mapping(const uint8_t id) {
+  for(int i=0; i<UAVCAN_MAX_NODES; i++) {
+    if(uavcan_node_ids[i].allocated_id == id) {
+      return &uavcan_node_ids[i];
+    }
+  }
+  return NULL;
+}
+
+static struct uavcan_node_mapping_t* get_free_id_mapping(void) {
+  // a free slot is stored with node id == 0;
+  return uavcan_get_node_id_mapping(0);
+}
+
+static int findFreeNodeID(const uint8_t preferred)
+{
+  // Search up
+  int candidate = (preferred > 0) ? preferred : 125;
+  while (candidate <= 125) {
+      if (!uavcan_get_node_id_mapping(candidate)) {
+        return candidate;
+      }
+      candidate++;
+  }
+  // Search down
+  candidate = (preferred > 0) ? preferred : 125;
+  while (candidate > 0) {
+      if (!uavcan_get_node_id_mapping(candidate)) {
+        return candidate;
+      }
+      candidate--;
+  }
+  // Not found
+  return -1;
+}
+
+
+static int detectRequestStage(struct uavcan_protocol_dynamic_node_id_Allocation* msg) {
+  if(msg->first_part_of_unique_id) {
+    return 1;
+  }
+  if(msg->unique_id.len == UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST) {
+    return 2;
+  }
+  if(msg->unique_id.len < UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST) {
+    return 3;
+  }
+  return INVALID_STAGE;  //invalid
+}
+
+static int getExpectedStage() {
+  if(current_unique_id.len == 0) {
+    return 1;
+  }
+  if(current_unique_id.len >= UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST*2) {
+    return 3;
+  }
+  if(current_unique_id.len >= UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST) {
+    return 2;
+  }
+  return INVALID_STAGE;    //invalid
+}
+
+static bool unique_id_identical(int index) {
+  return current_unique_id.len == uavcan_node_ids[index].unique_id.len &&
+        memcmp(current_unique_id.data, uavcan_node_ids[index].unique_id.data, current_unique_id.len) == 0;
+}
+
+static void handleAllocationRequest(struct uavcan_iface_t *iface, uint8_t preferred_node_id) {
+  uint8_t allocated_id = 0;
+
+  for(int i=0; i<UAVCAN_MAX_NODES; i++) {
+
+    if(uavcan_node_ids[i].allocated_id != 0) {
+      // allocation slot taken, check if unique id matches
+      if(unique_id_identical(i)) {
+        allocated_id = uavcan_node_ids[i].allocated_id;
+      }
+    }
+    else {
+      // free allocation slot
+
+      // copy unique id
+      for(int k=0; k<current_unique_id.len; k++) {
+        uavcan_node_ids[i].unique_id.data[k] = current_unique_id.data[k];
+      }
+      uavcan_node_ids[i].unique_id.len = current_unique_id.len;
+
+      int free_id = findFreeNodeID(preferred_node_id);
+
+      if(free_id != -1) {
+        uavcan_node_ids[i].allocated_id = free_id;
+        allocated_id = free_id;
+        break;
+      } else {
+        // error: not more free IDs
+        return;
+      }
+      
+    }
+  }
+
+  struct uavcan_protocol_dynamic_node_id_Allocation response_msg;
+  // copy unique id
+  for(int k=0; k<current_unique_id.len; k++) {
+    response_msg.unique_id.data[k] = current_unique_id.data[k];
+  }
+  response_msg.unique_id.len = current_unique_id.len;
+  response_msg.node_id = allocated_id;
+
+  uint8_t msg_buffer[50];
+
+  uint32_t total_size = uavcan_protocol_dynamic_node_id_Allocation_encode(&response_msg, msg_buffer);
+
+  uavcan_broadcast(
+    iface,
+    UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE, UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID,
+    CANARD_TRANSFER_PRIORITY_HIGH, msg_buffer, total_size);
+}
+
+
+static void id_alloc_uavcan_cb(struct uavcan_iface_t *iface, CanardRxTransfer *transfer)
+{
+  struct uavcan_protocol_dynamic_node_id_Allocation msg;
+  if(uavcan_protocol_dynamic_node_id_Allocation_decode(transfer, &msg)) {
+    return;   // decode error
+  }
+
+  const int unique_id_capacity = sizeof(msg.unique_id.data);
+
+  // only process anonymous transfers
+  if(transfer->source_node_id != 0) {
+    return;
+  }
+
+  uint32_t timestamp = transfer->timestamp_usec / 1000;
+
+  // Reset the expected stage on timeout
+  if(timestamp > last_message_timestamp + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_FOLLOWUP_TIMEOUT_MS) {
+    current_unique_id.len = 0;
+  }
+
+  // Checking if request stage matches the expected stage
+  int request_stage = detectRequestStage(&msg);
+  if(request_stage == INVALID_STAGE) { return; }
+  if(request_stage != getExpectedStage()) { return; }
+  if (msg.unique_id.len > (unique_id_capacity - current_unique_id.len)) { return; }
+
+  // Updating the local state
+  for(int i=0; i<msg.unique_id.len; i++) {
+    current_unique_id.data[current_unique_id.len] = msg.unique_id.data[i];
+    current_unique_id.len += 1;
+  }
+
+  if(current_unique_id.len == unique_id_capacity) {
+    // Proceeding with allocation.
+    handleAllocationRequest(iface, msg.node_id);
+    current_unique_id.len = 0;
+  } else {
+    struct uavcan_protocol_dynamic_node_id_Allocation response_msg;
+    // copy unique id
+    for(int k=0; k<current_unique_id.len; k++) {
+      response_msg.unique_id.data[k] = current_unique_id.data[k];
+    }
+    response_msg.unique_id.len = current_unique_id.len;
+
+    uint8_t msg_buffer[50];
+
+    uint32_t total_size = uavcan_protocol_dynamic_node_id_Allocation_encode(&response_msg, msg_buffer);
+
+    uavcan_broadcast(
+      iface,
+      UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE, UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID,
+      CANARD_TRANSFER_PRIORITY_HIGH, msg_buffer, total_size);
+  }
+
+  // It is important to update the timestamp only if the request has been processed successfully.
+  last_message_timestamp = timestamp;
+}
+
+static void node_info_resp_cb(struct uavcan_iface_t *iface, CanardRxTransfer *transfer) {
+  (void)iface;
+  struct uavcan_protocol_GetNodeInfoResponse msg;
+  if(uavcan_protocol_GetNodeInfoResponse_decode(transfer, &msg)) {
+    return;   // decode error
+  }
+
+  struct uavcan_node_mapping_t* mapping = uavcan_get_node_id_mapping(transfer->source_node_id);
+
+  if(!mapping) {
+    mapping = get_free_id_mapping();
+  }
+
+  
+  mapping->allocated_id = transfer->source_node_id;
+  for(int i=0; i<16; i++) {
+    mapping->unique_id.data[i] = msg.hardware_version.unique_id[i];
+  }
+  mapping->unique_id.len = 16;
+
+}
+
+void request_node_info(struct uavcan_iface_t *iface, uint8_t destination_node_id) {
+  CanardTxTransfer transfer;
+  canardInitTxTransfer(&transfer);
+  transfer.data_type_id = UAVCAN_PROTOCOL_GETNODEINFO_REQUEST_ID;
+  transfer.data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_REQUEST_SIGNATURE;
+  transfer.payload = NULL;
+  transfer.payload_len = 0;
+  transfer.priority = CANARD_TRANSFER_PRIORITY_HIGH;
+  uavcan_request(iface, destination_node_id, &transfer);
+}
+
+
+void uavcan_allocator_init(void) {
+  uavcan_bind(UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID, UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE,
+    &id_alloc_ev, &id_alloc_uavcan_cb);
+  
+  uavcan_bind(UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_ID, UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_SIGNATURE,
+    &node_info_ev, &node_info_resp_cb);
+}
+

sw/airborne/modules/uavcan/uavcan_allocator.h

@@ -0,0 +1,30 @@
+ /*
+ * Copyright (C) 2025 Fabien-B <fabien-b@github.com> 
+ * This file is part of paparazzi. See LICENCE file.
+ */
+
+/**
+ *  Dynamic node ID allocation.
+ *  See https://dronecan.github.io/Specification/6._Application_level_functions/#dynamic-node-id-allocation
+ */
+
+#pragma once
+#include "inttypes.h"
+
+struct uavcan_iface_t;
+
+struct uavcan_unique_id_t {
+  uint8_t len;
+  uint8_t data[16];
+};
+
+struct uavcan_node_mapping_t {
+  struct uavcan_unique_id_t unique_id;
+  uint8_t allocated_id;
+};
+
+struct uavcan_node_mapping_t* uavcan_get_node_id_mapping(const uint8_t id);
+void request_node_info(struct uavcan_iface_t *iface, uint8_t destination_node_id);
+
+void uavcan_allocator_init(void);
+void uavcan_allocator_periodic(void);

sw/airborne/modules/uavcan/uavcan_reporting.c

@@ -0,0 +1,112 @@
+#include "uavcan.h"
+#include "mcu_periph/can.h"
+#include "modules/core/threads.h"
+#include "uavcan.protocol.NodeStatus.h"
+#include "uavcan.protocol.GetNodeInfo_req.h"
+#include "uavcan.protocol.GetNodeInfo_res.h"
+#include "mcu_periph/sys_time.h"
+#include "generated/airframe.h"
+#include "uavcan_reporting.h"
+#include "uavcan/uavcan_allocator.h"
+
+#define AC_CAN_NAME_PREFIX "org.pprz."
+
+static uavcan_event node_info_ev;
+static uavcan_event node_status_ev;
+
+char ac_can_name[50] = {0};
+uint8_t ac_can_name_len = 0;
+
+static void node_info_cb(struct uavcan_iface_t *iface, CanardRxTransfer *transfer);
+static void node_status_cb(struct uavcan_iface_t *iface, CanardRxTransfer *transfer);
+static void get_uavcan_status(struct uavcan_protocol_NodeStatus* status);
+static void get_uavcan_software_version (struct uavcan_protocol_SoftwareVersion* software_version);
+static void get_uavcan_hardware_version(struct uavcan_protocol_HardwareVersion* hardware_version);
+
+
+void uavcan_init_reporting() {
+  strncpy(ac_can_name, AC_CAN_NAME_PREFIX, 50);
+  size_t prefix_len = strlen(AC_CAN_NAME_PREFIX);
+  strncpy(ac_can_name+prefix_len, AIRFRAME_NAME, 50-prefix_len);
+  ac_can_name_len = prefix_len + strlen(AIRFRAME_NAME);
+
+  uavcan_bind(UAVCAN_PROTOCOL_GETNODEINFO_REQUEST_ID, UAVCAN_PROTOCOL_GETNODEINFO_REQUEST_SIGNATURE, &node_info_ev, node_info_cb);
+  uavcan_bind(UAVCAN_PROTOCOL_NODESTATUS_ID, UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, &node_status_ev, node_status_cb);
+}
+
+/**
+ * request uniq_id of all nodes on the bus.
+ * Since nodes must periodicaly send a NodeStatus message, its a good place to do it.
+ */
+static void node_status_cb(struct uavcan_iface_t *iface, CanardRxTransfer *transfer) {
+  if(!uavcan_get_node_id_mapping(transfer->source_node_id)) {
+    request_node_info(iface, transfer->source_node_id);
+  }
+}
+
+void uavcan_reporting(void) {
+  struct uavcan_protocol_NodeStatus report;
+  get_uavcan_status(&report);
+
+  uint8_t buffer[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE];
+  uint32_t total_size = uavcan_protocol_NodeStatus_encode(&report, buffer);
+  
+#if UAVCAN_USE_CAN1
+  uavcan_broadcast(&uavcan1, UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, UAVCAN_PROTOCOL_NODESTATUS_ID, CANARD_TRANSFER_PRIORITY_MEDIUM, buffer, total_size);
+#endif
+#if UAVCAN_USE_CAN2
+  uavcan_broadcast(&uavcan2, UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, UAVCAN_PROTOCOL_NODESTATUS_ID, CANARD_TRANSFER_PRIORITY_MEDIUM, buffer, total_size);
+#endif
+}
+
+
+static void get_uavcan_status(struct uavcan_protocol_NodeStatus* status) {
+  status->uptime_sec = (uint32_t)get_sys_time_float();
+  status->health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK;
+  status->mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL;
+  status->sub_mode = 0;
+  status->vendor_specific_status_code = 0;
+}
+
+static void get_uavcan_software_version (struct uavcan_protocol_SoftwareVersion* software_version) {
+  software_version->major = 6;
+  software_version->minor = 0;
+  software_version->optional_field_flags = 0;
+  software_version->vcs_commit = 0;
+  software_version->image_crc = 0;
+}
+
+static void get_uavcan_hardware_version(struct uavcan_protocol_HardwareVersion* hardware_version) {
+  hardware_version->major = 2;
+  hardware_version->minor = 1;
+  memset(hardware_version->unique_id, 0, sizeof(hardware_version->unique_id));
+  hardware_version->certificate_of_authenticity.len = 0;
+}
+
+
+
+static void node_info_cb(struct uavcan_iface_t *iface, CanardRxTransfer *transfer __attribute__((unused))) {
+  struct uavcan_protocol_GetNodeInfoResponse msg;
+  get_uavcan_status(&msg.status);
+  get_uavcan_software_version(&msg.software_version);
+  get_uavcan_hardware_version(&msg.hardware_version);
+  uint8_t len = Min(sizeof(msg.name.data), ac_can_name_len);
+  strncpy((char*)msg.name.data, ac_can_name, len);
+  msg.name.len = len;
+
+  uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE];
+  uint32_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&msg, buffer);
+
+  CanardTxTransfer resp_transfer;
+  canardInitTxTransfer(&resp_transfer);
+  resp_transfer.data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_SIGNATURE;
+  resp_transfer.data_type_id = UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_ID;
+  resp_transfer.inout_transfer_id = &transfer->transfer_id;
+  resp_transfer.priority = CANARD_TRANSFER_PRIORITY_MEDIUM;
+  resp_transfer.payload = buffer;
+  resp_transfer.payload_len = total_size;
+
+  uavcan_response(iface, transfer->source_node_id, &resp_transfer);
+}
+
+

sw/airborne/modules/uavcan/uavcan_reporting.h

@@ -0,0 +1,3 @@
+#pragma once
+
+void uavcan_init_reporting(void);

sw/airborne/utils/framed_ring_buffer.h

@@ -31,7 +31,6 @@ enum cir_error {
   CIR_ERROR_NO_MSG = -1,                /**< circular buffer is empty */
   CIR_ERROR_BUFFER_TOO_SMALL = -2,      /**< destination buffer is too small */
   CIR_ERROR_NO_SPACE_AVAILABLE = -3,    /**< no space available in the circular buffer */
-  CIR_ERROR_LOCKED = -4,                /**< mutex locked */
 };
 
 /**

sw/airborne/utils/kv_store.c

@@ -0,0 +1,135 @@
+/*
+ * General purpose key-value store.
+ * Copyright (C) 2026 Fabien-B <fabien-b@github.com> 
+ * This file is part of paparazzi. See LICENCE file.
+ */
+
+#include "kv_store.h"
+#include "string.h"
+
+/**
+ * @brief Initializes a key-value store.
+ * 
+ * @param kv Pointer to the kv_store_t structure to initialize.
+ * @param capacity The maximum number of key-value pairs the store can hold.
+ * @param esize The size in bytes of each value element.
+ * @param keys Pointer to an array of uint32_t for storing keys. At least "capacity" long.
+ * @param values Pointer to the memory area for storing values. At least "capacity * esize" long.
+ * @param used Pointer to an array of uint8_t flags indicating if a slot is used. At least "capacity" long.
+ */
+void kv_init(kv_store_t *kv, size_t capacity, size_t esize,
+            uint32_t *keys, void *values, uint8_t *used) {
+
+  kv->capacity = capacity;
+  kv->esize = esize;
+  kv->keys = keys;
+  kv->values = (uint8_t *)values;
+  kv->used = used;
+
+  for (size_t i = 0; i < capacity; i++) {
+    kv->used[i] = 0;
+  }
+}
+
+/**
+ * @brief Finds the index of a given key in the store.
+ *
+ * @param kv Pointer to the kv_store_t structure.
+ * @param key The key to search for.
+ * @param index Pointer to a size_t where the index will be stored if found (can be NULL).
+ * @return 1 if the key is found, 0 otherwise.
+ */
+static int kv_find(const kv_store_t *kv, uint32_t key, size_t *index) {
+  for (size_t i = 0; i < kv->capacity; i++)
+  {
+    if (kv->used[i] && kv->keys[i] == key)
+    {
+      if (index)
+      {
+        *index = i;
+      }
+      return 1; // found
+    }
+  }
+  return 0; // not found
+}
+
+/**
+ * @brief Checks if a key exists in the store.
+ *
+ * @param kv Pointer to the kv_store_t structure.
+ * @param key The key to check for existence.
+ * @return 1 if the key exists, 0 otherwise.
+ */
+int kv_exists(const kv_store_t *kv, uint32_t key) {
+  return kv_find(kv, key, NULL);
+}
+
+/**
+ * @brief Sets a value for a given key.
+ *
+ * If the key already exists, its value is updated. If not, a new key-value pair is inserted
+ * into the first available slot. If the store is full, the operation fails.
+ *
+ * @param kv Pointer to the kv_store_t structure.
+ * @param key The key to set.
+ * @param value Pointer to the value to store. Value will be memcpy in the store, so the pointer does not need to be valid afterward.
+ * @return 0 on success, -1 if the store is full.
+ */
+int kv_set(kv_store_t *kv, uint32_t key, const void *value) {
+  size_t index;
+
+  // Update existing
+  if (kv_find(kv, key, &index))
+  {
+    memcpy(&kv->values[index * kv->esize], value, kv->esize);
+    return 0;
+  }
+
+  // Insert new
+  for (size_t i = 0; i < kv->capacity; i++)
+  {
+    if (!kv->used[i])
+    {
+      kv->used[i] = 1;
+      kv->keys[i] = key;
+      memcpy(&kv->values[i * kv->esize], value, kv->esize);
+      return 0;
+    }
+  }
+  
+  return -1;  // store full
+}
+
+/**
+ * @brief Retrieves the value associated with a given key.
+ *
+ * @param kv Pointer to the kv_store_t structure.
+ * @param key The key to retrieve the value for.
+ * @return Pointer to the value if the key exists, NULL otherwise.
+ */
+void* kv_get(const kv_store_t *kv, uint32_t key) {
+  size_t index;
+
+  if (!kv_find(kv, key, &index))
+    return NULL;
+
+  return &kv->values[index * kv->esize];
+}
+
+/**
+ * @brief Removes a key-value pair from the store.
+ *
+ * @param kv Pointer to the kv_store_t structure.
+ * @param key The key to remove.
+ * @return 0 on success, -1 if the key was not found.
+ */
+int kv_remove(kv_store_t *kv, uint32_t key) {
+  size_t index;
+
+  if (!kv_find(kv, key, &index))
+    return -1;
+
+  kv->used[index] = 0;
+  return 0;
+}

sw/airborne/utils/kv_store.h

@@ -0,0 +1,24 @@
+/*
+ * General purpose key-value store.
+ * Copyright (C) 2026 Fabien-B <fabien-b@github.com> 
+ * This file is part of paparazzi. See LICENCE file.
+ */
+
+#pragma once
+#include <stdint.h>
+#include <stddef.h>
+
+typedef struct {
+    size_t capacity;     // number of slots
+    size_t esize;        // size of each value
+    uint32_t *keys;      // [capacity]
+    uint8_t *values;     // [capacity * esize]
+    uint8_t *used;       // [capacity] 0 = free, 1 = occupied
+} kv_store_t;
+
+
+void kv_init(kv_store_t *kv, size_t capacity, size_t esize, uint32_t *keys, void *values, uint8_t *used);
+int kv_exists(const kv_store_t *kv, uint32_t key);
+int kv_set(kv_store_t *kv, uint32_t key, const void *value);
+void *kv_get(const kv_store_t *kv, uint32_t key);
+int kv_remove(kv_store_t *kv, uint32_t key);

sw/airborne/utils/ring_buffer.c

@@ -30,7 +30,7 @@ size_t ring_buffer_free_space(ring_buffer_t *ring_buffer) {
     return ring_buffer->size - ring_buffer_available(ring_buffer);
 }
 
-size_t ring_buffer_write(ring_buffer_t *rb, uint8_t *data, size_t len) {
+size_t ring_buffer_write(ring_buffer_t *rb, const uint8_t *data, size_t len) {
   size_t free_space = ring_buffer_free_space(rb);
   if (len > free_space) {
     len = free_space;

sw/airborne/utils/ring_buffer.h

@@ -28,7 +28,7 @@ void ring_buffer_init (ring_buffer_t *ring_buffer, uint8_t *buf, size_t size);
  * Write @param data of size @param len in @param ring_buffer.
  * @returns the number of byte effectively written in the ring_buffer.
  */
-size_t ring_buffer_write(ring_buffer_t *ring_buffer, uint8_t* data, size_t len);
+size_t ring_buffer_write(ring_buffer_t *ring_buffer, const uint8_t* data, size_t len);
 
 /**
  * Read @param len bytes from @param ring_buffer into @param read_buffer.

tests/modules/test_arch/mcu_periph/sys_time_arch.h

@@ -52,7 +52,4 @@ static inline uint32_t get_sys_time_msec(void)
          msec_of_cpu_ticks(sys_time.nb_sec_rem);
 }
 
-
-static inline void sys_time_usleep(uint32_t us __attribute__((unused))) {}
-
 #endif /* SYS_TIME_ARCH_H */