Upgrade libcanard to 3.0 and rename uavcan_v1 to cyphal

Did some prep work for redundant interfaces by introducing CanardHandle class to decouple physical interfaces from cyphal.cpp

src/drivers/cyphal/Actuators/EscClient.hpp

@@ -0,0 +1,190 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file EscClient.hpp
+ *
+ * Client-side implementation of UDRAL specification ESC service
+ *
+ * Publishes the following Cyphal messages:
+ *   reg.drone.service.actuator.common.sp.Value8.0.1
+ *   reg.drone.service.common.Readiness.0.1
+ *
+ * Subscribes to the following Cyphal messages:
+ *   reg.drone.service.actuator.common.Feedback.0.1
+ *   reg.drone.service.actuator.common.Status.0.1
+ *
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ * @author Jacob Crabill <jacob@flyvoly.com>
+ */
+
+#pragma once
+
+#include <lib/perf/perf_counter.h>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/esc_status.h>
+#include <drivers/drv_hrt.h>
+#include <lib/mixer_module/mixer_module.hpp>
+
+// UDRAL Specification Messages
+#include <reg/udral/service/actuator/common/sp/Vector31_0_1.h>
+#include <reg/udral/service/common/Readiness_0_1.h>
+
+/// TODO: Allow derived class of Subscription at same time, to handle ESC Feedback/Status
+class UavcanEscController : public UavcanPublisher
+{
+public:
+	static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS;
+
+	UavcanEscController(CanardHandle &handle, UavcanParamManager &pmgr) :
+		UavcanPublisher(handle, pmgr, "udral", "esc") { };
+
+	~UavcanEscController() {};
+
+	void update() override
+	{
+		if (_armed_sub.updated()) {
+			actuator_armed_s new_arming;
+			_armed_sub.update(&new_arming);
+
+			if (new_arming.armed != _armed.armed) {
+				_armed = new_arming;
+				size_t payload_size = reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
+
+				// Only publish if we have a valid publication ID set
+				if (_port_id == 0) {
+					return;
+				}
+
+				reg_udral_service_common_Readiness_0_1 msg_arming {};
+
+				if (_armed.armed) {
+					msg_arming.value = reg_udral_service_common_Readiness_0_1_ENGAGED;
+
+				} else if (_armed.prearmed) {
+					msg_arming.value = reg_udral_service_common_Readiness_0_1_STANDBY;
+
+				} else {
+					msg_arming.value = reg_udral_service_common_Readiness_0_1_SLEEP;
+				}
+
+				uint8_t arming_payload_buffer[reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
+
+				CanardPortID arming_pid = static_cast<CanardPortID>(static_cast<uint32_t>(_port_id) + 1);
+				const CanardTransferMetadata transfer_metadata = {
+					.priority       = CanardPriorityNominal,
+					.transfer_kind  = CanardTransferKindMessage,
+					.port_id        = arming_pid,                // This is the subject-ID.
+					.remote_node_id = CANARD_NODE_ID_UNSET,      // Messages cannot be unicast, so use UNSET.
+					.transfer_id    = _arming_transfer_id,
+				};
+
+				int result = reg_udral_service_common_Readiness_0_1_serialize_(&msg_arming, arming_payload_buffer,
+						&payload_size);
+
+				if (result == 0) {
+					// set the data ready in the buffer and chop if needed
+					++_arming_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
+					result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
+								       &transfer_metadata,
+								       payload_size,
+								       &arming_payload_buffer
+								      );
+				}
+			}
+		}
+	};
+
+	void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs)
+	{
+		if (_port_id > 0) {
+			reg_udral_service_actuator_common_sp_Vector31_0_1 msg_sp {0};
+			size_t payload_size = reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
+
+			for (uint8_t i = 0; i < MAX_ACTUATORS; i++) {
+				if (i < num_outputs) {
+					msg_sp.value[i] = static_cast<float>(outputs[i]);
+
+				} else {
+					// "unset" values published as NaN
+					msg_sp.value[i] = NAN;
+				}
+			}
+
+
+			PX4_INFO("Publish %d values %f, %f, %f, %f", num_outputs, (double)msg_sp.value[0], (double)msg_sp.value[1],
+				 (double)msg_sp.value[2], (double)msg_sp.value[3]);
+
+			uint8_t esc_sp_payload_buffer[reg_udral_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
+
+			const CanardTransferMetadata transfer_metadata = {
+				.priority       = CanardPriorityNominal,
+				.transfer_kind  = CanardTransferKindMessage,
+				.port_id        = _port_id,                // This is the subject-ID.
+				.remote_node_id = CANARD_NODE_ID_UNSET,       // Messages cannot be unicast, so use UNSET.
+				.transfer_id    = _transfer_id,
+			};
+
+			int result = reg_udral_service_actuator_common_sp_Vector31_0_1_serialize_(&msg_sp, esc_sp_payload_buffer,
+					&payload_size);
+
+			if (result == 0) {
+				// set the data ready in the buffer and chop if needed
+				++_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
+				result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
+							       &transfer_metadata,
+							       payload_size,
+							       &esc_sp_payload_buffer);
+			}
+		}
+	};
+
+	/**
+	 * Sets the number of rotors
+	 */
+	void set_rotor_count(uint8_t count) { _rotor_count = count; }
+
+private:
+	/**
+	 * ESC status message reception will be reported via this callback.
+	 */
+	void esc_status_sub_cb(const CanardRxTransfer &msg);
+
+	uint8_t _rotor_count {0};
+
+	uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
+	actuator_armed_s _armed {};
+
+	CanardTransferID _arming_transfer_id;
+};

src/drivers/cyphal/Actuators/EscServer.hpp

@@ -0,0 +1,74 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file EscServer.hpp
+ *
+ * Server-side implementation of UDRAL specification ESC service
+ * (Used for CANNode control of an ESC via PWM output)
+ *
+ * Subscribes to the following Cyphal messages:
+ *   reg.drone.service.actuator.common.sp.Value8.0.1
+ *   reg.drone.service.common.Readiness.0.1
+ *
+ * Publishes to the following Cyphal messages:
+ *   reg.drone.service.actuator.common.Feedback.0.1
+ *   reg.drone.service.actuator.common.Status.0.1
+ *
+ * And publishes to the following uORB topics:
+ *   actuator_armed
+ *   output_control_mc
+ *
+ * @author Jacob Crabill <jacob@flyvoly.com>
+ */
+
+#pragma once
+
+
+#include <lib/perf/perf_counter.h>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/esc_status.h>
+#include <drivers/drv_hrt.h>
+#include <lib/mixer_module/mixer_module.hpp>
+
+// UDRAL Specification Messages
+#include <reg/drone/service/actuator/common/sp/Vector31_0_1.h>
+#include <reg/drone/service/common/Readiness_0_1.h>
+
+#include "../Publishers/Publisher.hpp"
+
+class EscServer : public UavcanSubscription
+{
+/// TODO
+}

src/drivers/cyphal/CMakeLists.txt

@@ -0,0 +1,145 @@
+############################################################################
+#
+#   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)
+set(LEGACY_DSDL_DIR ${CMAKE_CURRENT_SOURCE_DIR}/legacy_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})
+px4_add_git_submodule(TARGET git_legacy_data_types PATH ${LEGACY_DSDL_DIR})
+
+find_program(NNVG_PATH nnvg)
+if(NNVG_PATH)
+	message("Generating Cyphal DSDL headers using Nunavut")
+	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 -I ${LEGACY_DSDL_DIR}/uavcan ${LEGACY_DSDL_DIR}/legacy)
+	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()
+
+set(SRCS)
+set(DPNDS)
+
+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()
+
+if(CONFIG_CYPHAL_NODE_MANAGER)
+    list(APPEND SRCS
+		NodeManager.hpp
+		NodeManager.cpp
+    )
+endif()
+
+if(CONFIG_CYPHAL_NODE_CLIENT)
+    list(APPEND SRCS
+		NodeClient.hpp
+		NodeClient.cpp
+    )
+    list(APPEND DPNDS
+        drivers_bootloaders
+    )
+endif()
+
+# Temporary measure to get Kconfig option as defines into this application
+# Ideally we want nicely decouple and define this in kconfig.cmake
+# But then we need to overhaul the src modules naming convention
+set(DRIVERS_CYPHAL_OPTIONS)
+get_cmake_property(_variableNames VARIABLES)
+foreach (_variableName ${_variableNames})
+    string(REGEX MATCH "^CONFIG_CYPHAL_" CYPHAL_OPTION ${_variableName})
+
+    if(CYPHAL_OPTION)
+        if(${${_variableName}} STREQUAL "y")
+            list(APPEND DRIVERS_CYPHAL_OPTIONS "-D${_variableName}=1")
+        else()
+            list(APPEND DRIVERS_CYPHAL_OPTIONS "-D${_variableName}=${${_variableName}}")
+        endif()
+    endif()
+endforeach()
+
+remove_definitions(-Werror)
+
+px4_add_module(
+	MODULE drivers__cyphal
+	MAIN cyphal
+	STACK_MAIN 4096
+	COMPILE_FLAGS
+		#-DCANARD_ASSERT
+		-DUINT32_C\(x\)=__UINT32_C\(x\)
+		-O0
+		${DRIVERS_CYPHAL_OPTIONS}
+	INCLUDES
+		${LIBCANARD_DIR}/libcanard/
+		${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated
+	SRCS
+		PublicationManager.cpp
+		PublicationManager.hpp
+		SubscriptionManager.cpp
+		SubscriptionManager.hpp
+		ParamManager.hpp
+		ParamManager.cpp
+		Cyphal.cpp
+		Cyphal.hpp
+		CanardHandle.cpp
+		Publishers/uORB/uorb_publisher.cpp
+		Subscribers/uORB/uorb_subscriber.cpp
+		${SRCS}
+		o1heap/o1heap.c
+		o1heap/o1heap.h
+		${LIBCANARD_DIR}/libcanard/canard.c
+		${LIBCANARD_DIR}/libcanard/canard.h
+	MODULE_CONFIG
+		module.yaml
+	DEPENDS
+		git_libcanard
+		git_public_regulated_data_types
+		git_legacy_data_types
+		version
+		${DPNDS}
+	)
+
+# libcanard 3.0 introduces this warning, for now no intention to fix it thus we ignore this warning
+set_source_files_properties(${LIBCANARD_DIR}/libcanard/canard.c PROPERTIES COMPILE_FLAGS -Wno-cast-align)

src/drivers/cyphal/CanardHandle.cpp

@@ -0,0 +1,216 @@
+/****************************************************************************
+ *
+ *   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 "CanardHandle.hpp"
+
+#include <net/if.h>
+#include <sys/ioctl.h>
+#include <string.h>
+
+#include <px4_platform_common/log.h>
+
+#include "o1heap/o1heap.h"
+
+#include "Subscribers/BaseSubscriber.hpp"
+
+#if defined(__PX4_NUTTX)
+# if defined(CONFIG_NET_CAN)
+#  include "CanardSocketCAN.hpp"
+# elif defined(CONFIG_CAN)
+#  include "CanardNuttXCDev.hpp"
+# endif // CONFIG_CAN
+#endif // NuttX
+
+
+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); }
+
+
+CanardHandle::CanardHandle(uint32_t node_id, const size_t capacity, const size_t mtu_bytes)
+{
+	_uavcan_heap = memalign(O1HEAP_ALIGNMENT, HeapSize);
+	uavcan_allocator = o1heapInit(_uavcan_heap, HeapSize, nullptr, nullptr);
+
+	if (uavcan_allocator == nullptr) {
+		PX4_ERR("o1heapInit failed with size %u", HeapSize);
+	}
+
+	_canard_instance = canardInit(&memAllocate, &memFree);
+
+	_canard_instance.node_id = node_id; // Defaults to anonymous; can be set up later at any point.
+
+	_queue = canardTxInit(capacity, mtu_bytes);
+
+#if defined(__PX4_NUTTX)
+# if defined(CONFIG_NET_CAN)
+	_can_interface = new CanardSocketCAN();
+# elif defined(CONFIG_CAN)
+	_can_interface = new CanardNuttXCDev();
+# endif // CONFIG_CAN
+#endif // NuttX
+
+}
+
+CanardHandle::~CanardHandle()
+{
+	_can_interface->close();
+	delete _can_interface;
+	_can_interface = nullptr;
+
+	delete static_cast<uint8_t *>(_uavcan_heap);
+	_uavcan_heap = nullptr;
+
+}
+
+
+bool CanardHandle::init()
+{
+	if (_can_interface) {
+		if (_can_interface->init() == PX4_OK) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
+void CanardHandle::receive()
+{
+	/* Process received messages */
+
+	uint8_t data[64] {};
+	CanardRxFrame received_frame{};
+	received_frame.frame.payload = &data;
+
+	while (_can_interface->receive(&received_frame) > 0) {
+		CanardRxTransfer receive{};
+		CanardRxSubscription *subscription = nullptr;
+		int32_t result = canardRxAccept(&_canard_instance, received_frame.timestamp_usec, &received_frame.frame, 0, &receive,
+						&subscription);
+
+		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 %" PRId32" \n", result);
+
+		} else if (result == 1) {
+			// A transfer has been received, process it.
+			// PX4_INFO("received Port ID: %d", receive.port_id);
+
+			if (subscription != nullptr) {
+				UavcanBaseSubscriber *sub_instance = (UavcanBaseSubscriber *)subscription->user_reference;
+				sub_instance->callback(receive);
+
+			} else {
+				PX4_ERR("No matching sub for %d", receive.metadata.port_id);
+			}
+
+			// Deallocate the dynamic memory afterwards.
+			_canard_instance.memory_free(&_canard_instance, (void *)receive.payload);
+
+		} else {
+			//PX4_INFO("RX canard %d", result);
+		}
+	}
+
+}
+
+void CanardHandle::transmit()
+{
+	// Look at the top of the TX queue.
+	for (const CanardTxQueueItem *ti = NULL; (ti = canardTxPeek(&_queue)) != NULL;) { // Peek at the top of the queue.
+		if ((0U == ti->tx_deadline_usec) || (ti->tx_deadline_usec > hrt_absolute_time())) { // Check the deadline.
+			// Send the frame. Redundant interfaces may be used here.
+			const int tx_res = _can_interface->transmit(*ti);
+
+			if (tx_res < 0) {
+				PX4_ERR("Transmit error %d, frame dropped, errno '%s'", tx_res, strerror(errno));
+
+			} else if (tx_res == 0) {
+				// Timeout - just exit and try again later
+				break;
+			}
+
+		}
+
+		// After the frame is transmitted or if it has timed out while waiting, pop it from the queue and deallocate:
+		_canard_instance.memory_free(&_canard_instance, canardTxPop(&_queue, ti));
+	}
+}
+
+int32_t CanardHandle::TxPush(const CanardMicrosecond             tx_deadline_usec,
+			     const CanardTransferMetadata *const metadata,
+			     const size_t                        payload_size,
+			     const void *const                   payload)
+{
+	return canardTxPush(&_queue, &_canard_instance, tx_deadline_usec, metadata, payload_size, payload);
+}
+
+int8_t CanardHandle::RxSubscribe(const CanardTransferKind    transfer_kind,
+				 const CanardPortID          port_id,
+				 const size_t                extent,
+				 const CanardMicrosecond     transfer_id_timeout_usec,
+				 CanardRxSubscription *const out_subscription)
+{
+	return canardRxSubscribe(&_canard_instance, transfer_kind, port_id, extent, transfer_id_timeout_usec, out_subscription);
+}
+
+int8_t CanardHandle::RxUnsubscribe(const CanardTransferKind transfer_kind,
+				   const CanardPortID       port_id)
+{
+	return canardRxUnsubscribe(&_canard_instance, transfer_kind, port_id);
+}
+
+CanardTreeNode *CanardHandle::getRxSubscriptions(CanardTransferKind kind)
+{
+	return _canard_instance.rx_subscriptions[kind];
+}
+
+int32_t CanardHandle::mtu()
+{
+	return _queue.mtu_bytes;
+}
+
+CanardNodeID CanardHandle::node_id()
+{
+	return _canard_instance.node_id;
+}
+
+void CanardHandle::set_node_id(CanardNodeID id)
+{
+	_canard_instance.node_id = id;
+}

src/drivers/cyphal/CanardHandle.hpp

