UAVCAN v1 GPS demo with MAVCAN autoconfigure

2020-12-22 10:21:22 +01:00 · 2020-12-22 10:21:22 +01:00 · 8242968b2b
parent 5f8ad65c40
commit 8242968b2b
52 changed files with 3294 additions and 404 deletions
--- a/.ci/Jenkinsfile-compile
+++ b/.ci/Jenkinsfile-compile
@ -9,10 +9,10 @@ pipeline {
        script {
          def build_nodes = [:]
          def docker_images = [
-            armhf: "px4io/px4-dev-armhf:2020-11-18",
-            arm64: "px4io/px4-dev-aarch64:2020-11-18",
-            base: "px4io/px4-dev-base-bionic:2020-11-18",
-            nuttx: "px4io/px4-dev-nuttx-focal:2020-11-18",
+            armhf: "px4io/px4-dev-armhf:2021-02-04",
+            arm64: "px4io/px4-dev-aarch64:2021-02-04",
+            base: "px4io/px4-dev-base-bionic:2021-02-04",
+            nuttx: "px4io/px4-dev-nuttx-focal:2021-02-04",
            snapdragon: "lorenzmeier/px4-dev-snapdragon:2020-04-01"
          ]

@ -128,7 +128,7 @@ pipeline {
    // TODO: actually upload artifacts to S3
    // stage('S3 Upload') {
    //   agent {
-    //     docker { image 'px4io/px4-dev-base-focal:2020-11-18' }
+    //     docker { image 'px4io/px4-dev-base-focal:2021-02-04' }
    //   }
    //   options {
    //         skipDefaultCheckout()
--- a/.devcontainer/devcontainer.json
+++ b/.devcontainer/devcontainer.json
@ -2,7 +2,7 @@
 // https://github.com/microsoft/vscode-dev-containers/tree/v0.134.0/containers/cpp
 {
 	"name": "px4-dev-nuttx",
-	"image": "px4io/px4-dev-nuttx-focal:2020-11-18",
+	"image": "px4io/px4-dev-nuttx-focal:2021-02-04",

 	"runArgs": [ "--cap-add=SYS_PTRACE", "--security-opt", "seccomp=unconfined" ],

--- a/.github/workflows/checks.yml
+++ b/.github/workflows/checks.yml
@ -28,7 +28,7 @@ jobs:
          "parameters_metadata",
        ]
    container:
-      image: px4io/px4-dev-nuttx-focal:2020-11-18
+      image: px4io/px4-dev-nuttx-focal:2021-02-04
      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
    steps:
    - uses: actions/checkout@v1
--- a/.github/workflows/clang-tidy.yml
+++ b/.github/workflows/clang-tidy.yml
@ -11,7 +11,7 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-clang:2020-11-18
+    container: px4io/px4-dev-clang:2021-02-04
    steps:
    - uses: actions/checkout@v1
      with:
--- a/.github/workflows/compile_linux.yml
+++ b/.github/workflows/compile_linux.yml
@ -11,7 +11,7 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-armhf:2020-11-18
+    container: px4io/px4-dev-armhf:2021-02-04
    strategy:
      matrix:
        config: [
--- a/.github/workflows/compile_linux_arm64.yml
+++ b/.github/workflows/compile_linux_arm64.yml
@ -11,7 +11,7 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-aarch64:2020-11-18
+    container: px4io/px4-dev-aarch64:2021-02-04
    strategy:
      matrix:
        config: [
--- a/.github/workflows/compile_nuttx.yml
+++ b/.github/workflows/compile_nuttx.yml
@ -11,7 +11,7 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-nuttx-focal:2020-11-18
+    container: px4io/px4-dev-nuttx-focal:2021-02-04
    strategy:
      matrix:
        config: [
@ -59,6 +59,7 @@ jobs:
          px4_fmu-v4_ctrlalloc,
          px4_fmu-v4_default,
          px4_fmu-v4_optimized,
+          px4_fmu-v4_uavcanv1,
          px4_fmu-v4pro_default,
          px4_fmu-v5_ctrlalloc,
          px4_fmu-v5_default,
@ -68,6 +69,7 @@ jobs:
          px4_fmu-v5_rover,
          px4_fmu-v5_rtps,
          px4_fmu-v5_stackcheck,
+          px4_fmu-v5_uavcanv1,
          px4_fmu-v5x_base_phy_DP83848C,
          px4_fmu-v5x_default,
          px4_fmu-v6x_default,
--- a/.github/workflows/compile_nuttx_cannode.yml
+++ b/.github/workflows/compile_nuttx_cannode.yml
@ -11,7 +11,7 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-nuttx-focal:2020-11-18
+    container: px4io/px4-dev-nuttx-focal:2021-02-04
    strategy:
      matrix:
        config: [
--- a/.github/workflows/mavros_avoidance_tests.yml
+++ b/.github/workflows/mavros_avoidance_tests.yml
@ -19,7 +19,7 @@ jobs:
          - {test_file: "mavros_posix_test_safe_landing.test", vehicle: "iris_obs_avoid", mission: "MC_safe_landing", build_type: "RelWithDebInfo"}

    container:
-      image: px4io/px4-dev-ros-melodic:2020-11-18
+      image: px4io/px4-dev-ros-melodic:2021-02-04
      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
    steps:
    - uses: actions/checkout@v1
--- a/.github/workflows/mavros_mission_tests.yml
+++ b/.github/workflows/mavros_mission_tests.yml
@ -25,7 +25,7 @@ jobs:
          #- {vehicle: "tiltrotor",     mission: "VTOL_mission_1",  build_type: "RelWithDebInfo"}

    container:
-      image: px4io/px4-dev-ros-melodic:2020-11-18
+      image: px4io/px4-dev-ros-melodic:2021-02-04
      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
    steps:
    - uses: actions/checkout@v1
--- a/.github/workflows/mavros_offboard_tests.yml
+++ b/.github/workflows/mavros_offboard_tests.yml
@ -20,7 +20,7 @@ jobs:
          #- {test_file: "mavros_posix_tests_offboard_rpyrt_ctl.test", vehicle: "iris", build_type: "RelWithDebInfo"}

    container:
-      image: px4io/px4-dev-ros-melodic:2020-11-18
+      image: px4io/px4-dev-ros-melodic:2021-02-04
      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
    steps:
    - uses: actions/checkout@v1
--- a/.github/workflows/metadata.yml
+++ b/.github/workflows/metadata.yml
@ -12,7 +12,7 @@ jobs:

  airframe:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2020-11-18
+    container: px4io/px4-dev-base-focal:2021-02-04
    steps:
    - uses: actions/checkout@v1
      with:
@ -27,7 +27,7 @@ jobs:

  module:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2020-11-18
+    container: px4io/px4-dev-base-focal:2021-02-04
    steps:
    - uses: actions/checkout@v1
      with:
@ -42,7 +42,7 @@ jobs:

  parameter:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2020-11-18
+    container: px4io/px4-dev-base-focal:2021-02-04
    steps:
    - uses: actions/checkout@v1
      with:
@ -57,7 +57,7 @@ jobs:

  uorb_graph:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-nuttx-focal:2020-11-18
+    container: px4io/px4-dev-nuttx-focal:2021-02-04
    steps:
    - uses: actions/checkout@v1
      with:
@ -72,7 +72,7 @@ jobs:

  micrortps_agent:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2020-11-18
+    container: px4io/px4-dev-base-focal:2021-02-04
    steps:
    - uses: actions/checkout@v1
      with:
@ -86,7 +86,7 @@ jobs:

  ROS_msgs:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2020-11-18
+    container: px4io/px4-dev-base-focal:2021-02-04
    steps:
    - uses: actions/checkout@v1
      with:
@ -99,7 +99,7 @@ jobs:

  ROS2_bridge:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2020-11-18
+    container: px4io/px4-dev-base-focal:2021-02-04
    steps:
    - uses: actions/checkout@v1
      with:
--- a/.github/workflows/sitl_tests.yml
+++ b/.github/workflows/sitl_tests.yml
@ -20,7 +20,7 @@ jobs:
          - {latitude:  "29.660316", longitude:  "-82.316658", altitude:  "30", build_type: "RelWithDebInfo", model: "tailsitter"    } # Florida
          - {latitude:  "47.397742", longitude:    "8.545594", altitude: "488", build_type: "Coverage",       model: "standard_vtol" } # Zurich
    container:
-      image: px4io/px4-dev-simulation-focal:2020-11-18
+      image: px4io/px4-dev-simulation-focal:2021-02-04
      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
    steps:
    - uses: actions/checkout@v1
--- a/.gitmodules
+++ b/.gitmodules
@ -30,12 +30,6 @@
 	path = src/drivers/gps/devices
 	url = https://github.com/PX4/PX4-GPSDrivers.git
 	branch = master
-[submodule "src/drivers/uavcan_v1/libcanard"]
-	path = src/drivers/uavcan_v1/libcanard
-	url = https://github.com/UAVCAN/libcanard.git
-[submodule "src/drivers/uavcan_v1/public_regulated_data_types"]
-	path = src/drivers/uavcan_v1/public_regulated_data_types
-	url = https://github.com/UAVCAN/public_regulated_data_types.git
 [submodule "src/modules/micrortps_bridge/micro-CDR"]
 	path = src/modules/micrortps_bridge/micro-CDR
 	url = https://github.com/PX4/Micro-CDR.git
@ -60,3 +54,15 @@
 [submodule "src/examples/gyro_fft/CMSIS_5"]
 	path = src/examples/gyro_fft/CMSIS_5
 	url = https://github.com/ARM-software/CMSIS_5.git
+[submodule "src/drivers/uavcan_v1/libcanard"]
+	path = src/drivers/uavcan_v1/libcanard
+	url = https://github.com/UAVCAN/libcanard.git
+[submodule "src/drivers/uavcan_v1/public_regulated_data_types"]
+	path = src/drivers/uavcan_v1/public_regulated_data_types
+	url = https://github.com/UAVCAN/public_regulated_data_types.git
+[submodule "src/drivers/uavcannode_gps_demo/public_regulated_data_types"]
+	path = src/drivers/uavcannode_gps_demo/public_regulated_data_types
+	url = https://github.com/UAVCAN/public_regulated_data_types
+[submodule "src/drivers/uavcannode_gps_demo/libcanard"]
+	path = src/drivers/uavcannode_gps_demo/libcanard
+	url = https://github.com/UAVCAN/libcanard
--- a/24
+++ b/24
@ -15,7 +15,7 @@ pipeline {
        // stage('Catkin build on ROS workspace') {
        //   agent {
        //     docker {
-        //       image 'px4io/px4-dev-ros-melodic:2020-11-18'
+        //       image 'px4io/px4-dev-ros-melodic:2021-02-04'
        //       args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw -e HOME=$WORKSPACE'
        //     }
        //   }
@ -56,7 +56,7 @@ pipeline {
        stage('Colcon build on ROS2 workspace') {
          agent {
            docker {
-              image 'px4io/px4-dev-ros2-foxy:2020-11-18'
+              image 'px4io/px4-dev-ros2-foxy:2021-02-04'
              args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw -e HOME=$WORKSPACE'
            }
          }
@ -87,7 +87,7 @@ pipeline {

        stage('Airframe') {
          agent {
-            docker { image 'px4io/px4-dev-base-focal:2020-11-18' }
+            docker { image 'px4io/px4-dev-base-focal:2021-02-04' }
          }
          steps {
            sh 'make distclean'
@ -106,7 +106,7 @@ pipeline {

        stage('Parameter') {
          agent {
-            docker { image 'px4io/px4-dev-base-focal:2020-11-18' }
+            docker { image 'px4io/px4-dev-base-focal:2021-02-04' }
          }
          steps {
            sh 'make distclean'
@ -125,7 +125,7 @@ pipeline {

        stage('Module') {
          agent {
-            docker { image 'px4io/px4-dev-base-focal:2020-11-18' }
+            docker { image 'px4io/px4-dev-base-focal:2021-02-04' }
          }
          steps {
            sh 'make distclean'
@ -145,7 +145,7 @@ pipeline {
        stage('uORB graphs') {
          agent {
            docker {
-              image 'px4io/px4-dev-nuttx-focal:2020-11-18'
+              image 'px4io/px4-dev-nuttx-focal:2021-02-04'
              args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
            }
          }
@ -174,7 +174,7 @@ pipeline {

        stage('Userguide') {
          agent {
-            docker { image 'px4io/px4-dev-base-focal:2020-11-18' }
+            docker { image 'px4io/px4-dev-base-focal:2021-02-04' }
          }
          steps {
            sh('export')
@ -204,7 +204,7 @@ pipeline {

        stage('QGroundControl') {
          agent {
-            docker { image 'px4io/px4-dev-base-focal:2020-11-18' }
+            docker { image 'px4io/px4-dev-base-focal:2021-02-04' }
          }
          steps {
            sh('export')
@ -232,7 +232,7 @@ pipeline {

        stage('microRTPS agent') {
          agent {
-            docker { image 'px4io/px4-dev-base-focal:2020-11-18' }
+            docker { image 'px4io/px4-dev-base-focal:2021-02-04' }
          }
          steps {
            sh('export')
@ -262,7 +262,7 @@ pipeline {

        stage('PX4 ROS msgs') {
          agent {
-            docker { image 'px4io/px4-dev-base-focal:2020-11-18' }
+            docker { image 'px4io/px4-dev-base-focal:2021-02-04' }
          }
          steps {
            sh('export')
@ -291,7 +291,7 @@ pipeline {

        stage('PX4 ROS2 bridge') {
          agent {
-            docker { image 'px4io/px4-dev-base-focal:2020-11-18' }
+            docker { image 'px4io/px4-dev-base-focal:2021-02-04' }
          }
          steps {
            sh('export')
@ -334,7 +334,7 @@ pipeline {

        stage('S3') {
          agent {
-            docker { image 'px4io/px4-dev-base-focal:2020-11-18' }
+            docker { image 'px4io/px4-dev-base-focal:2021-02-04' }
          }
          steps {
            sh('export')
--- a/Tools/docker_run.sh
+++ b/Tools/docker_run.sh
@ -4,10 +4,10 @@ if [ -z ${PX4_DOCKER_REPO+x} ]; then
 	echo "guessing PX4_DOCKER_REPO based on input";
 	if [[ $@ =~ .*px4_fmu.* ]]; then
 		# nuttx-px4fmu-v{1,2,3,4,5}
-		PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2020-11-18"
+		PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2021-02-04"
 	elif [[ $@ =~ .*ocpoc.* ]] || [[ $@ =~ .*navio2.* ]] || [[ $@ =~ .*raspberry.* ]] || [[ $@ =~ .*beaglebone.* ]] || [[ $@ =~ .*pilotpi.default ]]; then
 		# aerotenna_ocpoc_default, beaglebone_blue_default, emlid_navio2_default, px4_raspberrypi_default, scumaker_pilotpi_default
-		PX4_DOCKER_REPO="px4io/px4-dev-armhf:2020-11-18"
+		PX4_DOCKER_REPO="px4io/px4-dev-armhf:2021-02-04"
 	elif [[ $@ =~ .*pilotpi.arm64 ]]; then
 		# scumaker_pilotpi_arm64
 		PX4_DOCKER_REPO="px4io/px4-dev-aarch64:latest"
@ -16,13 +16,13 @@ if [ -z ${PX4_DOCKER_REPO+x} ]; then
 		PX4_DOCKER_REPO="lorenzmeier/px4-dev-snapdragon:2020-04-01"
 	elif [[ $@ =~ .*ocpoc.* ]] || [[ $@ =~ .*navio2.* ]] || [[ $@ =~ .*raspberry.* ]] || [[ $@ =~ .*bebop.* ]]; then
 		# aerotenna_ocpoc_default, posix_rpi_cross, posix_bebop_default
-		PX4_DOCKER_REPO="px4io/px4-dev-armhf:2020-11-18"
+		PX4_DOCKER_REPO="px4io/px4-dev-armhf:2021-02-04"
 	elif [[ $@ =~ .*clang.* ]] || [[ $@ =~ .*scan-build.* ]]; then
 		# clang tools
-		PX4_DOCKER_REPO="px4io/px4-dev-clang:2020-11-18"
+		PX4_DOCKER_REPO="px4io/px4-dev-clang:2021-02-04"
 	elif [[ $@ =~ .*tests* ]]; then
 		# run all tests with simulation
-		PX4_DOCKER_REPO="px4io/px4-dev-simulation-bionic:2020-11-18"
+		PX4_DOCKER_REPO="px4io/px4-dev-simulation-bionic:2021-02-04"
 	fi
 else
 	echo "PX4_DOCKER_REPO is set to '$PX4_DOCKER_REPO'";
--- a/boards/nxp/ucans32k146/default.cmake
+++ b/boards/nxp/ucans32k146/default.cmake
@ -28,6 +28,8 @@ px4_add_board(
 	CONSTRAINED_MEMORY
 	ROMFSROOT cannode
 	UAVCAN_INTERFACES 2
+	SERIAL_PORTS
+		GPS1:/dev/ttyS1
 	DRIVERS
 		#adc/board_adc
 		#barometer # all available barometer drivers
@ -45,7 +47,8 @@ px4_add_board(
 		#safety_button
 		#tone_alarm
 		#uavcannode # TODO: CAN driver needed
-		uavcan_v1
+		#uavcan_v1
+		uavcannode_gps_demo
 	MODULES
 		#ekf2
 		#load_mon
--- a/boards/px4/fmu-v4/default.cmake
+++ b/boards/px4/fmu-v4/default.cmake
@ -58,8 +58,7 @@ px4_add_board(
 		telemetry # all available telemetry drivers
 		test_ppm
 		tone_alarm
-		#uavcan
-		uavcan_v1
+		uavcan
 	MODULES
 		airspeed_selector
 		attitude_estimator_q
--- a/boards/px4/fmu-v4/nuttx-config/nsh/defconfig
+++ b/boards/px4/fmu-v4/nuttx-config/nsh/defconfig
@ -40,7 +40,6 @@ CONFIG_BOARD_LOOPSPERMSEC=16717
 CONFIG_BOARD_RESET_ON_ASSERT=2
 CONFIG_BUILTIN=y
 CONFIG_C99_BOOL8=y
-CONFIG_CAN_EXTID=y
 CONFIG_CDCACM=y
 CONFIG_CDCACM_PRODUCTID=0x0012
 CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v4.x"
@ -151,8 +150,6 @@ CONFIG_STM32_ADC1=y
 CONFIG_STM32_BBSRAM=y
 CONFIG_STM32_BBSRAM_FILES=5
 CONFIG_STM32_BKPSRAM=y
-CONFIG_STM32_CAN1=y
-CONFIG_STM32_CAN1_BAUD=1000000
 CONFIG_STM32_CCMDATARAM=y
 CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y
 CONFIG_STM32_DMA1=y
--- a/boards/px4/fmu-v4/nuttx-config/uavcanv1/defconfig
+++ b/boards/px4/fmu-v4/nuttx-config/uavcanv1/defconfig
@ -0,0 +1,239 @@
+#
+# This file is autogenerated: PLEASE DO NOT EDIT IT.
+#
+# You can use "make menuconfig" to make any modifications to the installed .config file.
+# You can then do "make savedefconfig" to generate a new defconfig file that includes your
+# modifications.
+#
+# CONFIG_DISABLE_ENVIRON is not set
+# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set
+# CONFIG_MMCSD_HAVE_CARDDETECT is not set
+# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set
+# CONFIG_MMCSD_MMCSUPPORT is not set
+# CONFIG_MMCSD_SPI is not set
+# CONFIG_NSH_DISABLEBG is not set
+# CONFIG_NSH_DISABLESCRIPT is not set
+# CONFIG_NSH_DISABLE_DF is not set
+# CONFIG_NSH_DISABLE_EXEC is not set
+# CONFIG_NSH_DISABLE_EXIT is not set
+# CONFIG_NSH_DISABLE_GET is not set
+# CONFIG_NSH_DISABLE_ITEF is not set
+# CONFIG_NSH_DISABLE_LOOPS is not set
+# CONFIG_NSH_DISABLE_SEMICOLON is not set
+# CONFIG_NSH_DISABLE_TIME is not set
+# CONFIG_STM32_CCMEXCLUDE is not set
+CONFIG_ARCH="arm"
+CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config"
+CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
+CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
+CONFIG_ARCH_CHIP="stm32"
+CONFIG_ARCH_CHIP_STM32=y
+CONFIG_ARCH_CHIP_STM32F427V=y
+CONFIG_ARCH_INTERRUPTSTACK=512
+CONFIG_ARCH_STACKDUMP=y
+CONFIG_ARMV7M_LAZYFPU=y
+CONFIG_ARMV7M_MEMCPY=y
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_BOARDCTL_RESET=y
+CONFIG_BOARD_CRASHDUMP=y
+CONFIG_BOARD_LOOPSPERMSEC=16717
+CONFIG_BOARD_RESET_ON_ASSERT=2
+CONFIG_BUILTIN=y
+CONFIG_C99_BOOL8=y
+CONFIG_CAN_EXTID=y
+CONFIG_CDCACM=y
+CONFIG_CDCACM_PRODUCTID=0x0012
+CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v4.x"
+CONFIG_CDCACM_RXBUFSIZE=600
+CONFIG_CDCACM_TXBUFSIZE=2000
+CONFIG_CDCACM_VENDORID=0x26ac
+CONFIG_CDCACM_VENDORSTR="3D Robotics"
+CONFIG_CLOCK_MONOTONIC=y
+CONFIG_DEBUG_FULLOPT=y
+CONFIG_DEBUG_HARDFAULT_ALERT=y
+CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEFAULT_SMALL=y
+CONFIG_DEV_FIFO_SIZE=0
+CONFIG_DEV_PIPE_MAXSIZE=1024
+CONFIG_DEV_PIPE_SIZE=70
+CONFIG_DISABLE_MQUEUE=y
+CONFIG_FAT_DMAMEMORY=y
+CONFIG_FAT_LCNAMES=y
+CONFIG_FAT_LFN=y
+CONFIG_FAT_LFN_ALIAS_HASH=y
+CONFIG_FDCLONE_STDIO=y
+CONFIG_FS_BINFS=y
+CONFIG_FS_CROMFS=y
+CONFIG_FS_FAT=y
+CONFIG_FS_FATTIME=y
+CONFIG_FS_PROCFS=y
+CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
+CONFIG_FS_PROCFS_REGISTER=y
+CONFIG_FS_ROMFS=y
+CONFIG_GRAN=y
+CONFIG_GRAN_INTR=y
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+CONFIG_I2C=y
+CONFIG_I2C_RESET=y
+CONFIG_IDLETHREAD_STACKSIZE=750
+CONFIG_LIBC_FLOATINGPOINT=y
+CONFIG_LIBC_LONG_LONG=y
+CONFIG_LIBC_STRERROR=y
+CONFIG_MEMSET_64BIT=y
+CONFIG_MEMSET_OPTSPEED=y
+CONFIG_MMCSD=y
+CONFIG_MMCSD_SDIO=y
+CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y
+CONFIG_MM_REGIONS=2
+CONFIG_MTD=y
+CONFIG_MTD_BYTE_WRITE=y
+CONFIG_MTD_PARTITION=y
+CONFIG_MTD_RAMTRON=y
+CONFIG_NAME_MAX=40
+CONFIG_NFILE_DESCRIPTORS=12
+CONFIG_NSH_ARCHINIT=y
+CONFIG_NSH_ARCHROMFS=y
+CONFIG_NSH_ARGCAT=y
+CONFIG_NSH_BUILTIN_APPS=y
+CONFIG_NSH_CMDPARMS=y
+CONFIG_NSH_CROMFSETC=y
+CONFIG_NSH_DISABLE_IFCONFIG=y
+CONFIG_NSH_DISABLE_IFUPDOWN=y
+CONFIG_NSH_DISABLE_MB=y
+CONFIG_NSH_DISABLE_MH=y
+CONFIG_NSH_DISABLE_TELNETD=y
+CONFIG_NSH_LINELEN=128
+CONFIG_NSH_MAXARGUMENTS=15
+CONFIG_NSH_NESTDEPTH=8
+CONFIG_NSH_QUOTE=y
+CONFIG_NSH_ROMFSETC=y
+CONFIG_NSH_ROMFSSECTSIZE=128
+CONFIG_NSH_STRERROR=y
+CONFIG_NSH_VARS=y
+CONFIG_PIPES=y
+CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
+CONFIG_PTHREAD_MUTEX_ROBUST=y
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_SETSPEED=y
+CONFIG_RAMTRON_WRITEWAIT=y
+CONFIG_RAM_SIZE=262144
+CONFIG_RAM_START=0x20000000
+CONFIG_RAW_BINARY=y
+CONFIG_RTC_DATETIME=y
+CONFIG_SCHED_ATEXIT=y
+CONFIG_SCHED_HPWORK=y
+CONFIG_SCHED_HPWORKPRIORITY=249
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_INSTRUMENTATION=y
+CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
+CONFIG_SCHED_WAITPID=y
+CONFIG_SDCLONE_DISABLE=y
+CONFIG_SDIO_BLOCKSETUP=y
+CONFIG_SEM_NNESTPRIO=8
+CONFIG_SEM_PREALLOCHOLDERS=0
+CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_SIG_DEFAULT=y
+CONFIG_SIG_SIGALRM_ACTION=y
+CONFIG_SIG_SIGUSR1_ACTION=y
+CONFIG_SIG_SIGUSR2_ACTION=y
+CONFIG_SIG_SIGWORK=4
+CONFIG_STACK_COLORATION=y
+CONFIG_START_DAY=30
+CONFIG_START_MONTH=11
+CONFIG_STDIO_BUFFER_SIZE=32
+CONFIG_STM32_ADC1=y
+CONFIG_STM32_BBSRAM=y
+CONFIG_STM32_BBSRAM_FILES=5
+CONFIG_STM32_BKPSRAM=y
+CONFIG_STM32_CAN1=y
+CONFIG_STM32_CAN1_BAUD=1000000
+CONFIG_STM32_CCMDATARAM=y
+CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y
+CONFIG_STM32_DMA1=y
+CONFIG_STM32_DMA2=y
+CONFIG_STM32_FLASH_CONFIG_I=y
+CONFIG_STM32_FLOWCONTROL_BROKEN=y
+CONFIG_STM32_I2C1=y
+CONFIG_STM32_I2CTIMEOMS=10
+CONFIG_STM32_I2CTIMEOTICKS=10
+CONFIG_STM32_JTAG_SW_ENABLE=y
+CONFIG_STM32_OTGFS=y
+CONFIG_STM32_PWR=y
+CONFIG_STM32_RTC=y
+CONFIG_STM32_RTC_HSECLOCK=y
+CONFIG_STM32_RTC_MAGIC=0xfacefeee
+CONFIG_STM32_RTC_MAGIC_REG=1
+CONFIG_STM32_RTC_MAGIC_TIME_SET=0xfacefeef
+CONFIG_STM32_SAVE_CRASHDUMP=y
+CONFIG_STM32_SDIO=y
+CONFIG_STM32_SDIO_CARD=y
+CONFIG_STM32_SERIALBRK_BSDCOMPAT=y
+CONFIG_STM32_SERIAL_DISABLE_REORDERING=y
+CONFIG_STM32_SPI1=y
+CONFIG_STM32_SPI1_DMA=y
+CONFIG_STM32_SPI1_DMA_BUFFER=1024
+CONFIG_STM32_SPI2=y
+CONFIG_STM32_SPI4=y
+CONFIG_STM32_SPI_DMA=y
+CONFIG_STM32_SPI_DMATHRESHOLD=8
+CONFIG_STM32_TIM10=y
+CONFIG_STM32_TIM11=y
+CONFIG_STM32_TIM8=y
+CONFIG_STM32_TIM9=y
+CONFIG_STM32_UART4=y
+CONFIG_STM32_UART7=y
+CONFIG_STM32_UART8=y
+CONFIG_STM32_USART1=y
+CONFIG_STM32_USART2=y
+CONFIG_STM32_USART3=y
+CONFIG_STM32_USART6=y
+CONFIG_STM32_USART_BREAKS=y
+CONFIG_STM32_USART_SINGLEWIRE=y
+CONFIG_STM32_WWDG=y
+CONFIG_SYSTEM_CDCACM=y
+CONFIG_SYSTEM_NSH=y
+CONFIG_TASK_NAME_SIZE=24
+CONFIG_UART4_BAUD=57600
+CONFIG_UART4_RXBUFSIZE=300
+CONFIG_UART4_RXDMA=y
+CONFIG_UART4_TXBUFSIZE=300
+CONFIG_UART7_BAUD=57600
+CONFIG_UART7_RXBUFSIZE=300
+CONFIG_UART7_RXDMA=y
+CONFIG_UART7_SERIAL_CONSOLE=y
+CONFIG_UART7_TXBUFSIZE=300
+CONFIG_UART8_BAUD=57600
+CONFIG_UART8_RXBUFSIZE=300
+CONFIG_UART8_TXBUFSIZE=300
+CONFIG_USART1_RXBUFSIZE=600
+CONFIG_USART1_RXDMA=y
+CONFIG_USART1_TXBUFSIZE=2500
+CONFIG_USART2_BAUD=57600
+CONFIG_USART2_IFLOWCONTROL=y
+CONFIG_USART2_OFLOWCONTROL=y
+CONFIG_USART2_RXBUFSIZE=600
+CONFIG_USART2_RXDMA=y
+CONFIG_USART2_TXBUFSIZE=1100
+CONFIG_USART3_BAUD=57600
+CONFIG_USART3_IFLOWCONTROL=y
+CONFIG_USART3_OFLOWCONTROL=y
+CONFIG_USART3_RXBUFSIZE=1200
+CONFIG_USART3_RXDMA=y
+CONFIG_USART3_TXBUFSIZE=900
+CONFIG_USART6_BAUD=57600
+CONFIG_USART6_RXBUFSIZE=300
+CONFIG_USART6_RXDMA=y
+CONFIG_USART6_TXBUFSIZE=300
+CONFIG_USBDEV=y
+CONFIG_USBDEV_BUSPOWERED=y
+CONFIG_USBDEV_MAXPOWER=500
+CONFIG_USEC_PER_TICK=1000
+CONFIG_USERMAIN_STACKSIZE=2944
+CONFIG_USER_ENTRYPOINT="nsh_main"
--- a/boards/px4/fmu-v4/uavcanv1.cmake
+++ b/boards/px4/fmu-v4/uavcanv1.cmake
@ -0,0 +1,137 @@
+
+px4_add_board(
+	PLATFORM nuttx
+	VENDOR px4
+	MODEL fmu-v4
+	LABEL uavcanv1
+	TOOLCHAIN arm-none-eabi
+	ARCHITECTURE cortex-m4
+	ROMFSROOT px4fmu_common
+	TESTING
+	UAVCAN_INTERFACES 1
+	SERIAL_PORTS
+		GPS1:/dev/ttyS3
+		TEL1:/dev/ttyS1
+		TEL2:/dev/ttyS2
+		WIFI:/dev/ttyS0
+
+	DRIVERS
+		adc/board_adc
+		adc/ads1115
+		barometer # all available barometer drivers
+		batt_smbus
+		camera_capture
+		camera_trigger
+		differential_pressure # all available differential pressure drivers
+		distance_sensor # all available distance sensor drivers
+		dshot
+		gps
+		heater
+		#imu # all available imu drivers
+		imu/adis16448
+		imu/adis16477
+		imu/adis16497
+		imu/invensense/icm20602
+		imu/invensense/icm20608g
+		imu/invensense/icm40609d
+		imu/invensense/mpu6500
+		imu/invensense/mpu9250
+		irlock
+		lights/blinkm
+		lights/rgbled
+		lights/rgbled_ncp5623c
+		magnetometer # all available magnetometer drivers
+		mkblctrl
+		optical_flow # all available optical flow drivers
+		#osd
+		pca9685
+		pca9685_pwm_out
+		#protocol_splitter
+		pwm_input
+		pwm_out_sim
+		pwm_out
+		rc_input
+		roboclaw
+		safety_button
+		tap_esc
+		telemetry # all available telemetry drivers
+		test_ppm
+		tone_alarm
+		#uavcan
+		uavcan_v1
+	MODULES
+		airspeed_selector
+		attitude_estimator_q
+		battery_status
+		camera_feedback
+		commander
+		dataman
+		ekf2
+		esc_battery
+		events
+		flight_mode_manager
+		fw_att_control
+		fw_pos_control_l1
+		land_detector
+		landing_target_estimator
+		load_mon
+		local_position_estimator
+		logger
+		mavlink
+		mc_att_control
+		mc_hover_thrust_estimator
+		mc_pos_control
+		mc_rate_control
+		#micrortps_bridge
+		navigator
+		rc_update
+		rover_pos_control
+		sensors
+		sih
+		temperature_compensation
+		uuv_att_control
+		vmount
+		vtol_att_control
+	SYSTEMCMDS
+		bl_update
+		#dmesg
+		dumpfile
+		esc_calib
+		gpio
+		hardfault_log
+		i2cdetect
+		led_control
+		mft
+		mixer
+		motor_ramp
+		motor_test
+		mtd
+		nshterm
+		param
+		perf
+		pwm
+		reboot
+		reflect
+		sd_bench
+		system_time
+		tests # tests and test runner
+		top
+		topic_listener
+		tune_control
+		usb_connected
+		ver
+		work_queue
+	EXAMPLES
+		fake_gyro
+		fake_magnetometer
+		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
+		gyro_fft
+		hello
+		hwtest # Hardware test
+		#matlab_csv_serial
+		px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html
+		px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html
+		rover_steering_control # Rover example app
+		uuv_example_app
+		work_item
+	)
--- a/boards/px4/fmu-v5/default.cmake
+++ b/boards/px4/fmu-v5/default.cmake
@ -60,8 +60,7 @@ px4_add_board(
 		telemetry # all available telemetry drivers
 		test_ppm
 		tone_alarm
-		#uavcan # legacy v0
-		uavcan_v1
+		uavcan
 	MODULES
 		airspeed_selector
 		attitude_estimator_q
--- a/boards/px4/fmu-v5/nuttx-config/nsh/defconfig
+++ b/boards/px4/fmu-v5/nuttx-config/nsh/defconfig
@ -44,8 +44,6 @@ CONFIG_BOARD_LOOPSPERMSEC=22114
 CONFIG_BOARD_RESET_ON_ASSERT=2
 CONFIG_BUILTIN=y
 CONFIG_C99_BOOL8=y
-CONFIG_CAN=y
-CONFIG_CAN_EXTID=y
 CONFIG_CDCACM=y
 CONFIG_CDCACM_PRODUCTID=0x0032
 CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v5.x"
@ -158,10 +156,6 @@ CONFIG_STM32F7_ADC1=y
 CONFIG_STM32F7_BBSRAM=y
 CONFIG_STM32F7_BBSRAM_FILES=5
 CONFIG_STM32F7_BKPSRAM=y
-CONFIG_STM32F7_CAN1=y
-CONFIG_STM32F7_CAN1_BAUD=1000000
-CONFIG_STM32F7_CAN_TSEG1=7
-CONFIG_STM32F7_CAN_TSEG2=1
 CONFIG_STM32F7_DMA1=y
 CONFIG_STM32F7_DMA2=y
 CONFIG_STM32F7_DMACAPABLE=y
--- a/boards/px4/fmu-v5/nuttx-config/uavcanv1/defconfig
+++ b/boards/px4/fmu-v5/nuttx-config/uavcanv1/defconfig
@ -0,0 +1,250 @@
+#
+# This file is autogenerated: PLEASE DO NOT EDIT IT.
+#
+# You can use "make menuconfig" to make any modifications to the installed .config file.
+# You can then do "make savedefconfig" to generate a new defconfig file that includes your
+# modifications.
+#
+# CONFIG_DISABLE_ENVIRON is not set
+# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set
+# CONFIG_FS_PROCFS_EXCLUDE_ENVIRON is not set
+# CONFIG_MMCSD_HAVE_CARDDETECT is not set
+# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set
+# CONFIG_MMCSD_MMCSUPPORT is not set
+# CONFIG_MMCSD_SPI is not set
+# CONFIG_NSH_DISABLEBG is not set
+# CONFIG_NSH_DISABLESCRIPT is not set
+# CONFIG_NSH_DISABLE_DF is not set
+# CONFIG_NSH_DISABLE_EXEC is not set
+# CONFIG_NSH_DISABLE_EXIT is not set
+# CONFIG_NSH_DISABLE_GET is not set
+# CONFIG_NSH_DISABLE_ITEF is not set
+# CONFIG_NSH_DISABLE_LOOPS is not set
+# CONFIG_NSH_DISABLE_SEMICOLON is not set
+# CONFIG_NSH_DISABLE_TIME is not set
+CONFIG_ARCH="arm"
+CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config"
+CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
+CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
+CONFIG_ARCH_CHIP="stm32f7"
+CONFIG_ARCH_CHIP_STM32F765II=y
+CONFIG_ARCH_CHIP_STM32F7=y
+CONFIG_ARCH_INTERRUPTSTACK=512
+CONFIG_ARCH_STACKDUMP=y
+CONFIG_ARMV7M_BASEPRI_WAR=y
+CONFIG_ARMV7M_DCACHE=y
+CONFIG_ARMV7M_DTCM=y
+CONFIG_ARMV7M_ICACHE=y
+CONFIG_ARMV7M_LAZYFPU=y
+CONFIG_ARMV7M_MEMCPY=y
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_BOARDCTL_RESET=y
+CONFIG_BOARD_CRASHDUMP=y
+CONFIG_BOARD_LOOPSPERMSEC=22114
+CONFIG_BOARD_RESET_ON_ASSERT=2
+CONFIG_BUILTIN=y
+CONFIG_C99_BOOL8=y
+CONFIG_CAN=y
+CONFIG_CAN_EXTID=y
+CONFIG_CDCACM=y
+CONFIG_CDCACM_PRODUCTID=0x0032
+CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v5.x"
+CONFIG_CDCACM_RXBUFSIZE=600
+CONFIG_CDCACM_TXBUFSIZE=12000
+CONFIG_CDCACM_VENDORID=0x26ac
+CONFIG_CDCACM_VENDORSTR="3D Robotics"
+CONFIG_CLOCK_MONOTONIC=y
+CONFIG_DEBUG_FULLOPT=y
+CONFIG_DEBUG_HARDFAULT_ALERT=y
+CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEFAULT_SMALL=y
+CONFIG_DEV_FIFO_SIZE=0
+CONFIG_DEV_PIPE_MAXSIZE=1024
+CONFIG_DEV_PIPE_SIZE=70
+CONFIG_DISABLE_MQUEUE=y
+CONFIG_FAT_DMAMEMORY=y
+CONFIG_FAT_LCNAMES=y
+CONFIG_FAT_LFN=y
+CONFIG_FAT_LFN_ALIAS_HASH=y
+CONFIG_FDCLONE_STDIO=y
+CONFIG_FS_BINFS=y
+CONFIG_FS_CROMFS=y
+CONFIG_FS_FAT=y
+CONFIG_FS_FATTIME=y
+CONFIG_FS_PROCFS=y
+CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
+CONFIG_FS_PROCFS_REGISTER=y
+CONFIG_FS_ROMFS=y
+CONFIG_GRAN=y
+CONFIG_GRAN_INTR=y
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+CONFIG_I2C=y
+CONFIG_I2C_RESET=y
+CONFIG_IDLETHREAD_STACKSIZE=750
+CONFIG_LIBC_FLOATINGPOINT=y
+CONFIG_LIBC_LONG_LONG=y
+CONFIG_LIBC_STRERROR=y
+CONFIG_MAX_TASKS=64
+CONFIG_MEMSET_64BIT=y
+CONFIG_MEMSET_OPTSPEED=y
+CONFIG_MMCSD=y
+CONFIG_MMCSD_SDIO=y
+CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y
+CONFIG_MM_REGIONS=3
+CONFIG_MTD=y
+CONFIG_MTD_BYTE_WRITE=y
+CONFIG_MTD_PARTITION=y
+CONFIG_MTD_RAMTRON=y
+CONFIG_NAME_MAX=40
+CONFIG_NFILE_DESCRIPTORS=12
+CONFIG_NSH_ARCHINIT=y
+CONFIG_NSH_ARCHROMFS=y
+CONFIG_NSH_ARGCAT=y
+CONFIG_NSH_BUILTIN_APPS=y
+CONFIG_NSH_CMDPARMS=y
+CONFIG_NSH_CROMFSETC=y
+CONFIG_NSH_DISABLE_IFCONFIG=y
+CONFIG_NSH_DISABLE_IFUPDOWN=y
+CONFIG_NSH_DISABLE_MB=y
+CONFIG_NSH_DISABLE_MH=y
+CONFIG_NSH_DISABLE_TELNETD=y
+CONFIG_NSH_LINELEN=128
+CONFIG_NSH_MAXARGUMENTS=15
+CONFIG_NSH_NESTDEPTH=8
+CONFIG_NSH_QUOTE=y
+CONFIG_NSH_ROMFSETC=y
+CONFIG_NSH_ROMFSSECTSIZE=128
+CONFIG_NSH_STRERROR=y
+CONFIG_NSH_VARS=y
+CONFIG_PIPES=y
+CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
+CONFIG_PTHREAD_MUTEX_ROBUST=y
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_SETSPEED=y
+CONFIG_RAMTRON_WRITEWAIT=y
+CONFIG_RAM_SIZE=245760
+CONFIG_RAM_START=0x20010000
+CONFIG_RAW_BINARY=y
+CONFIG_RTC_DATETIME=y
+CONFIG_SCHED_ATEXIT=y
+CONFIG_SCHED_HPWORK=y
+CONFIG_SCHED_HPWORKPRIORITY=249
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_INSTRUMENTATION=y
+CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
+CONFIG_SCHED_WAITPID=y
+CONFIG_SDCLONE_DISABLE=y
+CONFIG_SDMMC1_SDIO_MODE=y
+CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_NNESTPRIO=8
+CONFIG_SEM_PREALLOCHOLDERS=0
+CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_SIG_DEFAULT=y
+CONFIG_SIG_SIGALRM_ACTION=y
+CONFIG_SIG_SIGUSR1_ACTION=y
+CONFIG_SIG_SIGUSR2_ACTION=y
+CONFIG_SIG_SIGWORK=4
+CONFIG_STACK_COLORATION=y
+CONFIG_START_DAY=30
+CONFIG_START_MONTH=11
+CONFIG_STDIO_BUFFER_SIZE=256
+CONFIG_STM32F7_ADC1=y
+CONFIG_STM32F7_BBSRAM=y
+CONFIG_STM32F7_BBSRAM_FILES=5
+CONFIG_STM32F7_BKPSRAM=y
+CONFIG_STM32F7_CAN1=y
+CONFIG_STM32F7_CAN1_BAUD=1000000
+CONFIG_STM32F7_CAN_TSEG1=7
+CONFIG_STM32F7_CAN_TSEG2=1
+CONFIG_STM32F7_DMA1=y
+CONFIG_STM32F7_DMA2=y
+CONFIG_STM32F7_DMACAPABLE=y
+CONFIG_STM32F7_FLOWCONTROL_BROKEN=y
+CONFIG_STM32F7_I2C1=y
+CONFIG_STM32F7_I2C2=y
+CONFIG_STM32F7_I2C3=y
+CONFIG_STM32F7_I2C4=y
+CONFIG_STM32F7_I2C_DYNTIMEO=y
+CONFIG_STM32F7_I2C_DYNTIMEO_STARTSTOP=10
+CONFIG_STM32F7_OTGFS=y
+CONFIG_STM32F7_PROGMEM=y
+CONFIG_STM32F7_PWR=y
+CONFIG_STM32F7_RTC=y
+CONFIG_STM32F7_RTC_HSECLOCK=y
+CONFIG_STM32F7_RTC_MAGIC_REG=1
+CONFIG_STM32F7_SAVE_CRASHDUMP=y
+CONFIG_STM32F7_SDMMC1=y
+CONFIG_STM32F7_SDMMC_DMA=y
+CONFIG_STM32F7_SERIALBRK_BSDCOMPAT=y
+CONFIG_STM32F7_SERIAL_DISABLE_REORDERING=y
+CONFIG_STM32F7_SPI1=y
+CONFIG_STM32F7_SPI1_DMA=y
+CONFIG_STM32F7_SPI1_DMA_BUFFER=1024
+CONFIG_STM32F7_SPI2=y
+CONFIG_STM32F7_SPI4=y
+CONFIG_STM32F7_SPI5=y
+CONFIG_STM32F7_SPI6=y
+CONFIG_STM32F7_SPI_DMA=y
+CONFIG_STM32F7_SPI_DMATHRESHOLD=8
+CONFIG_STM32F7_TIM10=y
+CONFIG_STM32F7_TIM11=y
+CONFIG_STM32F7_UART4=y
+CONFIG_STM32F7_UART7=y
+CONFIG_STM32F7_UART8=y
+CONFIG_STM32F7_USART1=y
+CONFIG_STM32F7_USART2=y
+CONFIG_STM32F7_USART3=y
+CONFIG_STM32F7_USART6=y
+CONFIG_STM32F7_USART_BREAKS=y
+CONFIG_STM32F7_USART_INVERT=y
+CONFIG_STM32F7_USART_SINGLEWIRE=y
+CONFIG_STM32F7_USART_SWAP=y
+CONFIG_STM32F7_WWDG=y
+CONFIG_SYSTEM_CDCACM=y
+CONFIG_SYSTEM_NSH=y
+CONFIG_TASK_NAME_SIZE=24
+CONFIG_UART4_BAUD=57600
+CONFIG_UART4_RXBUFSIZE=600
+CONFIG_UART4_RXDMA=y
+CONFIG_UART4_TXBUFSIZE=1500
+CONFIG_UART7_BAUD=57600
+CONFIG_UART7_RXBUFSIZE=600
+CONFIG_UART7_SERIAL_CONSOLE=y
+CONFIG_UART7_TXBUFSIZE=1500
+CONFIG_UART8_BAUD=57600
+CONFIG_UART8_RXBUFSIZE=600
+CONFIG_UART8_RXDMA=y
+CONFIG_UART8_TXBUFSIZE=1500
+CONFIG_USART1_BAUD=57600
+CONFIG_USART1_RXBUFSIZE=600
+CONFIG_USART1_TXBUFSIZE=1500
+CONFIG_USART2_BAUD=57600
+CONFIG_USART2_IFLOWCONTROL=y
+CONFIG_USART2_OFLOWCONTROL=y
+CONFIG_USART2_RXBUFSIZE=600
+CONFIG_USART2_RXDMA=y
+CONFIG_USART2_TXBUFSIZE=1500
+CONFIG_USART3_BAUD=57600
+CONFIG_USART3_IFLOWCONTROL=y
+CONFIG_USART3_OFLOWCONTROL=y
+CONFIG_USART3_RXBUFSIZE=600
+CONFIG_USART3_RXDMA=y
+CONFIG_USART3_TXBUFSIZE=3000
+CONFIG_USART3_TXDMA=y
+CONFIG_USART6_BAUD=57600
+CONFIG_USART6_RXBUFSIZE=600
+CONFIG_USART6_RXDMA=y
+CONFIG_USART6_TXBUFSIZE=1500
+CONFIG_USBDEV=y
+CONFIG_USBDEV_BUSPOWERED=y
+CONFIG_USBDEV_MAXPOWER=500
+CONFIG_USEC_PER_TICK=1000
+CONFIG_USERMAIN_STACKSIZE=2944
+CONFIG_USER_ENTRYPOINT="nsh_main"
--- a/boards/px4/fmu-v5/uavcanv1.cmake
+++ b/boards/px4/fmu-v5/uavcanv1.cmake
@ -0,0 +1,140 @@
+
+px4_add_board(
+	PLATFORM nuttx
+	VENDOR px4
+	MODEL fmu-v5
+	LABEL uavcanv1
+	TOOLCHAIN arm-none-eabi
+	ARCHITECTURE cortex-m7
+	ROMFSROOT px4fmu_common
+	IO px4_io-v2_default
+	TESTING
+	UAVCAN_INTERFACES 2
+	SERIAL_PORTS
+		GPS1:/dev/ttyS0
+		TEL1:/dev/ttyS1
+		TEL2:/dev/ttyS2
+		TEL4:/dev/ttyS3
+	DRIVERS
+		adc/board_adc
+		adc/ads1115
+		barometer # all available barometer drivers
+		batt_smbus
+		camera_capture
+		camera_trigger
+		differential_pressure # all available differential pressure drivers
+		distance_sensor # all available distance sensor drivers
+		dshot
+		gps
+		#heater
+		#imu # all available imu drivers
+		imu/adis16448
+		imu/adis16477
+		imu/adis16497
+		imu/bosch/bmi055
+		imu/invensense/icm20602
+		imu/invensense/icm20689
+		#imu/mpu6000 # legacy icm20602/icm20689 driver
+		irlock
+		lights/blinkm
+		lights/rgbled
+		lights/rgbled_ncp5623c
+		lights/rgbled_pwm
+		magnetometer # all available magnetometer drivers
+		mkblctrl
+		optical_flow # all available optical flow drivers
+		#osd
+		pca9685
+		pca9685_pwm_out
+		power_monitor/ina226
+		#protocol_splitter
+		pwm_input
+		pwm_out_sim
+		pwm_out
+		px4io
+		rc_input
+		roboclaw
+		rpm
+		safety_button
+		tap_esc
+		telemetry # all available telemetry drivers
+		test_ppm
+		tone_alarm
+		#uavcan # legacy v0
+		uavcan_v1
+	MODULES
+		airspeed_selector
+		attitude_estimator_q
+		battery_status
+		camera_feedback
+		commander
+		dataman
+		ekf2
+		esc_battery
+		events
+		flight_mode_manager
+		fw_att_control
+		fw_pos_control_l1
+		land_detector
+		landing_target_estimator
+		load_mon
+		local_position_estimator
+		logger
+		mavlink
+		mc_att_control
+		mc_hover_thrust_estimator
+		mc_pos_control
+		mc_rate_control
+		#micrortps_bridge
+		navigator
+		rc_update
+		rover_pos_control
+		sensors
+		sih
+		temperature_compensation
+		uuv_att_control
+		vmount
+		vtol_att_control
+	SYSTEMCMDS
+		bl_update
+		dmesg
+		dumpfile
+		esc_calib
+		gpio
+		hardfault_log
+		i2cdetect
+		led_control
+		mft
+		mixer
+		motor_ramp
+		motor_test
+		mtd
+		nshterm
+		param
+		perf
+		pwm
+		reboot
+		reflect
+		sd_bench
+		system_time
+		tests # tests and test runner
+		top
+		topic_listener
+		tune_control
+		usb_connected
+		ver
+		work_queue
+	EXAMPLES
+		fake_gyro
+		fake_magnetometer
+		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
+		gyro_fft
+		hello
+		hwtest # Hardware test
+		#matlab_csv_serial
+		px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html
+		px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html
+		rover_steering_control # Rover example app
+		uuv_example_app
+		work_item
+	)
--- a/src/drivers/uavcan_v1/.Uavcan.cpp.kate-swp
+++ b/src/drivers/uavcan_v1/.Uavcan.cpp.kate-swp
--- a/src/drivers/uavcan_v1/CMakeLists.txt
+++ b/src/drivers/uavcan_v1/CMakeLists.txt
@ -45,10 +45,6 @@ else()
 	message(FATAL_ERROR "UAVCAN Nunavut nnvg not found")
 endif()

-add_definitions(
-	-DCANARD_DSDL_CONFIG_LITTLE_ENDIAN=1
-)
-
 set(SRCS)
 if(${PX4_PLATFORM} MATCHES "nuttx")
 	if(CONFIG_NET_CAN)
@ -80,8 +76,6 @@ px4_add_module(
 		${SRCS}
 		o1heap/o1heap.c
 		o1heap/o1heap.h
-		${LIBCANARD_DIR}/libcanard/canard_dsdl.c
-		${LIBCANARD_DIR}/libcanard/canard_dsdl.h
 		${LIBCANARD_DIR}/libcanard/canard.c
 		${LIBCANARD_DIR}/libcanard/canard.h
 	DEPENDS
--- a/src/drivers/uavcan_v1/Uavcan.cpp
+++ b/src/drivers/uavcan_v1/Uavcan.cpp
@ -182,11 +182,19 @@ void UavcanNode::Run()

 				canardRxSubscribe(&_canard_instance,
 						  CanardTransferKindMessage,
-							  test_port_id,
+						  bms_port_id,
 						  reg_drone_srv_battery_Status_0_1_EXTENT_BYTES_,
 						  CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
 						  &_drone_srv_battery_subscription);

+				canardRxSubscribe(&_canard_instance, //Temporory GPS message DSDL not defined yet
+						  CanardTransferKindMessage,
+						  gps_port_id,
+						  sizeof(struct sensor_gps_s),
+						  CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+						  &_drone_srv_gps_subscription);
+
+
 				_initialized = true;
 			}
 		}
@ -215,7 +223,7 @@ void UavcanNode::Run()

 	if (hrt_elapsed_time(&_uavcan_pnp_nodeidallocation_last) >= 1_s &&
 	    _node_register_setup != CANARD_NODE_ID_UNSET &&
-        _node_register_request_index == _node_register_last_received_index+1){
+	    _node_register_request_index == _node_register_last_received_index + 1) {

 		PX4_INFO("NodeID %i request register %i", _node_register_setup, _node_register_request_index);

@ -237,7 +245,7 @@ void UavcanNode::Run()

 		int32_t result = uavcan_register_List_Request_1_0_serialize_(&msg, request_payload_buffer, &request.payload_size);

-        if(result == 0){
+		if (result == 0) {
 			// set the data ready in the buffer and chop if needed
 			++_uavcan_register_list_request_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
 			result = canardTxPush(&_canard_instance, &request);
@ -334,7 +342,7 @@ void UavcanNode::Run()
 				uavcan_pnp_NodeIDAllocationData_1_0 msg;

 				size_t pnp_in_size_bits = receive.payload_size;
-                uavcan_pnp_NodeIDAllocationData_1_0_deserialize_(&msg, (const uint8_t*)receive.payload, &pnp_in_size_bits);
+				uavcan_pnp_NodeIDAllocationData_1_0_deserialize_(&msg, (const uint8_t *)receive.payload, &pnp_in_size_bits);

 				//TODO internal database with unique id to node ip mappings now we give an hardcoded ID back

@ -363,24 +371,24 @@ void UavcanNode::Run()

 				result = uavcan_pnp_NodeIDAllocationData_1_0_serialize_(&msg, node_id_alloc_payload_buffer, &transfer.payload_size);

-                if(result == 0) {
+				if (result == 0) {
 					// set the data ready in the buffer and chop if needed
 					++_uavcan_pnp_nodeidallocation_v1_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
 					result = canardTxPush(&_canard_instance, &transfer);
 				}
+
 			} if (receive.port_id == uavcan_register_List_1_0_FIXED_PORT_ID_) {
+
 				uavcan_register_List_Response_1_0 msg;

 				size_t register_in_size_bits = receive.payload_size;
-                uavcan_register_List_Response_1_0_deserialize_(&msg, (const uint8_t*)receive.payload, &register_in_size_bits);
+				uavcan_register_List_Response_1_0_deserialize_(&msg, (const uint8_t *)receive.payload, &register_in_size_bits);


-                
-                
-                
-                if(strncmp((char*)msg.name.name.elements, "uavcan.pub.battery_status.id", msg.name.name.count) == 0) { //Battery status publisher
+				if (strncmp((char *)msg.name.name.elements, "uavcan.pub.gnss_uorb.id",
+					    msg.name.name.count) == 0) { //Demo GPS status publisher
 					_node_register_setup = CANARD_NODE_ID_UNSET;
-                    PX4_INFO("NodeID %i battery_status publisher set PortID to %i", receive.remote_node_id, test_port_id);
+					PX4_INFO("NodeID %i GPS publisher set PortID to %i", receive.remote_node_id, gps_port_id);
 					_node_register_last_received_index++;

 					uavcan_register_Access_Request_1_0 request_msg;
@ -388,7 +396,7 @@ void UavcanNode::Run()

 					uavcan_register_Value_1_0_select_natural16_(&request_msg.value);
 					request_msg.value.natural16.value.count = 1;
-                    request_msg.value.natural16.value.elements[0] = test_port_id;
+					request_msg.value.natural16.value.elements[0] = gps_port_id;


 					uint8_t request_payload_buffer[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
@ -406,7 +414,43 @@ void UavcanNode::Run()

 					result = uavcan_register_Access_Request_1_0_serialize_(&request_msg, request_payload_buffer, &transfer.payload_size);

-                    if(result == 0) {
+					if (result == 0) {
+						// set the data ready in the buffer and chop if needed
+						++_uavcan_register_access_request_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
+						result = canardTxPush(&_canard_instance, &transfer);
+					}
+				}
+
+				if (strncmp((char *)msg.name.name.elements, "uavcan.pub.battery_status.id",
+					    msg.name.name.count) == 0) { //Battery status publisher
+					_node_register_setup = CANARD_NODE_ID_UNSET;
+					PX4_INFO("NodeID %i battery_status publisher set PortID to %i", receive.remote_node_id, bms_port_id);
+					_node_register_last_received_index++;
+
+					uavcan_register_Access_Request_1_0 request_msg;
+					memcpy(&request_msg.name, &msg.name, sizeof(uavcan_register_Name_1_0));
+
+					uavcan_register_Value_1_0_select_natural16_(&request_msg.value);
+					request_msg.value.natural16.value.count = 1;
+					request_msg.value.natural16.value.elements[0] = bms_port_id;
+
+
+					uint8_t request_payload_buffer[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
+
+					CanardTransfer transfer = {
+						.timestamp_usec = hrt_absolute_time(),      // Zero if transmission deadline is not limited.
+						.priority       = CanardPriorityNominal,
+						.transfer_kind  = CanardTransferKindRequest,
+						.port_id        = uavcan_register_Access_1_0_FIXED_PORT_ID_,                // This is the subject-ID.
+						.remote_node_id = receive.remote_node_id,       // Messages cannot be unicast, so use UNSET.
+						.transfer_id    = _uavcan_register_access_request_transfer_id,
+						.payload_size   = uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
+						.payload        = &request_payload_buffer,
+					};
+
+					result = uavcan_register_Access_Request_1_0_serialize_(&request_msg, request_payload_buffer, &transfer.payload_size);
+
+					if (result == 0) {
 						// set the data ready in the buffer and chop if needed
 						++_uavcan_register_access_request_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
 						result = canardTxPush(&_canard_instance, &transfer);
@ -414,10 +458,9 @@ void UavcanNode::Run()
 				}


-			} else if (receive.port_id == test_port_id) {
+			} else if (receive.port_id == bms_port_id) {
 				PX4_INFO("NodeID %i Battery Status msg", receive.remote_node_id);
 				//TODO deserialize
-                
 				/*

 						battery_status_s battery_status{};
@ -426,12 +469,20 @@ void UavcanNode::Run()
 						//battery_status.remaining = bms_status.remaining_capacity;
 						battery_status.timestamp = hrt_absolute_time();
 						_battery_status_pub.publish(battery_status);*/
+
+			} else if (receive.port_id == gps_port_id) {
+				PX4_INFO("NodeID %i GPS sensor msg", receive.remote_node_id);
+
+				sensor_gps_s *gps_msg = (sensor_gps_s *)receive.payload;
+
+				_sensor_gps_pub.publish(*gps_msg);
 			}

 			// Deallocate the dynamic memory afterwards.
 			_canard_instance.memory_free(&_canard_instance, (void *)receive.payload);
+
 		} else {
-                PX4_INFO("RX canard %d\r\n", result);
+			//PX4_INFO("RX canard %d\r\n", result);
 		}
 	}

--- a/src/drivers/uavcan_v1/Uavcan.hpp
+++ b/src/drivers/uavcan_v1/Uavcan.hpp
@ -47,6 +47,7 @@
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_gps.h>

 #include "o1heap/o1heap.h"

@ -67,25 +68,26 @@
 #include <uavcan/_register/Access_1_0.h>

 #define PNP1_PORT_ID                                 uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_
-#define PNP1_PAYLOAD_SIZE                            uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_
+#define PNP1_PAYLOAD_SIZE                            uavcan_pnp_NodeIDAllocationData_1_0_EXTENT_BYTES_
 #define PNP2_PORT_ID                                 uavcan_pnp_NodeIDAllocationData_2_0_FIXED_PORT_ID_
-#define PNP2_PAYLOAD_SIZE                            uavcan_pnp_NodeIDAllocationData_2_0_SERIALIZATION_BUFFER_SIZE_BYTES_
+#define PNP2_PAYLOAD_SIZE                            uavcan_pnp_NodeIDAllocationData_2_0_EXTENT_BYTES_

 #include "CanardInterface.hpp"

 class UavcanNode : public ModuleParams, public px4::ScheduledWorkItem
 {
 	/*
-	 * This memory is reserved for uavcan to use as over flow for message
-	 * Coming from multiple sources that my not be considered at development
-	 * time.
-	 *
-	 * The call to getNumFreeBlocks will tell how many blocks there are
-	 * free -and multiply it times getBlockSize to get the number of bytes
-	 *
+	* This memory is allocated for the 01Heap allocator used by
+	* libcanard to store incoming/outcoming data
+	* Current size of 8192 bytes is arbitrary, should be optimized further
+	* when more nodes and messages are on the CAN bus
 	*/
 	static constexpr unsigned HeapSize = 8192;

+	/*
+	 * Base interval, has to be complemented with events from the CAN driver
+	 * and uORB topics sending data, to decrease response time.
+	 */
 	static constexpr unsigned ScheduleIntervalMs = 10;

 public:
@ -123,6 +125,7 @@ private:
 	CanardRxSubscription _heartbeat_subscription;
 	CanardRxSubscription _pnp_v1_subscription;
 	CanardRxSubscription _drone_srv_battery_subscription;
+	CanardRxSubscription _drone_srv_gps_subscription;
 	CanardRxSubscription _register_access_subscription;
 	CanardRxSubscription _register_list_subscription;

@ -130,6 +133,7 @@ private:
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};

 	uORB::Publication<battery_status_s> _battery_status_pub{ORB_ID(battery_status)};
+	uORB::Publication<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};

 	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")};
 	perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")};
@ -139,7 +143,12 @@ private:
 	hrt_abstime _uavcan_node_heartbeat_last{0};
 	CanardTransferID _uavcan_node_heartbeat_transfer_id{0};

-    const uint16_t test_port_id = 1234;
+	/* Temporary hardcoded port IDs used by the register interface
+	* for demo purposes untill we have nice interface (QGC or latter)
+	* to configure the nodes
+	*/
+	const uint16_t bms_port_id = 1234;
+	const uint16_t gps_port_id = 1235;

 	CanardTransferID _uavcan_pnp_nodeidallocation_v1_transfer_id{0};
 	hrt_abstime _uavcan_pnp_nodeidallocation_last{0};
--- a/src/drivers/uavcan_v1/public_regulated_data_types
+++ b/src/drivers/uavcan_v1/public_regulated_data_types
@ -1 +1 @@
-Subproject commit d874225ea4b5a79a4639bcfcb9096ccd943138a9
+Subproject commit 81526eb6e27095701736dfb6e74842fe07a59f4f
--- a/src/drivers/uavcan_v1/templates/Header.j2
+++ b/src/drivers/uavcan_v1/templates/Header.j2
@ -1,32 +0,0 @@
-/*
- *
- * UAVCAN data structure definition.
- *
- * AUTOGENERATED, DO NOT EDIT.
- *
- * Source File:
- * {{ T.source_file_path }}
- *
- * Template:
- * {{ self._TemplateReference__context.name }}
- *
- * Generated at:  {{ now_utc }} UTC
- * Is deprecated: {{ T.deprecated and 'yes' or 'no' }}
- * Fixed port ID: {{ T.fixed_port_id }}
- * Full name:     {{ T.full_name }}
- * Version:       {{ T.version.major }}.{{ T.version.minor }}
- *
- */
-
-#ifndef {{T.full_name | c.macrofy}}
-#define {{T.full_name | c.macrofy}}
-{% for n in T | includes -%}
-#include {{ n }}
-{% endfor %}
-#include <canard_dsdl.h>
-{{T.full_namespace | open_namespace}}
-
-{%- block object -%}{%- endblock -%}
-
-{{T.full_namespace | close_namespace}}
-#endif // {{T.full_name | c.macrofy}}
--- a/src/drivers/uavcan_v1/templates/ServiceType.j2
+++ b/src/drivers/uavcan_v1/templates/ServiceType.j2
@ -1,10 +0,0 @@
-{% extends "Header.j2" %}
-{%- block object -%}
-{{ T | definition_begin }}
-{
-{% set composite_type = T.request_type -%}
-{% include '_composite_type.j2' %}
-{% set composite_type = T.response_type -%}
-{% include '_composite_type.j2' %}
-}{{ T | definition_end }}
-{% endblock -%}
--- a/src/drivers/uavcan_v1/templates/StructureType.j2
+++ b/src/drivers/uavcan_v1/templates/StructureType.j2
@ -1,5 +0,0 @@
-{% extends "Header.j2" %}
-{%- block object -%}
-{% set composite_type = T -%}
-{% include '_composite_type.j2' %}
-{% endblock -%}
--- a/src/drivers/uavcan_v1/templates/UnionType.j2
+++ b/src/drivers/uavcan_v1/templates/UnionType.j2
@ -1,5 +0,0 @@
-{% extends "Header.j2" %}
-{%- block object -%}
-{% set composite_type = T -%}
-{% include '_composite_type.j2' %}
-{% endblock -%}
--- a/src/drivers/uavcan_v1/templates/_composite_type.j2
+++ b/src/drivers/uavcan_v1/templates/_composite_type.j2
@ -1,115 +0,0 @@
-{{ composite_type | definition_begin }}
-{
-{%- if T.fixed_port_id is not None %}
-	static constexpr CanardPortID PORT_ID = {{T.fixed_port_id}};
-{%- endif %}
-
-{%- set total_size = namespace(value=0) -%}
-{%- for field in composite_type.fields -%}
-    {% set total_size.value = total_size.value + field.data_type.bit_length_set|max %}
-{%- endfor %}
-	static constexpr size_t SIZE = {{total_size.value}};
-
-	static constexpr auto getDataTypeFullName() { return "{{T}}"; }
-
-{% for constant in composite_type.constants %}
-	static constexpr {{ constant.data_type | declaration }} {{ constant.name | id }} = {{ constant.value.native_value.numerator }} / {{ constant.value.native_value.denominator }};
-{%- endfor -%}
-
-{% if composite_type is UnionType %}
-#error "TODO: UnionType
-{% else %}
-
-{% for field in composite_type.fields -%}
-    {%- if field is not padding %}
-        {%- if field.data_type is FloatType %}
-            {%- if field.data_type.bit_length_set|max <= 32 %}
-	float {{ field | id }}{NAN};
-            {%- else %}
-	double {{ field | id }}{NAN};
-            {%- endif %}
-        {%- elif field.data_type is BooleanType %}
-	bool {{ field | id }}{false};
-        {%- else %}
-	{{ field.data_type | declaration }} {{ field | id }}{};
-        {%- endif -%}
-    {%- endif -%}
-{%- endfor %}
-{% endif %}
-
-	void serializeToBuffer(uint8_t* const buffer, const size_t starting_bit = 0)
-	{
-{%- set bit_offset = namespace(value=0) -%}
-{%- for field in composite_type.fields -%}
-    {%- if field is not padding %}
-      {%- if field.data_type is SerializableType %}
-        {%- if field.data_type is IntegerType %}
-		canardDSDLSetUxx(buffer, starting_bit + {{ bit_offset.value }}, {{ field.name }}, {{ field.data_type.bit_length_set | max }});
-        {%- elif field.data_type is BooleanType %}
-		canardDSDLSetBit(buffer, starting_bit + {{ field.data_type.bit_length_set | max }}, {{ field.name }});
-        {%- elif field.data_type is FloatType %}
-            {%- if field.data_type.bit_length_set == 16 %}
-		canardDSDLSetF16(buffer, starting_bit + {{ field.data_type.bit_length_set | max }}, {{ field.name }});
-            {%- elif field.data_type.bit_length_set == 32 %}
-		canardDSDLSetF32(buffer, starting_bit + {{ field.data_type.bit_length_set | max }}, {{ field.name }});
-            {%- elif field.data_type.bit_length_set == 64 %}
-		canardDSDLSetF64(buffer, starting_bit + {{ field.data_type.bit_length_set | max }}, {{ field.name }});
-            {%- endif %}
-        {%- else %}
-		{{ field.name }}.serializeToBuffer(buffer, starting_bit + {{ bit_offset.value }});
-        {%- endif %}
-      {%- endif -%}
-    {%- endif -%}
-    {% set bit_offset.value = bit_offset.value + field.data_type.bit_length_set|max %}
-{%- endfor %}
-	}
-
-	static {{T | full_reference_name}} deserializeFromBuffer(const uint8_t* const buffer, const size_t buf_size, const size_t starting_bit = 0)
-	{
-		{{T | full_reference_name}} msg;
-{% set bit_offset = namespace(value=0) %}
-{%- for field in composite_type.fields -%}
-    {%- if field is not padding %}
-      {%- if field.data_type is SerializableType %}
-        {%- if field.data_type is UnsignedIntegerType %}
-            {%- if field.data_type.bit_length_set|max<= 8 %}
-		msg.{{ field.name }} = canardDSDLGetU8(buffer, buf_size, starting_bit + {{ bit_offset.value }}, {{ field.data_type.bit_length_set | max }});
-            {%- elif field.data_type.bit_length_set|max <= 16 %}
-		msg.{{ field.name }} = canardDSDLGetU16(buffer, buf_size, starting_bit + {{ bit_offset.value }}, {{ field.data_type.bit_length_set | max }});
-            {%- elif field.data_type.bit_length_set|max <= 32 %}
-		msg.{{ field.name }} = canardDSDLGetU32(buffer, buf_size, starting_bit + {{ bit_offset.value }}, {{ field.data_type.bit_length_set | max }});
-            {%- elif field.data_type.bit_length_set|max <= 64 %}
-		msg.{{ field.name }} = canardDSDLGetU64(buffer, buf_size, starting_bit + {{ bit_offset.value }}, {{ field.data_type.bit_length_set | max }});
-            {%- endif %}
-        {%- elif field.data_type is SignedIntegerType %}
-            {%- if field.data_type.bit_length_set|max<= 8 %}
-		msg.{{ field.name }} = canardDSDLGetI8(buffer, buf_size, starting_bit + {{ bit_offset.value }}, {{ field.data_type.bit_length_set | max }});
-            {%- elif field.data_type.bit_length_set|max <= 16 %}
-		msg.{{ field.name }} = canardDSDLGetI16(buffer, buf_size, starting_bit + {{ bit_offset.value }}, {{ field.data_type.bit_length_set | max }});
-            {%- elif field.data_type.bit_length_set|max <= 32 %}
-		msg.{{ field.name }} = canardDSDLGetI32(buffer, buf_size, starting_bit + {{ bit_offset.value }}, {{ field.data_type.bit_length_set | max }});
-            {%- elif field.data_type.bit_length_set|max <= 64 %}
-		msg.{{ field.name }} = canardDSDLGetI64(buffer, buf_size, starting_bit + {{ bit_offset.value }}, {{ field.data_type.bit_length_set | max }});
-            {%- endif %}
-        {%- elif field.data_type is BooleanType %}
-		msg.{{ field.name }} = canardDSDLGetBit(buffer, buf_size, starting_bit + {{ bit_offset.value }});
-        {%- elif field.data_type is FloatType %}
-            {%- if field.data_type.bit_length_set == 16 %}
-		msg.{{ field.name }} = canardDSDLGetF16(buffer, buf_size, starting_bit + {{ bit_offset.value }});
-            {%- elif field.data_type.bit_length_set == 32 %}
-		msg.{{ field.name }} = canardDSDLGetF32(buffer, buf_size, starting_bit + {{ bit_offset.value }});
-            {%- elif field.data_type.bit_length_set == 64 %}
-		msg.{{ field.name }} = canardDSDLGetF64(buffer, buf_size, starting_bit + {{ bit_offset.value }});
-            {%- endif %}
-        {%- else %}
-		msg.{{ field.name }} = {{ field.data_type | declaration }}::deserializeFromBuffer(buffer, buf_size, starting_bit + {{ bit_offset.value }});
-        {%- endif %}
-      {%- endif -%}
-    {%- endif -%}
-    {% set bit_offset.value = bit_offset.value + field.data_type.bit_length_set|max %}
-{%- endfor %}
-
-		return msg;
-	}
-
-}{{ composite_type | definition_end }}
--- a/src/drivers/uavcannode_gps_demo/CMakeLists.txt
+++ b/src/drivers/uavcannode_gps_demo/CMakeLists.txt
@ -0,0 +1,82 @@
+############################################################################
+#
+#   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+set(LIBCANARD_DIR ${CMAKE_CURRENT_SOURCE_DIR}/libcanard)
+set(DSDL_DIR ${CMAKE_CURRENT_SOURCE_DIR}/public_regulated_data_types)
+
+px4_add_git_submodule(TARGET git_libcanard PATH ${LIBCANARD_DIR})
+px4_add_git_submodule(TARGET git_public_regulated_data_types PATH ${DSDL_DIR})
+
+find_program(NNVG_PATH nnvg)
+if(NNVG_PATH)
+	execute_process(COMMAND ${NNVG_PATH} --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language c -I ${DSDL_DIR}/uavcan ${DSDL_DIR}/reg)
+	execute_process(COMMAND ${NNVG_PATH} --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language c ${DSDL_DIR}/uavcan)
+
+else()
+	message(FATAL_ERROR "UAVCAN Nunavut nnvg not found")
+endif()
+
+add_definitions(
+	-DCONFIG_EXAMPLES_LIBCANARDV1_DEV="can0"
+	-DCONFIG_EXAMPLES_LIBCANARDV1_NODE_MEM_POOL_SIZE=8192
+	-DCONFIG_EXAMPLES_LIBCANARDV1_DAEMON_STACK_SIZE=5000
+	-DCONFIG_EXAMPLES_LIBCANARDV1_DAEMON_PRIORITY=100
+	-DCANARD_DSDL_CONFIG_LITTLE_ENDIAN=1
+)
+
+px4_add_module(
+	MODULE drivers__uavcannode-gps-demo
+	MAIN uavcannode_gps_demo
+	COMPILE_FLAGS
+		-Wno-error
+	    -DUINT32_C\(x\)=__UINT32_C\(x\)
+	INCLUDES
+		${LIBCANARD_DIR}/libcanard/
+		${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated
+	SRCS
+		canard_main.c
+		socketcan.c
+		socketcan.h
+		uorb_converter.c
+		uorb_converter.h
+		o1heap.c
+		o1heap.h
+		libcancl/pnp.c
+		libcancl/registerinterface.c
+		${LIBCANARD_DIR}/libcanard/canard.c
+		${LIBCANARD_DIR}/libcanard/canard.h
+	DEPENDS
+		git_libcanard
+		git_public_regulated_data_types
+		version
+	)
--- a/src/drivers/uavcannode_gps_demo/canard_main.c
+++ b/src/drivers/uavcannode_gps_demo/canard_main.c
@ -0,0 +1,448 @@
+/****************************************************************************
+ * examples/canard/canard_main.c
+ *
+ *   Copyright (C) 2016 ETH Zuerich. All rights reserved.
+ *   Author: Matthias Renner <rennerm@ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <canard.h>
+#include <canard_dsdl.h>
+
+#include <sched.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <time.h>
+#include <string.h>
+#include <assert.h>
+#include <errno.h>
+
+#include <net/if.h>
+#include <sys/time.h>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+
+#include <poll.h>
+
+#include <nuttx/can.h>
+#include <netpacket/can.h>
+
+#include "socketcan.h"
+#include "o1heap.h"
+#include "uorb_converter.h"
+
+#include "libcancl/pnp.h"
+#include "libcancl/registerinterface.h"
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/* Arena for memory allocation, used by the library */
+#define O1_HEAP_SIZE CONFIG_EXAMPLES_LIBCANARDV1_NODE_MEM_POOL_SIZE
+#define UNIQUE_ID_LENGTH_BYTES                   16
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+O1HeapInstance *my_allocator;
+static uint8_t uavcan_heap[O1_HEAP_SIZE]
+__attribute__((aligned(O1HEAP_ALIGNMENT)));
+
+static uint8_t unique_id[UNIQUE_ID_LENGTH_BYTES] = { //FIXME real HW ID
+	0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x01
+};
+
+/* Node status variables */
+
+static bool g_canard_daemon_started;
+
+static int16_t gps_uorb_port_id = -1;
+static int16_t gps_fix_port_id = -1;
+static int16_t gps_aux_port_id = -1;
+
+struct pollfd fd;
+
+/****************************************************************************
+ * private functions
+ ****************************************************************************/
+
+
+//TODO move this to a seperate file probably
+
+int32_t set_gps_uorb_port_id(uavcan_register_Value_1_0 *value)
+{
+	if (uavcan_register_Value_1_0_is_natural16_(value) && value->natural16.value.count == 1) { // Natural 16
+		//TODO check validity
+		printf("Master: set uORB portID to %i\n", value->natural16.value.elements[0]);
+		gps_uorb_port_id = value->natural16.value.elements[0];
+		return 0;
+	}
+
+	return -UAVCAN_REGISTER_ERROR_SERIALIZATION;
+}
+
+uavcan_register_Value_1_0 get_gps_uorb_port_id()
+{
+	void *dataReturn;
+	uavcan_register_Value_1_0 value;
+
+	value.natural16.value.elements[0] = gps_uorb_port_id;
+	value.natural16.value.count = 1;
+	value._tag_ = 10; // TODO does nunavut generate ENUM/defines for this??
+	return value;
+}
+
+int32_t set_gps_fix_port_id(uavcan_register_Value_1_0 *value)
+{
+	if (uavcan_register_Value_1_0_is_natural16_(value) && value->natural16.value.count == 1) { // Natural 16
+		//TODO check validity
+		printf("Master: set FIX portID to %i\n", value->natural16.value.elements[0]);
+		gps_fix_port_id = value->natural16.value.elements[0];
+		return 0;
+	}
+
+	return -UAVCAN_REGISTER_ERROR_SERIALIZATION;
+}
+
+uavcan_register_Value_1_0 get_gps_fix_port_id()
+{
+	void *dataReturn;
+	uavcan_register_Value_1_0 value;
+
+	value.natural16.value.elements[0] = gps_fix_port_id;
+	value.natural16.value.count = 1;
+	value._tag_ = 10; // TODO does nunavut generate ENUM/defines for this??
+	return value;
+}
+
+int32_t set_gps_aux_port_id(uavcan_register_Value_1_0 *value)
+{
+	if (uavcan_register_Value_1_0_is_natural16_(value) && value->natural16.value.count == 1) { // Natural 16
+		//TODO check validity
+		printf("Master: set AUX portID to %i\n", value->natural16.value.elements[0]);
+		gps_fix_port_id = value->natural16.value.elements[0];
+		return 0;
+	}
+
+	return -UAVCAN_REGISTER_ERROR_SERIALIZATION;
+}
+
+uavcan_register_Value_1_0 get_gps_aux_port_id()
+{
+	void *dataReturn;
+	uavcan_register_Value_1_0 value;
+
+	value.natural16.value.elements[0] = gps_aux_port_id;
+	value.natural16.value.count = 1;
+	value._tag_ = 10; // TODO does nunavut generate ENUM/defines for this??
+	return value;
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: memAllocate
+ *
+ * Description:
+ *
+ ****************************************************************************/
+static void *memAllocate(CanardInstance *const ins, const size_t amount)
+{
+	(void) ins;
+	return o1heapAllocate(my_allocator, amount);
+}
+
+/****************************************************************************
+ * Name: memFree
+ *
+ * Description:
+ *
+ ****************************************************************************/
+
+static void memFree(CanardInstance *const ins, void *const pointer)
+{
+	(void) ins;
+	o1heapFree(my_allocator, pointer);
+}
+
+/****************************************************************************
+ * Name: getMonotonicTimestampUSec
+ *
+ * Description:
+ *
+ ****************************************************************************/
+
+
+uint64_t getMonotonicTimestampUSec(void)
+{
+	int ret;
+	struct timespec ts;
+
+	memset(&ts, 0, sizeof(ts));
+
+	ret = clock_gettime(CLOCK_MONOTONIC, &ts);
+
+	if (ret != 0) {
+		PX4_ERR("clock error %i", ret);
+		abort();
+	}
+
+	return ts.tv_sec * 1000000ULL + ts.tv_nsec / 1000ULL;
+}
+
+/****************************************************************************
+ * Name: processTxRxOnce
+ *
+ * Description:
+ *   Transmits all frames from the TX queue, receives up to one frame.
+ *
+ ****************************************************************************/
+
+void processTxRxOnce(CanardInstance *ins, CanardSocketInstance *sock_ins, int timeout_msec)
+{
+	int32_t result;
+
+	/* Transmitting */
+
+
+	for (const CanardFrame *txf = NULL; (txf = canardTxPeek(ins)) != NULL;) { // Look at the top of the TX queue.
+		if (txf->timestamp_usec > getMonotonicTimestampUSec()) { // Check if the frame has timed out.
+			if (socketcanTransmit(sock_ins, txf) == 0) {           // Send the frame. Redundant interfaces may be used here.
+				break;                             // If the driver is busy, break and retry later.
+			}
+
+		} else {
+			printf("Timeout??\n");
+		}
+
+		canardTxPop(ins);                         // Remove the frame from the queue after it's transmitted.
+		ins->memory_free(ins, (CanardFrame *)txf); // Deallocate the dynamic memory afterwards.
+	}
+
+
+	/* Poll receive */
+	if (poll(&fd, 1, timeout_msec) <= 0) {
+		return;
+	}
+
+	/* Receiving */
+	CanardFrame received_frame;
+
+	socketcanReceive(sock_ins, &received_frame);
+
+	CanardTransfer receive;
+	result = canardRxAccept(ins,
+				&received_frame, // The CAN frame received from the bus.
+				0,               // If the transport is not redundant, use 0.
+				&receive);
+
+	if (result < 0) {
+		// An error has occurred: either an argument is invalid or we've ran out of memory.
+		// It is possible to statically prove that an out-of-memory will never occur for a given application if
+		// the heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap.
+		// Reception of an invalid frame is NOT an error.
+		fprintf(stderr, "Receive error %d\n", result);
+
+	} else if (result == 1) {
+		printf("Receive portId %i\n", receive.port_id);
+
+		if (receive.port_id == PNPGetPortID(ins)) {
+			PNPProcess(ins, &receive);
+
+		} else {
+			uavcan_register_interface_process(ins, &receive);
+		}
+
+		ins->memory_free(ins, (void *)receive.payload); // Deallocate the dynamic memory afterwards.
+
+	} else {
+		// printf("RX canard %d\r\n", result);
+		// Nothing to do.
+		// The received frame is either invalid or it's a non-last frame of a multi-frame transfer.
+		// Reception of an invalid frame is NOT reported as an error because it is not an error.
+	}
+
+}
+
+/****************************************************************************
+ * Name: canard_daemon
+ *
+ * Description:
+ *
+ ****************************************************************************/
+
+static int canard_daemon(int argc, char *argv[])
+{
+	int errval = 0;
+	int can_fd = 0;
+
+	if (argc > 2) {
+		for (int args = 2; args < argc; args++) {
+			if (!strcmp(argv[args], "canfd")) {
+				can_fd = 1;
+			}
+		}
+	}
+
+	my_allocator = o1heapInit(&uavcan_heap, O1_HEAP_SIZE, NULL, NULL);
+
+	if (my_allocator == NULL) {
+		printf("o1heapInit failed with size %d\n", O1_HEAP_SIZE);
+		errval = 2;
+		goto errout_with_dev;
+	}
+
+	CanardInstance ins = canardInit(&memAllocate, &memFree);
+
+	if (can_fd) {
+		ins.mtu_bytes = CANARD_MTU_CAN_FD;
+
+	} else {
+		ins.mtu_bytes = CANARD_MTU_CAN_CLASSIC;
+	}
+
+	/* Open the CAN device for reading */
+	CanardSocketInstance sock_ins;
+	socketcanOpen(&sock_ins, CONFIG_EXAMPLES_LIBCANARDV1_DEV, can_fd);
+
+	/* setup poll fd */
+	fd.fd = sock_ins.s;
+	fd.events = POLLIN;
+
+	if (sock_ins.s < 0) {
+		printf("canard_daemon: ERROR: open %s failed: %d\n",
+		       CONFIG_EXAMPLES_LIBCANARDV1_DEV, errno);
+		errval = 2;
+		goto errout_with_dev;
+	}
+
+	/* libcancl functions */
+
+	/* Dynamic NodeId */
+
+	/* Init UAVCAN register interfaces */
+	uavcan_node_GetInfo_Response_1_0 node_information; // TODO ADD INFO
+	uavcan_register_interface_init(&ins, &node_information);
+	uavcan_register_interface_add_entry("gnss_uorb", set_gps_uorb_port_id, get_gps_uorb_port_id);
+	uavcan_register_interface_add_entry("gnss_fix", set_gps_fix_port_id, get_gps_fix_port_id);
+	uavcan_register_interface_add_entry("gnss_aux", set_gps_aux_port_id, get_gps_aux_port_id);
+
+	initPNPAllocatee(&ins, unique_id);
+
+	uint32_t random_no;
+	random_no = ((float)rand() / RAND_MAX) * (1000000);
+
+	uint64_t next_alloc_req = getMonotonicTimestampUSec() + random_no;
+
+	while (ins.node_id == CANARD_NODE_ID_UNSET) {
+		// process the TX and RX buffer
+		processTxRxOnce(&ins, &sock_ins, 10); //10Ms
+
+		const uint64_t ts = getMonotonicTimestampUSec();
+
+		if (ts >= next_alloc_req) {
+			next_alloc_req += ((float)rand() / RAND_MAX) * (1000000);
+			int32_t result = PNPAllocRequest(&ins);
+
+			if (result) {
+				ins.node_id = PNPGetNodeID();
+			}
+		}
+	}
+
+	printf("canard_daemon: canard initialized\n");
+	printf("start node (ID: %d MTU: %d)\n", ins.node_id,
+	       ins.mtu_bytes);
+
+	/* Initialize uORB publishers & subscribers */
+	uorbConverterInit(&ins, &gps_uorb_port_id, &gps_fix_port_id, &gps_aux_port_id);
+
+	g_canard_daemon_started = true;
+	uint64_t next_1hz_service_at = getMonotonicTimestampUSec();
+
+	for (;;) {
+		processTxRxOnce(&ins, &sock_ins, 10);
+
+		uorbProcessSub(10);
+
+	}
+
+errout_with_dev:
+
+	g_canard_daemon_started = false;
+	printf("canard_daemon: Terminating!\n");
+	fflush(stdout);
+	return errval;
+}
+
+
+/****************************************************************************
+ * Name: canard_main
+ *
+ * Description:
+ *
+ ****************************************************************************/
+
+int uavcannode_gps_demo_main(int argc, FAR char *argv[])
+{
+	int ret;
+
+	printf("canard_main: Starting canard_daemon\n");
+
+	if (g_canard_daemon_started) {
+		printf("canard_main: receive and send task already running\n");
+		return EXIT_SUCCESS;
+	}
+
+	ret = task_create("canard_daemon", CONFIG_EXAMPLES_LIBCANARDV1_DAEMON_PRIORITY,
+			  CONFIG_EXAMPLES_LIBCANARDV1_DAEMON_STACK_SIZE, canard_daemon,
+			  argv);
+
+	if (ret < 0) {
+		int errcode = errno;
+		printf("canard_main: ERROR: Failed to start canard_daemon: %d\n",
+		       errcode);
+		return EXIT_FAILURE;
+	}
+
+	printf("canard_main: canard_daemon started\n");
+	return EXIT_SUCCESS;
+}
--- a/src/drivers/uavcannode_gps_demo/libcanard
+++ b/src/drivers/uavcannode_gps_demo/libcanard
@ -0,0 +1 @@
+Subproject commit cde670347425023480a1417fcd603b27c8eb06c1
--- a/src/drivers/uavcannode_gps_demo/libcancl/README.md
+++ b/src/drivers/uavcannode_gps_demo/libcancl/README.md
@ -0,0 +1,12 @@
+# libcancl
+
+Libcanard client library, that implements common client functionality for use with UAVCAN/CAN
+Current features are
+
+- UAVCAN PNP client
+- UAVCAN Register interface
+- UAVCAN GetInfo responder
+
+# TODO's
+- UAVCAN ExecuteCommand implementation
+- UAVCAN diagnostic abstraction/helper
--- a/src/drivers/uavcannode_gps_demo/libcancl/pnp.c
+++ b/src/drivers/uavcannode_gps_demo/libcancl/pnp.c
@ -0,0 +1,264 @@
+/****************************************************************************
+ * nxp_bms/BMS_v1/src/UAVCAN/pnp.c
+ *
+ * BSD 3-Clause License
+ *
+ * Copyright 2020 NXP
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, this
+ *    list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * 3. Neither the name of the copyright holder nor the names of its
+ *    contributors may be used to endorse or promote products derived from
+ *    this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "pnp.h"
+
+// Use NuttX crc64 function TODO fallback header for other platforms
+#include <crc64.h>
+
+#include "uavcan/node/ID_1_0.h"
+#include "uavcan/pnp/NodeIDAllocationData_1_0.h"
+#include "uavcan/pnp/NodeIDAllocationData_2_0.h"
+
+#define PNP1_PORT_ID                                 uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_
+#define PNP1_PAYLOAD_SIZE                            uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_
+#define PNP2_PORT_ID                                 uavcan_pnp_NodeIDAllocationData_2_0_FIXED_PORT_ID_
+#define PNP2_PAYLOAD_SIZE                            uavcan_pnp_NodeIDAllocationData_2_0_SERIALIZATION_BUFFER_SIZE_BYTES_
+
+#define PNP_UNIQUE_ID_SIZE 16 // 128 bit unique id
+
+#define PNP_NODE_ID_NO_PREFERENCE CANARD_NODE_ID_MAX - 2 // No preference -> highest possible node ID, note: the two highest node-ID values are reserved for network maintenance tools
+
+/****************************************************************************
+ * private data
+ ****************************************************************************/
+
+CanardRxSubscription allocSub;
+
+// 128 bit unique id
+uint8_t local_unique_id[PNP_UNIQUE_ID_SIZE];
+
+const CanardNodeID preffered_node_id = PNP_NODE_ID_NO_PREFERENCE;
+
+CanardNodeID node_id = CANARD_NODE_ID_UNSET;
+
+CanardTransferID node_id_alloc_transfer_id = 0;
+
+/****************************************************************************
+ * private Functions declerations
+ ****************************************************************************/
+
+uint64_t makePseudoUniqueId(uint8_t *pUniqueID)
+{
+	// NuttX CRC64/WE implementation
+	return crc64(pUniqueID, PNP_UNIQUE_ID_SIZE);
+}
+
+
+/****************************************************************************
+ * public functions
+ ****************************************************************************/
+
+/* Rule A. On initialization:
+ * 1. The allocatee subscribes to this message.
+ * 2. The allocatee starts the Request Timer with a random interval [0, 1) sec of Trequest.
+ */
+
+uint32_t initPNPAllocatee(CanardInstance *ins, uint8_t *unique_id)
+{
+	// Store unique_id locally
+	memcpy(&local_unique_id[0], &unique_id[0], sizeof(local_unique_id));
+
+	// Create RX Subscriber so we can listen to NodeIDAllocationData msgs
+	(void) canardRxSubscribe(ins,   // Subscribe to messages uavcan.node.Heartbeat.
+				 CanardTransferKindMessage,
+				 (ins->mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PORT_ID : PNP1_PORT_ID),  // The fixed Subject-ID
+				 (ins->mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PAYLOAD_SIZE : PNP1_PAYLOAD_SIZE),
+				 CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+				 &allocSub);
+
+	// Callee should start a timer with a random interval between 0 and 1 sec
+}
+
+/* Rule B. On expiration of the Request Timer (started as per rules A, B, or C):
+ * 1. Request Timer restarts with a random interval [0, 1) sec of Trequest (chosen anew).
+ * 2. The allocatee broadcasts an allocation request message, where the fields are populated as follows:
+ *    unique_id_hash    - a 48-bit hash of the unique-ID of the allocatee.
+ *    allocated_node_id - empty (not populated).
+ */
+int32_t PNPAllocRequest(CanardInstance *ins)
+{
+	int32_t result;
+	CanardMicrosecond transmission_deadline = getMonotonicTimestampUSec() + 1000 * 10;
+
+	// Callee should restart timer
+
+	// Allocation already done, nothing to do TODO maybe stop subscribing
+	if (node_id != CANARD_NODE_ID_UNSET) {
+		return 1;
+	}
+
+	if (ins->mtu_bytes == CANARD_MTU_CAN_FD) {
+		// NodeIDAllocationData message
+		uavcan_pnp_NodeIDAllocationData_2_0 node_id_alloc_msg;
+		uint8_t node_id_alloc_payload_buffer[PNP2_PAYLOAD_SIZE];
+
+		memcpy(node_id_alloc_msg.unique_id, local_unique_id, sizeof(node_id_alloc_msg.unique_id));
+		node_id_alloc_msg.node_id.value = preffered_node_id;
+
+		CanardTransfer transfer = {
+			.timestamp_usec = transmission_deadline,      // Zero if transmission deadline is not limited.
+			.priority       = CanardPriorityNominal,
+			.transfer_kind  = CanardTransferKindMessage,
+			.port_id        = PNP2_PORT_ID,                // This is the subject-ID.
+			.remote_node_id = CANARD_NODE_ID_UNSET,       // Messages cannot be unicast, so use UNSET.
+			.transfer_id    = node_id_alloc_transfer_id,
+			.payload_size   = PNP2_PAYLOAD_SIZE,
+			.payload        = &node_id_alloc_payload_buffer,
+		};
+
+		result = uavcan_pnp_NodeIDAllocationData_2_0_serialize_(&node_id_alloc_msg, &node_id_alloc_payload_buffer,
+				&transfer.payload_size);
+
+		if (result == 0) {
+			// set the data ready in the buffer and chop if needed
+			++node_id_alloc_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
+			result = canardTxPush(ins, &transfer);
+		}
+
+	} else {
+		// NodeIDAllocationData message
+		uavcan_pnp_NodeIDAllocationData_1_0 node_id_alloc_msg;
+		uavcan_pnp_NodeIDAllocationData_1_0_initialize_(&node_id_alloc_msg);
+		uint8_t node_id_alloc_payload_buffer[PNP1_PAYLOAD_SIZE];
+
+		node_id_alloc_msg.unique_id_hash = (makePseudoUniqueId(local_unique_id) & 0xFFFFFFFFFFFF);
+
+		CanardTransfer transfer = {
+			.timestamp_usec = transmission_deadline,      // Zero if transmission deadline is not limited.
+			.priority       = CanardPriorityNominal,
+			.transfer_kind  = CanardTransferKindMessage,
+			.port_id        = PNP1_PORT_ID,                // This is the subject-ID.
+			.remote_node_id = CANARD_NODE_ID_UNSET,       // Messages cannot be unicast, so use UNSET.
+			.transfer_id    = node_id_alloc_transfer_id,
+			.payload_size   = PNP1_PAYLOAD_SIZE,
+			.payload        = &node_id_alloc_payload_buffer,
+		};
+
+		result = uavcan_pnp_NodeIDAllocationData_1_0_serialize_(&node_id_alloc_msg, &node_id_alloc_payload_buffer,
+				&transfer.payload_size);
+
+		if (result == 0) {
+			// set the data ready in the buffer and chop if needed
+			++node_id_alloc_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
+			result = canardTxPush(ins, &transfer);
+		}
+	}
+
+
+	if (result < 0) {
+		// An error has occurred: either an argument is invalid or we've ran out of memory.
+		// It is possible to statically prove that an out-of-memory will never occur for a given application if the
+		// heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap.
+		return result;
+	}
+
+	return 0;
+}
+
+/* Rule C. On any allocation message, even if other rules also match:
+ * 1. Request Timer restarts with a random interval of Trequest (chosen anew).
+ *
+ * Rule D. On an allocation message WHERE (source node-ID is non-anonymous, i.e., regular allocation response)
+ *                                  AND   (the field unique_id_hash matches the allocatee's 48-bit unique-ID hash)
+ *                                  AND   (the field allocated_node_id is populated):
+ * 1. Request Timer stops.
+ * 2. The allocatee initializes its node-ID with the received value.
+ * 3. The allocatee terminates its subscription to allocation messages.
+ * 4. Exit.
+ */
+int32_t PNPProcess(CanardInstance *ins, CanardTransfer *transfer)
+{
+
+	// Allocation already done, nothing to do
+	if (node_id != CANARD_NODE_ID_UNSET) {
+		return 1;
+	}
+
+	if (transfer->remote_node_id == CANARD_NODE_ID_UNSET) { // Another request, ignore.
+		return 0;
+	}
+
+	int32_t allocated = CANARD_NODE_ID_UNSET;
+
+	if (ins->mtu_bytes == CANARD_MTU_CAN_FD) {
+		uavcan_pnp_NodeIDAllocationData_2_0 msg;
+
+		size_t pnp_in_size_bits = transfer->payload_size;
+		uavcan_pnp_NodeIDAllocationData_2_0_deserialize_(&msg, transfer->payload, &pnp_in_size_bits);
+
+		if (memcmp(msg.unique_id, local_unique_id, sizeof(msg.unique_id)) == 0) {
+			allocated = msg.node_id.value;
+		}
+
+	} else {
+		uavcan_pnp_NodeIDAllocationData_1_0 msg;
+
+		size_t pnp_in_size_bits = transfer->payload_size;
+		uavcan_pnp_NodeIDAllocationData_1_0_deserialize_(&msg, transfer->payload, &pnp_in_size_bits);
+
+		if (msg.allocated_node_id.count > 0) {
+			if (msg.unique_id_hash == (makePseudoUniqueId(local_unique_id) & 0xFFFFFFFFFFFF)) {
+				allocated = msg.allocated_node_id.elements[0].value;
+			}
+		}
+	}
+
+	if (allocated == CANARD_NODE_ID_UNSET) {
+		return -1;        // UID mismatch.
+	}
+
+	if (allocated <= 0 || allocated >= CANARD_NODE_ID_MAX)
+		// Allocated node-ID ignored because it exceeds max_node_id
+	{
+		return -1;
+	}
+
+	// Plug-and-play allocation done: got node-ID %s from server %s', allocated, meta.source_node_id)
+	node_id = allocated;
+
+	return 1;
+}
+
+
+CanardNodeID PNPGetNodeID()
+{
+	return node_id;
+}
+
+
+CanardPortID PNPGetPortID(CanardInstance *ins)
+{
+	return (ins->mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PORT_ID : PNP1_PORT_ID);
+}
--- a/src/drivers/uavcannode_gps_demo/libcancl/pnp.h
+++ b/src/drivers/uavcannode_gps_demo/libcancl/pnp.h
@ -0,0 +1,74 @@
+/****************************************************************************
+ * nxp_bms/BMS_v1/inc/UAVCAN/pnp.h
+ *
+ * BSD 3-Clause License
+ *
+ * Copyright 2020 NXP
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, this
+ *    list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * 3. Neither the name of the copyright holder nor the names of its
+ *    contributors may be used to endorse or promote products derived from
+ *    this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ ** ###################################################################
+ **     Filename    : pnp.h
+ **     Project     : SmartBattery_RDDRONE_BMS772
+ **     Processor   : S32K144
+ **     Version     : 1.00
+ **     Date   		: 2020-10-29
+ **     Abstract    :
+ **        pnp module.
+ **
+ ** ###################################################################*/
+/*!
+ ** @file pnp.h
+ **
+ ** @version 01.00
+ **
+ ** @brief
+ **        pnp module. this module implements the UAVCAN plug and play protocol
+ **
+ */
+
+#ifndef UAVCAN_PNP_H_
+#define UAVCAN_PNP_H_
+
+#define NUNAVUT_ASSERT
+#include <canard.h>
+
+#include "time.h"
+
+
+uint32_t initPNPAllocatee(CanardInstance *ins, uint8_t *unique_id);
+
+int32_t PNPAllocRequest(CanardInstance *ins);
+
+int32_t PNPProcess(CanardInstance *ins, CanardTransfer *transfer);
+
+CanardNodeID PNPGetNodeID();
+
+const CanardPortID PNPGetPortID();
+
+
+
+#endif //UAVCAN_PNP_H_
--- a/src/drivers/uavcannode_gps_demo/libcancl/registerinterface.c
+++ b/src/drivers/uavcannode_gps_demo/libcancl/registerinterface.c
@ -0,0 +1,279 @@
+
+#include "registerinterface.h"
+
+#include "uavcan/_register/Access_1_0.h"
+#include "uavcan/_register/List_1_0.h"
+#include "uavcan/_register/Name_1_0.h"
+#include "uavcan/_register/Value_1_0.h"
+
+#include <stdio.h>
+#include <string.h>
+
+/****************************************************************************
+ * private data
+ ****************************************************************************/
+
+uavcan_node_GetInfo_Response_1_0 *node_info;
+CanardTransferID getinfo_response_transfer_id = 0;
+CanardTransferID register_access_response_transfer_id = 0;
+CanardTransferID register_list_response_transfer_id = 0;
+
+CanardRxSubscription getinfo_subscription;
+CanardRxSubscription register_access_subscription;
+CanardRxSubscription register_list_subscription;
+
+//TODO register list and data
+uavcan_register_interface_entry register_list[UAVCAN_REGISTER_COUNT];
+uint32_t register_list_size = 0;
+
+/****************************************************************************
+ * public functions
+ ****************************************************************************/
+
+int32_t uavcan_register_interface_init(CanardInstance *ins, uavcan_node_GetInfo_Response_1_0 *info)
+{
+	node_info = info; //TODO think about retention, copy isntead?
+
+	(void) canardRxSubscribe(ins,
+				 CanardTransferKindRequest,
+				 uavcan_node_GetInfo_1_0_FIXED_PORT_ID_,
+				 uavcan_node_GetInfo_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
+				 CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+				 &getinfo_subscription);
+
+	(void) canardRxSubscribe(ins,
+				 CanardTransferKindRequest,
+				 uavcan_register_Access_1_0_FIXED_PORT_ID_,
+				 uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
+				 CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+				 &register_access_subscription);
+
+	(void) canardRxSubscribe(ins,
+				 CanardTransferKindRequest,
+				 uavcan_register_List_1_0_FIXED_PORT_ID_,
+				 uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
+				 CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+				 &register_list_subscription);
+}
+
+int32_t uavcan_register_interface_add_entry(const char *name, register_access_set_callback cb_set,
+		register_access_get_callback cb_get)
+{
+	if (register_list_size < UAVCAN_REGISTER_COUNT) {
+		register_list[register_list_size].name = name;
+		register_list[register_list_size].cb_set = cb_set;
+		register_list[register_list_size].cb_get = cb_get;
+		register_list_size++;
+		return 0;
+
+	} else {
+		return -UAVCAN_REGISTER_ERROR_OUT_OF_MEMORY; // register list full
+	}
+
+}
+
+// Handler for all PortID registration related messages
+int32_t uavcan_register_interface_process(CanardInstance *ins, CanardTransfer *transfer)
+{
+	if (transfer->port_id == uavcan_node_GetInfo_1_0_FIXED_PORT_ID_) {
+		return uavcan_register_interface_get_info_response(ins, transfer);
+
+	} else if (transfer->port_id == uavcan_register_Access_1_0_FIXED_PORT_ID_) {
+		return uavcan_register_interface_access_response(ins, transfer);
+
+	} else if (transfer->port_id == uavcan_register_List_1_0_FIXED_PORT_ID_) {
+		return uavcan_register_interface_list_response(ins, transfer);
+	}
+
+	return 0; // Nothing to do
+}
+
+// Handler for node.GetInfo which yields a response
+int32_t uavcan_register_interface_get_info_response(CanardInstance *ins, CanardTransfer *request)
+{
+	uavcan_node_GetInfo_Request_1_0 msg;
+
+	size_t in_size_bits;
+
+	if (uavcan_node_GetInfo_Request_1_0_deserialize_(&msg, request->payload, request->payload_size) < 0) {
+		//Error deserialize failed
+		return -UAVCAN_REGISTER_ERROR_SERIALIZATION;
+	}
+
+	//Note so technically the uavcan_node_GetInfo_Request_1_0 is an empty message not sure if the code above is required
+
+	// Setup node.GetInfo response
+
+	uint8_t response_payload_buffer[uavcan_node_GetInfo_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
+
+	CanardMicrosecond transmission_deadline = getMonotonicTimestampUSec() + 1000 * 100;
+
+	CanardTransfer response = {
+		.timestamp_usec = transmission_deadline, // Zero if transmission deadline is not limited.
+		.priority       = CanardPriorityNominal,
+		.transfer_kind  = CanardTransferKindResponse,
+		.port_id        = uavcan_node_GetInfo_1_0_FIXED_PORT_ID_, // This is the subject-ID.
+		.remote_node_id = request->remote_node_id,       // Send back to request Node
+		.transfer_id    = getinfo_response_transfer_id,
+		.payload_size   = uavcan_node_GetInfo_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
+		.payload        = &response_payload_buffer,
+	};
+
+	int32_t result = uavcan_node_GetInfo_Response_1_0_serialize_(node_info, &response_payload_buffer,
+			 &response.payload_size);
+
+	if (result == 0) {
+		// set the data ready in the buffer and chop if needed
+		++getinfo_response_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
+		result = canardTxPush(ins, &response);
+	}
+
+	if (result < 0) {
+		// An error has occurred: either an argument is invalid or we've ran out of memory.
+		// It is possible to statically prove that an out-of-memory will never occur for a given application if the
+		// heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap.
+		return -UAVCAN_REGISTER_ERROR_SERIALIZATION;
+	}
+
+	return 1;
+
+}
+
+// Handler for register access interface
+int32_t uavcan_register_interface_access_response(CanardInstance *ins, CanardTransfer *request)
+{
+
+	int index;
+	{
+		uavcan_register_Access_Request_1_0 msg;
+
+		if (uavcan_register_Access_Request_1_0_deserialize_(&msg, request->payload, &request->payload_size) < 0) {
+			//Error deserialize failed
+			return -UAVCAN_REGISTER_ERROR_SERIALIZATION;
+		}
+
+		{
+			char register_string[255];
+			int reg_str_len;
+
+			for (index = 0; index < register_list_size; index++) {
+				reg_str_len = sprintf(register_string, "uavcan.pub.%s.id",
+						      register_list[index].name); //TODO more option then pub (sub rate etc)
+
+				if (strncmp(msg.name.name.elements, register_string, reg_str_len) == 0) {
+					if (msg.value._tag_ != 0) { // Value has been set thus we call set handler
+						if (register_list[index].cb_set(&msg.value) != 0) {
+							// TODO error ocurred check doc for correct response
+						}
+					}
+
+					break; // We're done exit loop
+				}
+			}
+		}
+	}
+
+	uavcan_register_Access_Response_1_0 response_msg;
+	uavcan_register_Access_Response_1_0_initialize_(&response_msg);
+
+	if (index < register_list_size)  { // Index is available
+		response_msg.value = register_list[index].cb_get();
+
+	} else { // Index not found return empty response
+		uavcan_register_Value_1_0_initialize_(&response_msg.value);
+		uavcan_register_Value_1_0_select_empty_(&response_msg.value);
+	}
+
+	uint8_t response_payload_buffer[uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
+
+	CanardMicrosecond transmission_deadline = getMonotonicTimestampUSec() + 1000 * 100;
+
+	CanardTransfer response = {
+		.timestamp_usec = transmission_deadline, // Zero if transmission deadline is not limited.
+		.priority       = CanardPriorityNominal,
+		.transfer_kind  = CanardTransferKindResponse,
+		.port_id        = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID.
+		.remote_node_id = request->remote_node_id,       // Send back to request Node
+		.transfer_id    = register_list_response_transfer_id,
+		.payload_size   = uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
+		.payload        = &response_payload_buffer,
+	};
+
+	int32_t result = uavcan_register_Access_Response_1_0_serialize_(&response_msg, &response_payload_buffer,
+			 &response.payload_size);
+
+	if (result == 0) {
+		// set the data ready in the buffer and chop if needed
+		++register_access_response_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
+		result = canardTxPush(ins, &response);
+	}
+
+	if (result < 0) {
+		// An error has occurred: either an argument is invalid or we've ran out of memory.
+		// It is possible to statically prove that an out-of-memory will never occur for a given application if the
+		// heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap.
+		return -UAVCAN_REGISTER_ERROR_SERIALIZATION;
+	}
+
+	return 1;
+}
+
+// Handler for register list interface
+int32_t uavcan_register_interface_list_response(CanardInstance *ins, CanardTransfer *request)
+{
+	uavcan_register_List_Request_1_0 msg;
+
+	if (uavcan_register_List_Request_1_0_deserialize_(&msg, request->payload, &request->payload_size) < 0) {
+		//Error deserialize failed
+		return -UAVCAN_REGISTER_ERROR_SERIALIZATION;
+	}
+
+	//Setup register response
+
+	uint8_t response_payload_buffer[uavcan_register_List_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; //TODO we know already how big our response is, don't overallocate.
+
+	CanardMicrosecond transmission_deadline = getMonotonicTimestampUSec() + 1000 * 100;
+
+	uavcan_register_List_Response_1_0 response_msg;
+
+	// Reponse magic start
+
+	if (msg.index <= register_list_size) {
+		response_msg.name.name.count = sprintf(response_msg.name.name.elements,
+						       "uavcan.pub.%s.id",
+						       register_list[msg.index].name);
+	}
+
+	//TODO more option then pub (sub rate
+
+	// Response magic end
+
+	CanardTransfer response = {
+		.timestamp_usec = transmission_deadline, // Zero if transmission deadline is not limited.
+		.priority       = CanardPriorityNominal,
+		.transfer_kind  = CanardTransferKindResponse,
+		.port_id        = uavcan_register_List_1_0_FIXED_PORT_ID_, // This is the subject-ID.
+		.remote_node_id = request->remote_node_id,       // Send back to request Node
+		.transfer_id    = register_list_response_transfer_id,
+		.payload_size   = uavcan_register_List_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, //See prev TODO
+		.payload        = &response_payload_buffer,
+	};
+
+	int32_t result = uavcan_register_List_Response_1_0_serialize_(&response_msg, &response_payload_buffer,
+			 &response.payload_size);
+
+	if (result == 0) {
+		// set the data ready in the buffer and chop if needed
+		++register_list_response_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
+		result = canardTxPush(ins, &response);
+	}
+
+	if (result < 0) {
+		// An error has occurred: either an argument is invalid or we've ran out of memory.
+		// It is possible to statically prove that an out-of-memory will never occur for a given application if the
+		// heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap.
+		return -UAVCAN_REGISTER_ERROR_SERIALIZATION;
+	}
+
+	return 1;
+}
--- a/src/drivers/uavcannode_gps_demo/libcancl/registerinterface.h
+++ b/src/drivers/uavcannode_gps_demo/libcancl/registerinterface.h
@ -0,0 +1,49 @@
+
+#ifndef UAVCAN_REGISTER_INTERFACE_H_
+#define UAVCAN_REGISTER_INTERFACE_H_
+
+#include <canard.h>
+
+#define NUNAVUT_ASSERT
+#include "uavcan/node/GetInfo_1_0.h"
+#include "uavcan/_register/Value_1_0.h"
+
+#include "time.h"
+
+//No of pre allocated register entries
+#ifndef UAVCAN_REGISTER_COUNT
+# define UAVCAN_REGISTER_COUNT 5
+#endif
+
+#define UAVCAN_REGISTER_ERROR_SERIALIZATION 1
+#define UAVCAN_REGISTER_ERROR_OUT_OF_MEMORY 2
+
+typedef int32_t (*register_access_set_callback)(uavcan_register_Value_1_0 *value);
+typedef uavcan_register_Value_1_0(*register_access_get_callback)(void);
+
+typedef struct {
+	/// uavcan.register.Name.1.0 name
+	const char *name;
+	register_access_set_callback cb_set;
+	register_access_get_callback cb_get;
+} uavcan_register_interface_entry;
+
+
+int32_t uavcan_register_interface_init(CanardInstance *ins, uavcan_node_GetInfo_Response_1_0 *info);
+
+int32_t uavcan_register_interface_add_entry(const char *name, register_access_set_callback cb_set,
+		register_access_get_callback cb_get);
+
+// Handler for all PortID registration related messages
+int32_t uavcan_register_interface_process(CanardInstance *ins, CanardTransfer *transfer);
+
+// Handler for node.GetInfo which yields a response
+int32_t uavcan_register_interface_get_info_response(CanardInstance *ins, CanardTransfer *request);
+
+// Handler for register access interface
+int32_t uavcan_register_interface_access_response(CanardInstance *ins, CanardTransfer *request);
+
+// Handler for register list interface
+int32_t uavcan_register_interface_list_response(CanardInstance *ins, CanardTransfer *request);
+
+#endif //UAVCAN_REGISTER_INTERFACE_H_
--- a/src/drivers/uavcannode_gps_demo/libcancl/time.h
+++ b/src/drivers/uavcannode_gps_demo/libcancl/time.h
@ -0,0 +1,6 @@
+#ifndef MAVCAN_TIME_H_
+#define MAVCAN_TIME_H_
+
+uint64_t getMonotonicTimestampUSec(void);
+
+#endif //MAVCAN_TIME_H_
--- a/src/drivers/uavcannode_gps_demo/o1heap.c
+++ b/src/drivers/uavcannode_gps_demo/o1heap.c
@ -0,0 +1,498 @@
+// Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated
+// documentation files (the "Software"), to deal in the Software without restriction, including without limitation
+// the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software,
+// and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in all copies or substantial portions
+// of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
+// WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS
+// OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
+// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+//
+// Copyright (c) 2020 Pavel Kirienko
+// Authors: Pavel Kirienko <pavel.kirienko@zubax.com>
+
+#include "o1heap.h"
+#include <assert.h>
+
+// ---------------------------------------- BUILD CONFIGURATION OPTIONS ----------------------------------------
+
+/// The assertion macro defaults to the standard assert().
+/// It can be overridden to manually suppress assertion checks or use a different error handling policy.
+#ifndef O1HEAP_ASSERT
+// Intentional violation of MISRA: the assertion check macro cannot be replaced with a function definition.
+#    define O1HEAP_ASSERT(x) assert(x)  // NOSONAR
+#endif
+
+/// Branch probability annotations are used to improve the worst case execution time (WCET). They are entirely optional.
+/// A stock implementation is provided for some well-known compilers; for other compilers it defaults to nothing.
+/// If you are using a different compiler, consider overriding this value.
+#ifndef O1HEAP_LIKELY
+#    if defined(__GNUC__) || defined(__clang__) || defined(__CC_ARM)
+// Intentional violation of MISRA: branch hinting macro cannot be replaced with a function definition.
+#        define O1HEAP_LIKELY(x) __builtin_expect((x), 1)  // NOSONAR
+#    else
+#        define O1HEAP_LIKELY(x) x
+#    endif
+#endif
+
+/// This option is used for testing only. Do not use in production.
+#if defined(O1HEAP_EXPOSE_INTERNALS) && O1HEAP_EXPOSE_INTERNALS
+#    define O1HEAP_PRIVATE
+#else
+#    define O1HEAP_PRIVATE static inline
+#endif
+
+// ---------------------------------------- INTERNAL DEFINITIONS ----------------------------------------
+
+#if !defined(__STDC_VERSION__) || (__STDC_VERSION__ < 199901L)
+#    error "Unsupported language: ISO C99 or a newer version is required."
+#endif
+
+#if __STDC_VERSION__ < 201112L
+// Intentional violation of MISRA: static assertion macro cannot be replaced with a function definition.
+#    define static_assert(x, ...) typedef char _static_assert_gl(_static_assertion_, __LINE__)[(x) ? 1 : -1]  // NOSONAR
+#    define _static_assert_gl(a, b) _static_assert_gl_impl(a, b)                                              // NOSONAR
+// Intentional violation of MISRA: the paste operator ## cannot be avoided in this context.
+#    define _static_assert_gl_impl(a, b) a##b  // NOSONAR
+#endif
+
+/// The overhead is at most O1HEAP_ALIGNMENT bytes large,
+/// then follows the user data which shall keep the next fragment aligned.
+#define FRAGMENT_SIZE_MIN (O1HEAP_ALIGNMENT * 2U)
+
+/// This is risky, handle with care: if the allocation amount plus per-fragment overhead exceeds 2**(b-1),
+/// where b is the pointer bit width, then ceil(log2(amount)) yields b; then 2**b causes an integer overflow.
+/// To avoid this, we put a hard limit on fragment size (which is amount + per-fragment overhead): 2**(b-1)
+#define FRAGMENT_SIZE_MAX ((SIZE_MAX >> 1U) + 1U)
+
+/// Normally we should subtract log2(FRAGMENT_SIZE_MIN) but log2 is bulky to compute using the preprocessor only.
+/// We will certainly end up with unused bins this way, but it is cheap to ignore.
+#define NUM_BINS_MAX (sizeof(size_t) * 8U)
+
+static_assert((O1HEAP_ALIGNMENT & (O1HEAP_ALIGNMENT - 1U)) == 0U, "Not a power of 2");
+static_assert((FRAGMENT_SIZE_MIN & (FRAGMENT_SIZE_MIN - 1U)) == 0U, "Not a power of 2");
+static_assert((FRAGMENT_SIZE_MAX & (FRAGMENT_SIZE_MAX - 1U)) == 0U, "Not a power of 2");
+
+typedef struct Fragment Fragment;
+
+typedef struct FragmentHeader {
+	Fragment *next;
+	Fragment *prev;
+	size_t    size;
+	bool      used;
+} FragmentHeader;
+static_assert(sizeof(FragmentHeader) <= O1HEAP_ALIGNMENT, "Memory layout error");
+
+struct Fragment {
+	FragmentHeader header;
+	// Everything past the header may spill over into the allocatable space. The header survives across alloc/free.
+	Fragment *next_free;  // Next free fragment in the bin; NULL in the last one.
+	Fragment *prev_free;  // Same but points back; NULL in the first one.
+};
+static_assert(sizeof(Fragment) <= FRAGMENT_SIZE_MIN, "Memory layout error");
+
+struct O1HeapInstance {
+	Fragment *bins[NUM_BINS_MAX];  ///< Smallest fragments are in the bin at index 0.
+	size_t    nonempty_bin_mask;   ///< Bit 1 represents a non-empty bin; bin at index 0 is for the smallest fragments.
+
+	O1HeapHook critical_section_enter;
+	O1HeapHook critical_section_leave;
+
+	O1HeapDiagnostics diagnostics;
+};
+
+/// The amount of space allocated for the heap instance.
+/// Its size is padded up to O1HEAP_ALIGNMENT to ensure correct alignment of the allocation arena that follows.
+#define INSTANCE_SIZE_PADDED ((sizeof(O1HeapInstance) + O1HEAP_ALIGNMENT - 1U) & ~(O1HEAP_ALIGNMENT - 1U))
+
+static_assert(INSTANCE_SIZE_PADDED >= sizeof(O1HeapInstance), "Invalid instance footprint computation");
+static_assert((INSTANCE_SIZE_PADDED % O1HEAP_ALIGNMENT) == 0U, "Invalid instance footprint computation");
+
+/// True if the argument is an integer power of two or zero.
+O1HEAP_PRIVATE bool isPowerOf2(const size_t x);
+O1HEAP_PRIVATE bool isPowerOf2(const size_t x)
+{
+	return (x & (x - 1U)) == 0U;
+}
+
+/// Special case: if the argument is zero, returns zero.
+O1HEAP_PRIVATE uint8_t log2Floor(const size_t x);
+O1HEAP_PRIVATE uint8_t log2Floor(const size_t x)
+{
+	size_t  tmp = x;
+	uint8_t y   = 0;
+
+	// This is currently the only exception to the statement "routines contain neither loops nor recursion".
+	// It is unclear if there is a better way to compute the binary logarithm than this.
+	while (tmp > 1U) {
+		tmp >>= 1U;
+		y++;
+	}
+
+	return y;
+}
+
+/// Special case: if the argument is zero, returns zero.
+O1HEAP_PRIVATE uint8_t log2Ceil(const size_t x);
+O1HEAP_PRIVATE uint8_t log2Ceil(const size_t x)
+{
+	return (uint8_t)(log2Floor(x) + (isPowerOf2(x) ? 0U : 1U));
+}
+
+/// Raise 2 into the specified power.
+/// You might be tempted to do something like (1U << power). WRONG! We humans are prone to forgetting things.
+/// If you forget to cast your 1U to size_t or ULL, you may end up with undefined behavior.
+O1HEAP_PRIVATE size_t pow2(const uint8_t power);
+O1HEAP_PRIVATE size_t pow2(const uint8_t power)
+{
+	return ((size_t) 1U) << power;
+}
+
+O1HEAP_PRIVATE void invoke(const O1HeapHook hook);
+O1HEAP_PRIVATE void invoke(const O1HeapHook hook)
+{
+	if (hook != NULL) {
+		hook();
+	}
+}
+
+/// Links two fragments so that their next/prev pointers point to each other; left goes before right.
+O1HEAP_PRIVATE void interlink(Fragment *const left, Fragment *const right);
+O1HEAP_PRIVATE void interlink(Fragment *const left, Fragment *const right)
+{
+	if (O1HEAP_LIKELY(left != NULL)) {
+		left->header.next = right;
+	}
+
+	if (O1HEAP_LIKELY(right != NULL)) {
+		right->header.prev = left;
+	}
+}
+
+/// Adds a new block into the appropriate bin and updates the lookup mask.
+O1HEAP_PRIVATE void rebin(O1HeapInstance *const handle, Fragment *const fragment);
+O1HEAP_PRIVATE void rebin(O1HeapInstance *const handle, Fragment *const fragment)
+{
+	O1HEAP_ASSERT(handle != NULL);
+	O1HEAP_ASSERT(fragment != NULL);
+	O1HEAP_ASSERT(fragment->header.size >= FRAGMENT_SIZE_MIN);
+	O1HEAP_ASSERT((fragment->header.size % FRAGMENT_SIZE_MIN) == 0U);
+	const uint8_t idx = log2Floor(fragment->header.size / FRAGMENT_SIZE_MIN);  // Round DOWN when inserting.
+	O1HEAP_ASSERT(idx < NUM_BINS_MAX);
+	// Add the new fragment to the beginning of the bin list.
+	// I.e., each allocation will be returning the least-recently-used fragment -- good for caching.
+	fragment->next_free = handle->bins[idx];
+	fragment->prev_free = NULL;
+
+	if (O1HEAP_LIKELY(handle->bins[idx] != NULL)) {
+		handle->bins[idx]->prev_free = fragment;
+	}
+
+	handle->bins[idx] = fragment;
+	handle->nonempty_bin_mask |= pow2(idx);
+}
+
+/// Removes the specified block from its bin.
+O1HEAP_PRIVATE void unbin(O1HeapInstance *const handle, const Fragment *const fragment);
+O1HEAP_PRIVATE void unbin(O1HeapInstance *const handle, const Fragment *const fragment)
+{
+	O1HEAP_ASSERT(handle != NULL);
+	O1HEAP_ASSERT(fragment != NULL);
+	O1HEAP_ASSERT(fragment->header.size >= FRAGMENT_SIZE_MIN);
+	O1HEAP_ASSERT((fragment->header.size % FRAGMENT_SIZE_MIN) == 0U);
+	const uint8_t idx = log2Floor(fragment->header.size / FRAGMENT_SIZE_MIN);  // Round DOWN when removing.
+	O1HEAP_ASSERT(idx < NUM_BINS_MAX);
+
+	// Remove the bin from the free fragment list.
+	if (O1HEAP_LIKELY(fragment->next_free != NULL)) {
+		fragment->next_free->prev_free = fragment->prev_free;
+	}
+
+	if (O1HEAP_LIKELY(fragment->prev_free != NULL)) {
+		fragment->prev_free->next_free = fragment->next_free;
+	}
+
+	// Update the bin header.
+	if (O1HEAP_LIKELY(handle->bins[idx] == fragment)) {
+		O1HEAP_ASSERT(fragment->prev_free == NULL);
+		handle->bins[idx] = fragment->next_free;
+
+		if (O1HEAP_LIKELY(handle->bins[idx] == NULL)) {
+			handle->nonempty_bin_mask &= ~pow2(idx);
+		}
+	}
+}
+
+// ---------------------------------------- PUBLIC API IMPLEMENTATION ----------------------------------------
+
+O1HeapInstance *o1heapInit(void *const      base,
+			   const size_t     size,
+			   const O1HeapHook critical_section_enter,
+			   const O1HeapHook critical_section_leave)
+{
+	O1HeapInstance *out = NULL;
+
+	if ((base != NULL) && ((((size_t) base) % O1HEAP_ALIGNMENT) == 0U) &&
+	    (size >= (INSTANCE_SIZE_PADDED + FRAGMENT_SIZE_MIN))) {
+		// Allocate the core heap metadata structure in the beginning of the arena.
+		O1HEAP_ASSERT(((size_t) base) % sizeof(O1HeapInstance *) == 0U);
+		out                         = (O1HeapInstance *) base;
+		out->nonempty_bin_mask      = 0U;
+		out->critical_section_enter = critical_section_enter;
+		out->critical_section_leave = critical_section_leave;
+
+		for (size_t i = 0; i < NUM_BINS_MAX; i++) {
+			out->bins[i] = NULL;
+		}
+
+		// Limit and align the capacity.
+		size_t capacity = size - INSTANCE_SIZE_PADDED;
+
+		if (capacity > FRAGMENT_SIZE_MAX) {
+			capacity = FRAGMENT_SIZE_MAX;
+		}
+
+		while ((capacity % FRAGMENT_SIZE_MIN) != 0) {
+			O1HEAP_ASSERT(capacity > 0U);
+			capacity--;
+		}
+
+		O1HEAP_ASSERT((capacity % FRAGMENT_SIZE_MIN) == 0);
+		O1HEAP_ASSERT((capacity >= FRAGMENT_SIZE_MIN) && (capacity <= FRAGMENT_SIZE_MAX));
+
+		// Initialize the root fragment.
+		Fragment *const frag = (Fragment *)(void *)(((uint8_t *) base) + INSTANCE_SIZE_PADDED);
+		O1HEAP_ASSERT((((size_t) frag) % O1HEAP_ALIGNMENT) == 0U);
+		frag->header.next = NULL;
+		frag->header.prev = NULL;
+		frag->header.size = capacity;
+		frag->header.used = false;
+		frag->next_free   = NULL;
+		frag->prev_free   = NULL;
+		rebin(out, frag);
+		O1HEAP_ASSERT(out->nonempty_bin_mask != 0U);
+
+		// Initialize the diagnostics.
+		out->diagnostics.capacity          = capacity;
+		out->diagnostics.allocated         = 0U;
+		out->diagnostics.peak_allocated    = 0U;
+		out->diagnostics.peak_request_size = 0U;
+		out->diagnostics.oom_count         = 0U;
+	}
+
+	return out;
+}
+
+void *o1heapAllocate(O1HeapInstance *const handle, const size_t amount)
+{
+	O1HEAP_ASSERT(handle != NULL);
+	O1HEAP_ASSERT(handle->diagnostics.capacity <= FRAGMENT_SIZE_MAX);
+	void *out = NULL;
+
+	// If the amount approaches approx. SIZE_MAX/2, an undetected integer overflow may occur.
+	// To avoid that, we do not attempt allocation if the amount exceeds the hard limit.
+	// We perform multiple redundant checks to account for a possible unaccounted overflow.
+	if (O1HEAP_LIKELY((amount > 0U) && (amount <= (handle->diagnostics.capacity - O1HEAP_ALIGNMENT)))) {
+		// Add the header size and align the allocation size to the power of 2.
+		// See "Timing-Predictable Memory Allocation In Hard Real-Time Systems", Herter, page 27.
+		const size_t fragment_size = pow2(log2Ceil(amount + O1HEAP_ALIGNMENT));
+		O1HEAP_ASSERT(fragment_size <= FRAGMENT_SIZE_MAX);
+		O1HEAP_ASSERT(fragment_size >= FRAGMENT_SIZE_MIN);
+		O1HEAP_ASSERT(fragment_size >= amount + O1HEAP_ALIGNMENT);
+		O1HEAP_ASSERT(isPowerOf2(fragment_size));
+
+		const uint8_t optimal_bin_index = log2Ceil(fragment_size / FRAGMENT_SIZE_MIN);  // Use CEIL when fetching.
+		O1HEAP_ASSERT(optimal_bin_index < NUM_BINS_MAX);
+		const size_t candidate_bin_mask = ~(pow2(optimal_bin_index) - 1U);
+
+		invoke(handle->critical_section_enter);
+
+		// Find the smallest non-empty bin we can use.
+		const size_t suitable_bins     = handle->nonempty_bin_mask & candidate_bin_mask;
+		const size_t smallest_bin_mask = suitable_bins & ~(suitable_bins - 1U);  // Clear all bits but the lowest.
+
+		if (O1HEAP_LIKELY(smallest_bin_mask != 0)) {
+			O1HEAP_ASSERT(isPowerOf2(smallest_bin_mask));
+			const uint8_t bin_index = log2Floor(smallest_bin_mask);
+			O1HEAP_ASSERT(bin_index >= optimal_bin_index);
+			O1HEAP_ASSERT(bin_index < NUM_BINS_MAX);
+
+			// The bin we found shall not be empty, otherwise it's a state divergence (memory corruption?).
+			Fragment *const frag = handle->bins[bin_index];
+			O1HEAP_ASSERT(frag != NULL);
+			O1HEAP_ASSERT(frag->header.size >= fragment_size);
+			O1HEAP_ASSERT((frag->header.size % FRAGMENT_SIZE_MIN) == 0U);
+			O1HEAP_ASSERT(!frag->header.used);
+			unbin(handle, frag);
+
+			// Split the fragment if it is too large.
+			const size_t leftover = frag->header.size - fragment_size;
+			frag->header.size     = fragment_size;
+			O1HEAP_ASSERT(leftover < handle->diagnostics.capacity);  // Overflow check.
+			O1HEAP_ASSERT(leftover % FRAGMENT_SIZE_MIN == 0U);       // Alignment check.
+
+			if (O1HEAP_LIKELY(leftover >= FRAGMENT_SIZE_MIN)) {
+				Fragment *const new_frag = (Fragment *)(void *)(((uint8_t *) frag) + fragment_size);
+				O1HEAP_ASSERT(((size_t) new_frag) % O1HEAP_ALIGNMENT == 0U);
+				new_frag->header.size = leftover;
+				new_frag->header.used = false;
+				interlink(new_frag, frag->header.next);
+				interlink(frag, new_frag);
+				rebin(handle, new_frag);
+			}
+
+			// Update the diagnostics.
+			O1HEAP_ASSERT((handle->diagnostics.allocated % FRAGMENT_SIZE_MIN) == 0U);
+			handle->diagnostics.allocated += fragment_size;
+			O1HEAP_ASSERT(handle->diagnostics.allocated <= handle->diagnostics.capacity);
+
+			if (O1HEAP_LIKELY(handle->diagnostics.peak_allocated < handle->diagnostics.allocated)) {
+				handle->diagnostics.peak_allocated = handle->diagnostics.allocated;
+			}
+
+			// Finalize the fragment we just allocated.
+			O1HEAP_ASSERT(frag->header.size >= amount + O1HEAP_ALIGNMENT);
+			frag->header.used = true;
+
+			out = ((uint8_t *) frag) + O1HEAP_ALIGNMENT;
+		}
+
+	} else {
+		invoke(handle->critical_section_enter);
+	}
+
+	// Update the diagnostics.
+	if (O1HEAP_LIKELY(handle->diagnostics.peak_request_size < amount)) {
+		handle->diagnostics.peak_request_size = amount;
+	}
+
+	if (O1HEAP_LIKELY((out == NULL) && (amount > 0U))) {
+		handle->diagnostics.oom_count++;
+	}
+
+	invoke(handle->critical_section_leave);
+	return out;
+}
+
+void o1heapFree(O1HeapInstance *const handle, void *const pointer)
+{
+	O1HEAP_ASSERT(handle != NULL);
+	O1HEAP_ASSERT(handle->diagnostics.capacity <= FRAGMENT_SIZE_MAX);
+
+	if (O1HEAP_LIKELY(pointer != NULL)) { // NULL pointer is a no-op.
+		Fragment *const frag = (Fragment *)(void *)(((uint8_t *) pointer) - O1HEAP_ALIGNMENT);
+
+		// Check for heap corruption in debug builds.
+		O1HEAP_ASSERT(((size_t) frag) % sizeof(Fragment *) == 0U);
+		O1HEAP_ASSERT(((size_t) frag) >= (((size_t) handle) + INSTANCE_SIZE_PADDED));
+		O1HEAP_ASSERT(((size_t) frag) <=
+			      (((size_t) handle) + INSTANCE_SIZE_PADDED + handle->diagnostics.capacity - FRAGMENT_SIZE_MIN));
+		O1HEAP_ASSERT(frag->header.used);  // Catch double-free
+		O1HEAP_ASSERT(((size_t) frag->header.next) % sizeof(Fragment *) == 0U);
+		O1HEAP_ASSERT(((size_t) frag->header.prev) % sizeof(Fragment *) == 0U);
+		O1HEAP_ASSERT(frag->header.size >= FRAGMENT_SIZE_MIN);
+		O1HEAP_ASSERT(frag->header.size <= handle->diagnostics.capacity);
+		O1HEAP_ASSERT((frag->header.size % FRAGMENT_SIZE_MIN) == 0U);
+
+		invoke(handle->critical_section_enter);
+
+		// Even if we're going to drop the fragment later, mark it free anyway to prevent double-free.
+		frag->header.used = false;
+
+		// Update the diagnostics. It must be done before merging because it invalidates the fragment size information.
+		O1HEAP_ASSERT(handle->diagnostics.allocated >= frag->header.size);  // Heap corruption check.
+		handle->diagnostics.allocated -= frag->header.size;
+
+		// Merge with siblings and insert the returned fragment into the appropriate bin and update metadata.
+		Fragment *const prev       = frag->header.prev;
+		Fragment *const next       = frag->header.next;
+		const bool      join_left  = (prev != NULL) && (!prev->header.used);
+		const bool      join_right = (next != NULL) && (!next->header.used);
+
+		if (join_left && join_right) { // [ prev ][ this ][ next ] => [ ------- prev ------- ]
+			unbin(handle, prev);
+			unbin(handle, next);
+			prev->header.size += frag->header.size + next->header.size;
+			frag->header.size = 0;  // Invalidate the dropped fragment headers to prevent double-free.
+			next->header.size = 0;
+			O1HEAP_ASSERT((prev->header.size % FRAGMENT_SIZE_MIN) == 0U);
+			interlink(prev, next->header.next);
+			rebin(handle, prev);
+
+		} else if (join_left) { // [ prev ][ this ][ next ] => [ --- prev --- ][ next ]
+			unbin(handle, prev);
+			prev->header.size += frag->header.size;
+			frag->header.size = 0;
+			O1HEAP_ASSERT((prev->header.size % FRAGMENT_SIZE_MIN) == 0U);
+			interlink(prev, next);
+			rebin(handle, prev);
+
+		} else if (join_right) { // [ prev ][ this ][ next ] => [ prev ][ --- this --- ]
+			unbin(handle, next);
+			frag->header.size += next->header.size;
+			next->header.size = 0;
+			O1HEAP_ASSERT((frag->header.size % FRAGMENT_SIZE_MIN) == 0U);
+			interlink(frag, next->header.next);
+			rebin(handle, frag);
+
+		} else {
+			rebin(handle, frag);
+		}
+
+		invoke(handle->critical_section_leave);
+	}
+}
+
+bool o1heapDoInvariantsHold(const O1HeapInstance *const handle)
+{
+	O1HEAP_ASSERT(handle != NULL);
+	bool valid = true;
+
+	invoke(handle->critical_section_enter);
+
+	// Check the bin mask consistency.
+	for (size_t i = 0; i < NUM_BINS_MAX; i++) { // Dear compiler, feel free to unroll this loop.
+		const bool mask_bit_set = (handle->nonempty_bin_mask & pow2((uint8_t) i)) != 0U;
+		const bool bin_nonempty = handle->bins[i] != NULL;
+		valid                   = valid && (mask_bit_set == bin_nonempty);
+	}
+
+	// Create a local copy of the diagnostics struct to check later and release the critical section early.
+	const O1HeapDiagnostics diag = handle->diagnostics;
+
+	invoke(handle->critical_section_leave);
+
+	// Capacity check.
+	valid = valid && (diag.capacity <= FRAGMENT_SIZE_MAX) && (diag.capacity >= FRAGMENT_SIZE_MIN) &&
+		((diag.capacity % FRAGMENT_SIZE_MIN) == 0U);
+
+	// Allocation info check.
+	valid = valid && (diag.allocated <= diag.capacity) && ((diag.allocated % FRAGMENT_SIZE_MIN) == 0U) &&
+		(diag.peak_allocated <= diag.capacity) && (diag.peak_allocated >= diag.allocated) &&
+		((diag.peak_allocated % FRAGMENT_SIZE_MIN) == 0U);
+
+	// Peak request check
+	valid = valid && ((diag.peak_request_size < diag.capacity) || (diag.oom_count > 0U));
+
+	if (diag.peak_request_size == 0U) {
+		valid = valid && (diag.peak_allocated == 0U) && (diag.allocated == 0U) && (diag.oom_count == 0U);
+
+	} else {
+		valid = valid &&  // Overflow on summation is possible but safe to ignore.
+			(((diag.peak_request_size + O1HEAP_ALIGNMENT) <= diag.peak_allocated) || (diag.oom_count > 0U));
+	}
+
+	return valid;
+}
+
+O1HeapDiagnostics o1heapGetDiagnostics(const O1HeapInstance *const handle)
+{
+	O1HEAP_ASSERT(handle != NULL);
+	invoke(handle->critical_section_enter);
+	const O1HeapDiagnostics out = handle->diagnostics;
+	invoke(handle->critical_section_leave);
+	return out;
+}
--- a/src/drivers/uavcannode_gps_demo/o1heap.h
+++ b/src/drivers/uavcannode_gps_demo/o1heap.h
@ -0,0 +1,142 @@
+// Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated
+// documentation files (the "Software"), to deal in the Software without restriction, including without limitation
+// the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software,
+// and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in all copies or substantial portions
+// of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
+// WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS
+// OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
+// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+//
+// Copyright (c) 2020 Pavel Kirienko
+// Authors: Pavel Kirienko <pavel.kirienko@zubax.com>
+//
+// READ THE DOCUMENTATION IN README.md.
+
+#ifndef O1HEAP_H_INCLUDED
+#define O1HEAP_H_INCLUDED
+
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/// The semantic version number of this distribution.
+#define O1HEAP_VERSION_MAJOR 1
+
+/// The guaranteed alignment depends on the platform pointer width.
+#define O1HEAP_ALIGNMENT (sizeof(void*) * 4U)
+
+/// The definition is private, so the user code can only operate on pointers. This is done to enforce encapsulation.
+typedef struct O1HeapInstance O1HeapInstance;
+
+/// A hook function invoked by the allocator. NULL hooks are silently not invoked (not an error).
+typedef void (*O1HeapHook)(void);
+
+/// Runtime diagnostic information. This information can be used to facilitate runtime self-testing,
+/// as required by certain safety-critical development guidelines.
+/// If assertion checks are not disabled, the library will perform automatic runtime self-diagnostics that trigger
+/// an assertion failure if a heap corruption is detected.
+/// Health checks and validation can be done with @ref o1heapDoInvariantsHold().
+typedef struct {
+	/// The total amount of memory available for serving allocation requests (heap size).
+	/// The maximum allocation size is (capacity - O1HEAP_ALIGNMENT).
+	/// This parameter does not include the overhead used up by @ref O1HeapInstance and arena alignment.
+	/// This parameter is constant.
+	size_t capacity;
+
+	/// The amount of memory that is currently allocated, including the per-fragment overhead and size alignment.
+	/// For example, if the application requested a fragment of size 1 byte, the value reported here may be 32 bytes.
+	size_t allocated;
+
+	/// The maximum value of 'allocated' seen since initialization. This parameter is never decreased.
+	size_t peak_allocated;
+
+	/// The largest amount of memory that the allocator has attempted to allocate (perhaps unsuccessfully)
+	/// since initialization (not including the rounding and the allocator's own per-fragment overhead,
+	/// so the total is larger). This parameter is never decreased. The initial value is zero.
+	size_t peak_request_size;
+
+	/// The number of times an allocation request could not be completed due to the lack of memory or
+	/// excessive fragmentation. OOM stands for "out of memory". This parameter is never decreased.
+	uint64_t oom_count;
+} O1HeapDiagnostics;
+
+/// The arena base pointer shall be aligned at @ref O1HEAP_ALIGNMENT, otherwise NULL is returned.
+///
+/// The total heap capacity cannot exceed approx. (SIZE_MAX/2). If the arena size allows for a larger heap,
+/// the excess will be silently truncated away (no error). This is not a realistic use case because a typical
+/// application is unlikely to be able to dedicate that much of the address space for the heap.
+///
+/// The critical section enter/leave callbacks will be invoked when the allocator performs an atomic transaction.
+/// There is at most one atomic transaction per allocation/deallocation.
+/// Either or both of the callbacks may be NULL if locking is not needed (i.e., the heap is not shared).
+/// It is guaranteed that a critical section will never be entered recursively.
+/// It is guaranteed that 'enter' is invoked the same number of times as 'leave', unless either of them are NULL.
+/// It is guaranteed that 'enter' is invoked before 'leave', unless either of them are NULL.
+/// The callbacks are never invoked from the initialization function itself.
+///
+/// The function initializes a new heap instance allocated in the provided arena, taking some of its space for its
+/// own needs (normally about 40..600 bytes depending on the architecture, but this parameter is not characterized).
+/// A pointer to the newly initialized instance is returned.
+///
+/// If the provided space is insufficient, NULL is returned.
+///
+/// An initialized instance does not hold any resources. Therefore, if the instance is no longer needed,
+/// it can be discarded without any de-initialization procedures.
+///
+/// The time complexity is unspecified.
+O1HeapInstance *o1heapInit(void *const      base,
+			   const size_t     size,
+			   const O1HeapHook critical_section_enter,
+			   const O1HeapHook critical_section_leave);
+
+/// The semantics follows malloc() with additional guarantees the full list of which is provided below.
+///
+/// If the allocation request is served successfully, a pointer to the newly allocated memory fragment is returned.
+/// The returned pointer is guaranteed to be aligned at @ref O1HEAP_ALIGNMENT.
+///
+/// If the allocation request cannot be served due to the lack of memory or its excessive fragmentation,
+/// a NULL pointer is returned.
+///
+/// The function is executed in constant time (unless the critical section management hooks are used and are not
+/// constant-time). The allocated memory is NOT zero-filled (because zero-filling is a variable-complexity operation).
+///
+/// The function may invoke critical_section_enter and critical_section_leave at most once each (NULL hooks ignored).
+void *o1heapAllocate(O1HeapInstance *const handle, const size_t amount);
+
+/// The semantics follows free() with additional guarantees the full list of which is provided below.
+///
+/// If the pointer does not point to a previously allocated block and is not NULL, the behavior is undefined.
+/// Builds where assertion checks are enabled may trigger an assertion failure for some invalid inputs.
+///
+/// The function is executed in constant time (unless the critical section management hooks are used and are not
+/// constant-time).
+///
+/// The function may invoke critical_section_enter and critical_section_leave at most once each (NULL hooks ignored).
+void o1heapFree(O1HeapInstance *const handle, void *const pointer);
+
+/// Performs a basic sanity check on the heap.
+/// This function can be used as a weak but fast method of heap corruption detection.
+/// It invokes critical_section_enter once (unless NULL) and then critical_section_leave once (unless NULL).
+/// If the handle pointer is NULL, the behavior is undefined.
+/// The time complexity is constant.
+/// The return value is truth if the heap looks valid, falsity otherwise.
+bool o1heapDoInvariantsHold(const O1HeapInstance *const handle);
+
+/// Samples and returns a copy of the diagnostic information, see @ref O1HeapDiagnostics.
+/// This function merely copies the structure from an internal storage, so it is fast to return.
+/// It invokes critical_section_enter once (unless NULL) and then critical_section_leave once (unless NULL).
+/// If the handle pointer is NULL, the behavior is undefined.
+O1HeapDiagnostics o1heapGetDiagnostics(const O1HeapInstance *const handle);
+
+#ifdef __cplusplus
+}
+#endif
+#endif  // O1HEAP_H_INCLUDED
--- a/src/drivers/uavcannode_gps_demo/public_regulated_data_types
+++ b/src/drivers/uavcannode_gps_demo/public_regulated_data_types
@ -0,0 +1 @@
+Subproject commit 7f5489e6e916cc8b13db0582dcf9930e225594b9
--- a/src/drivers/uavcannode_gps_demo/socketcan.c
+++ b/src/drivers/uavcannode_gps_demo/socketcan.c
@ -0,0 +1,171 @@
+#include "socketcan.h"
+
+#include <net/if.h>
+#include <sys/ioctl.h>
+#include <string.h>
+
+int16_t socketcanOpen(CanardSocketInstance *ins, const char *const can_iface_name, const bool can_fd)
+{
+	struct sockaddr_can addr;
+	struct ifreq ifr;
+
+	ins->can_fd = can_fd;
+
+	/* open socket */
+	if ((ins->s = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
+		perror("socket");
+		return -1;
+	}
+
+	strncpy(ifr.ifr_name, can_iface_name, IFNAMSIZ - 1);
+	ifr.ifr_name[IFNAMSIZ - 1] = '\0';
+	ifr.ifr_ifindex = if_nametoindex(ifr.ifr_name);
+
+	if (!ifr.ifr_ifindex) {
+		perror("if_nametoindex");
+		return -1;
+	}
+
+	memset(&addr, 0, sizeof(addr));
+	addr.can_family = AF_CAN;
+	addr.can_ifindex = ifr.ifr_ifindex;
+
+	const int on = 1;
+	/* RX Timestamping */
+
+	if (setsockopt(ins->s, SOL_SOCKET, SO_TIMESTAMP, &on, sizeof(on)) < 0) {
+		perror("SO_TIMESTAMP is disabled");
+		return -1;
+	}
+
+	/* NuttX Feature: Enable TX deadline when sending CAN frames
+	 * When a deadline occurs the driver will remove the CAN frame
+	 */
+
+	if (setsockopt(ins->s, SOL_CAN_RAW, CAN_RAW_TX_DEADLINE, &on, sizeof(on)) < 0) {
+		perror("CAN_RAW_TX_DEADLINE is disabled");
+		return -1;
+	}
+
+	if (can_fd) {
+		if (setsockopt(ins->s, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &on, sizeof(on)) < 0) {
+			perror("no CAN FD support");
+			return -1;
+		}
+	}
+
+	if (bind(ins->s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
+		perror("bind");
+		return -1;
+	}
+
+	// Setup TX msg
+	ins->send_iov.iov_base = &ins->send_frame;
+
+	if (ins->can_fd) {
+		ins->send_iov.iov_len = sizeof(struct canfd_frame);
+
+	} else {
+		ins->send_iov.iov_len = sizeof(struct can_frame);
+	}
+
+	memset(&ins->send_control, 0x00, sizeof(ins->send_control));
+
+	ins->send_msg.msg_iov    = &ins->send_iov;
+	ins->send_msg.msg_iovlen = 1;
+	ins->send_msg.msg_control = &ins->send_control;
+	ins->send_msg.msg_controllen = sizeof(ins->send_control);
+
+	ins->send_cmsg = CMSG_FIRSTHDR(&ins->send_msg);
+	ins->send_cmsg->cmsg_level = SOL_CAN_RAW;
+	ins->send_cmsg->cmsg_type = CAN_RAW_TX_DEADLINE;
+	ins->send_cmsg->cmsg_len = sizeof(struct timeval);
+	ins->send_tv = (struct timeval *)CMSG_DATA(&ins->send_cmsg);
+
+	// Setup RX msg
+	ins->recv_iov.iov_base = &ins->recv_frame;
+
+	if (can_fd) {
+		ins->recv_iov.iov_len = sizeof(struct canfd_frame);
+
+	} else {
+		ins->recv_iov.iov_len = sizeof(struct can_frame);
+	}
+
+	memset(&ins->recv_control, 0x00, sizeof(ins->recv_control));
+
+	ins->recv_msg.msg_iov    = &ins->recv_iov;
+	ins->recv_msg.msg_iovlen = 1;
+	ins->recv_msg.msg_control = &ins->recv_control;
+	ins->recv_msg.msg_controllen = sizeof(ins->recv_control);
+	ins->recv_cmsg = CMSG_FIRSTHDR(&ins->recv_msg);
+
+	return 0;
+}
+
+int16_t socketcanTransmit(CanardSocketInstance *ins, const CanardFrame *txf)
+{
+	/* Copy CanardFrame to can_frame/canfd_frame */
+
+	if (ins->can_fd) {
+		ins->send_frame.can_id = txf->extended_can_id;
+		ins->send_frame.can_id |= CAN_EFF_FLAG;
+		ins->send_frame.len = txf->payload_size;
+		memcpy(&ins->send_frame.data, txf->payload, txf->payload_size);
+
+	} else {
+		struct can_frame *frame = (struct can_frame *)&ins->send_frame;
+		frame->can_id = txf->extended_can_id;
+		frame->can_id |= CAN_EFF_FLAG;
+		frame->can_dlc = txf->payload_size;
+		memcpy(&frame->data, txf->payload, txf->payload_size);
+	}
+
+	/* Set CAN_RAW_TX_DEADLINE timestamp  */
+
+	ins->send_tv->tv_usec = txf->timestamp_usec % 1000000ULL;
+	ins->send_tv->tv_sec = (txf->timestamp_usec - ins->send_tv->tv_usec) / 1000000ULL;
+
+	return sendmsg(ins->s, &ins->send_msg, 0);
+}
+
+int16_t socketcanReceive(CanardSocketInstance *ins, CanardFrame *rxf)
+{
+	int32_t result = recvmsg(ins->s, &ins->recv_msg, 0);
+
+	if (result < 0) {
+		return result;
+	}
+
+	/* Copy CAN frame to CanardFrame */
+
+	if (ins->can_fd) {
+		struct canfd_frame *recv_frame = (struct canfd_frame *)&ins->recv_frame;
+		rxf->extended_can_id = recv_frame->can_id & CAN_EFF_MASK;
+		rxf->payload_size = recv_frame->len;
+		rxf->payload = &recv_frame->data;
+
+	} else {
+		struct can_frame *recv_frame = (struct can_frame *)&ins->recv_frame;
+		rxf->extended_can_id = recv_frame->can_id & CAN_EFF_MASK;
+		rxf->payload_size = recv_frame->can_dlc;
+		rxf->payload = &recv_frame->data; //FIXME either copy or clearly state the pointer reference
+	}
+
+	/* Read SO_TIMESTAMP value */
+
+	if (ins->recv_cmsg->cmsg_level == SOL_SOCKET && ins->recv_cmsg->cmsg_type == SO_TIMESTAMP) {
+		struct timeval *tv = (struct timeval *)CMSG_DATA(ins->recv_cmsg);
+		rxf->timestamp_usec = tv->tv_sec * 1000000ULL + tv->tv_usec;
+	}
+
+	return result;
+}
+
+
+/* TODO implement corresponding IOCTL */
+
+int16_t socketcanConfigureFilter(const fd_t fd, const size_t num_filters, const struct can_filter *filters)
+{
+	return -1;
+}
--- a/src/drivers/uavcannode_gps_demo/socketcan.h
+++ b/src/drivers/uavcannode_gps_demo/socketcan.h
@ -0,0 +1,69 @@
+
+#ifndef SOCKETCAN_H_INCLUDED
+#define SOCKETCAN_H_INCLUDED
+
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdint.h>
+
+#include <sys/time.h>
+#include <sys/socket.h>
+
+#include <nuttx/can.h>
+#include <netpacket/can.h>
+
+#include <canard.h>
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct CanardSocketInstance CanardSocketInstance;
+typedef int fd_t;
+
+struct CanardSocketInstance {
+	fd_t               s;
+	bool               can_fd;
+
+	//// Send msg structure
+	struct iovec       send_iov;
+	struct canfd_frame send_frame;
+	struct msghdr      send_msg;
+	struct cmsghdr     *send_cmsg;
+	struct timeval     *send_tv;   /* TX deadline timestamp */
+	uint8_t            send_control[sizeof(struct cmsghdr) + sizeof(struct timeval)];
+
+	//// Receive msg structure
+	struct iovec       recv_iov;
+	struct canfd_frame recv_frame;
+	struct msghdr      recv_msg;
+	struct cmsghdr     *recv_cmsg;
+	uint8_t            recv_control[sizeof(struct cmsghdr) + sizeof(struct timeval)];
+};
+
+/// Creates a SocketCAN socket for corresponding iface can_iface_name
+/// Also sets up the message structures required for socketcanTransmit & socketcanReceive
+/// can_fd determines to use CAN FD frame when is 1, and classical CAN frame when is 0
+/// The return value is 0 on succes and -1 on error
+int16_t socketcanOpen(CanardSocketInstance *ins, const char *const can_iface_name, const bool can_fd);
+
+/// Send a CanardFrame to the CanardSocketInstance socket
+/// This function is blocking
+/// The return value is number of bytes transferred, negative value on error.
+int16_t socketcanTransmit(CanardSocketInstance *ins, const CanardFrame *txf);
+
+/// Receive a CanardFrame from the CanardSocketInstance socket
+/// This function is blocking
+/// The return value is number of bytes received, negative value on error.
+int16_t socketcanReceive(CanardSocketInstance *ins, CanardFrame *rxf);
+
+// TODO implement ioctl for CAN filter
+int16_t socketcanConfigureFilter(const fd_t fd, const size_t num_filters, const struct can_filter *filters);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif //SOCKETCAN_H_INCLUDED
--- a/src/drivers/uavcannode_gps_demo/uorb_converter.c
+++ b/src/drivers/uavcannode_gps_demo/uorb_converter.c
@ -0,0 +1,107 @@
+/*
+ * uorb_converter.c
+ *
+ *  Created on: Apr 10, 2020
+ *      Author: hovergames
+ */
+
+#include <string.h>
+
+#include "uorb_converter.h"
+
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+int uorb_sub_fd;
+px4_pollfd_struct_t fds[1];
+int error_counter;
+CanardInstance *canard_ins;
+int raw_uorb_message_transfer_id;
+int fix_message_transfer_id;
+int aux_message_transfer_id;
+int16_t *gps_raw_uorb_port_id;
+int16_t *gps_fix_port_id;
+int16_t *gps_aux_port_id;
+
+
+void uorbConverterInit(CanardInstance *ins, int16_t *raw_uorb_port_id, int16_t *fix_port_id, int16_t *aux_port_id)
+{
+	canard_ins = ins;
+
+	gps_raw_uorb_port_id = raw_uorb_port_id;
+	gps_fix_port_id = fix_port_id;
+	gps_aux_port_id = aux_port_id;
+
+	/* subscribe to the uORB topic */
+	uorb_sub_fd = orb_subscribe(ORB_ID(sensor_gps));
+
+	orb_set_interval(uorb_sub_fd, 200);
+
+	/* one could wait for multiple topics with this technique, just using one here */
+	fds[0].fd = uorb_sub_fd;
+	fds[0].events = POLLIN;
+
+	error_counter = 0;
+	raw_uorb_message_transfer_id = 0;
+	fix_message_transfer_id = 0;
+	aux_message_transfer_id = 0;
+}
+
+void uorbProcessSub(int timeout_msec)
+{
+	/* wait for subscriber update of 1 file descriptor for timeout_msec ms */
+	int poll_ret = px4_poll(fds, 1, timeout_msec);
+
+	/* handle the poll result */
+	if (poll_ret == 0) {
+		/* this means none of our providers is giving us data */
+	} else if (poll_ret < 0) {
+		/* this is seriously bad - should be an emergency */
+		if (error_counter < 10 || error_counter % 50 == 0) {
+			/* use a counter to prevent flooding (and slowing us down) */
+			PX4_ERR("ERROR return value from poll(): %d", poll_ret);
+		}
+
+		error_counter++;
+
+	} else {
+		if (fds[0].revents & POLLIN) {
+			/* obtained data for the first file descriptor */
+			struct sensor_gps_s raw;
+			/* copy sensors raw data into local buffer */
+			orb_copy(ORB_ID(sensor_gps), uorb_sub_fd, &raw);
+
+			CanardMicrosecond transmission_deadline = getMonotonicTimestampUSec() + (uint64_t)(1000ULL * 100ULL);
+
+			// raw uORB sensor_gps message
+			if (*gps_raw_uorb_port_id != -1) {
+
+				CanardTransfer transfer = {
+					.timestamp_usec = transmission_deadline,      // Zero if transmission deadline is not limited.
+					.priority       = CanardPriorityNominal,
+					.transfer_kind  = CanardTransferKindMessage,
+					.port_id        = *gps_raw_uorb_port_id,                       // This is the subject-ID.
+					.remote_node_id = CANARD_NODE_ID_UNSET,       // Messages cannot be unicast, so use UNSET.
+					.transfer_id    = raw_uorb_message_transfer_id,
+					.payload_size   = sizeof(struct sensor_gps_s),
+					.payload        = &raw,
+				};
+
+				++raw_uorb_message_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
+				int32_t result = canardTxPush(canard_ins, &transfer);
+
+				if (result < 0) {
+					// An error has occurred: either an argument is invalid or we've ran out of memory.
+					// It is possible to statically prove that an out-of-memory will never occur for a given application if the
+					// heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap.
+					PX4_ERR("canardTxPush error %d", result);
+				}
+			}
+
+
+			PX4_INFO("Recv from uORB");
+		}
+	}
+}
+
--- a/src/drivers/uavcannode_gps_demo/uorb_converter.h
+++ b/src/drivers/uavcannode_gps_demo/uorb_converter.h
@ -0,0 +1,34 @@
+/*
+ * uorb_converter.h
+ *
+ *  Created on: Apr 10, 2020
+ *      Author: hovergames
+ */
+
+#ifndef SRC_DRIVERS_UAVCANNODE_V1_UORB_CONVERTER_H_
+#define SRC_DRIVERS_UAVCANNODE_V1_UORB_CONVERTER_H_
+
+
+/* uORB */
+#include <px4_platform_common/posix.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/sensor_gps.h>
+
+/* canard */
+#include <canard.h>
+#include <canard_dsdl.h>
+
+/* monotonic timestamp */
+#include "libcancl/time.h"
+
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+void uorbConverterInit(CanardInstance *ins, int16_t *raw_uorb_port_id, int16_t *fix_port_id, int16_t *aux_port_id);
+
+void uorbProcessSub(int timeout_msec);
+
+
+#endif /* SRC_DRIVERS_UAVCANNODE_V1_UORB_CONVERTER_H_ */
				`@ -0,0 +1 @@`
				`Subproject commit cde670347425023480a1417fcd603b27c8eb06c1`
				`@ -0,0 +1 @@`
				`Subproject commit 7f5489e6e916cc8b13db0582dcf9930e225594b9`