UAVCAN v1 bridge

- NuttX stm32f4/stm32f7 uses character device driver
 - NuttX kinetis and s32k uses socketcan

.gitmodules

@@ -30,6 +30,12 @@
 	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/PX4/libcanard.git
+[submodule "src/drivers/uavcan_v1/public_regulated_data_types"]
+	path = src/drivers/uavcan_v1/public_regulated_data_types
+	url = https://github.com/PX4/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

Tools/astyle/files_to_check_code_style.sh

@@ -12,6 +12,7 @@ exec find boards msg src platforms test \
     -path platforms/nuttx/NuttX -prune -o \
     -path platforms/qurt/dspal -prune -o \
     -path src/drivers/uavcan/libuavcan -prune -o \
+    -path src/drivers/uavcan_v1/libcanard -prune -o \
     -path src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis -prune -o \
     -path src/lib/ecl -prune -o \
     -path src/lib/matrix -prune -o \

Tools/setup/requirements.txt

@@ -6,6 +6,7 @@ empy>=3.3
 jinja2>=2.8
 matplotlib>=3.0.*
 numpy>=1.13
+nunavut
 packaging
 pandas>=0.21
 pkgconfig

boards/nxp/fmuk66-v3/socketcan.cmake

@@ -50,7 +50,7 @@ px4_add_board(
 		telemetry # all available telemetry drivers
 		#test_ppm # NOT Portable YET
 		tone_alarm
-		#uavcannode_v1
+		uavcan_v1
 	MODULES
 		airspeed_selector
 		attitude_estimator_q

boards/nxp/ucans32k146/default.cmake

@@ -45,6 +45,7 @@ px4_add_board(
 		#safety_button
 		#tone_alarm
 		#uavcannode # TODO: CAN driver needed
+		uavcan_v1
 	MODULES
 		#ekf2
 		#load_mon

boards/px4/fmu-v4/default.cmake

@@ -58,7 +58,8 @@ px4_add_board(
 		telemetry # all available telemetry drivers
 		test_ppm
 		tone_alarm
-		uavcan
+		#uavcan
+		uavcan_v1
 	MODULES
 		airspeed_selector
 		attitude_estimator_q

boards/px4/fmu-v4/nuttx-config/nsh/defconfig

@@ -40,6 +40,7 @@ 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"
@@ -150,6 +151,8 @@ 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

boards/px4/fmu-v5/default.cmake

@@ -60,7 +60,8 @@ px4_add_board(
 		telemetry # all available telemetry drivers
 		test_ppm
 		tone_alarm
-		uavcan
+		#uavcan # legacy v0
+		uavcan_v1
 	MODULES
 		airspeed_selector
 		attitude_estimator_q

boards/px4/fmu-v5/nuttx-config/nsh/defconfig

@@ -44,6 +44,8 @@ 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"
@@ -156,6 +158,10 @@ 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

src/drivers/uavcan_v1/CMakeLists.txt

@@ -0,0 +1,91 @@
+############################################################################
+#
+#   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} --templates ${CMAKE_CURRENT_SOURCE_DIR}/templates --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language cpp -I ${DSDL_DIR}/uavcan ${DSDL_DIR}/regulated)
+	execute_process(COMMAND ${NNVG_PATH} --templates ${CMAKE_CURRENT_SOURCE_DIR}/templates --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language cpp ${DSDL_DIR}/uavcan)
+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)
+		list(APPEND SRCS
+			CanardSocketCAN.cpp
+			CanardSocketCAN.hpp
+		)
+	elseif(CONFIG_CAN_EXTID)
+		list(APPEND SRCS
+			CanardNuttXCDev.cpp
+			CanardNuttXCDev.hpp
+		)
+	endif()
+endif()
+
+px4_add_module(
+	MODULE drivers__uavcan_v1
+	MAIN uavcan_v1
+	COMPILE_FLAGS
+		#-DCANARD_ASSERT
+		-DUINT32_C\(x\)=__UINT32_C\(x\)
+		-O0
+	INCLUDES
+		${LIBCANARD_DIR}/libcanard/
+		${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated
+	SRCS
+		Uavcan.cpp
+		Uavcan.hpp
+		${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
+		git_libcanard
+		git_public_regulated_data_types
+		version
+	)

src/drivers/uavcan_v1/CanardInterface.hpp

@@ -0,0 +1,60 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <canard.h>
+
+class CanardInterface
+{
+public:
+	CanardInterface() = default;
+	virtual ~CanardInterface() = default;
+
+	virtual int init() { return 0; };
+
+	virtual int close() { return 0; };
+
+	/// Send a CanardFrame
+	/// This function is blocking
+	/// The return value is number of bytes transferred, negative value on error.
+	virtual int16_t transmit(const CanardFrame &txframe, int timeout_ms = 0) = 0;
+
+	/// Receive a CanardFrame
+	/// This function is blocking
+	/// The return value is number of bytes received, negative value on error.
+	virtual int16_t receive(CanardFrame *rxf) = 0;
+
+private:
+
+};

src/drivers/uavcan_v1/CanardNuttXCDev.cpp

@@ -0,0 +1,151 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "CanardNuttXCDev.hpp"
+
+#include <fcntl.h>
+#include <poll.h>
+
+#include <nuttx/can/can.h>
+#include <arch/board/board.h>
+
+#include "stm32_can.h"
+
+#include <px4_platform_common/log.h>
+
+int CanardNuttXCDev::init()
+{
+	struct can_dev_s *can = stm32_caninitialize(1);
+
+	if (can == nullptr) {
+		PX4_ERR("Failed to get CAN interface");
+
+	} else {
+		/* Register the CAN driver at "/dev/can0" */
+		int ret = can_register("/dev/can0", can);
+
+		if (ret < 0) {
+			PX4_ERR("can_register failed: %d", ret);
+
+		} else {
+			_fd = ::open("/dev/can0", O_RDWR | O_NONBLOCK);
+		}
+	}
+
+	return 0;
+}
+
+int16_t CanardNuttXCDev::transmit(const CanardFrame &txf, int timeout_ms)
+{
+	if (_fd < 0) {
+		return -1;
+	}
+
+	struct pollfd fds {};
+
+	fds.fd = _fd;
+
+	fds.events |= POLLOUT;
+
+	const int poll_result = poll(&fds, 1, timeout_ms);
+
+	if (poll_result < 0) {
+		return -1;
+	}
+
+	if (poll_result == 0) {
+		return 0;
+	}
+
+	if ((fds.revents & POLLOUT) == 0) {
+		return -1;
+	}
+
+	struct can_msg_s transmit_msg {};
+
+	transmit_msg.cm_hdr.ch_id = txf.extended_can_id;
+
+	transmit_msg.cm_hdr.ch_dlc = txf.payload_size;
+
+	transmit_msg.cm_hdr.ch_extid = 1;
+
+	memcpy(transmit_msg.cm_data, txf.payload, txf.payload_size);
+
+	const size_t msg_len = CAN_MSGLEN(transmit_msg.cm_hdr.ch_dlc);
+
+	const ssize_t nbytes = ::write(_fd, &transmit_msg, msg_len);
+
+	if (nbytes < 0 || (size_t)nbytes != msg_len) {
+		return -1;
+	}
+
+	return 1;
+}
+
+int16_t CanardNuttXCDev::receive(CanardFrame *received_frame)
+{
+	if ((_fd < 0) || (received_frame == nullptr)) {
+		return -1;
+	}
+
+	// File desriptor for CAN.
+	struct pollfd fds {};
+	fds.fd = _fd;
+	fds.events = POLLIN;
+
+	// Any recieved CAN messages will cause the poll statement to unblock and run
+	// This way CAN read runs with minimal latency.
+	// Note that multiple messages may be received in a short time, so this will try to read any availible in a loop
+	::poll(&fds, 1, 10);
+
+	// Only execute this part if can0 is changed.
+	if (fds.revents & POLLIN) {
+
+		// Try to read.
+		struct can_msg_s receive_msg;
+		const ssize_t nbytes = ::read(fds.fd, &receive_msg, sizeof(receive_msg));
+
+		if (nbytes < 0 || (size_t)nbytes < CAN_MSGLEN(0) || (size_t)nbytes > sizeof(receive_msg)) {
+			// error
+			return -1;
+
+		} else {
+			received_frame->extended_can_id = receive_msg.cm_hdr.ch_id;
+			received_frame->payload_size = receive_msg.cm_hdr.ch_dlc;
+			memcpy((void *)received_frame->payload, receive_msg.cm_data, receive_msg.cm_hdr.ch_dlc);
+			return nbytes;
+		}
+	}
+
+	return 0;
+}

src/drivers/uavcan_v1/CanardNuttXCDev.hpp

@@ -0,0 +1,67 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <px4_platform_common/px4_config.h>
+
+#include <canard.h>
+
+#include "CanardInterface.hpp"
+
+class CanardNuttXCDev : public CanardInterface
+{
+public:
+	CanardNuttXCDev() = default;
+	~CanardNuttXCDev() override = default;
+
+	/// 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
+	int init();
+
+	/// 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 transmit(const CanardFrame &txframe, int timeout_ms = 0);
+
+	/// 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 receive(CanardFrame *rxf);
+
+private:
+	int _fd{-1};
+	bool _can_fd{false};
+};

src/drivers/uavcan_v1/CanardSocketCAN.cpp

@@ -0,0 +1,199 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "CanardSocketCAN.hpp"
+
+#include <net/if.h>
+#include <sys/ioctl.h>
+#include <string.h>
+
+#include <px4_platform_common/log.h>
+
+int CanardSocketCAN::init()
+{
+	const char *const can_iface_name = "can0";
+
+	struct sockaddr_can addr;
+	struct ifreq ifr;
+
+    //FIXME HOTFIX to make this code compile
+    bool can_fd = 0;
+    
+	_can_fd = can_fd;
+
+	/* open socket */
+	if ((_fd = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
+		PX4_ERR("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) {
+		PX4_ERR("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(_fd, SOL_SOCKET, SO_TIMESTAMP, &on, sizeof(on)) < 0) {
+		PX4_ERR("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(_fd, SOL_CAN_RAW, CAN_RAW_TX_DEADLINE, &on, sizeof(on)) < 0) {
+		PX4_ERR("CAN_RAW_TX_DEADLINE is disabled");
+		return -1;
+	}
+
+	if (can_fd) {
+		if (setsockopt(_fd, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &on, sizeof(on)) < 0) {
+			PX4_ERR("no CAN FD support");
+			return -1;
+		}
+	}
+
+	if (bind(_fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
+		PX4_ERR("bind");
+		return -1;
+	}
+
+	// Setup TX msg
+	_send_iov.iov_base = &_send_frame;
+
+	if (_can_fd) {
+		_send_iov.iov_len = sizeof(struct canfd_frame);
+
+	} else {
+		_send_iov.iov_len = sizeof(struct can_frame);
+	}
+
+	memset(&_send_control, 0x00, sizeof(_send_control));
+
+	_send_msg.msg_iov    = &_send_iov;
+	_send_msg.msg_iovlen = 1;
+	_send_msg.msg_control = &_send_control;
+	_send_msg.msg_controllen = sizeof(_send_control);
+
+	_send_cmsg = CMSG_FIRSTHDR(&_send_msg);
+	_send_cmsg->cmsg_level = SOL_CAN_RAW;
+	_send_cmsg->cmsg_type = CAN_RAW_TX_DEADLINE;
+	_send_cmsg->cmsg_len = sizeof(struct timeval);
+	_send_tv = (struct timeval *)CMSG_DATA(&_send_cmsg);
+
+	// Setup RX msg
+	_recv_iov.iov_base = &_recv_frame;
+
+	if (can_fd) {
+		_recv_iov.iov_len = sizeof(struct canfd_frame);
+
+	} else {
+		_recv_iov.iov_len = sizeof(struct can_frame);
+	}
+
+	memset(_recv_control, 0x00, sizeof(_recv_control));
+
+	_recv_msg.msg_iov = &_recv_iov;
+	_recv_msg.msg_iovlen = 1;
+	_recv_msg.msg_control = &_recv_control;
+	_recv_msg.msg_controllen = sizeof(_recv_control);
+	_recv_cmsg = CMSG_FIRSTHDR(&_recv_msg);
+
+	return 0;
+}
+
+int16_t CanardSocketCAN::transmit(const CanardFrame &txf, int timeout_ms)
+{
+	/* Copy CanardFrame to can_frame/canfd_frame */
+	if (_can_fd) {
+		_send_frame.can_id = txf.extended_can_id | CAN_EFF_FLAG;
+		_send_frame.len = txf.payload_size;
+		memcpy(&_send_frame.data, txf.payload, txf.payload_size);
+
+	} else {
+		struct can_frame *frame = (struct can_frame *)&_send_frame;
+		frame->can_id = txf.extended_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  */
+	_send_tv->tv_usec = txf.timestamp_usec % 1000000ULL;
+	_send_tv->tv_sec = (txf.timestamp_usec - _send_tv->tv_usec) / 1000000ULL;
+
+	return sendmsg(_fd, &_send_msg, 0);
+}
+
+int16_t CanardSocketCAN::receive(CanardFrame *rxf)
+{
+	int32_t result = recvmsg(_fd, &_recv_msg, 0);
+
+	if (result < 0) {
+		return result;
+	}
+
+	/* Copy CAN frame to CanardFrame */
+
+	if (_can_fd) {
+		struct canfd_frame *recv_frame = (struct canfd_frame *)&_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 *)&_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 (_recv_cmsg->cmsg_level == SOL_SOCKET && _recv_cmsg->cmsg_type == SO_TIMESTAMP) {
+		struct timeval *tv = (struct timeval *)CMSG_DATA(_recv_cmsg);
+		rxf->timestamp_usec = tv->tv_sec * 1000000ULL + tv->tv_usec;
+	}
+
+	return result;
+}

src/drivers/uavcan_v1/CanardSocketCAN.hpp

@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <px4_platform_common/px4_config.h>
+
+#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>
+
+#include "CanardInterface.hpp"
+
+class CanardSocketCAN : public CanardInterface
+{
+public:
+	CanardSocketCAN() = default;
+	~CanardSocketCAN() override = default;
+
+	/// 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
+	int init();
+
+	/// 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 transmit(const CanardFrame &txframe, int timeout_ms = 0);
+
+	/// 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 receive(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);
+
+private:
+
+	int               _fd{-1};
+	bool              _can_fd{false};
+
+	//// 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)] {};
+};

src/drivers/uavcan_v1/Uavcan.cpp

@@ -0,0 +1,407 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "Uavcan.hpp"
+
+#include <lib/ecl/geo/geo.h>
+#include <lib/version/version.h>
+
+#define REGULATED_DRONE_SENSOR_BMSSTATUS_ID 32080
+
+using namespace time_literals;
+
+UavcanNode *UavcanNode::_instance;
+
+O1HeapInstance *uavcan_allocator{nullptr};
+
+static void *memAllocate(CanardInstance *const ins, const size_t amount) { return o1heapAllocate(uavcan_allocator, amount); }
+static void memFree(CanardInstance *const ins, void *const pointer) { o1heapFree(uavcan_allocator, pointer); }
+
+#if defined(__PX4_NUTTX)
+# if defined(CONFIG_NET_CAN)
+#  include "CanardSocketCAN.hpp"
+# elif defined(CONFIG_CAN)
+#  include "CanardNuttXCDev.hpp"
+# endif // CONFIG_CAN
+#endif // NuttX
+
+UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan),
+	_can_interface(interface)
+{
+	pthread_mutex_init(&_node_mutex, nullptr);
+
+	_uavcan_heap = memalign(O1HEAP_ALIGNMENT, HeapSize);
+	uavcan_allocator = o1heapInit(_uavcan_heap, HeapSize, nullptr, nullptr);
+
+	if (uavcan_allocator == nullptr) {
+		PX4_ERR("o1heapInit failed with size %d", HeapSize);
+	}
+
+	_canard_instance = canardInit(&memAllocate, &memFree);
+
+	_canard_instance.node_id = node_id; // Defaults to anonymous; can be set up later at any point.
+
+	bool can_fd = false;
+
+	if (can_fd) {
+		_canard_instance.mtu_bytes = CANARD_MTU_CAN_FD;
+
+	} else {
+		_canard_instance.mtu_bytes = CANARD_MTU_CAN_CLASSIC;
+	}
+}
+
+UavcanNode::~UavcanNode()
+{
+	delete _can_interface;
+
+	if (_instance) {
+		/* tell the task we want it to go away */
+		_task_should_exit.store(true);
+		ScheduleNow();
+
+		unsigned i = 10;
+
+		do {
+			/* wait 5ms - it should wake every 10ms or so worst-case */
+			usleep(5000);
+
+			if (--i == 0) {
+				break;
+			}
+
+		} while (_instance);
+	}
+
+	perf_free(_cycle_perf);
+	perf_free(_interval_perf);
+
+	//delete _uavcan_heap;
+}
+
+int UavcanNode::start(uint32_t node_id, uint32_t bitrate)
+{
+	if (_instance != nullptr) {
+		PX4_WARN("Already started");
+		return -1;
+	}
+
+#if defined(__PX4_NUTTX)
+# if defined(CONFIG_NET_CAN)
+	CanardInterface *interface = new CanardSocketCAN();
+# elif defined(CONFIG_CAN)
+	CanardInterface *interface = new CanardNuttXCDev();
+# endif // CONFIG_CAN
+#endif // NuttX
+
+	_instance = new UavcanNode(interface, node_id);
+
+	if (_instance == nullptr) {
+		PX4_ERR("Out of memory");
+		return -1;
+	}
+
+	// Keep the bit rate for reboots on BenginFirmware updates
+	_instance->active_bitrate = bitrate;
+
+	_instance->ScheduleOnInterval(ScheduleIntervalMs * 1000);
+
+	return PX4_OK;
+}
+
+void UavcanNode::Run()
+{
+	pthread_mutex_lock(&_node_mutex);
+
+	if (!_initialized) {
+		// interface init
+		if (_can_interface) {
+			if (_can_interface->init() == PX4_OK) {
+
+				// Subscribe to messages uavcan.node.Heartbeat.
+				canardRxSubscribe(&_canard_instance,
+						  CanardTransferKindMessage,
+						  uavcan::node::Heartbeat_1_0::PORT_ID,
+						  uavcan::node::Heartbeat_1_0::SIZE,
+						  CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+						  &_heartbeat_subscription);
+
+				if (_param_uavcan_v1_bat_md.get() == 1) {
+					canardRxSubscribe(&_canard_instance,
+							  CanardTransferKindMessage,
+							  static_cast<CanardPortID>(_param_uavcan_v1_bat_id.get()),
+							  regulated::drone::sensor::BMSStatus_1_0::SIZE,
+							  CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+							  &_drone_sensor_BMSStatus_subscription);
+				}
+
+				_initialized = true;
+			}
+		}
+
+		// return early if still not initialized
+		if (!_initialized) {
+			pthread_mutex_unlock(&_node_mutex);
+			return;
+		}
+	}
+
+	// check for parameter updates
+	if (_parameter_update_sub.updated()) {
+		// clear update
+		parameter_update_s pupdate;
+		_parameter_update_sub.copy(&pupdate);
+
+		// update parameters from storage
+		updateParams();
+	}
+
+	perf_begin(_cycle_perf);
+	perf_count(_interval_perf);
+
+
+
+	// send uavcan::node::Heartbeat_1_0 @ 1 Hz
+	if (hrt_elapsed_time(&_uavcan_node_heartbeat_last) >= 1_s) {
+
+		uavcan::node::Heartbeat_1_0 heartbeat{};
+		heartbeat.uptime = _uavcan_node_heartbeat_transfer_id; // TODO: use real uptime
+		heartbeat.health = uavcan::node::Heartbeat_1_0::HEALTH_NOMINAL;
+		heartbeat.mode = uavcan::node::Heartbeat_1_0::MODE_OPERATIONAL;
+
+		heartbeat.serializeToBuffer(_uavcan_node_heartbeat_buffer);
+
+		const CanardTransfer transfer = {
+			.timestamp_usec = hrt_absolute_time(),
+			.priority       = CanardPriorityNominal,
+			.transfer_kind  = CanardTransferKindMessage,
+			.port_id        = uavcan::node::Heartbeat_1_0::PORT_ID,
+			.remote_node_id = CANARD_NODE_ID_UNSET,
+			.transfer_id    = _uavcan_node_heartbeat_transfer_id++,
+			.payload_size   = uavcan::node::Heartbeat_1_0::SIZE,
+			.payload        = &_uavcan_node_heartbeat_buffer,
+		};
+
+		int32_t result = canardTxPush(&_canard_instance, &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("Heartbeat transmit error %d", result);
+		}
+
+		_uavcan_node_heartbeat_last = transfer.timestamp_usec;
+	}
+
+	// send regulated::drone::sensor::BMSStatus_1_0 @ 1 Hz
+	if (_param_uavcan_v1_bat_md.get() == 2) {
+		if (hrt_elapsed_time(&_regulated_drone_sensor_bmsstatus_last) >= 1_s) {
+
+			battery_status_s battery_status;
+
+			if (_battery_status_sub.update(&battery_status)) {
+				regulated::drone::sensor::BMSStatus_1_0 bmsstatus{};
+				//bmsstatus.timestamp = battery_status.timestamp;
+				bmsstatus.remaining_capacity = battery_status.remaining;
+
+				bmsstatus.serializeToBuffer(_regulated_drone_sensor_bmsstatus_buffer);
+
+				const CanardTransfer transfer = {
+					.timestamp_usec = hrt_absolute_time(),
+					.priority       = CanardPriorityNominal,
+					.transfer_kind  = CanardTransferKindMessage,
+					.port_id        = static_cast<CanardPortID>(_param_uavcan_v1_bat_id.get()),
+					.remote_node_id = CANARD_NODE_ID_UNSET,
+					.transfer_id    = _regulated_drone_sensor_bmsstatus_transfer_id++,
+					.payload_size   = regulated::drone::sensor::BMSStatus_1_0::SIZE,
+					.payload        = &_regulated_drone_sensor_bmsstatus_buffer,
+				};
+
+				int32_t result = canardTxPush(&_canard_instance, &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("Battery transmit error %d", result);
+				}
+
+				_regulated_drone_sensor_bmsstatus_last = transfer.timestamp_usec;
+			}
+		}
+	}
+
+	// Transmitting
+	// Look at the top of the TX queue.
+	for (const CanardFrame *txf = nullptr; (txf = canardTxPeek(&_canard_instance)) != nullptr;) {
+		// Check if the frame has timed out.
+		if (hrt_absolute_time() > txf->timestamp_usec) {
+			// Send the frame. Redundant interfaces may be used here.
+			const int tx_res = _can_interface->transmit(*txf);
+
+			if (tx_res < 0) {
+				// Failure - drop the frame and report
+				canardTxPop(&_canard_instance);
+
+				// Deallocate the dynamic memory afterwards.
+				_canard_instance.memory_free(&_canard_instance, (CanardFrame *)txf);
+				PX4_ERR("Transmit error %d, frame dropped, errno '%s'", tx_res, strerror(errno));
+
+			} else if (tx_res > 0) {
+				// Success - just drop the frame
+				canardTxPop(&_canard_instance);
+
+				// Deallocate the dynamic memory afterwards.
+				_canard_instance.memory_free(&_canard_instance, (CanardFrame *)txf);
+
+			} else {
+				// Timeout - just exit and try again later
+				break;
+			}
+		}
+	}
+
+	uint8_t data[64] {};
+	CanardFrame received_frame{};
+	received_frame.payload = &data;
+
+	if (_can_interface->receive(&received_frame) > 0) {
+
+		CanardTransfer receive{};
+		int32_t result = canardRxAccept(&_canard_instance, &received_frame, 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.
+			PX4_ERR("Receive error %d\n", result);
+
+		} else if (result == 1) {
+			// A transfer has been received, process it.
+			PX4_DEBUG("received Port ID: %d", receive.port_id);
+
+			if (receive.port_id == static_cast<CanardPortID>(_param_uavcan_v1_bat_id.get())) {
+				auto bms_status = regulated::drone::sensor::BMSStatus_1_0::deserializeFromBuffer((const uint8_t *)receive.payload,
+						  receive.payload_size);
+
+				battery_status_s battery_status{};
+				battery_status.id = bms_status.battery_id;
+				battery_status.remaining = bms_status.remaining_capacity;
+				battery_status.timestamp = hrt_absolute_time();
+				_battery_status_pub.publish(battery_status);
+			}
+
+			// Deallocate the dynamic memory afterwards.
+			_canard_instance.memory_free(&_canard_instance, (void *)receive.payload);
+		}
+	}
+
+	perf_end(_cycle_perf);
+
+	if (_task_should_exit.load()) {
+		_can_interface->close();
+
+		ScheduleClear();
+		_instance = nullptr;
+	}
+
+	pthread_mutex_unlock(&_node_mutex);
+}
+
+void UavcanNode::print_info()
+{
+	pthread_mutex_lock(&_node_mutex);
+
+	perf_print_counter(_cycle_perf);
+	perf_print_counter(_interval_perf);
+
+	pthread_mutex_unlock(&_node_mutex);
+}
+
+static void print_usage()
+{
+	PX4_INFO("usage: \n"
+		 "\tuavcannode {start|status|stop}");
+}
+
+extern "C" __EXPORT int uavcan_v1_main(int argc, char *argv[])
+{
+	if (argc < 2) {
+		print_usage();
+		return 1;
+	}
+
+	if (!strcmp(argv[1], "start")) {
+		if (UavcanNode::instance()) {
+			PX4_ERR("already started");
+			return 1;
+		}
+
+		// CAN bitrate
+		int32_t bitrate = 0;
+		param_get(param_find("UAVCAN_V1_BAUD"), &bitrate);
+
+		// Node ID
+		int32_t node_id = 0;
+		param_get(param_find("UAVCAN_V1_ID"), &node_id);
+
+		// Start
+		PX4_INFO("Node ID %u, bitrate %u", node_id, bitrate);
+		return UavcanNode::start(node_id, bitrate);
+	}
+
+	/* commands below require the app to be started */
+	UavcanNode *const inst = UavcanNode::instance();
+
+	if (!inst) {
+		PX4_ERR("application not running");
+		return 1;
+	}
+
+	if (!strcmp(argv[1], "status") || !strcmp(argv[1], "info")) {
+		inst->print_info();
+		return 0;
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		delete inst;
+		return 0;
+	}
+
+	print_usage();
+	return 1;
+}

src/drivers/uavcan_v1/Uavcan.hpp

@@ -0,0 +1,136 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/atomic.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+
+#include <lib/parameters/param.h>
+#include <lib/perf/perf_counter.h>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/parameter_update.h>
+
+#include "o1heap/o1heap.h"
+
+#include <canard.h>
+#include <canard_dsdl.h>
+
+#include <regulated/drone/sensor/BMSStatus_1_0.hpp>
+#include <uavcan/node/Heartbeat_1_0.hpp>
+
+#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
+	 *
+	 */
+	static constexpr unsigned HeapSize = 8192;
+
+	static constexpr unsigned ScheduleIntervalMs = 10;
+
+public:
+
+	UavcanNode(CanardInterface *interface, uint32_t node_id);
+	~UavcanNode() override;
+
+	static int start(uint32_t node_id, uint32_t bitrate);
+
+	void print_info();
+
+	static UavcanNode *instance() { return _instance; }
+
+	/* The bit rate that can be passed back to the bootloader */
+	int32_t active_bitrate{0};
+
+private:
+	void Run() override;
+	void fill_node_info();
+
+	void *_uavcan_heap{nullptr};
+
+	CanardInterface *const _can_interface;
+
+	CanardInstance _canard_instance;
+
+	px4::atomic_bool _task_should_exit{false};	///< flag to indicate to tear down the CAN driver
+
+	bool _initialized{false};		///< number of actuators currently available
+
+	static UavcanNode *_instance;
+
+	pthread_mutex_t _node_mutex;
+
+	CanardRxSubscription _heartbeat_subscription;
+	CanardRxSubscription _drone_sensor_BMSStatus_subscription;
+
+	uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
+	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
+
+	uORB::Publication<battery_status_s> _battery_status_pub{ORB_ID(battery_status)};
+
+	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")};
+
+	// uavcan::node::Heartbeat_1_0
+	uint8_t _uavcan_node_heartbeat_buffer[uavcan::node::Heartbeat_1_0::SIZE];
+	hrt_abstime _uavcan_node_heartbeat_last{0};
+	CanardTransferID _uavcan_node_heartbeat_transfer_id{0};
+
+	// regulated::drone::sensor::BMSStatus_1_0
+	uint8_t _regulated_drone_sensor_bmsstatus_buffer[regulated::drone::sensor::BMSStatus_1_0::SIZE];
+	hrt_abstime _regulated_drone_sensor_bmsstatus_last{0};
+	CanardTransferID _regulated_drone_sensor_bmsstatus_transfer_id{0};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::UAVCAN_V1_ENABLE>) _param_uavcan_v1_enable,
+		(ParamInt<px4::params::UAVCAN_V1_ID>) _param_uavcan_v1_id,
+		(ParamInt<px4::params::UAVCAN_V1_BAUD>) _param_uavcan_v1_baud,
+		(ParamInt<px4::params::UAVCAN_V1_BAT_MD>) _param_uavcan_v1_bat_md,
+		(ParamInt<px4::params::UAVCAN_V1_BAT_ID>) _param_uavcan_v1_bat_id
+	)
+};

src/drivers/uavcan_v1/libcanard

@@ -0,0 +1 @@
+Subproject commit bec890304a2888bc516416e4ebf252f761558b92

src/drivers/uavcan_v1/o1heap/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;
+}

src/drivers/uavcan_v1/o1heap/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

src/drivers/uavcan_v1/parameters.c

@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * UAVCAN v1
+ *
+ *  0 - UAVCAN disabled.
+ *  1 - Enables UAVCANv1
+ *
+ * @boolean
+ * @reboot_required true
+ * @group UAVCAN v1
+ */
+PARAM_DEFINE_INT32(UAVCAN_V1_ENABLE, 0);
+
+/**
+ * UAVCAN v1 Node ID.
+ *
+ * Read the specs at http://uavcan.org to learn more about Node ID.
+ *
+ * @min 1
+ * @max 125
+ * @reboot_required true
+ * @group UAVCANv1
+ */
+PARAM_DEFINE_INT32(UAVCAN_V1_ID, 1);
+
+/**
+ * UAVCAN/CAN v1 bus bitrate.
+ *
+ * @unit bit/s
+ * @min 20000
+ * @max 1000000
+ * @reboot_required true
+ * @group UAVCAN v1
+ */
+PARAM_DEFINE_INT32(UAVCAN_V1_BAUD, 1000000);
+
+/**
+ * UAVCAN v1 battery mode.
+ *
+ * @value 0 Disable
+ * @value 1 Receive BMSStatus
+ * @value 2 Send BMSStatus
+ * @reboot_required true
+ * @group UAVCAN v1
+ */
+PARAM_DEFINE_INT32(UAVCAN_V1_BAT_MD, 0);
+
+/**
+ * UAVCAN v1 battery port ID.
+ *
+ * @min 1
+ * @max 32767
+ * @group UAVCAN v1
+ */
+PARAM_DEFINE_INT32(UAVCAN_V1_BAT_ID, 4242);

src/drivers/uavcan_v1/public_regulated_data_types

@@ -0,0 +1 @@
+Subproject commit e378c3a4c5eb38ad30866059b838d4099a85f2ea

src/drivers/uavcan_v1/templates/Header.j2

@@ -0,0 +1,32 @@
+/*
+ *
+ * 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}}

src/drivers/uavcan_v1/templates/ServiceType.j2

@@ -0,0 +1,10 @@
+{% 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 -%}

src/drivers/uavcan_v1/templates/StructureType.j2

@@ -0,0 +1,5 @@
+{% extends "Header.j2" %}
+{%- block object -%}
+{% set composite_type = T -%}
+{% include '_composite_type.j2' %}
+{% endblock -%}

src/drivers/uavcan_v1/templates/UnionType.j2

@@ -0,0 +1,5 @@
+{% extends "Header.j2" %}
+{%- block object -%}
+{% set composite_type = T -%}
+{% include '_composite_type.j2' %}
+{% endblock -%}

src/drivers/uavcan_v1/templates/_composite_type.j2

@@ -0,0 +1,115 @@
+{{ 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 }}