@@ -0,0 +1,85 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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>
+#include "CanardInterface.hpp"
+
+class CanardHandle
+{
+	/*
+	* 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;
+
+public:
+	CanardHandle(uint32_t node_id, const size_t capacity, const size_t mtu_bytes);
+	~CanardHandle();
+
+	bool init();
+
+	void receive();
+	void transmit();
+
+	int32_t TxPush(const CanardMicrosecond             tx_deadline_usec,
+		       const CanardTransferMetadata *const metadata,
+		       const size_t                        payload_size,
+		       const void *const                   payload);
+
+	int8_t RxSubscribe(const CanardTransferKind    transfer_kind,
+			   const CanardPortID          port_id,
+			   const size_t                extent,
+			   const CanardMicrosecond     transfer_id_timeout_usec,
+			   CanardRxSubscription *const out_subscription);
+	int8_t RxUnsubscribe(const CanardTransferKind transfer_kind,
+			     const CanardPortID       port_id);
+	CanardTreeNode *getRxSubscriptions(CanardTransferKind kind);
+
+	int32_t mtu();
+	CanardNodeID node_id();
+	void set_node_id(CanardNodeID id);
+
+private:
+	CanardInterface *_can_interface;
+
+	CanardInstance _canard_instance;
+
+	CanardTxQueue _queue;
+
+	void *_uavcan_heap{nullptr};
+
+};

src/drivers/cyphal/CanardInterface.hpp

@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ *   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>
+
+/// One frame stored in the transmission queue along with its metadata.
+struct CanardRxFrame {
+	CanardMicrosecond timestamp_usec;
+
+	/// The actual CAN frame data.
+	CanardFrame frame;
+};
+
+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 CanardTxQueueItem &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(CanardRxFrame *rxf) = 0;
+
+private:
+
+};

src/drivers/cyphal/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 CanardTxQueueItem &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.frame.extended_can_id;
+
+	transmit_msg.cm_hdr.ch_dlc = txf.frame.payload_size;
+
+	transmit_msg.cm_hdr.ch_extid = 1;
+
+	memcpy(transmit_msg.cm_data, txf.frame.payload, txf.frame.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(CanardRxFrame *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, 0);
+
+	// 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->frame.extended_can_id = receive_msg.cm_hdr.ch_id;
+			received_frame->frame.payload_size = receive_msg.cm_hdr.ch_dlc;
+			memcpy((void *)received_frame->frame.payload, receive_msg.cm_data, receive_msg.cm_hdr.ch_dlc);
+			return nbytes;
+		}
+	}
+
+	return 0;
+}

src/drivers/cyphal/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 CanardTxQueueItem to the CanardSocketInstance socket
+	/// This function is blocking
+	/// The return value is number of bytes transferred, negative value on error.
+	int16_t transmit(const CanardTxQueueItem &txframe, int timeout_ms = 0);
+
+	/// Receive a CanardRxFrame from the CanardSocketInstance socket
+	/// This function is blocking
+	/// The return value is number of bytes received, negative value on error.
+	int16_t receive(CanardRxFrame *rxf);
+
+private:
+	int _fd{-1};
+	bool _can_fd{false};
+};

src/drivers/cyphal/CanardSocketCAN.cpp

@@ -0,0 +1,211 @@
+/****************************************************************************
+ *
+ *   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>
+
+uint64_t getMonotonicTimestampUSec(void)
+{
+	struct timespec ts {};
+
+	clock_gettime(CLOCK_MONOTONIC, &ts);
+
+	return ts.tv_sec * 1000000ULL + ts.tv_nsec / 1000ULL;
+}
+
+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 CanardTxQueueItem &txf, int timeout_ms)
+{
+	/* Copy CanardFrame to can_frame/canfd_frame */
+	if (_can_fd) {
+		_send_frame.can_id = txf.frame.extended_can_id | CAN_EFF_FLAG;
+		_send_frame.len = txf.frame.payload_size;
+		memcpy(&_send_frame.data, txf.frame.payload, txf.frame.payload_size);
+
+	} else {
+		struct can_frame *frame = (struct can_frame *)&_send_frame;
+		frame->can_id = txf.frame.extended_can_id | CAN_EFF_FLAG;
+		frame->can_dlc = txf.frame.payload_size;
+		memcpy(&frame->data, txf.frame.payload, txf.frame.payload_size);
+	}
+
+	uint64_t deadline_systick = getMonotonicTimestampUSec() + (txf.tx_deadline_usec - hrt_absolute_time()) +
+				    CONFIG_USEC_PER_TICK; // Compensate for precision loss when converting hrt to systick
+
+	/* Set CAN_RAW_TX_DEADLINE timestamp  */
+	_send_tv->tv_usec = deadline_systick % 1000000ULL;
+	_send_tv->tv_sec = (deadline_systick - _send_tv->tv_usec) / 1000000ULL;
+
+	return sendmsg(_fd, &_send_msg, 0);
+}
+
+int16_t CanardSocketCAN::receive(CanardRxFrame *rxf)
+{
+	int32_t result = recvmsg(_fd, &_recv_msg, MSG_DONTWAIT);
+
+	if (result < 0) {
+		return result;
+	}
+
+	/* Copy CAN frame to CanardFrame */
+
+	if (_can_fd) {
+		struct canfd_frame *recv_frame = (struct canfd_frame *)&_recv_frame;
+		rxf->frame.extended_can_id = recv_frame->can_id & CAN_EFF_MASK;
+		rxf->frame.payload_size = recv_frame->len;
+		rxf->frame.payload = &recv_frame->data;
+
+	} else {
+		struct can_frame *recv_frame = (struct can_frame *)&_recv_frame;
+		rxf->frame.extended_can_id = recv_frame->can_id & CAN_EFF_MASK;
+		rxf->frame.payload_size = recv_frame->can_dlc;
+		rxf->frame.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/cyphal/CanardSocketCAN.hpp

@@ -0,0 +1,103 @@
+/****************************************************************************
+ *
+ *   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 <unistd.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();
+
+	/// Close socket connection
+	int close()
+	{
+		return ::close(_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 transmit(const CanardTxQueueItem &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(CanardRxFrame *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/cyphal/Cyphal.cpp

@@ -0,0 +1,408 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020-2021 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 "Cyphal.hpp"
+
+#include <lib/geo/geo.h>
+#include <lib/version/version.h>
+
+
+#ifdef CONFIG_CYPHAL_APP_DESCRIPTOR
+#include "boot_app_shared.h"
+/*
+ * This is the AppImageDescriptor used
+ * by the make_can_boot_descriptor.py tool to set
+ * the application image's descriptor so that the
+ * uavcan bootloader has the ability to validate the
+ * image crc, size etc of this application
+*/
+boot_app_shared_section app_descriptor_t AppDescriptor = {
+	.signature = APP_DESCRIPTOR_SIGNATURE,
+	.image_crc = 0,
+	.image_size = 0,
+	.git_hash  = 0,
+	.major_version = APP_VERSION_MAJOR,
+	.minor_version = APP_VERSION_MINOR,
+	.board_id = HW_VERSION_MAJOR << 8 | HW_VERSION_MINOR,
+	.reserved = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff }
+};
+#endif
+
+using namespace time_literals;
+
+CyphalNode *CyphalNode::_instance;
+
+CyphalNode::CyphalNode(uint32_t node_id, size_t capacity, size_t mtu_bytes) :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan),
+	_canard_handle(node_id, capacity, mtu_bytes)
+{
+	pthread_mutex_init(&_node_mutex, nullptr);
+
+
+#ifdef CONFIG_CYPHAL_NODE_MANAGER
+	_node_manager.subscribe();
+#endif
+
+#ifdef CONFIG_CYPHAL_NODE_CLIENT
+	_node_client = new NodeClient(_canard_handle, _param_manager);
+
+	_node_client->subscribe();
+#endif
+
+	_pub_manager.updateParams();
+
+	_sub_manager.subscribe();
+}
+
+CyphalNode::~CyphalNode()
+{
+	if (_instance) {
+		/* tell the task we want it to go away */
+		_task_should_exit.store(true);
+		ScheduleNow();
+
+		unsigned i = 1000;
+
+		do {
+			/* Wait for it to exit or timeout */
+			usleep(5000);
+
+			if (--i == 0) {
+				PX4_ERR("Failed to Stop Task - reboot needed");
+				break;
+			}
+
+		} while (_instance);
+	}
+
+	perf_free(_cycle_perf);
+	perf_free(_interval_perf);
+}
+
+int CyphalNode::start(uint32_t node_id, uint32_t bitrate)
+{
+	if (_instance != nullptr) {
+		PX4_WARN("Already started");
+		return -1;
+	}
+
+	bool can_fd = false;
+
+	if (can_fd) {
+		_instance = new CyphalNode(node_id, 8, CANARD_MTU_CAN_FD);
+
+	} else {
+		_instance = new CyphalNode(node_id, 64, CANARD_MTU_CAN_CLASSIC);
+	}
+
+	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 CyphalNode::init()
+{
+	// interface init
+	if (_canard_handle.init()) {
+		_initialized = true;
+	}
+
+}
+
+void CyphalNode::Run()
+{
+	pthread_mutex_lock(&_node_mutex);
+
+	if (_instance != nullptr && !_initialized) {
+		init();
+
+		// 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();
+
+		// Update dynamic pub/sub objects based on Port ID params
+		_pub_manager.updateParams();
+		_sub_manager.updateParams();
+
+		_mixing_output.updateParams();
+	}
+
+	perf_begin(_cycle_perf);
+	perf_count(_interval_perf);
+
+	if (_canard_handle.node_id() != CANARD_NODE_ID_UNSET) {
+		// send uavcan::node::Heartbeat_1_0 @ 1 Hz
+		sendHeartbeat();
+
+		// Check all publishers
+		_pub_manager.update();
+
+#ifdef CONFIG_CYPHAL_NODE_MANAGER
+		_node_manager.update();
+#endif
+	}
+
+#ifdef CONFIG_CYPHAL_NODE_CLIENT
+
+	else if (_node_client != nullptr) {
+		if (_canard_handle.node_id() == CANARD_NODE_ID_UNSET) {
+			_node_client->update();
+
+		} else {
+			delete _node_client;
+		}
+	}
+
+#endif
+
+	_canard_handle.transmit();
+
+	_canard_handle.receive();
+
+	// Pop canardTx queue to send out responses to requests
+	_canard_handle.transmit();
+
+	perf_end(_cycle_perf);
+
+	if (_instance && _task_should_exit.load()) {
+		ScheduleClear();
+
+		if (_initialized) {
+			_initialized = false;
+		}
+
+		_instance = nullptr;
+	}
+
+	pthread_mutex_unlock(&_node_mutex);
+}
+
+template <typename As, typename F>
+static void traverseTree(const CanardTreeNode *const root, const F &op)  // NOLINT this recursion is tightly bounded
+{
+	if (root != nullptr) {
+		traverseTree<As, F>(root->lr[0], op);
+		op(static_cast<const As *>(static_cast<const void *>(root)));
+		traverseTree<As, F>(root->lr[1], op);
+	}
+}
+
+void CyphalNode::print_info()
+{
+	pthread_mutex_lock(&_node_mutex);
+
+	perf_print_counter(_cycle_perf);
+	perf_print_counter(_interval_perf);
+
+	/*O1HeapDiagnostics heap_diagnostics = o1heapGetDiagnostics(uavcan_allocator);
+
+	PX4_INFO("Heap status %zu/%zu Peak alloc %zu Peak req %zu OOM count %" PRIu64,
+		 heap_diagnostics.allocated, heap_diagnostics.capacity,
+		 heap_diagnostics.peak_allocated, heap_diagnostics.peak_request_size,
+		 heap_diagnostics.oom_count);*/
+
+	_pub_manager.printInfo();
+
+	traverseTree<CanardRxSubscription>(_canard_handle.getRxSubscriptions(CanardTransferKindMessage),
+	[&](const CanardRxSubscription * const sub) {
+		if (sub->user_reference == nullptr) {
+			PX4_INFO("Message port id %d", sub->port_id);
+
+		} else {
+			((UavcanBaseSubscriber *)sub->user_reference)->printInfo();
+		}
+	});
+
+
+	traverseTree<CanardRxSubscription>(_canard_handle.getRxSubscriptions(CanardTransferKindRequest),
+	[&](const CanardRxSubscription * const sub) {
+		if (sub->user_reference == nullptr) {
+			PX4_INFO("Service response port id %d", sub->port_id);
+
+		} else {
+			((UavcanBaseSubscriber *)sub->user_reference)->printInfo();
+		}
+	});
+
+	traverseTree<CanardRxSubscription>(_canard_handle.getRxSubscriptions(CanardTransferKindResponse),
+	[&](const CanardRxSubscription * const sub) {
+		if (sub->user_reference == nullptr) {
+			PX4_INFO("Service request port id %d", sub->port_id);
+
+		} else {
+			((UavcanBaseSubscriber *)sub->user_reference)->printInfo();
+		}
+	});
+
+	_mixing_output.printInfo();
+
+	pthread_mutex_unlock(&_node_mutex);
+}
+
+static void print_usage()
+{
+	PX4_INFO("usage: \n"
+		 "\tuavcannode {start|status|stop}");
+}
+
+extern "C" __EXPORT int cyphal_main(int argc, char *argv[])
+{
+	if (argc < 2) {
+		print_usage();
+		return 1;
+	}
+
+	if (!strcmp(argv[1], "start")) {
+		if (CyphalNode::instance()) {
+			PX4_ERR("already started");
+			return 1;
+		}
+
+		// CAN bitrate
+		int32_t bitrate = 0;
+		param_get(param_find("CYPHAL_BAUD"), &bitrate);
+
+		// Node ID
+		int32_t node_id = 0;
+		param_get(param_find("CYPHAL_ID"), &node_id);
+
+		if (node_id == -1) {
+			node_id = CANARD_NODE_ID_UNSET;
+		}
+
+		// Start
+		PX4_INFO("Node ID %" PRIu32 ", bitrate %" PRIu32, node_id, bitrate);
+		return CyphalNode::start(node_id, bitrate);
+	}
+
+	/* commands below require the app to be started */
+	CyphalNode *const inst = CyphalNode::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;
+}
+
+void CyphalNode::sendHeartbeat()
+{
+	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.value = uavcan_node_Health_1_0_NOMINAL;
+		heartbeat.mode.value = uavcan_node_Mode_1_0_OPERATIONAL;
+		const hrt_abstime now = hrt_absolute_time();
+		size_t payload_size = uavcan_node_Heartbeat_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_;
+
+		const CanardTransferMetadata transfer_metadata = {
+			.priority       = CanardPriorityNominal,
+			.transfer_kind  = CanardTransferKindMessage,
+			.port_id        = uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_,
+			.remote_node_id = CANARD_NODE_ID_UNSET,
+			.transfer_id    = _uavcan_node_heartbeat_transfer_id++
+		};
+
+		uavcan_node_Heartbeat_1_0_serialize_(&heartbeat, _uavcan_node_heartbeat_buffer, &payload_size);
+
+		int32_t result = _canard_handle.TxPush(now + PUBLISHER_DEFAULT_TIMEOUT_USEC,
+						       &transfer_metadata,
+						       payload_size,
+						       &_uavcan_node_heartbeat_buffer
+						      );
+
+		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 %" PRId32 "", result);
+		}
+
+		_uavcan_node_heartbeat_last = now;
+	}
+}
+
+bool UavcanMixingInterface::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
+		unsigned num_control_groups_updated)
+{
+	// Note: This gets called from MixingOutput from within its update() function
+	// Hence, the mutex lock in UavcanMixingInterface::Run() is in effect
+
+	/// TODO: Need esc/servo metadata / bitmask(s)
+	_esc_controller.update_outputs(stop_motors, outputs, num_outputs);
+	// _servo_controller.update_outputs(stop_motors, outputs, num_outputs);
+
+	return true;
+}
+
+void UavcanMixingInterface::Run()
+{
+	pthread_mutex_lock(&_node_mutex);
+	_mixing_output.update();
+	_mixing_output.updateSubscriptions();
+	pthread_mutex_unlock(&_node_mutex);
+}

src/drivers/cyphal/Cyphal.hpp

@@ -0,0 +1,188 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020-2021 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/mixer_module/mixer_module.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/actuator_outputs.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_gps.h>
+
+#include <canard.h>
+
+#include "CanardInterface.hpp"
+
+#include "Publishers/Publisher.hpp"
+#include "Publishers/uORB/uorb_publisher.hpp"
+
+#ifdef CONFIG_CYPHAL_NODE_MANAGER
+#include "NodeManager.hpp"
+#endif
+
+#ifdef CONFIG_CYPHAL_NODE_CLIENT
+#include "NodeClient.hpp"
+#endif
+
+#include "PublicationManager.hpp"
+#include "SubscriptionManager.hpp"
+
+#include "Actuators/EscClient.hpp" /// TODO: Add EscServer.hpp for node-side service
+
+/**
+ * UAVCAN mixing class.
+ * It is separate from CyphalNode to have 2 WorkItems and therefore allowing independent scheduling
+ * (I.e. UavcanMixingInterface runs upon actuator_control updates, whereas CyphalNode runs at
+ * a fixed rate or upon bus updates).
+ * Both work items are expected to run on the same work queue.
+ */
+class UavcanMixingInterface : public OutputModuleInterface
+{
+public:
+	UavcanMixingInterface(pthread_mutex_t &node_mutex,
+			      UavcanEscController &esc_controller) //, UavcanServoController &servo_controller)
+		: OutputModuleInterface(MODULE_NAME "-actuators", px4::wq_configurations::uavcan),
+		  _node_mutex(node_mutex),
+		  _esc_controller(esc_controller)/*,
+		  _servo_controller(servo_controller)*/ {}
+
+	bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
+			   unsigned num_outputs, unsigned num_control_groups_updated) override;
+
+	void mixerChanged() override {};
+
+	void printInfo() { _mixing_output.printStatus(); }
+
+	MixingOutput &mixingOutput() { return _mixing_output; }
+
+	/// For use with PR-16808 once merged
+	// const char *get_param_prefix() override { return "UCAN1_ACT"; }
+
+protected:
+	void Run() override;
+private:
+	friend class CyphalNode;
+	pthread_mutex_t &_node_mutex;
+	UavcanEscController &_esc_controller;
+	// UavcanServoController &_servo_controller;
+	MixingOutput _mixing_output{"UCAN1_ESC", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
+};
+
+class CyphalNode : public ModuleParams, public px4::ScheduledWorkItem
+{
+
+	/*
+	 * 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:
+
+	CyphalNode(uint32_t node_id, size_t capacity, size_t mtu_bytes);
+	~CyphalNode() override;
+
+	static int start(uint32_t node_id, uint32_t bitrate);
+
+	void print_info();
+
+	static CyphalNode *instance() { return _instance; }
+
+	/* The bit rate that can be passed back to the bootloader */
+	int32_t active_bitrate{0};
+
+private:
+	void init();
+	void Run() override;
+	void fill_node_info();
+
+	// Sends a heartbeat at 1s intervals
+	void sendHeartbeat();
+
+	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 CyphalNode *_instance;
+
+	CanardHandle _canard_handle;
+
+	pthread_mutex_t _node_mutex;
+
+	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
+
+	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_EXTENT_BYTES_];
+	hrt_abstime _uavcan_node_heartbeat_last{0};
+	CanardTransferID _uavcan_node_heartbeat_transfer_id{0};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::CYPHAL_ENABLE>) _param_uavcan_v1_enable,
+		(ParamInt<px4::params::CYPHAL_ID>) _param_uavcan_v1_id,
+		(ParamInt<px4::params::CYPHAL_BAUD>) _param_uavcan_v1_baud
+	)
+
+	UavcanParamManager _param_manager;
+
+#ifdef CONFIG_CYPHAL_NODE_MANAGER
+	NodeManager _node_manager {_canard_handle, _param_manager};
+#endif
+
+#ifdef CONFIG_CYPHAL_NODE_CLIENT
+	NodeClient *_node_client {nullptr};
+#endif
+
+	PublicationManager _pub_manager {_canard_handle, _param_manager};
+	SubscriptionManager _sub_manager {_canard_handle, _param_manager};
+
+	/// TODO: Integrate with PublicationManager
+	UavcanEscController _esc_controller {_canard_handle, _param_manager};
+
+	UavcanMixingInterface _mixing_output {_node_mutex, _esc_controller};
+
+};

src/drivers/cyphal/Kconfig

@@ -0,0 +1,110 @@
+#
+# For a description of the syntax of this configuration file,
+# see the file kconfig-language.txt in the NuttX tools repository.
+#
+menuconfig DRIVERS_CYPHAL
+	bool "Cyphal"
+	default n
+	---help---
+		Enable support for Cyphal
+
+if DRIVERS_CYPHAL
+    choice
+        prompt "Cyphal Mode"
+
+    config CYPHAL_FMU
+        bool "Server (FMU)"
+
+    config CYPHAL_CLIENT
+        bool "Client (Peripheral)"
+
+    endchoice
+
+    config CYPHAL_NODE_MANAGER
+        bool "Node manager"
+        default y
+        depends on CYPHAL_FMU
+        help
+            Implement Cyphal PNP server functionality and manages discovered nodes
+
+    config CYPHAL_NODE_CLIENT
+        bool "Node client"
+        default y
+        depends on CYPHAL_CLIENT
+        help
+            Implement Cyphal PNP client functionality
+
+    config CYPHAL_APP_DESCRIPTOR
+        bool "UAVCAN v0 bootloader app descriptor"
+        default n
+        depends on CYPHAL_CLIENT && DRIVERS_BOOTLOADERS
+        help
+            When the board uses the UAVCANv0 bootloader functionality you need a AppImageDescriptor defined
+
+
+    menu "Publisher support"
+
+        config CYPHAL_GNSS_PUBLISHER
+            bool "GNSS Publisher"
+            default n
+
+        config CYPHAL_ESC_CONTROLLER
+            bool "ESC Controller"
+            default n
+
+        config CYPHAL_READINESS_PUBLISHER
+            bool "Readiness Publisher"
+            default n
+
+        config CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER
+            bool "uORB actuator_outputs publisher"
+            default n
+
+        config CYPHAL_UORB_SENSOR_GPS_PUBLISHER
+            bool "uORB sensor_gps publisher"
+            default n
+
+    endmenu
+
+    menu "Subscriber support"
+
+        config CYPHAL_ESC_SUBSCRIBER
+            bool "ESC Subscriber"
+            default n
+
+        config CYPHAL_GNSS_SUBSCRIBER_0
+            bool "GNSS Subscriber 0"
+            default n
+
+        config CYPHAL_GNSS_SUBSCRIBER_1
+            bool "GNSS Subscriber 1"
+            default n
+
+        config CYPHAL_BMS_SUBSCRIBER
+            bool "BMS Subscriber"
+            default n
+
+        config CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER
+            bool "uORB sensor_gps Subscriber"
+            default n
+    endmenu
+
+    menu "Advertised Services"
+        config CYPHAL_GETINFO_RESPONDER
+            bool "GetInfo1.0 responder"
+            default y
+            help
+                Responds to uavcan.node.GetInfo.1.0 request sending over node information
+                See https://github.com/UAVCAN/public_regulated_data_types/blob/master/uavcan/node/430.GetInfo.1.0.uavcan for full response
+
+        config CYPHAL_EXECUTECOMMAND_RESPONDER
+            bool "ExecuteCommand1.0 responder"
+            default n
+            help
+                To be implemented
+    endmenu
+
+    menu "Service invokers"
+    endmenu
+
+endif #DRIVERS_CYPHAL

src/drivers/cyphal/NodeClient.cpp

@@ -0,0 +1,175 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file NodeClient.cpp
+ *
+ * Defines basic implementation of client UAVCAN PNP for requesting a Node ID
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#define PNP_UNIQUE_ID_SIZE 16 // 128 bit unique id
+
+#include <crc64.h>
+#include "NodeClient.hpp"
+
+void NodeClient::callback(const CanardRxTransfer &receive)
+{
+	if (receive.metadata.remote_node_id != CANARD_NODE_ID_UNSET && _canard_handle.node_id() == CANARD_NODE_ID_UNSET) {
+
+		int32_t allocated = CANARD_NODE_ID_UNSET;
+		px4_guid_t px4_guid;
+		board_get_px4_guid(px4_guid);
+
+		if (_canard_handle.mtu() == CANARD_MTU_CAN_FD) {
+			uavcan_pnp_NodeIDAllocationData_2_0 msg;
+
+			size_t msg_size_in_bytes = receive.payload_size;
+			uavcan_pnp_NodeIDAllocationData_2_0_deserialize_(&msg, (const uint8_t *)receive.payload,
+					&msg_size_in_bytes);
+
+			if (memcmp(msg.unique_id, px4_guid, sizeof(msg.unique_id)) == 0) {
+				allocated = msg.node_id.value;
+			}
+
+		} else {
+			uavcan_pnp_NodeIDAllocationData_1_0 msg;
+
+			size_t msg_size_in_bytes = receive.payload_size;
+			uavcan_pnp_NodeIDAllocationData_1_0_deserialize_(&msg, (const uint8_t *)receive.payload,
+					&msg_size_in_bytes);
+
+			if (msg.allocated_node_id.count > 0) {
+				if (msg.unique_id_hash == (crc64(px4_guid, PNP_UNIQUE_ID_SIZE) & 0xFFFFFFFFFFFF)) {
+					allocated = msg.allocated_node_id.elements[0].value;
+				}
+			}
+		}
+
+		if (allocated == CANARD_NODE_ID_UNSET) {
+			return;        // UID mismatch.
+		}
+
+		if (allocated <= 0 || allocated >= (int32_t)CANARD_NODE_ID_MAX)
+			// Allocated node-ID ignored because it exceeds max_node_id
+		{
+			return;
+		}
+
+		_canard_handle.set_node_id(allocated);
+
+		PX4_INFO("Allocated Node ID %d", _canard_handle.node_id());
+
+	}
+}
+
+
+void NodeClient::update()
+{
+	if (hrt_elapsed_time(&_nodealloc_request_last) >= hrt_abstime(2 *
+			1000000ULL)) { // Compiler hates me here, some 1_s doesn't work
+
+		int32_t result;
+
+		// Allocation already done, nothing to do
+		if (_canard_handle.node_id() != CANARD_NODE_ID_UNSET) {
+			return;
+		}
+
+		if (_canard_handle.mtu() == 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];
+			size_t payload_size = PNP2_PAYLOAD_SIZE;
+
+			px4_guid_t px4_guid;
+			board_get_px4_guid(px4_guid);
+			memcpy(node_id_alloc_msg.unique_id, px4_guid, sizeof(node_id_alloc_msg.unique_id));
+			//node_id_alloc_msg.node_id.value = preffered_node_id; //FIXME preffered ID PX4 Param
+
+			const CanardTransferMetadata transfer_metadata = {
+				.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,
+			};
+
+			result = uavcan_pnp_NodeIDAllocationData_2_0_serialize_(&node_id_alloc_msg, (uint8_t *)&node_id_alloc_payload_buffer,
+					&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.
+				_canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
+						      &transfer_metadata,
+						      payload_size,
+						      &node_id_alloc_payload_buffer);
+			}
+
+		} 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];
+			size_t payload_size = PNP1_PAYLOAD_SIZE;
+
+			px4_guid_t px4_guid;
+			board_get_px4_guid(px4_guid);
+			node_id_alloc_msg.unique_id_hash = (crc64(px4_guid, PNP_UNIQUE_ID_SIZE) & 0xFFFFFFFFFFFF);
+
+			const CanardTransferMetadata transfer_metadata = {
+				.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,
+			};
+
+			result = uavcan_pnp_NodeIDAllocationData_1_0_serialize_(&node_id_alloc_msg, (uint8_t *)&node_id_alloc_payload_buffer,
+					&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.
+				_canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
+						      &transfer_metadata,
+						      payload_size,
+						      &node_id_alloc_payload_buffer);
+			}
+		}
+
+		_nodealloc_request_last = hrt_absolute_time();
+	}
+}

src/drivers/cyphal/NodeClient.hpp

@@ -0,0 +1,92 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file NodeClient.hpp
+ *
+ * Defines basic implementation of UAVCAN PNP for dynamic Node ID
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#pragma once
+
+
+#include <px4_platform_common/defines.h>
+#include <drivers/drv_hrt.h>
+
+#include "CanardInterface.hpp"
+
+#include <uavcan/node/ID_1_0.h>
+#include <uavcan/pnp/NodeIDAllocationData_1_0.h>
+#include <uavcan/pnp/NodeIDAllocationData_2_0.h>
+
+#include "Services/AccessRequest.hpp"
+#include "Services/ListRequest.hpp"
+
+#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_
+
+class NodeClient : public UavcanBaseSubscriber
+{
+public:
+	NodeClient(CanardHandle &handle, UavcanParamManager &pmgr) : UavcanBaseSubscriber(handle, "", "NodeIDAllocationData",
+				0),
+		_canard_handle(handle) { };
+
+	void subscribe() override
+	{
+
+		_canard_handle.RxSubscribe(CanardTransferKindMessage,
+					   (_canard_handle.mtu() == CANARD_MTU_CAN_FD ? PNP2_PORT_ID : PNP1_PORT_ID),  // The fixed Subject-ID
+					   (_canard_handle.mtu() == CANARD_MTU_CAN_FD ? PNP2_PAYLOAD_SIZE : PNP1_PAYLOAD_SIZE),
+					   CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+					   &_subj_sub._canard_sub);
+	}
+
+	bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg);
+	bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_2_0 &msg);
+
+	void callback(const CanardRxTransfer &receive); // NodeIDAllocation callback
+
+	void update();
+
+private:
+
+	CanardHandle &_canard_handle;
+	CanardTransferID _node_id_alloc_transfer_id{0};
+
+	hrt_abstime _nodealloc_request_last{0};
+};

src/drivers/cyphal/NodeManager.cpp

@@ -0,0 +1,186 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file NodeIDManager.cpp
+ *
+ * Defines basic implementation of UAVCAN PNP for dynamic Node ID
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#define RETRY_COUNT 10
+
+#include "NodeManager.hpp"
+
+void NodeManager::callback(const CanardRxTransfer &receive)
+{
+	if (_canard_handle.mtu() == CANARD_MTU_CAN_FD) {
+		uavcan_pnp_NodeIDAllocationData_2_0 node_id_alloc_msg {};
+		size_t msg_size_in_bytes = receive.payload_size;
+		uavcan_pnp_NodeIDAllocationData_2_0_deserialize_(&node_id_alloc_msg, (const uint8_t *)receive.payload,
+				&msg_size_in_bytes);
+		/// do something with the data
+		HandleNodeIDRequest(node_id_alloc_msg);
+
+	} else {
+		uavcan_pnp_NodeIDAllocationData_1_0 node_id_alloc_msg {};
+		size_t msg_size_in_bytes = receive.payload_size;
+		uavcan_pnp_NodeIDAllocationData_1_0_deserialize_(&node_id_alloc_msg, (const uint8_t *)receive.payload,
+				&msg_size_in_bytes);
+		/// do something with the data
+		HandleNodeIDRequest(node_id_alloc_msg);
+	}
+}
+
+bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg)
+{
+	if (msg.allocated_node_id.count == 0) {
+		uint32_t i;
+
+		msg.allocated_node_id.count = 1;
+		msg.allocated_node_id.elements[0].value = CANARD_NODE_ID_UNSET;
+
+		/* Search for an available NodeID to assign */
+		for (i = 1; i < sizeof(nodeid_registry) / sizeof(nodeid_registry[0]); i++) {
+			if (i == _canard_handle.node_id()) {
+				continue; // Don't give our NodeID to a node
+
+			} else if (nodeid_registry[i].node_id == 0) { // Unused
+				nodeid_registry[i].node_id = i;
+				memcpy(&nodeid_registry[i].unique_id, &msg.unique_id_hash, 6);
+				break;
+
+			} else if (memcmp(&nodeid_registry[i].unique_id[0], &msg.unique_id_hash, 6) == 0) {
+				msg.allocated_node_id.elements[0].value = nodeid_registry[i].node_id; // Existing NodeID
+				break;
+			}
+		}
+
+		nodeid_registry[i].register_setup = false; // Re-instantiate register setup
+		nodeid_registry[i].register_index = 0;
+		nodeid_registry[i].retry_count = 0;
+
+		if (msg.allocated_node_id.elements[0].value != CANARD_NODE_ID_UNSET) {
+
+			PX4_INFO("Received NodeID allocation request assigning %i", msg.allocated_node_id.elements[0].value);
+
+			uint8_t node_id_alloc_payload_buffer[uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
+			size_t payload_size = uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_;
+
+			const CanardTransferMetadata transfer_metadata = {
+				.priority       = CanardPriorityNominal,
+				.transfer_kind  = CanardTransferKindMessage,
+				.port_id        = uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_, // This is the subject-ID.
+				.remote_node_id = CANARD_NODE_ID_UNSET,       // Messages cannot be unicast, so use UNSET.
+				.transfer_id    = _uavcan_pnp_nodeidallocation_v1_transfer_id,
+			};
+
+			int result = uavcan_pnp_NodeIDAllocationData_1_0_serialize_(&msg, node_id_alloc_payload_buffer, &payload_size);
+
+			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.
+				_canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
+						      &transfer_metadata,
+						      payload_size,
+						      &node_id_alloc_payload_buffer);
+			}
+
+			_register_request_last = hrt_absolute_time();
+
+			return result >= 0;
+		}
+	}
+
+	return false;
+}
+
+bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_2_0 &msg)
+{
+	//TODO v2 node id
+	return false;
+}
+
+
+void NodeManager::HandleListResponse(const CanardRxTransfer &receive)
+{
+	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);
+
+	if (msg.name.name.count == 0) {
+		// Index doesn't exist, we've parsed through all registers
+		for (uint32_t i = 0; i < sizeof(nodeid_registry) / sizeof(nodeid_registry[0]); i++) {
+			if (nodeid_registry[i].node_id == receive.metadata.remote_node_id) {
+				nodeid_registry[i].register_setup = true; // Don't update anymore
+			}
+		}
+
+	} else {
+		for (uint32_t i = 0; i < sizeof(nodeid_registry) / sizeof(nodeid_registry[0]); i++) {
+			if (nodeid_registry[i].node_id == receive.metadata.remote_node_id) {
+				nodeid_registry[i].register_index++; // Increment index counter for next update()
+				nodeid_registry[i].register_setup = false;
+				nodeid_registry[i].retry_count = 0;
+			}
+		}
+
+		if (_access_request.setPortId(receive.metadata.remote_node_id, msg.name, NULL)) { //FIXME confirm handler
+			PX4_INFO("Set portID succesfull");
+
+		} else {
+			PX4_INFO("Register not found %.*s", msg.name.name.count, msg.name.name.elements);
+		}
+	}
+}
+void NodeManager::update()
+{
+	if (hrt_elapsed_time(&_register_request_last) >= hrt_abstime(2 *
+			1000000ULL)) { // Compiler hates me here, some 1_s doesn't work
+		for (uint32_t i = 0; i < sizeof(nodeid_registry) / sizeof(nodeid_registry[0]); i++) {
+			if (nodeid_registry[i].node_id != 0 && nodeid_registry[i].register_setup == false) {
+				//Setting up registers
+				_list_request.getIndex(nodeid_registry[i].node_id, nodeid_registry[i].register_index, this);
+				nodeid_registry[i].retry_count++;
+
+				if (nodeid_registry[i].retry_count > RETRY_COUNT) {
+					nodeid_registry[i].register_setup = true; // Don't update anymore
+				}
+			}
+		}
+
+		_register_request_last = hrt_absolute_time();
+	}
+}

src/drivers/cyphal/NodeManager.hpp

@@ -0,0 +1,113 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file NodeManager.hpp
+ *
+ * Defines basic implementation of UAVCAN PNP for dynamic Node ID
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#pragma once
+
+
+#include <px4_platform_common/defines.h>
+#include <drivers/drv_hrt.h>
+
+#include "CanardInterface.hpp"
+
+#include <uavcan/node/ID_1_0.h>
+#include <uavcan/pnp/NodeIDAllocationData_1_0.h>
+#include <uavcan/pnp/NodeIDAllocationData_2_0.h>
+
+#include "Services/AccessRequest.hpp"
+#include "Services/ListRequest.hpp"
+
+#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_
+
+typedef struct {
+	uint8_t   node_id;
+	uint8_t   unique_id[16];
+	bool      register_setup;
+	uint16_t  register_index;
+	uint16_t  retry_count;
+} UavcanNodeEntry;
+
+class NodeManager : public UavcanBaseSubscriber, public UavcanServiceRequestInterface
+{
+public:
+	NodeManager(CanardHandle &handle, UavcanParamManager &pmgr) : UavcanBaseSubscriber(handle, "", "NodeIDAllocationData",
+				0),
+		_canard_handle(handle), _access_request(handle, pmgr), _list_request(handle) { };
+
+	void subscribe() override
+	{
+		_access_request.subscribe();
+		_list_request.subscribe();
+
+		_canard_handle.RxSubscribe(CanardTransferKindMessage,
+					   (_canard_handle.mtu() == CANARD_MTU_CAN_FD ? PNP2_PORT_ID : PNP1_PORT_ID),  // The fixed Subject-ID
+					   (_canard_handle.mtu() == CANARD_MTU_CAN_FD ? PNP2_PAYLOAD_SIZE : PNP1_PAYLOAD_SIZE),
+					   CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+					   &_subj_sub._canard_sub);
+	}
+
+	bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg);
+	bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_2_0 &msg);
+
+	void response_callback(const CanardRxTransfer &receive) override
+	{
+		HandleListResponse(receive);
+	}
+	void callback(const CanardRxTransfer &receive); // NodeIDAllocation callback
+
+	void HandleListResponse(const CanardRxTransfer &receive);
+
+	void update();
+
+private:
+	CanardHandle &_canard_handle;
+	CanardTransferID _uavcan_pnp_nodeidallocation_v1_transfer_id{0};
+	UavcanNodeEntry nodeid_registry[16] {0}; //TODO configurable or just rewrite
+
+	UavcanAccessServiceRequest _access_request;
+	UavcanListServiceRequest _list_request;
+
+	bool nodeRegisterSetup = 0;
+
+	hrt_abstime _register_request_last{0};
+};

src/drivers/cyphal/ParamManager.cpp

@@ -0,0 +1,107 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ParamManager.cpp
+ *
+ * Implements basic functionality of UAVCAN parameter management class
+ *
+ * @author Jacob Crabill <jacob@flyvoly.com>
+ */
+
+#include "ParamManager.hpp"
+#include <px4_platform_common/defines.h>
+
+bool UavcanParamManager::GetParamByName(const char *param_name, uavcan_register_Value_1_0 &value)
+{
+	for (auto &param : _uavcan_params) {
+		if (strcmp(param_name, param.uavcan_name) == 0) {
+			param_t param_handle = param_find(param.px4_name);
+
+			if (param_handle == PARAM_INVALID) {
+				return false;
+			}
+
+			return param.px4_param_to_register_value(param_handle, value);
+		}
+	}
+
+	return false;
+}
+
+bool UavcanParamManager::GetParamByName(const uavcan_register_Name_1_0 &name, uavcan_register_Value_1_0 &value)
+{
+	for (auto &param : _uavcan_params) {
+		if (strncmp((char *)name.name.elements, param.uavcan_name, name.name.count) == 0) {
+			param_t param_handle = param_find(param.px4_name);
+
+			if (param_handle == PARAM_INVALID) {
+				return false;
+			}
+
+			return param.px4_param_to_register_value(param_handle, value);
+		}
+	}
+
+	return false;
+}
+
+bool UavcanParamManager::GetParamName(uint32_t id, uavcan_register_Name_1_0 &name)
+{
+	if (id >= sizeof(_uavcan_params) / sizeof(UavcanParamBinder)) {
+		return false;
+	}
+
+	strncpy((char *)name.name.elements, _uavcan_params[id].uavcan_name, uavcan_register_Name_1_0_name_ARRAY_CAPACITY_);
+
+	name.name.count = strlen(_uavcan_params[id].uavcan_name);
+
+	return true;
+}
+
+bool UavcanParamManager::SetParamByName(const uavcan_register_Name_1_0 &name, const uavcan_register_Value_1_0 &value)
+{
+	for (auto &param : _uavcan_params) {
+		if (strncmp((char *)name.name.elements, param.uavcan_name, name.name.count) == 0) {
+			param_t param_handle = param_find(param.px4_name);
+
+			if (param_handle == PARAM_INVALID) {
+				return false;
+			}
+
+			return param.register_value_to_px4_param(value, param_handle);
+		}
+	}
+
+	return false;
+}

src/drivers/cyphal/ParamManager.hpp

@@ -0,0 +1,136 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ParamManager.hpp
+ *
+ * Defines basic functionality of UAVCAN parameter management class
+ *
+ * @author Jacob Crabill <jacob@flyvoly.com>
+ */
+
+#pragma once
+
+#include <lib/parameters/param.h>
+#include <px4_platform_common/defines.h>
+
+#include <uavcan/_register/Name_1_0.h>
+#include <uavcan/_register/Value_1_0.h>
+
+static constexpr uint16_t CANARD_PORT_ID_UNSET = 65535U;
+static constexpr uint16_t CANARD_PORT_ID_MAX   = 32767U;
+
+static bool px4_param_to_uavcan_port_id(param_t &in, uavcan_register_Value_1_0 &out)
+{
+	if (param_type(in) == PARAM_TYPE_INT32) {
+		int32_t out_val {};
+		const int res = param_get(in, &out_val);
+
+		if (res != PX4_OK) {
+			// Parameter not found / internal error
+			return false;
+		}
+
+		if (out_val >= 0 && out_val <= CANARD_PORT_ID_MAX) {
+			out.natural16.value.elements[0] = (uint16_t)out_val;
+
+		} else {
+			// "Invalid" value -- set to "UNSET"
+			out.natural16.value.elements[0] = CANARD_PORT_ID_UNSET;
+		}
+
+		out.natural16.value.count = 1;
+		uavcan_register_Value_1_0_select_natural16_(&out);
+		return true;
+	}
+
+	return false;
+};
+
+static bool uavcan_port_id_to_px4_param(const uavcan_register_Value_1_0 &in, param_t &out)
+{
+	if (uavcan_register_Value_1_0_is_natural16_(&in) && in.natural16.value.count == 1) {
+		if (param_type(out) == PARAM_TYPE_INT32) {
+			int32_t in_val = in.natural16.value.elements[0];
+			param_set(out, &in_val);
+			return true;
+		}
+	}
+
+	return false;
+};
+
+using param_2_reg_t = bool(*)(param_t &in, uavcan_register_Value_1_0 &out);
+using reg_2_param_t = bool(*)(const uavcan_register_Value_1_0 &in, param_t &out);
+
+
+typedef struct {
+	const char *uavcan_name;
+	const char *px4_name;
+	param_2_reg_t px4_param_to_register_value;
+	reg_2_param_t register_value_to_px4_param;
+	bool is_mutable {true};
+	bool is_persistent {true};
+} UavcanParamBinder;
+
+
+class UavcanParamManager
+{
+public:
+
+	bool GetParamByName(const char *param_name, uavcan_register_Value_1_0 &value);
+	bool GetParamByName(const uavcan_register_Name_1_0 &name, uavcan_register_Value_1_0 &value);
+	bool GetParamName(uint32_t id, uavcan_register_Name_1_0 &name);
+	bool SetParamByName(const uavcan_register_Name_1_0 &name, const uavcan_register_Value_1_0 &value);
+
+private:
+
+
+	const UavcanParamBinder _uavcan_params[13] {
+		{"uavcan.pub.esc.0.id",                "UCAN1_ESC_PUB",		    px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.pub.servo.0.id",              "UCAN1_SERVO_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.pub.gps.0.id",                "UCAN1_GPS_PUB",		    px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.pub.actuator_outputs.0.id",   "UCAN1_ACTR_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.esc.0.id",                "UCAN1_ESC0_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.gps.0.id",                "UCAN1_GPS0_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.gps.1.id",                "UCAN1_GPS1_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.energy_source.0.id",      "UCAN1_BMS_ES_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.battery_status.0.id",     "UCAN1_BMS_BS_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.battery_parameters.0.id", "UCAN1_BMS_BP_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.legacy_bms.0.id",         "UCAN1_LG_BMS_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.uorb.sensor_gps.0.id",    "UCAN1_UORB_GPS",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.pub.uorb.sensor_gps.0.id",    "UCAN1_UORB_GPS_P",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		//{"uavcan.sub.bms.0.id",   "UCAN1_BMS0_SUB"}, //FIXME instancing
+		//{"uavcan.sub.bms.1.id",   "UCAN1_BMS1_SUB"},
+	};
+};

src/drivers/cyphal/PublicationManager.cpp

@@ -0,0 +1,122 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file PublicationManager.cpp
+ *
+ * Manages the dynamic (run-time configurable) UAVCAN publications
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ * @author Jacob Crabill <jacob@flyvoly.com>
+ */
+
+
+#include "PublicationManager.hpp"
+
+PublicationManager::~PublicationManager()
+{
+	_dynpublishers.clear();
+}
+
+void PublicationManager::updateDynamicPublications()
+{
+	for (auto &sub : _uavcan_pubs) {
+
+		bool found_publisher = false;
+
+		for (auto &dynpub : _dynpublishers) {
+			// Check if subscriber has already been created
+			char full_subj_name[200];
+			snprintf(full_subj_name, sizeof(full_subj_name), "%s%s", dynpub->getPrefixName(), dynpub->getSubjectName());
+			const uint8_t instance = dynpub->getInstance();
+
+			if (strcmp(full_subj_name, sub.subject_name) == 0 && instance == sub.instance) {
+				found_publisher = true;
+				break;
+			}
+		}
+
+		if (found_publisher) {
+			continue;
+		}
+
+		char uavcan_param[90];
+		snprintf(uavcan_param, sizeof(uavcan_param), "uavcan.pub.%s.%d.id", sub.subject_name, sub.instance);
+		uavcan_register_Value_1_0 value;
+
+		if (_param_manager.GetParamByName(uavcan_param, value)) {
+			uint16_t port_id = value.natural16.value.elements[0];
+
+			if (port_id <= CANARD_PORT_ID_MAX) { // PortID is set, create a subscriber
+				UavcanPublisher *dynpub = sub.create_pub(_canard_handle, _param_manager);
+
+				if (dynpub == nullptr) {
+					PX4_ERR("Out of memory");
+					return;
+				}
+
+				_dynpublishers.add(dynpub);
+
+				dynpub->updateParam();
+			}
+
+		} else {
+			PX4_ERR("Port ID param for publisher %s.%u not found", sub.subject_name, sub.instance);
+			return;
+		}
+	}
+}
+
+void PublicationManager::printInfo()
+{
+	for (auto &dynpub : _dynpublishers) {
+		dynpub->printInfo();
+	}
+}
+
+void PublicationManager::updateParams()
+{
+	for (auto &dynpub : _dynpublishers) {
+		dynpub->updateParam();
+	}
+
+	// Check for any newly-enabled publication
+	updateDynamicPublications();
+}
+
+void PublicationManager::update()
+{
+	for (auto &dynpub : _dynpublishers) {
+		dynpub->update();
+	}
+}

src/drivers/cyphal/PublicationManager.hpp

@@ -0,0 +1,164 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file PublicationManager.hpp
+ *
+ * Manages the dynamic (run-time configurable) UAVCAN publications
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ * @author Jacob Crabill <jacob@flyvoly.com>
+ */
+
+#pragma once
+
+#include <px4_platform_common/px4_config.h>
+
+#ifndef CONFIG_CYPHAL_GNSS_PUBLISHER
+#define CONFIG_CYPHAL_GNSS_PUBLISHER 0
+#endif
+
+#ifndef CONFIG_CYPHAL_ESC_CONTROLLER
+#define CONFIG_CYPHAL_ESC_CONTROLLER 0
+#endif
+
+#ifndef CONFIG_CYPHAL_READINESS_PUBLISHER
+#define CONFIG_CYPHAL_READINESS_PUBLISHER 0
+#endif
+
+#ifndef CONFIG_CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER
+#define CONFIG_CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER 0
+#endif
+
+#ifndef CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER
+#define CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER 0
+#endif
+
+/* Preprocessor calculation of publisher count */
+
+#define UAVCAN_PUB_COUNT CONFIG_CYPHAL_GNSS_PUBLISHER + \
+	CONFIG_CYPHAL_ESC_CONTROLLER + \
+	CONFIG_CYPHAL_READINESS_PUBLISHER + \
+	CONFIG_CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER + \
+	CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER
+
+#include <px4_platform_common/defines.h>
+#include <drivers/drv_hrt.h>
+#include "Publishers/Publisher.hpp"
+#include "CanardInterface.hpp"
+
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/sensor_gps.h>
+
+#include "Actuators/EscClient.hpp"
+#include "Publishers/udral/Readiness.hpp"
+#include "Publishers/udral/Gnss.hpp"
+#include "Publishers/uORB/uorb_publisher.hpp"
+
+typedef struct {
+	UavcanPublisher *(*create_pub)(CanardHandle &handle, UavcanParamManager &pmgr) {};
+	const char *subject_name;
+	const uint8_t instance;
+} UavcanDynPubBinder;
+
+class PublicationManager
+{
+public:
+	PublicationManager(CanardHandle &handle, UavcanParamManager &pmgr) : _canard_handle(handle), _param_manager(pmgr) {}
+	~PublicationManager();
+
+	void update();
+	void printInfo();
+	void updateParams();
+
+private:
+	void updateDynamicPublications();
+
+	CanardHandle &_canard_handle;
+	UavcanParamManager &_param_manager;
+	List<UavcanPublisher *> _dynpublishers;
+
+
+	const UavcanDynPubBinder _uavcan_pubs[UAVCAN_PUB_COUNT] {
+#if CONFIG_CYPHAL_GNSS_PUBLISHER
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanPublisher *
+			{
+				return new UavcanGnssPublisher(handle, pmgr, 0);
+			},
+			"gps",
+			0
+		},
+#endif
+#if CONFIG_CYPHAL_ESC_CONTROLLER
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanPublisher *
+			{
+				return new UavcanEscController(handle, pmgr);
+			},
+			"esc",
+			0
+		},
+#endif
+#if CONFIG_CYPHAL_READINESS_PUBLISHER
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanPublisher *
+			{
+				return new UavcanReadinessPublisher(handle, pmgr, 0);
+			},
+			"readiness",
+			0
+		},
+#endif
+#if CONFIG_CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanPublisher *
+			{
+				return new uORB_over_UAVCAN_Publisher<actuator_outputs_s>(handle, pmgr, ORB_ID(actuator_outputs));
+			},
+			"uorb.actuator_outputs",
+			0
+		},
+#endif
+#if CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanPublisher *
+			{
+				return new uORB_over_UAVCAN_Publisher<sensor_gps_s>(handle, pmgr, ORB_ID(sensor_gps));
+			},
+			"uorb.sensor_gps",
+			0
+		},
+#endif
+	};
+};

src/drivers/cyphal/Publishers/Publisher.hpp

@@ -0,0 +1,146 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Publisher.hpp
+ *
+ * Defines basic functionality of Cyphal publisher class
+ *
+ * @author Jacob Crabill <jacob@flyvoly.com>
+ */
+
+#pragma once
+
+#include <px4_platform_common/px4_config.h>
+
+#include <lib/parameters/param.h>
+#include <containers/List.hpp>
+
+#include <uavcan/_register/Access_1_0.h>
+
+#include "../CanardHandle.hpp"
+#include "../CanardInterface.hpp"
+#include "../ParamManager.hpp"
+
+/* This is a default baseline timeout for publishers
+ * Still it's recommended for implementers to check if their publisher
+ * has timing requirements and if it should drop messages that are too old in favour of newer messages
+ */
+#define PUBLISHER_DEFAULT_TIMEOUT_USEC 100000UL
+
+class UavcanPublisher : public ListNode<UavcanPublisher *>
+{
+public:
+	UavcanPublisher(CanardHandle &handle, UavcanParamManager &pmgr, const char *prefix_name, const char *subject_name,
+			uint8_t instance = 0) :
+		_canard_handle(handle), _param_manager(pmgr), _prefix_name(prefix_name), _subject_name(subject_name),
+		_instance(instance) { };
+
+	virtual ~UavcanPublisher() = default;
+
+	// Update the uORB Subscription and broadcast a UAVCAN message
+	virtual void update() = 0;
+
+	CanardPortID id() { return _port_id; };
+
+	void updateParam()
+	{
+		char uavcan_param[256];
+		snprintf(uavcan_param, sizeof(uavcan_param), "uavcan.pub.%s%s.%d.id", _prefix_name, _subject_name, _instance);
+
+		// Set _port_id from _uavcan_param
+		uavcan_register_Value_1_0 value;
+
+		if (_param_manager.GetParamByName(uavcan_param, value)) {
+			uint16_t new_id = value.natural16.value.elements[0];
+
+			if (_port_id != new_id) {
+				if (new_id == CANARD_PORT_ID_UNSET) {
+					PX4_INFO("Disabling publication of subject %s%s.%d", _prefix_name, _subject_name, _instance);
+					_port_id = CANARD_PORT_ID_UNSET;
+
+				} else {
+					_port_id = (CanardPortID)new_id;
+					PX4_INFO("Enabling subject %s%s.%d on port %d", _prefix_name, _subject_name, _instance, _port_id);
+				}
+			}
+		}
+	};
+
+	void printInfo()
+	{
+		if (_port_id != CANARD_PORT_ID_UNSET) {
+			PX4_INFO("Enabled subject %s%s.%d on port %d", _prefix_name, _subject_name, _instance, _port_id);
+
+		} else {
+			PX4_INFO("Subject %s%s.%d disabled", _prefix_name, _subject_name, _instance);
+		}
+	}
+
+	const char *getSubjectName()
+	{
+		return _subject_name;
+	}
+
+	const char *getPrefixName()
+	{
+		return _prefix_name;
+	}
+
+	uint8_t getInstance()
+	{
+		return _instance;
+	}
+
+	UavcanPublisher *next()
+	{
+		return _next_pub;
+	}
+
+	void setNext(UavcanPublisher *next)
+	{
+		_next_pub = next;
+	}
+
+protected:
+	CanardHandle &_canard_handle;
+	UavcanParamManager &_param_manager;
+	const char *_prefix_name;
+	const char *_subject_name;
+	uint8_t _instance {0};
+
+	CanardPortID _port_id {CANARD_PORT_ID_UNSET};
+	CanardTransferID _transfer_id {0};
+
+	UavcanPublisher *_next_pub {nullptr};
+};

src/drivers/cyphal/Publishers/uORB/uorb_publisher.cpp

@@ -0,0 +1,53 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file uorb_template.cpp
+ *
+* Defines generic, templatized uORB over UAVCANv1 publisher
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ * @author Jacob Crabill <jacob@flyvoly.com>
+ */
+
+#include "uorb_publisher.hpp"
+
+/* ---- Specializations of get_payload_size() to reduce wasted bandwidth where possible ---- */
+
+template<>
+size_t uORB_over_UAVCAN_Publisher<actuator_outputs_s>::get_payload_size(actuator_outputs_s *msg)
+{
+	// Remove unvalid output & padding from payload_size to save bandwidth
+	return sizeof(struct actuator_outputs_s) - sizeof(msg->_padding0) -
+	       ((sizeof(msg->output) / sizeof(msg->output[0]) - msg->noutputs) * sizeof(msg->output[0]));
+}

src/drivers/cyphal/Publishers/uORB/uorb_publisher.hpp

@@ -0,0 +1,103 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file uorb_template.hpp
+ *
+* Defines generic, templatized uORB over UAVCANv1 publisher
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#pragma once
+
+#include "../Publisher.hpp"
+
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/actuator_outputs.h>
+
+template <class T>
+class uORB_over_UAVCAN_Publisher : public UavcanPublisher
+{
+public:
+	uORB_over_UAVCAN_Publisher(CanardHandle &handle, UavcanParamManager &pmgr, const orb_metadata *meta,
+				   uint8_t instance = 0) :
+		UavcanPublisher(handle, pmgr, "uorb.", meta->o_name, instance),
+		_uorb_meta{meta},
+		_uorb_sub(meta)
+	{};
+
+	~uORB_over_UAVCAN_Publisher() override = default;
+
+	// Update the uORB Subscription and broadcast a UAVCAN message
+	virtual void update() override
+	{
+		// Not sure if actuator_armed is a good indication of readiness but seems close to it
+		if (_uorb_sub.updated() && _port_id != CANARD_PORT_ID_UNSET) {
+			T data {};
+			_uorb_sub.update(&data);
+
+			const CanardTransferMetadata transfer_metadata = {
+				.priority       = CanardPriorityNominal,
+				.transfer_kind  = CanardTransferKindMessage,
+				.port_id        = _port_id, // This is the subject-ID.
+				.remote_node_id = CANARD_NODE_ID_UNSET,
+				.transfer_id    = _transfer_id
+			};
+
+			// set the data ready in the buffer and chop if needed
+			++_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
+			_canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
+					      &transfer_metadata,
+					      get_payload_size(&data),
+					      &data);
+		}
+	};
+
+protected:
+	// Default payload-size function -- can specialize in derived class
+	size_t get_payload_size(T *msg)
+	{
+		(void)msg;
+		return sizeof(T);
+	}
+
+private:
+	const orb_metadata *_uorb_meta;
+	uORB::Subscription _uorb_sub;
+};
+
+/* ---- Specializations of get_payload_size() to reduce wasted bandwidth where possible ---- */
+
+template<>
+size_t uORB_over_UAVCAN_Publisher<actuator_outputs_s>::get_payload_size(actuator_outputs_s *msg);

src/drivers/cyphal/Publishers/udral/Gnss.hpp

@@ -0,0 +1,102 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Gnss.hpp
+ *
+ * Defines basic functionality of Cyphal GNSS publisher
+ *
+ * @author Jacob Crabill <jacob@flyvoly.com>
+ */
+
+#pragma once
+
+// UDRAL Specification Messages
+#include <reg/udral/physics/kinematics/geodetic/Point_0_1.h>
+
+#include "../Publisher.hpp"
+
+class UavcanGnssPublisher : public UavcanPublisher
+{
+public:
+	UavcanGnssPublisher(CanardHandle &handle, UavcanParamManager &pmgr, uint8_t instance = 0) :
+		UavcanPublisher(handle, pmgr, "udral", "gps", instance)
+	{
+
+	};
+
+	~UavcanGnssPublisher() override = default;
+
+	// Update the uORB Subscription and broadcast a UAVCAN message
+	virtual void update() override
+	{
+		if (_gps_sub.updated() && _port_id != CANARD_PORT_ID_UNSET) {
+			sensor_gps_s gps {};
+			_gps_sub.update(&gps);
+			size_t payload_size = reg_udral_physics_kinematics_geodetic_Point_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
+
+			reg_udral_physics_kinematics_geodetic_Point_0_1 geo {};
+			geo.latitude = gps.lat;
+			geo.longitude = gps.lon;
+			geo.altitude = uavcan_si_unit_length_WideScalar_1_0 { .meter = static_cast<double>(gps.alt) };
+
+			uint8_t geo_payload_buffer[reg_udral_physics_kinematics_geodetic_Point_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
+
+			const CanardTransferMetadata transfer_metadata = {
+				.priority       = CanardPriorityNominal,
+				.transfer_kind  = CanardTransferKindMessage,
+				.port_id        = _port_id, // This is the subject-ID.
+				.remote_node_id = CANARD_NODE_ID_UNSET,
+				.transfer_id    = _transfer_id,
+			};
+
+			int32_t result = reg_udral_physics_kinematics_geodetic_Point_0_1_serialize_(&geo, geo_payload_buffer,
+					 &payload_size);
+
+			if (result == 0) {
+				// set the data ready in the buffer and chop if needed
+				++_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
+				result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
+							       &transfer_metadata,
+							       payload_size,
+							       &geo_payload_buffer);
+			}
+		}
+	};
+
+private:
+
+	/// TODO: Allow >1 instance
+	uORB::Subscription _gps_sub{ORB_ID(sensor_gps)};
+	CanardTransferID _transfer_id_2 {0};
+};

src/drivers/cyphal/Publishers/udral/Readiness.hpp

@@ -0,0 +1,105 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Readiness.hpp
+ *
+ * Defines the Cyphal readiness publisher
+ * readiness state is used to command or report the availability status
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#pragma once
+
+// UDRAL Specification Messages
+#include <reg/udral/service/common/Readiness_0_1.h>
+
+#include "../Publisher.hpp"
+
+class UavcanReadinessPublisher : public UavcanPublisher
+{
+public:
+	UavcanReadinessPublisher(CanardHandle &handle, UavcanParamManager &pmgr, uint8_t instance = 0) :
+		UavcanPublisher(handle, pmgr, "udral", "readiness", instance)
+	{
+
+	};
+
+	~UavcanReadinessPublisher() override = default;
+
+	// Update the uORB Subscription and broadcast a UAVCAN message
+	virtual void update() override
+	{
+		// Not sure if actuator_armed is a good indication of readiness but seems close to it
+		if (_actuator_armed_sub.updated() && _port_id != CANARD_PORT_ID_UNSET) {
+			actuator_armed_s armed {};
+			_actuator_armed_sub.update(&armed);
+			size_t payload_size;
+
+			reg_udral_service_common_Readiness_0_1 readiness {};
+
+			if (armed.armed) {
+				readiness.value = reg_udral_service_common_Readiness_0_1_ENGAGED;
+
+			} else {
+				readiness.value = reg_udral_service_common_Readiness_0_1_STANDBY;
+			}
+
+			uint8_t readiness_payload_buffer[reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
+
+			const CanardTransferMetadata transfer_metadata = {
+				.priority       = CanardPriorityNominal,
+				.transfer_kind  = CanardTransferKindMessage,
+				.port_id        = _port_id, // This is the subject-ID.
+				.remote_node_id = CANARD_NODE_ID_UNSET,
+				.transfer_id    = _transfer_id,
+			};
+
+			int32_t result = reg_udral_service_common_Readiness_0_1_serialize_(&readiness, readiness_payload_buffer,
+					 &payload_size);
+
+			if (result == 0) {
+				// set the data ready in the buffer and chop if needed
+				++_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
+				result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
+							       &transfer_metadata,
+							       payload_size,
+							       &readiness_payload_buffer);
+			}
+		}
+	};
+
+private:
+	uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};
+};

src/drivers/cyphal/ServiceClients/Access.hpp

@@ -0,0 +1,131 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Access.hpp
+ *
+ * Defines response to a Access request
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#pragma once
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/module.h>
+#include <version/version.h>
+
+#include "../ParamManager.hpp"
+
+#include <uavcan/node/ID_1_0.h>
+#include <uavcan/node/GetInfo_1_0.h>
+
+#include "../Subscribers/BaseSubscriber.hpp"
+
+class UavcanAccessResponse : public UavcanBaseSubscriber
+{
+public:
+	UavcanAccessResponse(CanardHandle &handle, UavcanParamManager &pmgr) :
+		UavcanBaseSubscriber(handle, "", "Access", 0),  _param_manager(pmgr) { };
+
+	void subscribe() override
+	{
+		// Subscribe to requests uavcan.pnp.NodeIDAllocationData
+		_canard_handle.RxSubscribe(CanardTransferKindRequest,
+					   uavcan_register_Access_1_0_FIXED_PORT_ID_,
+					   uavcan_register_Access_Response_1_0_EXTENT_BYTES_,
+					   CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+					   &_subj_sub._canard_sub);
+
+	};
+
+	void callback(const CanardRxTransfer &receive) override
+	{
+		PX4_INFO("Access request");
+
+		size_t payload_size = uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_;
+
+		uavcan_register_Access_Request_1_0 msg;
+		uavcan_register_Access_Request_1_0_initialize_(&msg);
+
+		size_t register_in_size_bits = receive.payload_size;
+		uavcan_register_Access_Request_1_0_deserialize_(&msg, (const uint8_t *)receive.payload, &register_in_size_bits);
+
+		int result {0};
+
+		uavcan_register_Value_1_0 value = msg.value;
+		uavcan_register_Name_1_0 name = msg.name;
+
+		/// TODO: get/set parameter based on whether empty or not
+		if (uavcan_register_Value_1_0_is_empty_(&value)) { // Tag Type: uavcan_primitive_Empty_1_0
+			// Value is empty -- 'Get' only
+			result = _param_manager.GetParamByName(name, value) ? 0 : -1;
+
+		} else {
+			// Set value
+			result = _param_manager.SetParamByName(name, value) ? 0 : -1;
+
+		}
+
+		/// TODO: Access_Response
+		uavcan_register_Access_Response_1_0 response {};
+		response.value = value;
+
+		uint8_t response_payload_buffer[uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
+
+		const CanardTransferMetadata transfer_metadata = {
+			.priority       = CanardPriorityNominal,
+			.transfer_kind  = CanardTransferKindResponse,
+			.port_id        = uavcan_register_Access_1_0_FIXED_PORT_ID_,                // This is the subject-ID.
+			.remote_node_id = receive.metadata.remote_node_id,       // Messages cannot be unicast, so use UNSET.
+			.transfer_id    = receive.metadata.transfer_id,
+		};
+
+		result = uavcan_register_Access_Response_1_0_serialize_(&response, response_payload_buffer, &payload_size);
+
+		if (result == 0) {
+			// set the data ready in the buffer and chop if needed
+			result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
+						       &transfer_metadata,
+						       payload_size,
+						       &response_payload_buffer);
+		}
+
+		//return result;
+
+	};
+
+private:
+	UavcanParamManager &_param_manager;
+
+};

src/drivers/cyphal/ServiceClients/GetInfo.hpp

@@ -0,0 +1,139 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file GetInfo.hpp
+ *
+ * Defines response to a GetInfo request
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#pragma once
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/module.h>
+#include <version/version.h>
+
+#include <uavcan/node/ID_1_0.h>
+#include <uavcan/node/GetInfo_1_0.h>
+
+#include "../Subscribers/BaseSubscriber.hpp"
+#include "../Publishers/Publisher.hpp"
+
+class UavcanGetInfoResponse : public UavcanBaseSubscriber
+{
+public:
+	UavcanGetInfoResponse(CanardHandle &handle) :
+		UavcanBaseSubscriber(handle, "", "GetInfo", 0) { };
+
+	void subscribe() override
+	{
+		// Subscribe to requests uavcan.pnp.NodeIDAllocationData
+		_canard_handle.RxSubscribe(
+			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,
+			&_subj_sub._canard_sub);
+
+	};
+
+	void callback(const CanardRxTransfer &receive) override
+	{
+		PX4_INFO("GetInfo request");
+
+		// Setup node.GetInfo response
+
+		uavcan_node_GetInfo_Response_1_0 node_info;
+		size_t payload_size = uavcan_node_GetInfo_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_;
+
+		uavcan_node_GetInfo_Response_1_0_initialize_(&node_info);
+
+		node_info.protocol_version.major = 1;
+		node_info.protocol_version.minor = 0;
+
+#if defined(BOARD_HAS_VERSIONING)
+		node_info.hardware_version.major = (uint8_t)px4_board_hw_version();
+		node_info.hardware_version.minor = (uint8_t)px4_board_hw_revision();
+#endif
+
+		unsigned fwver = px4_firmware_version();
+		node_info.software_version.major = (fwver >> (8 * 3)) & 0xFF;
+		node_info.software_version.minor = (fwver >> (8 * 2)) & 0xFF;
+
+		node_info.software_vcs_revision_id = px4_firmware_version_binary();
+
+		px4_guid_t px4_guid;
+		board_get_px4_guid(px4_guid);
+		memcpy(node_info.unique_id, px4_guid, sizeof(node_info.unique_id));
+
+		//TODO proper name
+		strncpy((char *)node_info.name.elements,
+			px4_board_name(),
+			uavcan_node_GetInfo_Response_1_0_name_ARRAY_CAPACITY_);
+
+		node_info.name.count = strlen(px4_board_name());
+
+		uint8_t response_payload_buffer[uavcan_node_GetInfo_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
+
+		const CanardTransferMetadata transfer_metadata = {
+			.priority       = CanardPriorityNominal,
+			.transfer_kind  = CanardTransferKindResponse,
+			.port_id        = uavcan_node_GetInfo_1_0_FIXED_PORT_ID_, // This is the subject-ID.
+			.remote_node_id = receive.metadata.remote_node_id,       // Send back to request Node
+			.transfer_id    = receive.metadata.transfer_id
+		};
+
+		int32_t result = uavcan_node_GetInfo_Response_1_0_serialize_(&node_info, (uint8_t *)&response_payload_buffer,
+				 &payload_size);
+
+		if (result == 0) {
+			// set the data ready in the buffer and chop if needed
+			result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
+						       &transfer_metadata,
+						       payload_size,
+						       &response_payload_buffer);
+		}
+
+		//TODO proper error handling
+		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;
+		}
+
+	};
+
+};

src/drivers/cyphal/ServiceClients/List.hpp

@@ -0,0 +1,118 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file List.hpp
+ *
+ * Defines response to a List request
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#pragma once
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/module.h>
+#include <version/version.h>
+
+#include "../ParamManager.hpp"
+
+#include <uavcan/_register/List_1_0.h>
+
+#include "../Subscribers/BaseSubscriber.hpp"
+
+class UavcanListResponse : public UavcanBaseSubscriber
+{
+public:
+	UavcanListResponse(CanardHandle &handle, UavcanParamManager &pmgr) :
+		UavcanBaseSubscriber(handle, "", "List", 0),  _param_manager(pmgr) { };
+
+	void subscribe() override
+	{
+		// Subscribe to requests uavcan.pnp.NodeIDAllocationData
+
+		_canard_handle.RxSubscribe(CanardTransferKindRequest,
+					   uavcan_register_List_1_0_FIXED_PORT_ID_,
+					   uavcan_register_List_Response_1_0_EXTENT_BYTES_,
+					   CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+					   &_subj_sub._canard_sub);
+
+	};
+
+	void callback(const CanardRxTransfer &receive) override
+	{
+		PX4_INFO("List request");
+
+		size_t payload_size = uavcan_register_List_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_;
+
+		uavcan_register_List_Request_1_0 msg;
+		uavcan_register_List_Response_1_0 response;
+
+		uavcan_register_List_Request_1_0_initialize_(&msg);
+		uavcan_register_List_Response_1_0_initialize_(&response);
+
+		size_t register_in_size_bits = receive.payload_size;
+		uavcan_register_List_Request_1_0_deserialize_(&msg, (const uint8_t *)receive.payload, &register_in_size_bits);
+
+		int result {0};
+
+		if (_param_manager.GetParamName(msg.index, response.name) == 0) {
+			response.name.name.count = 0;
+		}
+
+		uint8_t response_payload_buffer[uavcan_register_List_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
+
+		const CanardTransferMetadata transfer_metadata = {
+			.priority       = CanardPriorityNominal,
+			.transfer_kind  = CanardTransferKindResponse,
+			.port_id        = uavcan_register_List_1_0_FIXED_PORT_ID_,                // This is the subject-ID.
+			.remote_node_id = receive.metadata.remote_node_id,       // Messages cannot be unicast, so use UNSET.
+			.transfer_id    = receive.metadata.transfer_id
+		};
+
+		result = uavcan_register_List_Response_1_0_serialize_(&response, response_payload_buffer, &payload_size);
+
+		if (result == 0) {
+			// set the data ready in the buffer and chop if needed
+			result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
+						       &transfer_metadata,
+						       payload_size,
+						       &response_payload_buffer);
+		}
+
+	};
+
+private:
+	UavcanParamManager &_param_manager;
+
+};

src/drivers/cyphal/Services/AccessRequest.hpp

@@ -0,0 +1,108 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Access.hpp
+ *
+ * Defines a Access Service invoker and process Access responses
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#pragma once
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/module.h>
+#include <version/version.h>
+
+#include <uavcan/_register/Access_1_0.h>
+
+#include "ServiceRequest.hpp"
+#include "../ParamManager.hpp"
+#include "../Publishers/Publisher.hpp"
+
+class UavcanAccessServiceRequest : public UavcanServiceRequest
+{
+public:
+	UavcanAccessServiceRequest(CanardHandle &handle, UavcanParamManager &pmgr) :
+		UavcanServiceRequest(handle, "", "Access", uavcan_register_Access_1_0_FIXED_PORT_ID_,
+				     uavcan_register_Access_Response_1_0_EXTENT_BYTES_),  _param_manager(pmgr)  { };
+
+	bool setPortId(CanardNodeID node_id, uavcan_register_Name_1_0 &name, UavcanServiceRequestInterface *handler)
+	{
+		int result {0};
+
+		uavcan_register_Access_Request_1_0 request_msg;
+		request_msg.value.natural16.value.count = 1;
+		uavcan_register_Value_1_0_select_natural16_(&request_msg.value); // Set to natural16 so that ParamManager casts type
+
+		//FIXME ParamManager only has notion of being either sub/pub have to find a portable way to address trhis
+		name.name.elements[7] = 's'; //HACK Change pub into sub
+
+		if (_param_manager.GetParamByName(name, request_msg.value)) {
+			size_t payload_size = uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_;
+			name.name.elements[7] = 'p'; //HACK Change sub into pub
+			memcpy(&request_msg.name, &name, sizeof(request_msg.name));
+
+			uint8_t request_payload_buffer[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
+
+			const CanardTransferMetadata transfer_metadata = {
+				.priority       = CanardPriorityNominal,
+				.transfer_kind  = CanardTransferKindRequest,
+				.port_id        = _portID,                // This is the subject-ID.
+				.remote_node_id = node_id,       // Messages cannot be unicast, so use UNSET.
+				.transfer_id    = request_transfer_id
+			};
+
+			result = uavcan_register_Access_Request_1_0_serialize_(&request_msg, request_payload_buffer, &payload_size);
+
+			if (result == 0) {
+				return request(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
+					       &transfer_metadata,
+					       payload_size,
+					       &request_payload_buffer,
+					       handler);
+
+			} else {
+				return false;
+			}
+
+		} else {
+			return false;
+		}
+	};
+
+private:
+	UavcanParamManager &_param_manager;
+
+};

src/drivers/cyphal/Services/ListRequest.hpp

@@ -0,0 +1,91 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ListRequest.hpp
+ *
+ * Defines a List Service invoker and process List responses
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#pragma once
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/module.h>
+#include <version/version.h>
+
+#include <uavcan/_register/List_1_0.h>
+
+#include "ServiceRequest.hpp"
+#include "../ParamManager.hpp"
+#include "../Publishers/Publisher.hpp"
+
+class UavcanListServiceRequest : public UavcanServiceRequest
+{
+public:
+	UavcanListServiceRequest(CanardHandle &handle) :
+		UavcanServiceRequest(handle, "", "List", uavcan_register_List_1_0_FIXED_PORT_ID_,
+				     uavcan_register_List_Response_1_0_EXTENT_BYTES_) { };
+
+
+	bool getIndex(CanardNodeID node_id, uint16_t index, UavcanServiceRequestInterface *handler)
+	{
+		uavcan_register_List_Request_1_0 msg;
+		size_t payload_size = uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_;
+		msg.index = index;
+
+		uint8_t request_payload_buffer[uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
+
+		const CanardTransferMetadata transfer_metadata = {
+			.priority       = CanardPriorityNominal,
+			.transfer_kind  = CanardTransferKindRequest,
+			.port_id        = uavcan_register_List_1_0_FIXED_PORT_ID_, // This is the subject-ID.
+			.remote_node_id = node_id,
+			.transfer_id    = request_transfer_id
+		};
+
+		if (uavcan_register_List_Request_1_0_serialize_(&msg, request_payload_buffer, &payload_size) == 0) {
+			return request(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
+				       &transfer_metadata,
+				       payload_size,
+				       &request_payload_buffer,
+				       handler);
+
+		} else {
+			return false;
+		}
+	};
+
+
+};

src/drivers/cyphal/Services/ServiceRequest.hpp

@@ -0,0 +1,113 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ServiceRequest.hpp
+ *
+ * Defines a Service invoker base class and process responses
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#pragma once
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/module.h>
+#include <version/version.h>
+
+#include <uavcan/_register/List_1_0.h>
+
+#include "../Subscribers/BaseSubscriber.hpp"
+
+
+class UavcanServiceRequestInterface
+{
+public:
+	virtual void response_callback(const CanardRxTransfer &receive) = 0;
+};
+
+class UavcanServiceRequest : public UavcanBaseSubscriber
+{
+public:
+	UavcanServiceRequest(CanardHandle &handle, const char *prefix_name, const char *subject_name, CanardPortID portID,
+			     size_t extent) :
+		UavcanBaseSubscriber(handle, prefix_name, subject_name, 0), _portID(portID), _extent(extent) { };
+
+
+	void subscribe() override
+	{
+		// Subscribe to requests response
+		_canard_handle.RxSubscribe(CanardTransferKindResponse,
+					   _portID,
+					   _extent,
+					   CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+					   &_subj_sub._canard_sub);
+	};
+
+	bool request(const CanardMicrosecond             tx_deadline_usec,
+		     const CanardTransferMetadata        *transfer_metadata,
+		     const size_t                        payload_size,
+		     const void *const                   payload,
+		     UavcanServiceRequestInterface       *handler)
+	{
+		_response_callback = handler;
+		remote_node_id = transfer_metadata->remote_node_id;
+		++request_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
+		return _canard_handle.TxPush(tx_deadline_usec,
+					     transfer_metadata,
+					     payload_size,
+					     payload) > 0;
+	}
+
+	void callback(const CanardRxTransfer &receive) override
+	{
+		PX4_INFO("Response");
+
+		if (_response_callback != nullptr &&
+		    receive.metadata.transfer_id == (request_transfer_id - 1) &&
+		    receive.metadata.remote_node_id == remote_node_id) {
+			_response_callback->response_callback(receive);
+		}
+	};
+
+
+
+protected:
+	CanardTransferID request_transfer_id = 0;
+	CanardNodeID remote_node_id = CANARD_NODE_ID_UNSET;
+
+	const CanardPortID _portID;
+	const size_t _extent;
+	UavcanServiceRequestInterface *_response_callback = nullptr;
+
+};

src/drivers/cyphal/Subscribers/BaseSubscriber.hpp

@@ -0,0 +1,154 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file BaseSubscriber.hpp
+ *
+ * Defines basic functionality of Cyphal subscriber class
+ *
+ * @author Jacob Crabill <jacob@flyvoly.com>
+ */
+
+#pragma once
+
+#include <px4_platform_common/px4_config.h>
+
+#include <lib/parameters/param.h>
+
+#include "../CanardHandle.hpp"
+#include "../CanardInterface.hpp"
+#include "../ParamManager.hpp"
+
+class UavcanBaseSubscriber
+{
+public:
+	UavcanBaseSubscriber(CanardHandle &handle, const char *prefix_name, const char *subject_name, uint8_t instance = 0) :
+		_canard_handle(handle), _prefix_name(prefix_name), _instance(instance)
+	{
+		_subj_sub._subject_name = subject_name;
+		_subj_sub._canard_sub.user_reference = this;
+		_subj_sub._canard_sub.port_id = CANARD_PORT_ID_UNSET;
+	}
+
+	virtual ~UavcanBaseSubscriber()
+	{
+		unsubscribe();
+	}
+
+	bool isValidPortId(int32_t id) const { return id >= 0 && id <= CANARD_PORT_ID_MAX; }
+
+	virtual void subscribe() = 0;
+	virtual void unsubscribe()
+	{
+		SubjectSubscription *curSubj = &_subj_sub;
+
+		while (curSubj != nullptr) {
+			_canard_handle.RxUnsubscribe(CanardTransferKindMessage, curSubj->_canard_sub.port_id);
+			curSubj = curSubj->next;
+		}
+	};
+
+	virtual void callback(const CanardRxTransfer &msg) = 0;
+
+	CanardPortID id(uint32_t instance = 0)
+	{
+		uint32_t i = 0;
+		SubjectSubscription *curSubj = &_subj_sub;
+
+		while (curSubj != nullptr) {
+			if (instance == i) {
+				return curSubj->_canard_sub.port_id;
+			}
+
+			curSubj = curSubj->next;
+			i++;
+		}
+
+		return CANARD_PORT_ID_UNSET; // Wrong id return unset
+	}
+
+	bool hasPortID(CanardPortID port_id)
+	{
+		if (!isValidPortId((int32_t)port_id)) {
+			return false;
+		}
+
+		SubjectSubscription *curSubj = &_subj_sub;
+
+		while (curSubj != nullptr) {
+			if (port_id == curSubj->_canard_sub.port_id) {
+				return true;
+			}
+
+			curSubj = curSubj->next;
+		}
+
+		return false;
+	}
+
+	const char *getSubjectName()
+	{
+		return _subj_sub._subject_name;
+	}
+
+	uint8_t getInstance()
+	{
+		return _instance;
+	}
+
+	void printInfo()
+	{
+		SubjectSubscription *curSubj = &_subj_sub;
+
+		while (curSubj != nullptr) {
+			if (curSubj->_canard_sub.port_id != CANARD_PORT_ID_UNSET) {
+				PX4_INFO("Subscribed %s.%d on port %d", curSubj->_subject_name, _instance, curSubj->_canard_sub.port_id);
+			}
+
+			curSubj = curSubj->next;
+		}
+	}
+
+protected:
+	struct SubjectSubscription {
+		CanardRxSubscription _canard_sub;
+		const char *_subject_name;
+		struct SubjectSubscription *next {nullptr};
+	};
+
+	CanardHandle &_canard_handle;
+	const char *_prefix_name;
+	SubjectSubscription _subj_sub;
+	uint8_t _instance {0};
+	/// TODO: 'type' parameter? uavcan.pub.PORT_NAME.type (see 384.Access.1.0.uavcan)
+};

src/drivers/cyphal/Subscribers/DynamicPortSubscriber.hpp

@@ -0,0 +1,117 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file DynamicPortSubscriber.hpp
+ *
+ * Defines basic functionality of Cyphal subscriber class with Non-fixed unregulated port identifier
+ *
+ * @author Jacob Crabill <jacob@flyvoly.com>
+ */
+
+#pragma once
+
+#include <px4_platform_common/px4_config.h>
+
+#include <lib/parameters/param.h>
+
+#include <uavcan/_register/Access_1_0.h>
+
+#include "../CanardInterface.hpp"
+#include "../ParamManager.hpp"
+#include "BaseSubscriber.hpp"
+
+class UavcanDynamicPortSubscriber : public UavcanBaseSubscriber
+{
+public:
+	UavcanDynamicPortSubscriber(CanardHandle &handle, UavcanParamManager &pmgr, const char *prefix_name,
+				    const char *subject_name,  uint8_t instance = 0) :
+		UavcanBaseSubscriber(handle, prefix_name, subject_name, instance), _param_manager(pmgr) { };
+
+	void updateParam()
+	{
+		SubjectSubscription *curSubj = &_subj_sub;
+
+		while (curSubj != nullptr) {
+			char uavcan_param[90];
+			snprintf(uavcan_param, sizeof(uavcan_param), "uavcan.sub.%s%s.%d.id", _prefix_name, curSubj->_subject_name, _instance);
+
+			// Set _port_id from _uavcan_param
+			uavcan_register_Value_1_0 value;
+
+			if (_param_manager.GetParamByName(uavcan_param, value)) {
+				uint16_t new_id = value.natural16.value.elements[0];
+
+				/* FIXME how about partial subscribing */
+				if (curSubj->_canard_sub.port_id != new_id) {
+					if (new_id == CANARD_PORT_ID_UNSET) {
+						// Cancel subscription
+						unsubscribe();
+
+					} else {
+						if (curSubj->_canard_sub.port_id != CANARD_PORT_ID_UNSET) {
+							// Already active; unsubscribe first
+							unsubscribe();
+						}
+
+						// Subscribe on the new port ID
+						curSubj->_canard_sub.port_id = (CanardPortID)new_id;
+						PX4_INFO("Subscribing %s%s.%d on port %d", _prefix_name, curSubj->_subject_name, _instance,
+							 curSubj->_canard_sub.port_id);
+						subscribe();
+					}
+				}
+
+			} else if (curSubj->_canard_sub.port_id != CANARD_PORT_ID_UNSET) { // No valid sub id unsubscribe when neccesary
+				// Already active; unsubscribe first
+				unsubscribe();
+			}
+
+			curSubj = curSubj->next;
+		}
+	};
+
+	UavcanDynamicPortSubscriber *next()
+	{
+		return _next_sub;
+	}
+
+	void setNext(UavcanDynamicPortSubscriber *next)
+	{
+		_next_sub = next;
+	}
+
+protected:
+	UavcanParamManager &_param_manager;
+	UavcanDynamicPortSubscriber *_next_sub {nullptr};
+};

src/drivers/cyphal/Subscribers/Heartbeat.hpp

@@ -0,0 +1,72 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Heartbeat.hpp
+ *
+ * Defines basic functionality of UAVCAN Heartbeat.1.0 subscription
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#pragma once
+
+#include "../NodeManager.hpp"
+
+#include <uavcan/node/Heartbeat_1_0.h>
+
+#include "BaseSubscriber.hpp"
+
+class UavcanHeartbeatSubscriber : public UavcanBaseSubscriber
+{
+public:
+	UavcanHeartbeatSubscriber(CanardHandle &handle) :
+		UavcanBaseSubscriber(handle, "", "Heartbeat", 0) { };
+
+	void subscribe() override
+	{
+		// Subscribe to heartbeat messages
+		_canard_handle.RxSubscribe(CanardTransferKindMessage,
+					   uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_,  // The fixed Subject-ID
+					   uavcan_node_Heartbeat_1_0_EXTENT_BYTES_,
+					   CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+					   &_subj_sub._canard_sub);
+
+	};
+
+	void callback(const CanardRxTransfer &receive) override
+	{
+		//TODO heartbeat management
+	};
+
+};

src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp

@@ -0,0 +1,128 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Battery.hpp
+ *
+ * Defines basic functionality of UAVCAN legacy Battery subscription
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#pragma once
+
+#include <uORB/topics/battery_status.h>
+#include <uORB/PublicationMulti.hpp>
+
+// Legacy message from UAVCANv0
+#include <legacy/equipment/power/BatteryInfo_1_0.h>
+
+#include "../DynamicPortSubscriber.hpp"
+
+class UavcanLegacyBatteryInfoSubscriber : public UavcanDynamicPortSubscriber
+{
+public:
+	UavcanLegacyBatteryInfoSubscriber(CanardHandle &handle, UavcanParamManager &pmgr, uint8_t instance = 0) :
+		UavcanDynamicPortSubscriber(handle, pmgr, "legacy.", "legacy_bms", instance) { };
+
+	void subscribe() override
+	{
+		// Subscribe to messages reg.drone.service.battery.Status.0.1
+		_canard_handle.RxSubscribe(CanardTransferKindMessage,
+					   _subj_sub._canard_sub.port_id,
+					   legacy_equipment_power_BatteryInfo_1_0_EXTENT_BYTES_,
+					   CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC * 100, //FIXME timeout caused by scheduler
+					   &_subj_sub._canard_sub);
+	};
+
+	void callback(const CanardRxTransfer &receive) override
+	{
+		PX4_INFO("Legacy BmsCallback");
+
+		legacy_equipment_power_BatteryInfo_1_0 bat_info {};
+		size_t bat_info_size_in_bytes = receive.payload_size;
+		legacy_equipment_power_BatteryInfo_1_0_deserialize_(&bat_info, (const uint8_t *)receive.payload,
+				&bat_info_size_in_bytes);
+
+		battery_status_s bat_status {0};
+		bat_status.timestamp = hrt_absolute_time();
+		bat_status.voltage_filtered_v = bat_info.voltage;
+		bat_status.current_filtered_a = bat_info.current;
+		bat_status.current_average_a = bat_info.average_power_10sec;
+		bat_status.remaining = bat_info.state_of_charge_pct / 100.0f;
+		bat_status.scale = -1;
+
+		if (bat_info.status_flags & legacy_equipment_power_BatteryInfo_1_0_STATUS_FLAG_TEMP_HOT) {
+			bat_status.temperature = 100;
+
+		} else if (bat_info.status_flags & legacy_equipment_power_BatteryInfo_1_0_STATUS_FLAG_TEMP_COLD) {
+			bat_status.temperature = -30;
+
+		} else {
+			bat_status.temperature = 20; // Temp okay ?
+		}
+
+		bat_status.cell_count = 0; // Unknown
+		bat_status.connected = bat_info.status_flags & legacy_equipment_power_BatteryInfo_1_0_STATUS_FLAG_IN_USE;
+		bat_status.source = 1; // External
+		bat_status.capacity = bat_info.full_charge_capacity_wh;
+		bat_status.serial_number = bat_info.model_instance_id & 0xFFFF; // Take first 16 bits
+		bat_status.state_of_health = bat_info.state_of_health_pct; // External
+		bat_status.id = bat_info.battery_id;
+
+		/* Missing fields in UAVCANv0 legacy message
+		 * temperature (partly)
+		 * cell_count
+		 * connected (partly)
+		 * priority
+		 * cycle_count
+		 * time_remaining_s
+		 * average_time_to_empty
+		 * manufacture_date
+		 * max_error
+		 * interface_error
+		 * voltage_cell_v
+		 * max_cell_voltage_delta
+		 * is_powering_off
+		 * warning
+		 */
+
+
+		_battery_status_pub.publish(bat_status);
+		print_message(ORB_ID(battery_status), bat_status);
+	};
+
+private:
+	uORB::PublicationMulti<battery_status_s> _battery_status_pub{ORB_ID(battery_status)};
+
+};

src/drivers/cyphal/Subscribers/uORB/uorb_subscriber.cpp

@@ -0,0 +1,54 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file uorb_template.cpp
+ *
+* Defines generic, templatized uORB over UAVCANv1 subscriber
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ * @author Jacob Crabill <jacob@flyvoly.com>
+ */
+
+#include "uorb_subscriber.hpp"
+
+/* ---- Specializations of get_payload_size() to reduce wasted bandwidth where possible ---- */
+
+/* ---- Specializations of convert() to convert incompatbile data, instance no. timestamp ---- */
+
+template<>
+void uORB_over_UAVCAN_Subscriber<sensor_gps_s>::convert(sensor_gps_s *data)
+{
+	/* HOTFIX as long as we don't have UAVCAN timesyncronization we update the timestamp on arrival */
+	data->timestamp = hrt_absolute_time();
+}

src/drivers/cyphal/Subscribers/uORB/uorb_subscriber.hpp

@@ -0,0 +1,114 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file uorb_template.hpp
+ *
+* Defines generic, templatized uORB over UAVCANv1 publisher
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#pragma once
+
+#include "../DynamicPortSubscriber.hpp"
+
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/actuator_outputs.h>
+
+#include <uORB/topics/sensor_gps.h>
+
+template <class T>
+class uORB_over_UAVCAN_Subscriber : public UavcanDynamicPortSubscriber
+{
+public:
+	uORB_over_UAVCAN_Subscriber(CanardHandle &handle, UavcanParamManager &pmgr, const orb_metadata *meta,
+				    uint8_t instance = 0) :
+		UavcanDynamicPortSubscriber(handle, pmgr, "uorb.", meta->o_name, instance),
+		_uorb_meta{meta},
+		_uorb_pub(meta)
+	{};
+
+	~uORB_over_UAVCAN_Subscriber() override = default;
+
+	void subscribe() override
+	{
+		T *data = NULL;
+
+		// Subscribe to messages uORB sensor_gps payload over UAVCAN
+		_canard_handle.RxSubscribe(CanardTransferKindMessage,
+					   _subj_sub._canard_sub.port_id,
+					   get_payload_size(data),
+					   CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC * 10000,
+					   &_subj_sub._canard_sub);
+	};
+
+	void callback(const CanardRxTransfer &receive) override
+	{
+		T *data = (T *)receive.payload;
+
+		if (receive.payload_size == get_payload_size(data)) {
+
+			/* Data type specific conversion if necceary  */
+			convert(data);
+
+			_uorb_pub.publish(*data);
+
+		} else {
+			PX4_ERR("uORB over UAVCAN %s payload size mismatch got %d expected %d",
+				_subj_sub._subject_name, receive.payload_size, get_payload_size(data));
+		}
+	};
+
+protected:
+	// Default payload-size function -- can specialize in derived class
+	size_t get_payload_size(const T *msg)
+	{
+		(void)msg;
+		return sizeof(T);
+	};
+
+	void convert(T *data) {};
+
+private:
+	const orb_metadata *_uorb_meta;
+	uORB::PublicationMulti<T> _uorb_pub;
+};
+
+/* ---- Specializations of get_payload_size() to reduce wasted bandwidth where possible ---- */
+
+
+/* ---- Specializations of convert() to convert incompatbile data, instance no. timestamp ---- */
+
+template<>
+void uORB_over_UAVCAN_Subscriber<sensor_gps_s>::convert(sensor_gps_s *data);

src/drivers/cyphal/Subscribers/udral/Battery.hpp

@@ -0,0 +1,186 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Battery.hpp
+ *
+ * Defines basic functionality of Cyphal Battery subscription
+ *
+ * @author Jacob Crabill <jacob@flyvoly.com>
+ */
+
+#pragma once
+
+#include <uORB/topics/battery_status.h>
+#include <uORB/PublicationMulti.hpp>
+
+// UDRAL Specification Messages
+#include <reg/udral/physics/electricity/SourceTs_0_1.h>
+#include <reg/udral/service/battery/Parameters_0_3.h>
+#include <reg/udral/service/battery/Status_0_2.h>
+
+#include "../DynamicPortSubscriber.hpp"
+
+#define KELVIN_OFFSET 273.15f
+#define WH_TO_JOULE   3600
+
+class UavcanBmsSubscriber : public UavcanDynamicPortSubscriber
+{
+public:
+	UavcanBmsSubscriber(CanardHandle &handle, UavcanParamManager &pmgr, uint8_t instance = 0) :
+		UavcanDynamicPortSubscriber(handle, pmgr, "udral.", "energy_source", instance)
+	{
+		_subj_sub.next = &_status_sub;
+
+		_status_sub._subject_name = _status_name;
+		_status_sub._canard_sub.user_reference = this;
+		_status_sub.next = &_parameters_sub;
+
+		_parameters_sub._subject_name = _parameters_name;
+		_parameters_sub._canard_sub.user_reference = this;
+		_parameters_sub.next = nullptr;
+	}
+
+	void subscribe() override
+	{
+		// Subscribe to messages reg.drone.physics.electricity.SourceTs.0.1
+		_canard_handle.RxSubscribe(CanardTransferKindMessage,
+					   _subj_sub._canard_sub.port_id,
+					   reg_udral_physics_electricity_SourceTs_0_1_EXTENT_BYTES_,
+					   CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+					   &_subj_sub._canard_sub);
+
+		// Subscribe to messages reg.drone.service.battery.Status.0.2
+		_canard_handle.RxSubscribe(CanardTransferKindMessage,
+					   _status_sub._canard_sub.port_id,
+					   reg_udral_service_battery_Status_0_2_EXTENT_BYTES_,
+					   CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+					   &_status_sub._canard_sub);
+
+		// Subscribe to messages reg.drone.service.battery.Parameters.0.3
+		_canard_handle.RxSubscribe(CanardTransferKindMessage,
+					   _parameters_sub._canard_sub.port_id,
+					   reg_udral_service_battery_Parameters_0_3_EXTENT_BYTES_,
+					   CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+					   &_parameters_sub._canard_sub);
+	};
+
+	void callback(const CanardRxTransfer &receive) override
+	{
+		if (receive.metadata.port_id == _subj_sub._canard_sub.port_id) {
+			reg_udral_physics_electricity_SourceTs_0_1 source_ts {};
+			size_t source_ts_size_in_bytes = receive.payload_size;
+			reg_udral_physics_electricity_SourceTs_0_1_deserialize_(&source_ts,
+					(const uint8_t *)receive.payload,
+					&source_ts_size_in_bytes);
+			bat_status.timestamp = hrt_absolute_time(); //FIXME timesyncronization source_ts.timestamp.microsecond;
+			bat_status.voltage_v = source_ts.value.power.voltage.volt;
+			bat_status.current_a = source_ts.value.power.current.ampere;
+
+			bat_status.connected = true; // Based on some threshold or an error??
+
+			// Estimate discharged mah from Joule
+			if (_nominal_voltage != NAN) {
+				bat_status.discharged_mah = (source_ts.value.full_energy.joule - source_ts.value.energy.joule)
+							    / (_nominal_voltage * WH_TO_JOULE);
+			}
+
+			bat_status.remaining = source_ts.value.energy.joule / source_ts.value.full_energy.joule;
+
+			// TODO uORB publication rate limiting
+			_battery_status_pub.publish(bat_status);
+
+		} else if (receive.metadata.port_id == _status_sub._canard_sub.port_id) {
+			reg_udral_service_battery_Status_0_2 bat {};
+			size_t bat_size_in_bytes = receive.payload_size;
+			reg_udral_service_battery_Status_0_2_deserialize_(&bat, (const uint8_t *)receive.payload, &bat_size_in_bytes);
+
+			bat_status.scale = -1; // What does the mean?
+
+			bat_status.temperature = bat.temperature_min_max[1].kelvin - KELVIN_OFFSET; // PX4 uses degC we assume
+
+			bat_status.cell_count = bat.cell_voltages.count;
+
+			uint32_t cell_count = bat_status.cell_count;
+
+			if (cell_count > (sizeof(bat_status.voltage_cell_v) / sizeof(bat_status.voltage_cell_v[0]))) {
+				cell_count = sizeof(bat_status.voltage_cell_v) / sizeof(bat_status.voltage_cell_v[0]);
+			}
+
+			float voltage_cell_min = FLT_MAX_EXP;
+			float voltage_cell_max = FLT_MIN_EXP;
+
+			for (uint32_t i = 0; i < cell_count; i++) {
+				bat_status.voltage_cell_v[i] = bat.cell_voltages.elements[i];
+
+				if (bat_status.voltage_cell_v[i] > voltage_cell_max) {
+					voltage_cell_max = bat_status.voltage_cell_v[i];
+				}
+
+				if (bat_status.voltage_cell_v[i] < voltage_cell_min) {
+					voltage_cell_min = bat_status.voltage_cell_v[i];
+				}
+			}
+
+			bat_status.max_cell_voltage_delta = voltage_cell_max - voltage_cell_min; // Current delta or max delta over time?
+
+		} else if (receive.metadata.port_id == _parameters_sub._canard_sub.port_id) {
+			reg_udral_service_battery_Parameters_0_3 parameters {};
+			size_t parameters_size_in_bytes = receive.payload_size;
+			reg_udral_service_battery_Parameters_0_3_deserialize_(&parameters,
+					(const uint8_t *)receive.payload,
+					&parameters_size_in_bytes);
+
+			bat_status.capacity = parameters.design_capacity.coulomb / 3.6f; // Coulomb -> mAh
+			bat_status.cycle_count = parameters.cycle_count;
+			bat_status.serial_number = parameters.unique_id & 0xFFFF;
+			bat_status.state_of_health = parameters.state_of_health_pct;
+			bat_status.max_error = 1; // UAVCAN didn't spec'ed this, but we're optimistic
+			bat_status.id = 0; //TODO instancing
+			_nominal_voltage = parameters.nominal_voltage.volt;
+		}
+	}
+
+private:
+	float _nominal_voltage = NAN;
+
+	uORB::PublicationMulti<battery_status_s> _battery_status_pub{ORB_ID(battery_status)};
+
+	SubjectSubscription _status_sub;
+	SubjectSubscription _parameters_sub;
+
+	const char *_status_name = "battery_status";
+	const char *_parameters_name = "battery_parameters";
+
+	battery_status_s bat_status {0};
+};

src/drivers/cyphal/Subscribers/udral/Esc.hpp

@@ -0,0 +1,111 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Esc.hpp
+ *
+ * Defines basic functionality of Cyphal ESC setpoint subscription
+ * (For use on a CAN-->PWM node)
+ *
+ * @author Jacob Crabill <jacob@flyvoly.com>
+ */
+
+#pragma once
+
+/// For use with PR-16808 once merged
+// #include <uORB/topics/output_control.h>
+
+// UDRAL Specification Messages
+#include <reg/udral/service/actuator/common/sp/Vector8_0_1.h>
+#include <reg/udral/service/common/Readiness_0_1.h>
+
+#include "../DynamicPortSubscriber.hpp"
+
+class UavcanEscSubscriber : public UavcanDynamicPortSubscriber
+{
+public:
+	UavcanEscSubscriber(CanardHandle &handle, UavcanParamManager &pmgr, uint8_t instance = 0) :
+		UavcanDynamicPortSubscriber(handle, pmgr, "udral.", "esc", instance) { };
+
+	void subscribe() override
+	{
+		// Subscribe to messages reg.drone.service.actuator.common.sp.Vector8.0.1
+		_canard_handle.RxSubscribe(CanardTransferKindMessage,
+					   _subj_sub._canard_sub.port_id,
+					   reg_udral_service_actuator_common_sp_Vector8_0_1_EXTENT_BYTES_,
+					   CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+					   &_subj_sub._canard_sub);
+
+		// Subscribe to messages reg.drone.service.common.Readiness.0.1
+		_canard_handle.RxSubscribe(CanardTransferKindMessage,
+					   static_cast<CanardPortID>(static_cast<uint32_t>(_subj_sub._canard_sub.port_id) + 1),
+					   reg_udral_service_common_Readiness_0_1_EXTENT_BYTES_,
+					   CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+					   &_canard_sub_readiness);
+	};
+
+	void callback(const CanardRxTransfer &receive) override
+	{
+		// Test with Yakut:
+		// export YAKUT_TRANSPORT="pyuavcan.transport.can.CANTransport(pyuavcan.transport.can.media.slcan.SLCANMedia('/dev/serial/by-id/usb-Zubax_Robotics_Zubax_Babel_23002B000E514E413431302000000000-if00', 8, 115200), 42)"
+		// yakut pub 22.reg.drone.service.actuator.common.sp.Vector8.0.1 '{value: [1000, 2000, 3000, 4000, 0, 0, 0, 0]}'
+		PX4_INFO("EscCallback");
+
+		reg_udral_service_actuator_common_sp_Vector8_0_1 esc {};
+		size_t esc_size_in_bits = receive.payload_size;
+		reg_udral_service_actuator_common_sp_Vector8_0_1_deserialize_(&esc, (const uint8_t *)receive.payload,
+				&esc_size_in_bits);
+
+		double val1 = static_cast<double>(esc.value[0]);
+		double val2 = static_cast<double>(esc.value[1]);
+		double val3 = static_cast<double>(esc.value[2]);
+		double val4 = static_cast<double>(esc.value[3]);
+		PX4_INFO("values[0-3] = {%f, %f, %f, %f}", val1, val2, val3, val4);
+		/// do something with the data
+
+		/// For use with PR-16808 once merged
+		// output_control_s outputs;
+
+		// for (uint8_t i = 0; i < 8; i++) {
+		// 	outputs.value[i] = 2.f * (esc.value[i] / 8191.f) - 1.f;
+		// }
+
+		// _output_pub.publish(outputs);
+	};
+
+private:
+	/// For use with PR-16808 once merged
+	// uORB::Publication<output_control_s> _output_pub{ORB_ID(output_control_mc)};
+
+	CanardRxSubscription _canard_sub_readiness;
+};

src/drivers/cyphal/Subscribers/udral/Gnss.hpp

@@ -0,0 +1,98 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Gnss.hpp
+ *
+ * Defines basic functionality of Cyphal GNSS subscription
+ *
+ * @author Jacob Crabill <jacob@flyvoly.com>
+ */
+
+#pragma once
+
+// UDRAL Specification Messages
+#include <reg/udral/physics/kinematics/geodetic/Point_0_1.h>
+
+#include "../DynamicPortSubscriber.hpp"
+
+class UavcanGnssSubscriber : public UavcanDynamicPortSubscriber
+{
+public:
+	UavcanGnssSubscriber(CanardHandle &handle, UavcanParamManager &pmgr, uint8_t instance = 0) :
+		UavcanDynamicPortSubscriber(handle, pmgr, "udral.", "gps", instance) { };
+
+	void subscribe() override
+	{
+		// Subscribe to messages reg.drone.physics.kinematics.geodetic.Point.0.1
+		_canard_handle.RxSubscribe(CanardTransferKindMessage,
+					   _subj_sub._canard_sub.port_id,
+					   reg_udral_physics_kinematics_geodetic_Point_0_1_EXTENT_BYTES_,
+					   CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+					   &_subj_sub._canard_sub);
+
+		/** TODO: Add additional GPS-data messages: (reg.drone.service.gnss._.0.1.uavcan):
+		 * # A compliant implementation of this service should publish the following subjects:
+		 * #
+		 * #   PUBLISHED SUBJECT NAME      SUBJECT TYPE                                            TYP. RATE [Hz]
+		 * #   point_kinematics            reg.drone.physics.kinematics.geodetic.PointStateVarTs   1...100
+		 * #   time                        reg.drone.service.gnss.Time                             1...10
+		 * #   heartbeat                   reg.drone.service.gnss.Heartbeat                        ~1
+		 * #   sensor_status               reg.drone.service.sensor.Status                         ~1
+		 *
+		 * Not mentioned, but should also be included: Dilution of Precision
+		 *   (reg.drone.service.gnss.DilutionOfPrecision.0.1.uavcan)
+		 * For PX4, only the PointStateVarTs, DilutionOfPrecision, and perhaps Time would be needed
+		 * to publish 'sensor_gps'
+		 */
+	};
+
+	void callback(const CanardRxTransfer &receive) override
+	{
+		// Test with Yakut:
+		// export YAKUT_TRANSPORT="pyuavcan.transport.can.CANTransport(pyuavcan.transport.can.media.slcan.SLCANMedia('/dev/serial/by-id/usb-Zubax_Robotics_Zubax_Babel_23002B000E514E413431302000000000-if00', 8, 115200), 42)"
+		// yakut pub 1500.reg.drone.physics.kinematics.geodetic.Point.0.1 '{latitude: 1.234, longitude: 2.34, altitude: {meter: 0.5}}'
+		PX4_INFO("GpsCallback");
+
+		reg_udral_physics_kinematics_geodetic_Point_0_1 geo {};
+		size_t geo_size_in_bits = receive.payload_size;
+		reg_udral_physics_kinematics_geodetic_Point_0_1_deserialize_(&geo, (const uint8_t *)receive.payload, &geo_size_in_bits);
+
+		double lat = geo.latitude;
+		double lon = geo.longitude;
+		double alt = geo.altitude.meter;
+		PX4_INFO("Latitude: %f, Longitude: %f, Altitude: %f", lat, lon, alt);
+		/// do something with the data
+	};
+
+};

src/drivers/cyphal/SubscriptionManager.cpp

@@ -0,0 +1,157 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file SubscriptionManager.cpp
+ *
+ * Manages the UAVCAN subscriptions
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+
+#include "SubscriptionManager.hpp"
+
+#include "ParamManager.hpp"
+
+SubscriptionManager::~SubscriptionManager()
+{
+	UavcanDynamicPortSubscriber *dynsub;
+
+	while (_dynsubscribers != nullptr) {
+		dynsub = _dynsubscribers;
+		_dynsubscribers = dynsub->next();
+		delete dynsub;
+	}
+}
+
+void SubscriptionManager::subscribe()
+{
+	_heartbeat_sub.subscribe();
+
+#if CONFIG_CYPHAL_GETINFO_RESPONDER
+	_getinfo_rsp.subscribe();
+#endif
+
+	_access_rsp.subscribe();
+	_list_rsp.subscribe();
+
+	updateDynamicSubscriptions();
+}
+
+void SubscriptionManager::updateDynamicSubscriptions()
+{
+	for (auto &sub : _uavcan_subs) {
+
+		bool found_subscriber = false;
+		UavcanDynamicPortSubscriber *dynsub = _dynsubscribers;
+
+		while (dynsub != nullptr) {
+			// Check if subscriber has already been created
+			const char *subj_name = dynsub->getSubjectName();
+			const uint8_t instance = dynsub->getInstance();
+
+			if (strcmp(subj_name, sub.subject_name) == 0 && instance == sub.instance) {
+				found_subscriber = true;
+				break;
+			}
+
+			dynsub = dynsub->next();
+		}
+
+		if (found_subscriber) {
+			continue;
+		}
+
+		char uavcan_param[90];
+		snprintf(uavcan_param, sizeof(uavcan_param), "uavcan.sub.%s.%d.id", sub.subject_name, sub.instance);
+		uavcan_register_Value_1_0 value;
+
+		if (_param_manager.GetParamByName(uavcan_param, value)) {
+			uint16_t port_id = value.natural16.value.elements[0];
+
+			if (port_id <= CANARD_PORT_ID_MAX) { // PortID is set, create a subscriber
+				dynsub = sub.create_sub(_canard_handle, _param_manager);
+
+				if (dynsub == nullptr) {
+					PX4_ERR("Out of memory");
+					return;
+				}
+
+				if (_dynsubscribers == nullptr) {
+					// Set the head of our linked list
+					_dynsubscribers = dynsub;
+
+				} else {
+					// Append the new subscriber to our linked list
+					UavcanDynamicPortSubscriber *tmp = _dynsubscribers;
+
+					while (tmp->next() != nullptr) {
+						tmp = tmp->next();
+					}
+
+					tmp->setNext(dynsub);
+				}
+
+				dynsub->updateParam();
+			}
+
+		} else {
+			PX4_ERR("Port ID param for subscriber %s.%u not found", sub.subject_name, sub.instance);
+			return;
+		}
+	}
+}
+
+void SubscriptionManager::printInfo()
+{
+	UavcanDynamicPortSubscriber *dynsub = _dynsubscribers;
+
+	while (dynsub != nullptr) {
+		dynsub->printInfo();
+		dynsub = dynsub->next();
+	}
+}
+
+void SubscriptionManager::updateParams()
+{
+	UavcanDynamicPortSubscriber *dynsub = _dynsubscribers;
+
+	while (dynsub != nullptr) {
+		dynsub->updateParam();
+		dynsub = dynsub->next();
+	}
+
+	// Check for any newly-enabled subscriptions
+	updateDynamicSubscriptions();
+}

src/drivers/cyphal/SubscriptionManager.hpp

@@ -0,0 +1,184 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file SubscriptionManager.hpp
+ *
+ * Manages the UAVCAN subscriptions
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#pragma once
+
+#ifndef CONFIG_CYPHAL_ESC_SUBSCRIBER
+#define CONFIG_CYPHAL_ESC_SUBSCRIBER 0
+#endif
+
+#ifndef CONFIG_CYPHAL_GNSS_SUBSCRIBER_0
+#define CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 0
+#endif
+
+#ifndef CONFIG_CYPHAL_GNSS_SUBSCRIBER_1
+#define CONFIG_CYPHAL_GNSS_SUBSCRIBER_1 0
+#endif
+
+#ifndef CONFIG_CYPHAL_BMS_SUBSCRIBER
+#define CONFIG_CYPHAL_BMS_SUBSCRIBER 0
+#endif
+
+#ifndef CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER
+#define CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER 0
+#endif
+
+/* Preprocessor calculation of Subscribers count */
+
+#define UAVCAN_SUB_COUNT CONFIG_CYPHAL_ESC_SUBSCRIBER + \
+	CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 + \
+	CONFIG_CYPHAL_GNSS_SUBSCRIBER_1 + \
+	CONFIG_CYPHAL_BMS_SUBSCRIBER + \
+	CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER
+
+#include <px4_platform_common/defines.h>
+#include <drivers/drv_hrt.h>
+#include "Subscribers/DynamicPortSubscriber.hpp"
+#include "CanardInterface.hpp"
+
+#include "ServiceClients/GetInfo.hpp"
+#include "ServiceClients/Access.hpp"
+#include "ServiceClients/List.hpp"
+#include "Subscribers/BaseSubscriber.hpp"
+#include "Subscribers/Heartbeat.hpp"
+#include "Subscribers/udral/Battery.hpp"
+#include "Subscribers/udral/Esc.hpp"
+#include "Subscribers/udral/Gnss.hpp"
+#include "Subscribers/legacy/LegacyBatteryInfo.hpp"
+#include "Subscribers/uORB/uorb_subscriber.hpp"
+
+typedef struct {
+	UavcanDynamicPortSubscriber *(*create_sub)(CanardHandle &handle, UavcanParamManager &pmgr) {};
+	const char *subject_name;
+	const uint8_t instance;
+} UavcanDynSubBinder;
+
+class SubscriptionManager
+{
+public:
+	SubscriptionManager(CanardHandle &handle, UavcanParamManager &pmgr) : _canard_handle(handle), _param_manager(pmgr) {}
+	~SubscriptionManager();
+
+	void subscribe();
+	void printInfo();
+	void updateParams();
+
+private:
+	void updateDynamicSubscriptions();
+
+	CanardHandle &_canard_handle;
+	UavcanParamManager &_param_manager;
+	UavcanDynamicPortSubscriber *_dynsubscribers {nullptr};
+
+	UavcanHeartbeatSubscriber _heartbeat_sub {_canard_handle};
+
+#if CONFIG_CYPHAL_GETINFO_RESPONDER
+	// GetInfo response
+	UavcanGetInfoResponse _getinfo_rsp {_canard_handle};
+#endif
+
+	// Process register requests
+	UavcanAccessResponse  _access_rsp {_canard_handle, _param_manager};
+	UavcanListResponse  _list_rsp {_canard_handle, _param_manager};
+
+	const UavcanDynSubBinder _uavcan_subs[UAVCAN_SUB_COUNT] {
+#if CONFIG_CYPHAL_ESC_SUBSCRIBER
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
+			{
+				return new UavcanEscSubscriber(handle, pmgr, 0);
+			},
+			"esc",
+			0
+		},
+#endif
+#if CONFIG_CYPHAL_GNSS_SUBSCRIBER_0
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
+			{
+				return new UavcanGnssSubscriber(handle, pmgr, 0);
+			},
+			"gps",
+			0
+		},
+#endif
+#if CONFIG_CYPHAL_GNSS_SUBSCRIBER_1 //FIXME decouple handletanceing
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
+			{
+				return new UavcanGnssSubscriber(handle, pmgr, 1);
+			},
+			"gps",
+			1
+		},
+#endif
+#if CONFIG_CYPHAL_BMS_SUBSCRIBER
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
+			{
+				return new UavcanBmsSubscriber(handle, pmgr, 0);
+			},
+			"energy_source",
+			0
+		},
+#endif
+#if 0 //Obsolete to be removed
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
+			{
+				return new UavcanLegacyBatteryInfoSubscriber(handle, pmgr, 0);
+			},
+			"legacy_bms",
+			0
+		},
+#endif
+#if CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
+			{
+				return new uORB_over_UAVCAN_Subscriber<sensor_gps_s>(handle, pmgr, ORB_ID(sensor_gps));
+			},
+			"uorb.sensor_gps",
+			0
+		},
+#endif
+	};
+};

src/drivers/cyphal/module.yaml

@@ -0,0 +1,10 @@
+module_name: UAVCANv1
+actuator_output:
+  output_groups:
+    - param_prefix: UCAN1_ESC
+      channel_label: 'ESC'
+      standard_params:
+        min: { min: 0, max: 8191, default: 1 }
+        max: { min: 0, max: 8191, default: 8191 }
+        failsafe: { min: 0, max: 8191 }
+      num_channels: 16

src/drivers/cyphal/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/cyphal/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/cyphal/parameters.c

@@ -0,0 +1,190 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * Cyphal
+ *
+ *  0 - Cyphal disabled.
+ *  1 - Enables Cyphal
+ *
+ * @boolean
+ * @reboot_required true
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(CYPHAL_ENABLE, 0);
+
+/**
+ * Cyphal Node ID.
+ *
+ * Read the specs at http://uavcan.org to learn more about Node ID.
+ *
+ * @min -1
+ * @max 125
+ * @reboot_required true
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(CYPHAL_ID, 1);
+
+/**
+ * UAVCAN/CAN v1 bus bitrate.
+ *
+ * @unit bit/s
+ * @min 20000
+ * @max 1000000
+ * @reboot_required true
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(CYPHAL_BAUD, 1000000);
+
+/* Subscription port ID, -1 will be treated as unset */
+
+/**
+ * ESC 0 subscription port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_ESC0_SUB, -1);
+
+/**
+ * GPS 0 subscription port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_GPS0_SUB, -1);
+
+/**
+ * GPS 1 subscription port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_GPS1_SUB, -1);
+
+/**
+ * UDRAL battery energy source subscription port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_BMS_ES_SUB, -1);
+
+/**
+ * UDRAL battery status subscription port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_BMS_BS_SUB, -1);
+
+/**
+ * UDRAL battery parameters subscription  port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_BMS_BP_SUB, -1);
+
+/**
+ * Cyphal leagcy battery port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_LG_BMS_SUB, -1);
+
+
+/**
+ * sensor_gps uORB over Cyphal subscription port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_UORB_GPS, -1);
+
+
+/**
+ * sensor_gps uORB over Cyphal publication port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_UORB_GPS_P, -1);
+
+// Publication Port IDs
+
+/**
+ * Cyphal ESC publication port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_ESC_PUB, -1);
+
+/**
+ * Cyphal GPS publication port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_GPS_PUB, -1);
+
+/**
+ * Cyphal Servo publication port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_SERVO_PUB, -1);
+
+/**
+ * actuator_outputs uORB over Cyphal publication port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_ACTR_PUB, -1);