move uavcan from modules to drivers

src/drivers/uavcan/.gitignore

@@ -0,0 +1 @@
+./dsdlc_generated/

src/drivers/uavcan/CMakeLists.txt

@@ -0,0 +1,106 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+
+px4_add_git_submodule(TARGET git_uavcan PATH "libuavcan")
+
+set(UAVCAN_USE_CPP03 ON CACHE BOOL "uavcan cpp03")
+
+if(CONFIG_ARCH_CHIP)
+
+	if (${CONFIG_ARCH_CHIP} MATCHES "kinetis")
+		set(UAVCAN_PLATFORM "kinetis")
+		set(UAVCAN_TIMER 1)
+	elseif (${CONFIG_ARCH_CHIP} MATCHES "stm32")
+		set(UAVCAN_PLATFORM "stm32")
+		set(UAVCAN_TIMER 5) # The default timer the 5
+	endif()
+endif()
+
+if(NOT config_uavcan_num_ifaces)
+	message(FATAL_ERROR "config_uavcan_num_ifaces not set")
+endif()
+
+string(TOUPPER "${PX4_PLATFORM}" OS_UPPER)
+string(TOUPPER "${UAVCAN_PLATFORM}" UAVCAN_PLATFORM_UPPER)
+add_definitions(
+		-DUAVCAN_CPP_VERSION=UAVCAN_CPP03
+		-DUAVCAN_MEM_POOL_BLOCK_SIZE=48
+		-DUAVCAN_NO_ASSERTIONS
+		-DUAVCAN_PLATFORM=${UAVCAN_PLATFORM}
+		-DUAVCAN_DRIVER=uavcan_${UAVCAN_PLATFORM}
+		-DUAVCAN_${UAVCAN_PLATFORM_UPPER}_${OS_UPPER}=1
+		-DUAVCAN_${UAVCAN_PLATFORM_UPPER}_NUM_IFACES=${config_uavcan_num_ifaces}
+		-DUAVCAN_${UAVCAN_PLATFORM_UPPER}_TIMER_NUMBER=${UAVCAN_TIMER}
+		)
+
+add_subdirectory(libuavcan EXCLUDE_FROM_ALL)
+add_dependencies(uavcan prebuild_targets)
+
+px4_add_module(
+	MODULE modules__uavcan
+	MAIN uavcan
+	STACK_MAIN 3200
+	STACK_MAX 1500
+	INCLUDES
+		${PX4_SOURCE_DIR}/mavlink/include/mavlink
+		libuavcan/libuavcan/include
+		libuavcan/libuavcan/include/dsdlc_generated
+		libuavcan/libuavcan_drivers/posix/include
+		libuavcan/libuavcan_drivers/${UAVCAN_PLATFORM}/driver/include
+	SRCS
+		# Main
+		uavcan_main.cpp
+		uavcan_servers.cpp
+		uavcan_params.c
+
+		# Actuators
+		actuators/esc.cpp
+		actuators/hardpoint.cpp
+
+		# Sensors
+		sensors/sensor_bridge.cpp
+		sensors/gnss.cpp
+		sensors/mag.cpp
+		sensors/baro.cpp
+
+	DEPENDS
+		mixer
+		version
+
+		git_uavcan
+
+		# within libuavcan
+		libuavcan_dsdlc
+		uavcan
+		uavcan_${UAVCAN_PLATFORM}_driver
+	)

src/drivers/uavcan/actuators/esc.cpp

@@ -0,0 +1,232 @@
+/****************************************************************************
+ *
+ *   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 esc.cpp
+ *
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include "esc.hpp"
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+
+#define MOTOR_BIT(x) (1<<(x))
+
+UavcanEscController::UavcanEscController(uavcan::INode &node) :
+	_node(node),
+	_uavcan_pub_raw_cmd(node),
+	_uavcan_sub_status(node),
+	_orb_timer(node)
+{
+	_uavcan_pub_raw_cmd.setPriority(UAVCAN_COMMAND_TRANSFER_PRIORITY);
+
+	if (_perfcnt_invalid_input == nullptr) {
+		errx(1, "uavcan: couldn't allocate _perfcnt_invalid_input");
+	}
+
+	if (_perfcnt_scaling_error == nullptr) {
+		errx(1, "uavcan: couldn't allocate _perfcnt_scaling_error");
+	}
+}
+
+UavcanEscController::~UavcanEscController()
+{
+	perf_free(_perfcnt_invalid_input);
+	perf_free(_perfcnt_scaling_error);
+}
+
+int UavcanEscController::init()
+{
+	// ESC status subscription
+	int res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb));
+
+	if (res < 0) {
+		warnx("ESC status sub failed %i", res);
+		return res;
+	}
+
+	// ESC status will be relayed from UAVCAN bus into ORB at this rate
+	_orb_timer.setCallback(TimerCbBinder(this, &UavcanEscController::orb_pub_timer_cb));
+	_orb_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / ESC_STATUS_UPDATE_RATE_HZ));
+
+	return res;
+}
+
+void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
+{
+	if ((outputs == nullptr) ||
+	    (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) ||
+	    (num_outputs > esc_status_s::CONNECTED_ESC_MAX)) {
+		perf_count(_perfcnt_invalid_input);
+		return;
+	}
+
+	/*
+	 * Rate limiting - we don't want to congest the bus
+	 */
+	const auto timestamp = _node.getMonotonicTime();
+
+	if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) {
+		return;
+	}
+
+	_prev_cmd_pub = timestamp;
+
+	/*
+	 * Fill the command message
+	 * If unarmed, we publish an empty message anyway
+	 */
+	uavcan::equipment::esc::RawCommand msg;
+
+	actuator_outputs_s actuator_outputs = {};
+	actuator_outputs.noutputs = num_outputs;
+	actuator_outputs.timestamp = hrt_absolute_time();
+
+	static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max();
+	const float cmd_min = _run_at_idle_throttle_when_armed ? 1.0F : 0.0F;
+
+	for (unsigned i = 0; i < num_outputs; i++) {
+		if (_armed_mask & MOTOR_BIT(i)) {
+			float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max;
+
+			// trim negative values back to minimum
+			if (scaled < cmd_min) {
+				scaled = cmd_min;
+				perf_count(_perfcnt_scaling_error);
+			}
+
+			if (scaled > cmd_max) {
+				scaled = cmd_max;
+				perf_count(_perfcnt_scaling_error);
+			}
+
+			msg.cmd.push_back(static_cast<int>(scaled));
+
+			_esc_status.esc[i].esc_setpoint_raw = abs(static_cast<int>(scaled));
+			actuator_outputs.output[i] = scaled;
+
+		} else {
+			msg.cmd.push_back(static_cast<unsigned>(0));
+			actuator_outputs.output[i] = 0.0f;
+		}
+	}
+
+	/*
+	 * Remove channels that are always zero.
+	 * The objective of this optimization is to avoid broadcasting multi-frame transfers when a single frame
+	 * transfer would be enough. This is a valid optimization as the UAVCAN specification implies that all
+	 * non-specified ESC setpoints should be considered zero.
+	 * The positive outcome is a (marginally) lower bus traffic and lower CPU load.
+	 *
+	 * From the standpoint of the PX4 architecture, however, this is a hack. It should be investigated why
+	 * the mixer returns more outputs than are actually used.
+	 */
+	for (int index = int(msg.cmd.size()) - 1; index >= _max_number_of_nonzero_outputs; index--) {
+		if (msg.cmd[index] != 0) {
+			_max_number_of_nonzero_outputs = index + 1;
+			break;
+		}
+	}
+
+	msg.cmd.resize(_max_number_of_nonzero_outputs);
+
+	/*
+	 * Publish the command message to the bus
+	 * Note that for a quadrotor it takes one CAN frame
+	 */
+	(void)_uavcan_pub_raw_cmd.broadcast(msg);
+
+	// Publish actuator outputs
+	if (_actuator_outputs_pub != nullptr) {
+		orb_publish(ORB_ID(actuator_outputs), _actuator_outputs_pub, &actuator_outputs);
+
+	} else {
+		int instance;
+		_actuator_outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &actuator_outputs,
+					&instance, ORB_PRIO_DEFAULT);
+	}
+
+}
+
+void UavcanEscController::arm_all_escs(bool arm)
+{
+	if (arm) {
+		_armed_mask = -1;
+
+	} else {
+		_armed_mask = 0;
+	}
+}
+
+void UavcanEscController::arm_single_esc(int num, bool arm)
+{
+	if (arm) {
+		_armed_mask = MOTOR_BIT(num);
+
+	} else {
+		_armed_mask = 0;
+	}
+}
+
+void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)
+{
+	if (msg.esc_index < esc_status_s::CONNECTED_ESC_MAX) {
+		_esc_status.esc_count = uavcan::max<int>(_esc_status.esc_count, msg.esc_index + 1);
+		_esc_status.timestamp = msg.getMonotonicTimestamp().toUSec();
+
+		auto &ref = _esc_status.esc[msg.esc_index];
+
+		ref.esc_address = msg.getSrcNodeID().get();
+
+		ref.esc_voltage     = msg.voltage;
+		ref.esc_current     = msg.current;
+		ref.esc_temperature = msg.temperature;
+		ref.esc_setpoint    = msg.power_rating_pct;
+		ref.esc_rpm         = msg.rpm;
+		ref.esc_errorcount  = msg.error_count;
+	}
+}
+
+void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent &)
+{
+	_esc_status.counter += 1;
+	_esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN;
+
+	if (_esc_status_pub != nullptr) {
+		(void)orb_publish(ORB_ID(esc_status), _esc_status_pub, &_esc_status);
+
+	} else {
+		_esc_status_pub = orb_advertise(ORB_ID(esc_status), &_esc_status);
+	}
+}

src/drivers/uavcan/actuators/esc.hpp

@@ -0,0 +1,119 @@
+/****************************************************************************
+ *
+ *   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 esc.hpp
+ *
+ * UAVCAN <--> ORB bridge for ESC messages:
+ *     uavcan.equipment.esc.RawCommand
+ *     uavcan.equipment.esc.RPMCommand
+ *     uavcan.equipment.esc.Status
+ *
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <uavcan/uavcan.hpp>
+#include <uavcan/equipment/esc/RawCommand.hpp>
+#include <uavcan/equipment/esc/Status.hpp>
+#include <perf/perf_counter.h>
+#include <uORB/topics/esc_status.h>
+#include <uORB/topics/actuator_outputs.h>
+
+
+class UavcanEscController
+{
+public:
+	UavcanEscController(uavcan::INode &node);
+	~UavcanEscController();
+
+	int init();
+
+	void update_outputs(float *outputs, unsigned num_outputs);
+
+	void arm_all_escs(bool arm);
+	void arm_single_esc(int num, bool arm);
+
+	void enable_idle_throttle_when_armed(bool value) { _run_at_idle_throttle_when_armed = value; }
+
+private:
+	/**
+	 * ESC status message reception will be reported via this callback.
+	 */
+	void esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg);
+
+	/**
+	 * ESC status will be published to ORB from this callback (fixed rate).
+	 */
+	void orb_pub_timer_cb(const uavcan::TimerEvent &event);
+
+
+	static constexpr unsigned MAX_RATE_HZ = 200;			///< XXX make this configurable
+	static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 10;
+	static constexpr unsigned UAVCAN_COMMAND_TRANSFER_PRIORITY = 5;	///< 0..31, inclusive, 0 - highest, 31 - lowest
+
+	typedef uavcan::MethodBinder<UavcanEscController *,
+		void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)>
+		StatusCbBinder;
+
+	typedef uavcan::MethodBinder<UavcanEscController *, void (UavcanEscController::*)(const uavcan::TimerEvent &)>
+	TimerCbBinder;
+
+	bool		_armed = false;
+	bool		_run_at_idle_throttle_when_armed = false;
+	esc_status_s	_esc_status = {};
+	orb_advert_t	_esc_status_pub = nullptr;
+	orb_advert_t _actuator_outputs_pub = nullptr;
+
+	/*
+	 * libuavcan related things
+	 */
+	uavcan::MonotonicTime							_prev_cmd_pub;   ///< rate limiting
+	uavcan::INode								&_node;
+	uavcan::Publisher<uavcan::equipment::esc::RawCommand>			_uavcan_pub_raw_cmd;
+	uavcan::Subscriber<uavcan::equipment::esc::Status, StatusCbBinder>	_uavcan_sub_status;
+	uavcan::TimerEventForwarder<TimerCbBinder>				_orb_timer;
+
+	/*
+	 * ESC states
+	 */
+	uint32_t 			_armed_mask = 0;
+	uint8_t				_max_number_of_nonzero_outputs = 0;
+
+	/*
+	 * Perf counters
+	 */
+	perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input");
+	perf_counter_t _perfcnt_scaling_error = perf_alloc(PC_COUNT, "uavcan_esc_scaling_error");
+};

src/drivers/uavcan/actuators/hardpoint.cpp

@@ -0,0 +1,90 @@
+/****************************************************************************
+ *
+ *   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 hardpoint.cpp
+ *
+ * @author Andreas Jochum <Andreas@NicaDrone.com>
+ */
+
+#include "hardpoint.hpp"
+#include <systemlib/err.h>
+
+UavcanHardpointController::UavcanHardpointController(uavcan::INode &node) :
+	_node(node),
+	_uavcan_pub_raw_cmd(node),
+	_timer(node)
+{
+	_uavcan_pub_raw_cmd.setPriority(uavcan::TransferPriority::MiddleLower);
+}
+
+
+UavcanHardpointController::~UavcanHardpointController()
+{
+
+}
+
+int UavcanHardpointController::init()
+{
+	/*
+	 * Setup timer and call back function for periodic updates
+	 */
+	_timer.setCallback(TimerCbBinder(this, &UavcanHardpointController::periodic_update));
+	return 0;
+}
+
+void UavcanHardpointController::set_command(uint8_t hardpoint_id, uint16_t command)
+{
+	_cmd.command = command;
+	_cmd.hardpoint_id = hardpoint_id;
+
+	/*
+	 * Publish the command message to the bus
+	 */
+	(void)_uavcan_pub_raw_cmd.broadcast(_cmd);
+
+	/*
+	 * Start the periodic update timer after a command is set
+	 */
+	if (!_timer.isRunning()) {
+		_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / MAX_RATE_HZ));
+	}
+
+}
+void UavcanHardpointController::periodic_update(const uavcan::TimerEvent &)
+{
+	/*
+	 * Broadcast command at MAX_RATE_HZ
+	 */
+	(void)_uavcan_pub_raw_cmd.broadcast(_cmd);
+}

src/drivers/uavcan/actuators/hardpoint.hpp

@@ -0,0 +1,89 @@
+/****************************************************************************
+ *
+ *   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 hardpoint.hpp
+ *
+ * @author Andreas Jochum <Andreas@NicaDrone.com>
+ */
+
+#pragma once
+
+#include <uavcan/uavcan.hpp>
+#include <uavcan/equipment/hardpoint/Command.hpp>
+#include <uavcan/equipment/hardpoint/Status.hpp>
+#include <perf/perf_counter.h>
+
+/**
+ * @brief The UavcanHardpointController class
+ */
+
+class UavcanHardpointController
+{
+public:
+	UavcanHardpointController(uavcan::INode &node);
+	~UavcanHardpointController();
+
+	/*
+	* setup periodic updater
+	*/
+	int init();
+
+
+	/*
+	 * set command
+	 */
+	void set_command(uint8_t hardpoint_id, uint16_t command);
+
+private:
+	/*
+	 * Max update rate to avoid exessive bus traffic
+	 */
+	static constexpr unsigned			MAX_RATE_HZ = 1;	///< XXX make this configurable
+
+	uavcan::equipment::hardpoint::Command		_cmd;
+
+	void periodic_update(const uavcan::TimerEvent &);
+
+	typedef uavcan::MethodBinder<UavcanHardpointController *, void (UavcanHardpointController::*)(const uavcan::TimerEvent &)>
+	TimerCbBinder;
+
+	pthread_mutex_t					_node_mutex;
+	/*
+	 * libuavcan related things
+	 */
+	uavcan::INode							&_node;
+	uavcan::Publisher<uavcan::equipment::hardpoint::Command>	_uavcan_pub_raw_cmd;
+	uavcan::TimerEventForwarder<TimerCbBinder>			_timer;
+
+};

src/drivers/uavcan/allocator.hpp

@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2015 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <systemlib/err.h>
+#include <uavcan/uavcan.hpp>
+#include <uavcan/helpers/heap_based_pool_allocator.hpp>
+
+// TODO: Entire UAVCAN application should be moved into a namespace later; this is the first step.
+namespace uavcan_node
+{
+
+struct AllocatorSynchronizer {
+	const ::irqstate_t state = ::enter_critical_section();
+	~AllocatorSynchronizer() { ::leave_critical_section(state); }
+};
+
+struct Allocator : public uavcan::HeapBasedPoolAllocator<uavcan::MemPoolBlockSize, AllocatorSynchronizer> {
+	static constexpr unsigned CapacitySoftLimit = 250;
+	static constexpr unsigned CapacityHardLimit = 500;
+
+	Allocator() :
+		uavcan::HeapBasedPoolAllocator<uavcan::MemPoolBlockSize, AllocatorSynchronizer>(CapacitySoftLimit, CapacityHardLimit)
+	{ }
+
+	~Allocator()
+	{
+		if (getNumAllocatedBlocks() > 0) {
+			warnx("UAVCAN LEAKS MEMORY: %u BLOCKS (%u BYTES) LOST",
+			      getNumAllocatedBlocks(), getNumAllocatedBlocks() * uavcan::MemPoolBlockSize);
+		}
+	}
+};
+
+}

src/drivers/uavcan/sensors/baro.cpp

@@ -0,0 +1,142 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014, 2015 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <drivers/drv_hrt.h>
+#include "baro.hpp"
+#include <cmath>
+
+const char *const UavcanBarometerBridge::NAME = "baro";
+
+UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node) :
+	UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_BASE_DEVICE_PATH, ORB_ID(sensor_baro)),
+	_sub_air_pressure_data(node),
+	_sub_air_temperature_data(node),
+	_reports(2, sizeof(sensor_baro_s))
+{ }
+
+int UavcanBarometerBridge::init()
+{
+	int res = device::CDev::init();
+
+	if (res < 0) {
+		return res;
+	}
+
+	res = _sub_air_pressure_data.start(AirPressureCbBinder(this, &UavcanBarometerBridge::air_pressure_sub_cb));
+
+	if (res < 0) {
+		DEVICE_LOG("failed to start uavcan sub: %d", res);
+		return res;
+	}
+
+	res = _sub_air_temperature_data.start(AirTemperatureCbBinder(this, &UavcanBarometerBridge::air_temperature_sub_cb));
+
+	if (res < 0) {
+		DEVICE_LOG("failed to start uavcan sub: %d", res);
+		return res;
+	}
+
+	return 0;
+}
+
+ssize_t UavcanBarometerBridge::read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(sensor_baro_s);
+	sensor_baro_s *baro_buf = reinterpret_cast<sensor_baro_s *>(buffer);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1) {
+		return -ENOSPC;
+	}
+
+	while (count--) {
+		if (_reports.get(baro_buf)) {
+			ret += sizeof(*baro_buf);
+			baro_buf++;
+		}
+	}
+
+	/* if there was no data, warn the caller */
+	return ret ? ret : -EAGAIN;
+}
+
+int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+	case SENSORIOCSPOLLRATE: {
+			// not supported yet, pretend that everything is ok
+			return OK;
+		}
+
+	default: {
+			return CDev::ioctl(filp, cmd, arg);
+		}
+	}
+}
+
+void UavcanBarometerBridge::air_temperature_sub_cb(const
+		uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &msg)
+{
+	last_temperature_kelvin = msg.static_temperature;
+}
+
+void UavcanBarometerBridge::air_pressure_sub_cb(const
+		uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure> &msg)
+{
+	sensor_baro_s report{};
+
+	/*
+	 * FIXME HACK
+	 * This code used to rely on msg.getMonotonicTimestamp().toUSec() instead of HRT.
+	 * It stopped working when the time sync feature has been introduced, because it caused libuavcan
+	 * to use an independent time source (based on hardware TIM5) instead of HRT.
+	 * The proper solution is to be developed.
+	 */
+	report.timestamp   = hrt_absolute_time();
+	report.temperature = last_temperature_kelvin - 273.15F;
+	report.pressure    = msg.static_pressure / 100.0F;  // Convert to millibar
+	report.error_count = 0;
+
+	/* TODO get device ID for sensor */
+	report.device_id = 0;
+
+	// add to the ring buffer
+	_reports.force(&report);
+
+	publish(msg.getSrcNodeID().get(), &report);
+}

src/drivers/uavcan/sensors/baro.hpp

@@ -0,0 +1,84 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014, 2015 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include "sensor_bridge.hpp"
+#include <drivers/drv_baro.h>
+#include <drivers/device/ringbuffer.h>
+
+#include <uavcan/equipment/air_data/StaticPressure.hpp>
+#include <uavcan/equipment/air_data/StaticTemperature.hpp>
+
+class RingBuffer;
+
+class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase
+{
+public:
+	static const char *const NAME;
+
+	UavcanBarometerBridge(uavcan::INode &node);
+
+	const char *get_name() const override { return NAME; }
+
+	int init() override;
+
+private:
+	ssize_t read(struct file *filp, char *buffer, size_t buflen);
+	int ioctl(struct file *filp, int cmd, unsigned long arg) override;
+
+	void air_pressure_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure> &msg);
+	void air_temperature_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &msg);
+
+	typedef uavcan::MethodBinder < UavcanBarometerBridge *,
+		void (UavcanBarometerBridge::*)
+		(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure> &) >
+		AirPressureCbBinder;
+
+	typedef uavcan::MethodBinder < UavcanBarometerBridge *,
+		void (UavcanBarometerBridge::*)
+		(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &) >
+		AirTemperatureCbBinder;
+
+	uavcan::Subscriber<uavcan::equipment::air_data::StaticPressure, AirPressureCbBinder> _sub_air_pressure_data;
+	uavcan::Subscriber<uavcan::equipment::air_data::StaticTemperature, AirTemperatureCbBinder> _sub_air_temperature_data;
+
+	ringbuffer::RingBuffer _reports;
+
+	float last_temperature_kelvin = 0.0f;
+
+};

src/drivers/uavcan/sensors/gnss.cpp

@@ -0,0 +1,463 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014, 2015 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.cpp
+ *
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ * @author Andrew Chambers <achamber@gmail.com>
+ *
+ */
+
+#include "gnss.hpp"
+#include <drivers/drv_hrt.h>
+#include <systemlib/err.h>
+#include <mathlib/mathlib.h>
+
+const char *const UavcanGnssBridge::NAME = "gnss";
+
+UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
+	_node(node),
+	_sub_fix(node),
+	_sub_fix2(node),
+	_pub_fix2(node),
+	_orb_to_uavcan_pub_timer(node, TimerCbBinder(this, &UavcanGnssBridge::broadcast_from_orb)),
+	_report_pub(nullptr)
+{
+}
+
+UavcanGnssBridge::~UavcanGnssBridge()
+{
+	(void) orb_unsubscribe(_orb_sub_gnss);
+}
+
+int UavcanGnssBridge::init()
+{
+	int res = _pub_fix2.init(uavcan::TransferPriority::MiddleLower);
+
+	if (res < 0) {
+		PX4_WARN("GNSS fix2 pub failed %i", res);
+		return res;
+	}
+
+	res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb));
+
+	if (res < 0) {
+		PX4_WARN("GNSS fix sub failed %i", res);
+		return res;
+	}
+
+	res = _sub_fix2.start(Fix2CbBinder(this, &UavcanGnssBridge::gnss_fix2_sub_cb));
+
+	if (res < 0) {
+		PX4_WARN("GNSS fix2 sub failed %i", res);
+		return res;
+	}
+
+	_orb_to_uavcan_pub_timer.startPeriodic(
+		uavcan::MonotonicDuration::fromUSec(1000000U / ORB_TO_UAVCAN_FREQUENCY_HZ));
+
+	return res;
+}
+
+unsigned UavcanGnssBridge::get_num_redundant_channels() const
+{
+	return (_receiver_node_id < 0) ? 0 : 1;
+}
+
+void UavcanGnssBridge::print_status() const
+{
+	printf("RX errors: %d, using old Fix: %d, receiver node id: ",
+	       _sub_fix.getFailureCount(), int(_old_fix_subscriber_active));
+
+	if (_receiver_node_id < 0) {
+		printf("N/A\n");
+
+	} else {
+		printf("%d\n", _receiver_node_id);
+	}
+}
+
+void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg)
+{
+	const bool valid_pos_cov = !msg.position_covariance.empty();
+	const bool valid_vel_cov = !msg.velocity_covariance.empty();
+
+	float pos_cov[9];
+	msg.position_covariance.unpackSquareMatrix(pos_cov);
+
+	float vel_cov[9];
+	msg.velocity_covariance.unpackSquareMatrix(vel_cov);
+
+	process_fixx(msg, pos_cov, vel_cov, valid_pos_cov, valid_vel_cov);
+}
+
+void UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix2> &msg)
+{
+	if (_old_fix_subscriber_active) {
+		PX4_WARN("GNSS Fix2 message detected, disabling support for the old Fix message");
+		_sub_fix.stop();
+		_old_fix_subscriber_active = false;
+		_receiver_node_id = -1;
+	}
+
+	float pos_cov[9] {};
+	float vel_cov[9] {};
+	bool valid_covariances = true;
+
+	switch (msg.covariance.size()) {
+	// Scalar matrix
+	case 1: {
+			const auto x = msg.covariance[0];
+
+			pos_cov[0] = x;
+			pos_cov[4] = x;
+			pos_cov[8] = x;
+
+			vel_cov[0] = x;
+			vel_cov[4] = x;
+			vel_cov[8] = x;
+			break;
+		}
+
+	// Diagonal matrix (the most common case)
+	case 6: {
+			pos_cov[0] = msg.covariance[0];
+			pos_cov[4] = msg.covariance[1];
+			pos_cov[8] = msg.covariance[2];
+
+			vel_cov[0] = msg.covariance[3];
+			vel_cov[4] = msg.covariance[4];
+			vel_cov[8] = msg.covariance[5];
+			break;
+		}
+
+	// Upper triangular matrix.
+	// This code has been carefully optimized by hand. We could use unpackSquareMatrix(), but it's slow.
+	// Sub-matrix indexes (empty squares contain velocity-position covariance data):
+	// 0  1  2
+	// 1  6  7
+	// 2  7 11
+	//         15 16 17
+	//         16 18 19
+	//         17 19 20
+	case 21: {
+			pos_cov[0] = msg.covariance[0];
+			pos_cov[1] = msg.covariance[1];
+			pos_cov[2] = msg.covariance[2];
+			pos_cov[3] = msg.covariance[1];
+			pos_cov[4] = msg.covariance[6];
+			pos_cov[5] = msg.covariance[7];
+			pos_cov[6] = msg.covariance[2];
+			pos_cov[7] = msg.covariance[7];
+			pos_cov[8] = msg.covariance[11];
+
+			vel_cov[0] = msg.covariance[15];
+			vel_cov[1] = msg.covariance[16];
+			vel_cov[2] = msg.covariance[17];
+			vel_cov[3] = msg.covariance[16];
+			vel_cov[4] = msg.covariance[18];
+			vel_cov[5] = msg.covariance[19];
+			vel_cov[6] = msg.covariance[17];
+			vel_cov[7] = msg.covariance[19];
+			vel_cov[8] = msg.covariance[20];
+		}
+
+	/* FALLTHROUGH */
+
+	// Full matrix 6x6.
+	// This code has been carefully optimized by hand. We could use unpackSquareMatrix(), but it's slow.
+	// Sub-matrix indexes (empty squares contain velocity-position covariance data):
+	//  0  1  2
+	//  6  7  8
+	// 12 13 14
+	//          21 22 23
+	//          27 28 29
+	//          33 34 35
+	case 36: {
+			pos_cov[0] = msg.covariance[0];
+			pos_cov[1] = msg.covariance[1];
+			pos_cov[2] = msg.covariance[2];
+			pos_cov[3] = msg.covariance[6];
+			pos_cov[4] = msg.covariance[7];
+			pos_cov[5] = msg.covariance[8];
+			pos_cov[6] = msg.covariance[12];
+			pos_cov[7] = msg.covariance[13];
+			pos_cov[8] = msg.covariance[14];
+
+			vel_cov[0] = msg.covariance[21];
+			vel_cov[1] = msg.covariance[22];
+			vel_cov[2] = msg.covariance[23];
+			vel_cov[3] = msg.covariance[27];
+			vel_cov[4] = msg.covariance[28];
+			vel_cov[5] = msg.covariance[29];
+			vel_cov[6] = msg.covariance[33];
+			vel_cov[7] = msg.covariance[34];
+			vel_cov[8] = msg.covariance[35];
+		}
+
+	/* FALLTHROUGH */
+
+	// Either empty or invalid sized, interpret as zero matrix
+	default: {
+			valid_covariances = false;
+			break;	// Nothing to do
+		}
+	}
+
+	process_fixx(msg, pos_cov, vel_cov, valid_covariances, valid_covariances);
+}
+
+template <typename FixType>
+void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType> &msg,
+				    const float (&pos_cov)[9],
+				    const float (&vel_cov)[9],
+				    const bool valid_pos_cov,
+				    const bool valid_vel_cov)
+{
+	// This bridge does not support redundant GNSS receivers yet.
+	if (_receiver_node_id < 0) {
+		_receiver_node_id = msg.getSrcNodeID().get();
+		PX4_WARN("GNSS receiver node ID: %d", _receiver_node_id);
+
+	} else {
+		if (_receiver_node_id != msg.getSrcNodeID().get()) {
+			return;  // This GNSS receiver is the redundant one, ignore it.
+		}
+	}
+
+	auto report = ::vehicle_gps_position_s();
+
+	/*
+	 * FIXME HACK
+	 * There used to be the following line of code:
+	 * 	report.timestamp_position = msg.getMonotonicTimestamp().toUSec();
+	 * It stopped working when the time sync feature has been introduced, because it caused libuavcan
+	 * to use an independent time source (based on hardware TIM5) instead of HRT.
+	 * The proper solution is to be developed.
+	 */
+	report.timestamp = hrt_absolute_time();
+
+	report.lat           = msg.latitude_deg_1e8 / 10;
+	report.lon           = msg.longitude_deg_1e8 / 10;
+	report.alt           = msg.height_msl_mm;
+	report.alt_ellipsoid = msg.height_ellipsoid_mm;
+
+	if (valid_pos_cov) {
+		// Horizontal position uncertainty
+		const float horizontal_pos_variance = math::max(pos_cov[0], pos_cov[4]);
+		report.eph = (horizontal_pos_variance > 0) ? sqrtf(horizontal_pos_variance) : -1.0F;
+
+		// Vertical position uncertainty
+		report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F;
+
+	} else {
+		report.eph = -1.0F;
+		report.epv = -1.0F;
+	}
+
+	if (valid_vel_cov) {
+		report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]);
+
+		/* There is a nonlinear relationship between the velocity vector and the heading.
+		 * Use Jacobian to transform velocity covariance to heading covariance
+		 *
+		 * Nonlinear equation:
+		 * heading = atan2(vel_e_m_s, vel_n_m_s)
+		 * For math, see http://en.wikipedia.org/wiki/Atan2#Derivative
+		 *
+		 * To calculate the variance of heading from the variance of velocity,
+		 * cov(heading) = J(velocity)*cov(velocity)*J(velocity)^T
+		 */
+		float vel_n = msg.ned_velocity[0];
+		float vel_e = msg.ned_velocity[1];
+		float vel_n_sq = vel_n * vel_n;
+		float vel_e_sq = vel_e * vel_e;
+		report.c_variance_rad =
+			(vel_e_sq * vel_cov[0] +
+			 -2 * vel_n * vel_e * vel_cov[1] +	// Covariance matrix is symmetric
+			 vel_n_sq * vel_cov[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq));
+
+	} else {
+		report.s_variance_m_s = -1.0F;
+		report.c_variance_rad = -1.0F;
+	}
+
+	report.fix_type = msg.status;
+
+	report.vel_n_m_s = msg.ned_velocity[0];
+	report.vel_e_m_s = msg.ned_velocity[1];
+	report.vel_d_m_s = msg.ned_velocity[2];
+	report.vel_m_s = sqrtf(report.vel_n_m_s * report.vel_n_m_s +
+			       report.vel_e_m_s * report.vel_e_m_s +
+			       report.vel_d_m_s * report.vel_d_m_s);
+	report.cog_rad = atan2f(report.vel_e_m_s, report.vel_n_m_s);
+	report.vel_ned_valid = true;
+
+	report.timestamp_time_relative = 0;
+
+	const std::uint64_t gnss_ts_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec();
+
+	switch (msg.gnss_time_standard) {
+	case FixType::GNSS_TIME_STANDARD_UTC: {
+			report.time_utc_usec = gnss_ts_usec;
+			break;
+		}
+
+	case FixType::GNSS_TIME_STANDARD_GPS: {
+			if (msg.num_leap_seconds > 0) {
+				report.time_utc_usec = gnss_ts_usec - msg.num_leap_seconds + 9;
+			}
+
+			break;
+		}
+
+	case FixType::GNSS_TIME_STANDARD_TAI: {
+			if (msg.num_leap_seconds > 0) {
+				report.time_utc_usec = gnss_ts_usec - msg.num_leap_seconds - 10;
+			}
+
+			break;
+		}
+
+	default: {
+			break;
+		}
+	}
+
+//TODO px4_clock_settime does nothing on the Snapdragon platform
+#ifndef __PX4_QURT
+
+	// If we haven't already done so, set the system clock using GPS data
+	if (valid_pos_cov && !_system_clock_set) {
+		timespec ts;
+		memset(&ts, 0, sizeof(ts));
+		// get the whole microseconds
+		ts.tv_sec = report.time_utc_usec / 1000000ULL;
+		// get the remainder microseconds and convert to nanoseconds
+		ts.tv_nsec = (report.time_utc_usec % 1000000ULL) * 1000;
+		px4_clock_settime(CLOCK_REALTIME, &ts);
+		_system_clock_set = true;
+	}
+
+#endif
+
+	report.satellites_used = msg.sats_used;
+
+	// Using PDOP for HDOP and VDOP
+	// Relevant discussion: https://github.com/PX4/Firmware/issues/5153
+	report.hdop = msg.pdop;
+	report.vdop = msg.pdop;
+
+	report.heading = NAN;
+	report.heading_offset = NAN;
+
+	// Publish to a multi-topic
+	int32_t gps_orb_instance;
+	orb_publish_auto(ORB_ID(vehicle_gps_position), &_report_pub, &report, &gps_orb_instance,
+			 ORB_PRIO_HIGH);
+
+	// Doing less time critical stuff here
+	if (_orb_to_uavcan_pub_timer.isRunning()) {
+		_orb_to_uavcan_pub_timer.stop();
+		PX4_WARN("GNSS ORB->UAVCAN bridge stopped, because there are other GNSS publishers");
+	}
+}
+
+void UavcanGnssBridge::broadcast_from_orb(const uavcan::TimerEvent &)
+{
+	if (_orb_sub_gnss < 0) {
+		// ORB subscription stops working if this is relocated into init()
+		_orb_sub_gnss = orb_subscribe(ORB_ID(vehicle_gps_position));
+
+		if (_orb_sub_gnss < 0) {
+			PX4_WARN("GNSS ORB sub errno %d", errno);
+			return;
+		}
+
+		PX4_WARN("GNSS ORB fd %d", _orb_sub_gnss);
+	}
+
+	{
+		bool updated = false;
+		const int res = orb_check(_orb_sub_gnss, &updated);
+
+		if (res < 0) {
+			PX4_WARN("GNSS ORB check err %d %d", res, errno);
+			return;
+		}
+
+		if (!updated) {
+			return;
+		}
+	}
+
+	auto orb_msg = ::vehicle_gps_position_s();
+	const int res = orb_copy(ORB_ID(vehicle_gps_position), _orb_sub_gnss, &orb_msg);
+
+	if (res < 0) {
+		PX4_WARN("GNSS ORB read errno %d", errno);
+		return;
+	}
+
+	// Convert to UAVCAN
+	using uavcan::equipment::gnss::Fix2;
+	Fix2 msg;
+
+	msg.gnss_timestamp = uavcan::UtcTime::fromUSec(orb_msg.time_utc_usec);
+	msg.gnss_time_standard = Fix2::GNSS_TIME_STANDARD_UTC;
+
+	msg.longitude_deg_1e8   = std::int64_t(orb_msg.lon) * 10LL;
+	msg.latitude_deg_1e8    = std::int64_t(orb_msg.lat) * 10LL;
+	msg.height_ellipsoid_mm = orb_msg.alt_ellipsoid;
+	msg.height_msl_mm       = orb_msg.alt;
+
+	msg.ned_velocity[0] = orb_msg.vel_n_m_s;
+	msg.ned_velocity[1] = orb_msg.vel_e_m_s;
+	msg.ned_velocity[2] = orb_msg.vel_d_m_s;
+
+	msg.sats_used = orb_msg.satellites_used;
+	msg.status    = orb_msg.fix_type;
+	// mode skipped
+	// sub mode skipped
+
+	// diagonal covariance matrix
+	msg.covariance.resize(2, orb_msg.eph * orb_msg.eph);
+	msg.covariance.resize(3, orb_msg.epv * orb_msg.epv);
+	msg.covariance.resize(6, orb_msg.s_variance_m_s * orb_msg.s_variance_m_s);
+
+	msg.pdop = (orb_msg.hdop > orb_msg.vdop) ? orb_msg.hdop : orb_msg.vdop;  // this is a hack :(
+
+	// Publishing now
+	(void) _pub_fix2.broadcast(msg);
+}

src/drivers/uavcan/sensors/gnss.hpp

@@ -0,0 +1,116 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014, 2015 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
+ *
+ * UAVCAN <--> ORB bridge for GNSS messages:
+ *     uavcan.equipment.gnss.Fix (deprecated, but still supported for backward compatibility)
+ *     uavcan.equipment.gnss.Fix2
+ *
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ * @author Andrew Chambers <achamber@gmail.com>
+ */
+
+#pragma once
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_gps_position.h>
+
+#include <uavcan/uavcan.hpp>
+#include <uavcan/equipment/gnss/Fix.hpp>
+#include <uavcan/equipment/gnss/Fix2.hpp>
+
+#include "sensor_bridge.hpp"
+
+class UavcanGnssBridge : public IUavcanSensorBridge
+{
+	static constexpr unsigned ORB_TO_UAVCAN_FREQUENCY_HZ = 10;
+
+public:
+	static const char *const NAME;
+
+	UavcanGnssBridge(uavcan::INode &node);
+	~UavcanGnssBridge();
+
+	const char *get_name() const override { return NAME; }
+
+	int init() override;
+
+	unsigned get_num_redundant_channels() const override;
+
+	void print_status() const override;
+
+private:
+	/**
+	 * GNSS fix message will be reported via this callback.
+	 */
+	void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg);
+	void gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix2> &msg);
+
+	template <typename FixType>
+	void process_fixx(const uavcan::ReceivedDataStructure<FixType> &msg,
+			  const float (&pos_cov)[9],
+			  const float (&vel_cov)[9],
+			  const bool valid_pos_cov,
+			  const bool valid_vel_cov);
+
+	void broadcast_from_orb(const uavcan::TimerEvent &);
+
+	typedef uavcan::MethodBinder < UavcanGnssBridge *,
+		void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &) >
+		FixCbBinder;
+
+	typedef uavcan::MethodBinder < UavcanGnssBridge *,
+		void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix2> &) >
+		Fix2CbBinder;
+
+	typedef uavcan::MethodBinder<UavcanGnssBridge *,
+		void (UavcanGnssBridge::*)(const uavcan::TimerEvent &)>
+		TimerCbBinder;
+
+	uavcan::INode &_node;
+	uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder> _sub_fix;
+	uavcan::Subscriber<uavcan::equipment::gnss::Fix2, Fix2CbBinder> _sub_fix2;
+	uavcan::Publisher<uavcan::equipment::gnss::Fix2> _pub_fix2;
+	uavcan::TimerEventForwarder<TimerCbBinder> _orb_to_uavcan_pub_timer;
+	int _receiver_node_id = -1;
+
+	bool _old_fix_subscriber_active = true;
+
+	orb_advert_t _report_pub;                ///< uORB pub for gnss position
+
+	int _orb_sub_gnss = -1;                  ///< uORB sub for gnss position, used for bridging uORB --> UAVCAN
+
+	bool _system_clock_set = false; ///< Have we set the system clock at least once from GNSS data?
+};

src/drivers/uavcan/sensors/mag.cpp

@@ -0,0 +1,152 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014, 2015 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include "mag.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <systemlib/err.h>
+
+const char *const UavcanMagnetometerBridge::NAME = "mag";
+
+UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode &node) :
+	UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_BASE_DEVICE_PATH, ORB_ID(sensor_mag)),
+	_sub_mag(node)
+{
+	_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;     // <-- Why?
+
+	_scale.x_scale = 1.0F;
+	_scale.y_scale = 1.0F;
+	_scale.z_scale = 1.0F;
+}
+
+int UavcanMagnetometerBridge::init()
+{
+	int res = device::CDev::init();
+
+	if (res < 0) {
+		return res;
+	}
+
+	res = _sub_mag.start(MagCbBinder(this, &UavcanMagnetometerBridge::mag_sub_cb));
+
+	if (res < 0) {
+		DEVICE_LOG("failed to start uavcan sub: %d", res);
+		return res;
+	}
+
+	return 0;
+}
+
+ssize_t UavcanMagnetometerBridge::read(struct file *filp, char *buffer, size_t buflen)
+{
+	static uint64_t last_read = 0;
+	struct mag_report *mag_buf = reinterpret_cast<struct mag_report *>(buffer);
+
+	/* buffer must be large enough */
+	unsigned count = buflen / sizeof(struct mag_report);
+
+	if (count < 1) {
+		return -ENOSPC;
+	}
+
+	if (last_read < _report.timestamp) {
+		/* copy report */
+		lock();
+		*mag_buf = _report;
+		last_read = _report.timestamp;
+		unlock();
+		return sizeof(struct mag_report);
+
+	} else {
+		/* no new data available, warn caller */
+		return -EAGAIN;
+	}
+}
+
+int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case MAGIOCSSCALE: {
+			std::memcpy(&_scale, reinterpret_cast<const void *>(arg), sizeof(_scale));
+			return 0;
+		}
+
+	case MAGIOCGSCALE: {
+			std::memcpy(reinterpret_cast<void *>(arg), &_scale, sizeof(_scale));
+			return 0;
+		}
+
+	case MAGIOCGEXTERNAL: {
+			return 1;           // declare it external rise it's priority and to allow for correct orientation compensation
+		}
+
+	case MAGIOCCALIBRATE:
+	case MAGIOCSRANGE:
+	case MAGIOCEXSTRAP: {
+			return -EINVAL;
+		}
+
+	default: {
+			return CDev::ioctl(filp, cmd, arg);
+		}
+	}
+}
+
+void UavcanMagnetometerBridge::mag_sub_cb(const
+		uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength>
+		&msg)
+{
+	lock();
+	/*
+	 * FIXME HACK
+	 * This code used to rely on msg.getMonotonicTimestamp().toUSec() instead of HRT.
+	 * It stopped working when the time sync feature has been introduced, because it caused libuavcan
+	 * to use an independent time source (based on hardware TIM5) instead of HRT.
+	 * The proper solution is to be developed.
+	 */
+	_report.timestamp = hrt_absolute_time();
+	_report.device_id = _device_id.devid;
+	_report.is_external = true;
+
+	_report.x = (msg.magnetic_field_ga[0] - _scale.x_offset) * _scale.x_scale;
+	_report.y = (msg.magnetic_field_ga[1] - _scale.y_offset) * _scale.y_scale;
+	_report.z = (msg.magnetic_field_ga[2] - _scale.z_offset) * _scale.z_scale;
+	unlock();
+
+	publish(msg.getSrcNodeID().get(), &_report);
+}

src/drivers/uavcan/sensors/mag.hpp

@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014, 2015 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include "sensor_bridge.hpp"
+#include <drivers/drv_mag.h>
+
+#include <uavcan/equipment/ahrs/MagneticFieldStrength.hpp>
+
+class UavcanMagnetometerBridge : public UavcanCDevSensorBridgeBase
+{
+public:
+	static const char *const NAME;
+
+	UavcanMagnetometerBridge(uavcan::INode &node);
+
+	const char *get_name() const override { return NAME; }
+
+	int init() override;
+
+private:
+	ssize_t	read(struct file *filp, char *buffer, size_t buflen);
+	int ioctl(struct file *filp, int cmd, unsigned long arg) override;
+
+	void mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength> &msg);
+
+	typedef uavcan::MethodBinder < UavcanMagnetometerBridge *,
+		void (UavcanMagnetometerBridge::*)
+		(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength> &) >
+		MagCbBinder;
+
+	uavcan::Subscriber<uavcan::equipment::ahrs::MagneticFieldStrength, MagCbBinder> _sub_mag;
+	struct mag_calibration_s _scale = {};
+	mag_report _report =  {};
+};

src/drivers/uavcan/sensors/sensor_bridge.cpp

@@ -0,0 +1,166 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include "sensor_bridge.hpp"
+#include <cassert>
+
+#include "gnss.hpp"
+#include "mag.hpp"
+#include "baro.hpp"
+
+/*
+ * IUavcanSensorBridge
+ */
+void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge *> &list)
+{
+	list.add(new UavcanBarometerBridge(node));
+	list.add(new UavcanMagnetometerBridge(node));
+	list.add(new UavcanGnssBridge(node));
+}
+
+/*
+ * UavcanCDevSensorBridgeBase
+ */
+UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase()
+{
+	for (unsigned i = 0; i < _max_channels; i++) {
+		if (_channels[i].node_id >= 0) {
+			(void)unregister_class_devname(_class_devname, _channels[i].class_instance);
+		}
+	}
+
+	delete [] _channels;
+}
+
+void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
+{
+	assert(report != nullptr);
+
+	Channel *channel = nullptr;
+
+	// Checking if such channel already exists
+	for (unsigned i = 0; i < _max_channels; i++) {
+		if (_channels[i].node_id == node_id) {
+			channel = _channels + i;
+			break;
+		}
+	}
+
+	// No such channel - try to create one
+	if (channel == nullptr) {
+		if (_out_of_channels) {
+			return;           // Give up immediately - saves some CPU time
+		}
+
+		DEVICE_LOG("adding channel %d...", node_id);
+
+		// Search for the first free channel
+		for (unsigned i = 0; i < _max_channels; i++) {
+			if (_channels[i].node_id < 0) {
+				channel = _channels + i;
+				break;
+			}
+		}
+
+		// No free channels left
+		if (channel == nullptr) {
+			_out_of_channels = true;
+			DEVICE_LOG("out of channels");
+			return;
+		}
+
+		// update device id as we now know our device node_id
+		_device_id.devid_s.address = static_cast<uint8_t>(node_id);
+
+		// Ask the CDev helper which class instance we can take
+		const int class_instance = register_class_devname(_class_devname);
+
+		if (class_instance < 0 || class_instance >= int(_max_channels)) {
+			_out_of_channels = true;
+			DEVICE_LOG("out of class instances");
+			(void)unregister_class_devname(_class_devname, class_instance);
+			return;
+		}
+
+		// Publish to the appropriate topic, abort on failure
+		channel->node_id        = node_id;
+		channel->class_instance = class_instance;
+
+		channel->orb_advert = orb_advertise_multi(_orb_topic, report, &channel->orb_instance, ORB_PRIO_VERY_HIGH);
+
+		if (channel->orb_advert == nullptr) {
+			DEVICE_LOG("ADVERTISE FAILED");
+			(void)unregister_class_devname(_class_devname, class_instance);
+			*channel = Channel();
+			return;
+		}
+
+		DEVICE_LOG("channel %d class instance %d ok", channel->node_id, channel->class_instance);
+	}
+
+	assert(channel != nullptr);
+
+	(void)orb_publish(_orb_topic, channel->orb_advert, report);
+}
+
+unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const
+{
+	unsigned out = 0;
+
+	for (unsigned i = 0; i < _max_channels; i++) {
+		if (_channels[i].node_id >= 0) {
+			out += 1;
+		}
+	}
+
+	return out;
+}
+
+void UavcanCDevSensorBridgeBase::print_status() const
+{
+	printf("devname: %s\n", _class_devname);
+
+	for (unsigned i = 0; i < _max_channels; i++) {
+		if (_channels[i].node_id >= 0) {
+			printf("channel %d: node id %d --> class instance %d\n",
+			       i, _channels[i].node_id, _channels[i].class_instance);
+
+		} else {
+			printf("channel %d: empty\n", i);
+		}
+	}
+}

src/drivers/uavcan/sensors/sensor_bridge.hpp

@@ -0,0 +1,132 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014, 2015 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <containers/List.hpp>
+#include <uavcan/uavcan.hpp>
+#include <drivers/device/device.h>
+#include <drivers/drv_orb_dev.h>
+
+/**
+ * A sensor bridge class must implement this interface.
+ */
+class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode<IUavcanSensorBridge *>
+{
+public:
+	static constexpr unsigned MAX_NAME_LEN = 20;
+
+	virtual ~IUavcanSensorBridge() { }
+
+	/**
+	 * Returns ASCII name of the bridge.
+	 */
+	virtual const char *get_name() const = 0;
+
+	/**
+	 * Starts the bridge.
+	 * @return Non-negative value on success, negative on error.
+	 */
+	virtual int init() = 0;
+
+	/**
+	 * Returns number of active redundancy channels.
+	 */
+	virtual unsigned get_num_redundant_channels() const = 0;
+
+	/**
+	 * Prints current status in a human readable format to stdout.
+	 */
+	virtual void print_status() const = 0;
+
+	/**
+	 * Sensor bridge factory.
+	 * Creates all known sensor bridges and puts them in the linked list.
+	 */
+	static void make_all(uavcan::INode &node, List<IUavcanSensorBridge *> &list);
+};
+
+/**
+ * This is the base class for redundant sensors with an independent ORB topic per each redundancy channel.
+ * For example, sensor_mag0, sensor_mag1, etc.
+ */
+class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CDev
+{
+	struct Channel {
+		int node_id              = -1;
+		orb_advert_t orb_advert  = nullptr;
+		int class_instance       = -1;
+		int orb_instance	 = -1;
+	};
+
+	const unsigned _max_channels;
+	const char *const _class_devname;
+	const orb_id_t _orb_topic;
+	Channel *const _channels;
+	bool _out_of_channels = false;
+
+protected:
+	static constexpr unsigned DEFAULT_MAX_CHANNELS = 5; // 640 KB ought to be enough for anybody
+
+	UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname,
+				   const orb_id_t orb_topic_sensor,
+				   const unsigned max_channels = DEFAULT_MAX_CHANNELS) :
+		device::CDev(name, devname),
+		_max_channels(max_channels),
+		_class_devname(class_devname),
+		_orb_topic(orb_topic_sensor),
+		_channels(new Channel[max_channels])
+	{
+		_device_id.devid_s.bus_type = DeviceBusType_UAVCAN;
+		_device_id.devid_s.bus = 0;
+	}
+
+	/**
+	 * Sends one measurement into appropriate ORB topic.
+	 * New redundancy channels will be registered automatically.
+	 * @param node_id Sensor's Node ID
+	 * @param report  Pointer to ORB message object
+	 */
+	void publish(const int node_id, const void *report);
+
+public:
+	virtual ~UavcanCDevSensorBridgeBase();
+
+	unsigned get_num_redundant_channels() const override;
+
+	void print_status() const override;
+};

src/drivers/uavcan/uavcan_driver.hpp

@@ -0,0 +1,45 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2018 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @author David Sidrane <david_s5@nscdg.com>
+ */
+
+#pragma once
+#if defined(UAVCAN_KINETIS_NUTTX)
+#  include <uavcan_kinetis/uavcan_kinetis.hpp>
+#elif defined(UAVCAN_STM32_NUTTX)
+#  include <uavcan_stm32/uavcan_stm32.hpp>
+#else
+#  error "Unsupported driver"
+#endif

src/drivers/uavcan/uavcan_main.hpp

@@ -0,0 +1,235 @@
+/****************************************************************************
+ *
+ *   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 uavcan_main.hpp
+ *
+ * Defines basic functinality of UAVCAN node.
+ *
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ *		 Andreas Jochum <Andreas@NicaDrone.com>
+ */
+
+#pragma once
+
+#include <px4_config.h>
+
+#include "uavcan_driver.hpp"
+#include <uavcan/helpers/heap_based_pool_allocator.hpp>
+#include <uavcan/protocol/global_time_sync_master.hpp>
+#include <uavcan/protocol/global_time_sync_slave.hpp>
+#include <uavcan/protocol/node_status_monitor.hpp>
+#include <uavcan/protocol/param/GetSet.hpp>
+#include <uavcan/protocol/param/ExecuteOpcode.hpp>
+#include <uavcan/protocol/RestartNode.hpp>
+
+#include <drivers/device/device.h>
+#include <perf/perf_counter.h>
+
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/test_motor.h>
+#include <uORB/topics/actuator_direct.h>
+
+#include "actuators/esc.hpp"
+#include "actuators/hardpoint.hpp"
+#include "sensors/sensor_bridge.hpp"
+
+#include "uavcan_servers.hpp"
+#include "allocator.hpp"
+
+#define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN	4
+
+// we add two to allow for actuator_direct and busevent
+#define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+2)
+
+/**
+ * A UAVCAN node.
+ */
+class UavcanNode : public cdev::CDev
+{
+	static constexpr unsigned MaxBitRatePerSec	= 1000000;
+	static constexpr unsigned bitPerFrame		= 148;
+	static constexpr unsigned FramePerSecond	= MaxBitRatePerSec / bitPerFrame;
+	static constexpr unsigned FramePerMSecond	= ((FramePerSecond / 1000) + 1);
+
+	static constexpr unsigned PollTimeoutMs		= 3;
+
+
+	/*
+	 * This memory is reserved for uavcan to use for queuing CAN frames.
+	 * At 1Mbit there is approximately one CAN frame every 145 uS.
+	 * The number of buffers sets how long you can go without calling
+	 * node_spin_xxxx. Since our task is the only one running and the
+	 * driver will light the fd when there is a CAN frame we can nun with
+	 * a minimum number of buffers to conserver memory. Each buffer is
+	 * 32 bytes. So 5 buffers costs 160 bytes and gives us a poll rate
+	 * of ~1 mS
+	 *  1000000/200
+	 */
+
+	static constexpr unsigned RxQueueLenPerIface	= FramePerMSecond * PollTimeoutMs; // At
+	static constexpr unsigned StackSize		= 2400;
+
+public:
+	typedef UAVCAN_DRIVER::CanInitHelper<RxQueueLenPerIface> CanInitHelper;
+	enum eServerAction {None, Start, Stop, CheckFW, Busy};
+
+	UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock);
+
+	virtual		~UavcanNode();
+
+	virtual int	ioctl(file *filp, int cmd, unsigned long arg);
+
+	static int	start(uavcan::NodeID node_id, uint32_t bitrate);
+
+	uavcan::Node<>	&get_node() { return _node; }
+
+	// TODO: move the actuator mixing stuff into the ESC controller class
+	static int	control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input);
+
+	void		subscribe();
+
+	int		teardown();
+
+	int		arm_actuators(bool arm);
+
+	void		print_info();
+
+	void		shrink();
+
+	void		hardpoint_controller_set(uint8_t hardpoint_id, uint16_t command);
+
+	static UavcanNode *instance() { return _instance; }
+	static int		 getHardwareVersion(uavcan::protocol::HardwareVersion &hwver);
+	int			 fw_server(eServerAction action);
+	void			attachITxQueueInjector(ITxQueueInjector *injector) {_tx_injector = injector;}
+	int			 list_params(int remote_node_id);
+	int			 save_params(int remote_node_id);
+	int			 set_param(int remote_node_id, const char *name, char *value);
+	int			 get_param(int remote_node_id, const char *name);
+	int			 reset_node(int remote_node_id);
+
+private:
+	void		fill_node_info();
+	int		init(uavcan::NodeID node_id);
+	void		node_spin_once();
+	int		run();
+	int		add_poll_fd(int fd);			///< add a fd to poll list, returning index into _poll_fds[]
+	int		start_fw_server();
+	int		stop_fw_server();
+	int		request_fw_check();
+	int		print_params(uavcan::protocol::param::GetSet::Response &resp);
+	int		get_set_param(int nodeid, const char *name, uavcan::protocol::param::GetSet::Request &req);
+	void 		update_params();
+	void		set_setget_response(uavcan::protocol::param::GetSet::Response *resp)
+	{
+		_setget_response = resp;
+	}
+	void		free_setget_response(void)
+	{
+		_setget_response = nullptr;
+	}
+
+	int			_task = -1;			///< handle to the OS task
+	bool			_task_should_exit = false;	///< flag to indicate to tear down the CAN driver
+	volatile eServerAction	_fw_server_action;
+	int			 _fw_server_status;
+	int			_armed_sub = -1;		///< uORB subscription of the arming status
+	actuator_armed_s	_armed = {};			///< the arming request of the system
+	bool			_is_armed = false;		///< the arming status of the actuators on the bus
+
+	int			_test_motor_sub = -1;   ///< uORB subscription of the test_motor status
+	test_motor_s		_test_motor = {};
+	bool			_test_in_progress = false;
+
+	unsigned		_output_count = 0;		///< number of actuators currently available
+
+	static UavcanNode	*_instance;			///< singleton pointer
+
+	uavcan_node::Allocator	 _pool_allocator;
+
+	uavcan::Node<>			_node;				///< library instance
+	pthread_mutex_t			_node_mutex;
+	px4_sem_t			_server_command_sem;
+	UavcanEscController		_esc_controller;
+	UavcanHardpointController	_hardpoint_controller;
+	uavcan::GlobalTimeSyncMaster	_time_sync_master;
+	uavcan::GlobalTimeSyncSlave	_time_sync_slave;
+	uavcan::NodeStatusMonitor	_node_status_monitor;
+
+	List<IUavcanSensorBridge *>	_sensor_bridges;		///< List of active sensor bridges
+
+	MixerGroup			*_mixers = nullptr;
+	ITxQueueInjector		*_tx_injector;
+	uint32_t			_groups_required = 0;
+	uint32_t			_groups_subscribed = 0;
+	int				_control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN];
+	actuator_controls_s		_controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
+	orb_id_t			_control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
+	pollfd				_poll_fds[UAVCAN_NUM_POLL_FDS] = {};
+	unsigned			_poll_fds_num = 0;
+	int32_t 			_idle_throttle_when_armed = 0;
+
+	int				_actuator_direct_sub = -1;   ///< uORB subscription of the actuator_direct topic
+	uint8_t				_actuator_direct_poll_fd_num = 0;
+	actuator_direct_s		_actuator_direct = {};
+
+	actuator_outputs_s		_outputs = {};
+
+	perf_counter_t			_perf_control_latency;
+
+	bool 				_airmode = false;
+
+	// index into _poll_fds for each _control_subs handle
+	uint8_t				_poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN];
+
+	void handle_time_sync(const uavcan::TimerEvent &);
+
+	typedef uavcan::MethodBinder<UavcanNode *, void (UavcanNode::*)(const uavcan::TimerEvent &)> TimerCallback;
+	uavcan::TimerEventForwarder<TimerCallback> _master_timer;
+
+	bool _callback_success;
+	uavcan::protocol::param::GetSet::Response *_setget_response;
+	typedef uavcan::MethodBinder<UavcanNode *,
+		void (UavcanNode::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &)> GetSetCallback;
+	typedef uavcan::MethodBinder<UavcanNode *,
+		void (UavcanNode::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &)> ExecuteOpcodeCallback;
+	typedef uavcan::MethodBinder<UavcanNode *,
+		void (UavcanNode::*)(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &)> RestartNodeCallback;
+	void cb_setget(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
+	void cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result);
+	void cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result);
+
+};

src/drivers/uavcan/uavcan_module.hpp

@@ -0,0 +1,82 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <px4_config.h>
+
+#include <drivers/device/device.h>
+
+/**
+ * @file uavcan.hpp
+ *
+ * Public header for the UAVCAN module
+ *
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ * @author David Sidrane <david_s5@nscdg.com>
+ * @author Holger Steinhaus <holger@steinhaus-home.de>
+ */
+
+// firmware paths
+#define UAVCAN_MAX_PATH_LENGTH (128 + 40)
+#define UAVCAN_FIRMWARE_PATH   "/fs/microsd/fw"
+#define UAVCAN_ROMFS_FW_PATH   "/etc/firmware/uavcan"
+#define UAVCAN_ROMFS_FW_PREFIX "_"
+
+// logging
+#define UAVCAN_NODE_DB_PATH "/fs/microsd/uavcan.db"
+#define UAVCAN_LOG_FILE     UAVCAN_NODE_DB_PATH"/trace.log"
+
+// device files
+// TODO: split IOCTL interface in ESC and node related functionality, then change UAVCAN_DEVICE_PATH to "/dev/uavcan/node"
+#define UAVCAN_DEVICE_PATH     "/dev/uavcan/esc"
+#define UAVCAN_ESC_DEVICE_PATH "/dev/uavcan/esc"
+
+// ioctl interface
+#define _UAVCAN_IOC(_n)               (_IOC(_UAVCAN_IOCBASE, _n))
+#define _UAVCAN_IOCBASE               (0x4000)                        // IOCTL base for module UAVCAN
+/*
+ * Query if node identification is in progress. Returns:
+ *      EINVAL - not applicable in the current operating mode
+ *      ETIME  - network discovery complete
+ *      OK (0) - network discovery in progress
+ */
+#define UAVCAN_IOCG_NODEID_INPROGRESS _UAVCAN_IOC(1)
+/*
+ * Set hardpoint command. Accepts a pointer to uavcan::equipment::hardpoint::Command; returns nothing.
+ * The pointer may be invalidated once the call returns.
+ */
+#define UAVCAN_IOCS_HARDPOINT_SET       _UAVCAN_IOC(10)
+
+// public prototypes
+extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);

src/drivers/uavcan/uavcan_params.c

@@ -0,0 +1,90 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014-2017 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <px4_config.h>
+#include <parameters/param.h>
+
+/**
+ * UAVCAN mode
+ *
+ *  0 - UAVCAN disabled.
+ *  1 - Enables support for UAVCAN sensors without dynamic node ID allocation and firmware update.
+ *  2 - Enables support for UAVCAN sensors with dynamic node ID allocation and firmware update.
+ *  3 - Enables support for UAVCAN sensors and actuators with dynamic node ID allocation and firmware update. Also sets the motor control outputs to UAVCAN.
+ *
+ * @min 0
+ * @max 3
+ * @value 0 Disabled
+ * @value 1 Sensors Manual Config
+ * @value 2 Sensors Automatic Config
+ * @value 3 Sensors and Actuators (ESCs) Automatic Config
+ * @reboot_required true
+ * @group UAVCAN
+ */
+PARAM_DEFINE_INT32(UAVCAN_ENABLE, 0);
+
+/**
+ * UAVCAN Node ID.
+ *
+ * Read the specs at http://uavcan.org to learn more about Node ID.
+ *
+ * @min 1
+ * @max 125
+ * @reboot_required true
+ * @group UAVCAN
+ */
+PARAM_DEFINE_INT32(UAVCAN_NODE_ID, 1);
+
+/**
+ * UAVCAN CAN bus bitrate.
+ *
+ * @unit bit/s
+ * @min 20000
+ * @max 1000000
+ * @reboot_required true
+ * @group UAVCAN
+ */
+PARAM_DEFINE_INT32(UAVCAN_BITRATE, 1000000);
+
+/**
+ * UAVCAN ESC will spin at idle throttle when armed, even if the mixer outputs zero setpoints.
+ *
+ * @boolean
+ * @reboot_required true
+ * @group UAVCAN
+ */
+PARAM_DEFINE_INT32(UAVCAN_ESC_IDLT, 1);

src/drivers/uavcan/uavcan_servers.hpp

@@ -0,0 +1,220 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <px4_config.h>
+#include "uavcan_driver.hpp"
+#include <drivers/device/device.h>
+#include <perf/perf_counter.h>
+
+#include <uavcan/node/sub_node.hpp>
+#include <uavcan/protocol/node_status_monitor.hpp>
+#include <uavcan/protocol/param/GetSet.hpp>
+#include <uavcan/protocol/param/ExecuteOpcode.hpp>
+
+#include <uavcan/protocol/dynamic_node_id_server/centralized.hpp>
+#include <uavcan/protocol/node_info_retriever.hpp>
+#include <uavcan_posix/basic_file_server_backend.hpp>
+#include <uavcan/protocol/firmware_update_trigger.hpp>
+#include <uavcan/protocol/file_server.hpp>
+#include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
+#include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
+#include <uavcan_posix/firmware_version_checker.hpp>
+#include <uavcan/equipment/esc/RawCommand.hpp>
+#include <uavcan/equipment/indication/BeepCommand.hpp>
+#include <uavcan/protocol/enumeration/Begin.hpp>
+#include <uavcan/protocol/enumeration/Indication.hpp>
+
+#include "uavcan_module.hpp"
+#include "uavcan_virtual_can_driver.hpp"
+
+/**
+ * @file uavcan_servers.hpp
+ *
+ * Defines basic functionality of UAVCAN node.
+ *
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ * @author David Sidrane <david_s5@nscdg.com>
+ */
+
+/**
+ * A UAVCAN Server Sub node.
+ */
+class __EXPORT UavcanServers
+{
+	static constexpr unsigned NumIfaces = 1;  // UAVCAN_STM32_NUM_IFACES
+
+	static constexpr unsigned StackSize  = 6000;
+	static constexpr unsigned Priority  =  120;
+
+	static constexpr unsigned VirtualIfaceBlockAllocationQuota =  80;
+
+	static constexpr float BeepFrequencyGenericIndication       = 1000.0F;
+	static constexpr float BeepFrequencySuccess                 = 2000.0F;
+	static constexpr float BeepFrequencyError                   = 100.0F;
+
+public:
+	UavcanServers(uavcan::INode &main_node);
+
+	virtual		~UavcanServers();
+
+	static int      start(uavcan::INode &main_node);
+	static int      stop();
+
+	uavcan::SubNode<> &get_node() { return _subnode; }
+
+	static UavcanServers *instance() { return _instance; }
+
+	/*
+	 *  Set the main node's pointer to to the injector
+	 *  This is a work around as main_node.getDispatcher().remeveRxFrameListener();
+	 *  would require a dynamic cast and rtti is not enabled.
+	 */
+	void attachITxQueueInjector(ITxQueueInjector **injector) {*injector = &_vdriver;}
+
+	void requestCheckAllNodesFirmwareAndUpdate() { _check_fw = true; }
+
+	bool guessIfAllDynamicNodesAreAllocated() { return _server_instance.guessIfAllDynamicNodesAreAllocated(); }
+
+private:
+	pthread_t         _subnode_thread;
+	pthread_mutex_t   _subnode_mutex;
+	volatile bool     _subnode_thread_should_exit = false;
+
+	int		init();
+
+	pthread_addr_t	run(pthread_addr_t);
+
+	static UavcanServers	*_instance;            ///< singleton pointer
+
+	VirtualCanDriver _vdriver;
+
+	uavcan::SubNode<>  _subnode;
+	uavcan::INode      &_main_node;
+
+	uavcan_posix::dynamic_node_id_server::FileEventTracer _tracer;
+	uavcan_posix::dynamic_node_id_server::FileStorageBackend _storage_backend;
+	uavcan_posix::FirmwareVersionChecker _fw_version_checker;
+	uavcan::dynamic_node_id_server::CentralizedServer _server_instance;  ///< server singleton pointer
+	uavcan_posix::BasicFileServerBackend  _fileserver_backend;
+	uavcan::NodeInfoRetriever   _node_info_retriever;
+	uavcan::FirmwareUpdateTrigger   _fw_upgrade_trigger;
+	uavcan::BasicFileServer         _fw_server;
+
+	/*
+	 * The MAVLink parameter bridge needs to know the maximum parameter index
+	 * of each node so that clients can determine when parameter listings have
+	 * finished. We do that by querying a node's entire parameter set whenever
+	 * a parameter message is received for a node with a zero _param_count,
+	 * and storing the count here. If a node doesn't respond, or doesn't have
+	 * any parameters, its count will stay at zero and we'll try to query the
+	 * set again next time.
+	 *
+	 * The node's UAVCAN ID is used as the index into the _param_counts array.
+	 */
+	uint8_t _param_counts[128] = {};
+	bool _count_in_progress = false;
+	uint8_t _count_index = 0;
+
+	bool _param_in_progress = false;
+	uint8_t _param_index = 0;
+	bool _param_list_in_progress = false;
+	bool _param_list_all_nodes = false;
+	uint8_t _param_list_node_id = 1;
+
+	uint32_t _param_dirty_bitmap[4] = {};
+	uint8_t _param_save_opcode = 0;
+
+	bool _cmd_in_progress = false;
+
+	// uORB topic handle for MAVLink parameter responses
+	orb_advert_t _param_response_pub = nullptr;
+	orb_advert_t _command_ack_pub = nullptr;
+
+	typedef uavcan::MethodBinder<UavcanServers *,
+		void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &)> GetSetCallback;
+	typedef uavcan::MethodBinder<UavcanServers *,
+		void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &)>
+		ExecuteOpcodeCallback;
+	typedef uavcan::MethodBinder<UavcanServers *,
+		void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &)> RestartNodeCallback;
+	void cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
+	void cb_count(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
+	void cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result);
+	void cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result);
+
+	uavcan::ServiceClient<uavcan::protocol::param::GetSet, GetSetCallback> _param_getset_client;
+	uavcan::ServiceClient<uavcan::protocol::param::ExecuteOpcode, ExecuteOpcodeCallback> _param_opcode_client;
+	uavcan::ServiceClient<uavcan::protocol::RestartNode, RestartNodeCallback> _param_restartnode_client;
+	void param_count(uavcan::NodeID node_id);
+	void param_opcode(uavcan::NodeID node_id);
+
+	uint8_t get_next_active_node_id(uint8_t base);
+	uint8_t get_next_dirty_node_id(uint8_t base);
+	void set_node_params_dirty(uint8_t node_id) { _param_dirty_bitmap[node_id >> 5] |= 1 << (node_id & 31); }
+	void clear_node_params_dirty(uint8_t node_id) { _param_dirty_bitmap[node_id >> 5] &= ~(1 << (node_id & 31)); }
+	bool are_node_params_dirty(uint8_t node_id) const { return bool((_param_dirty_bitmap[node_id >> 5] >> (node_id & 31)) & 1); }
+
+	void beep(float frequency);
+
+	bool _mutex_inited = false;
+	volatile bool _check_fw = false;
+
+	// ESC enumeration
+	bool _esc_enumeration_active = false;
+	uint8_t _esc_enumeration_ids[uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize];
+	uint8_t _esc_enumeration_index = 0;
+	uint8_t _esc_count = 0;
+
+	typedef uavcan::MethodBinder<UavcanServers *,
+		void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::enumeration::Begin> &)>
+		EnumerationBeginCallback;
+	typedef uavcan::MethodBinder<UavcanServers *,
+		void (UavcanServers::*)(const uavcan::ReceivedDataStructure<uavcan::protocol::enumeration::Indication>&)>
+		EnumerationIndicationCallback;
+	void cb_enumeration_begin(const uavcan::ServiceCallResult<uavcan::protocol::enumeration::Begin> &result);
+	void cb_enumeration_indication(const uavcan::ReceivedDataStructure<uavcan::protocol::enumeration::Indication> &msg);
+	void cb_enumeration_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
+	void cb_enumeration_save(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result);
+
+	uavcan::Publisher<uavcan::equipment::indication::BeepCommand> _beep_pub;
+	uavcan::Subscriber<uavcan::protocol::enumeration::Indication, EnumerationIndicationCallback>
+	_enumeration_indication_sub;
+	uavcan::ServiceClient<uavcan::protocol::enumeration::Begin, EnumerationBeginCallback> _enumeration_client;
+	uavcan::ServiceClient<uavcan::protocol::param::GetSet, GetSetCallback> _enumeration_getset_client;
+	uavcan::ServiceClient<uavcan::protocol::param::ExecuteOpcode, ExecuteOpcodeCallback> _enumeration_save_client;
+
+	void unpackFwFromROMFS(const char *sd_path, const char *romfs_path);
+	int copyFw(const char *dst, const char *src);
+};

src/drivers/uavcan/uavcan_virtual_can_driver.hpp

@@ -0,0 +1,512 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *               Pavel Kirienko <pavel.kirienko@zubax.com>
+ *               David Sidrane <david_s5@nscdg.com>
+ *
+ * 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 <nuttx/config.h>
+
+#include <cstdlib>
+#include <cstdint>
+#include <cstring>
+#include <cstdio>
+#include <fcntl.h>
+#include <pthread.h>
+#include <semaphore.h>
+#include <debug.h>
+
+#include <uavcan/node/sub_node.hpp>
+#include <uavcan/protocol/node_status_monitor.hpp>
+
+/*
+ * General purpose wrapper around os's mutual exclusion
+ * mechanism.
+ *
+ * It supports the
+ *
+ * Note the naming of thier_mutex_ implies that the underlying
+ * mutex is owned by class using the Lock.
+ * and this wrapper provides service to initialize and de initialize
+ * the mutex.
+ */
+class Lock
+{
+	pthread_mutex_t &thier_mutex_;
+
+public:
+
+	Lock(pthread_mutex_t &m) :
+		thier_mutex_(m)
+	{
+		(void)pthread_mutex_lock(&m);
+	}
+
+	~Lock()
+	{
+		(void)pthread_mutex_unlock(&thier_mutex_);
+	}
+
+	static int init(pthread_mutex_t &thier_mutex_)
+	{
+		return pthread_mutex_init(&thier_mutex_, NULL);
+	}
+
+	static int deinit(pthread_mutex_t &thier_mutex_)
+	{
+		return pthread_mutex_destroy(&thier_mutex_);
+	}
+
+};
+
+/**
+ * Generic queue based on the linked list class defined in libuavcan.
+ * This class does not use heap memory.
+ */
+template <typename T>
+class Queue
+{
+	struct Item : public uavcan::LinkedListNode<Item> {
+		T payload;
+
+		template <typename... Args>
+		Item(Args... args) : payload(args...) { }
+	};
+
+	uavcan::LimitedPoolAllocator allocator_;
+	uavcan::LinkedListRoot<Item> list_;
+
+public:
+	Queue(uavcan::IPoolAllocator &arg_allocator, std::size_t block_allocation_quota) :
+		allocator_(arg_allocator, block_allocation_quota)
+	{
+		uavcan::IsDynamicallyAllocatable<Item>::check();
+	}
+
+	~Queue()
+	{
+		while (!isEmpty()) {
+			pop();
+		}
+	}
+
+	bool isEmpty() const { return list_.isEmpty(); }
+
+	/**
+	 * Creates one item in-place at the end of the list.
+	 * Returns true if the item was appended successfully, false if there's not enough memory.
+	 * Complexity is O(N) where N is queue length.
+	 */
+	template <typename... Args>
+	bool tryEmplace(Args... args)
+	{
+		// Allocating memory
+		void *const ptr = allocator_.allocate(sizeof(Item));
+
+		if (ptr == nullptr) {
+			return false;
+		}
+
+		// Constructing the new item
+		Item *const item = new (ptr) Item(args...);
+		assert(item != nullptr);
+
+		// Inserting the new item at the end of the list
+		Item *p = list_.get();
+
+		if (p == nullptr) {
+			list_.insert(item);
+
+		} else {
+			while (p->getNextListNode() != nullptr) {
+				p = p->getNextListNode();
+			}
+
+			assert(p->getNextListNode() == nullptr);
+			p->setNextListNode(item);
+			assert(p->getNextListNode()->getNextListNode() == nullptr);
+		}
+
+		return true;
+	}
+
+	/**
+	 * Accesses the first element.
+	 * Nullptr will be returned if the queue is empty.
+	 * Complexity is O(1).
+	 */
+	T       *peek()       { return isEmpty() ? nullptr : &list_.get()->payload; }
+	const T *peek() const { return isEmpty() ? nullptr : &list_.get()->payload; }
+
+	/**
+	 * Removes the first element.
+	 * If the queue is empty, nothing will be done and assertion failure will be triggered.
+	 * Complexity is O(1).
+	 */
+	void pop()
+	{
+		Item *const item = list_.get();
+		assert(item != nullptr);
+
+		if (item != nullptr) {
+			list_.remove(item);
+			item->~Item();
+			allocator_.deallocate(item);
+		}
+	}
+};
+
+/**
+ * Objects of this class are owned by the sub-node thread.
+ * This class does not use heap memory.
+ */
+class VirtualCanIface : public uavcan::ICanIface,
+	uavcan::Noncopyable
+{
+	/**
+	 * This class re-defines uavcan::RxCanFrame with flags.
+	 * Simple inheritance or composition won't work here, because the 40 byte limit will be exceeded,
+	 * rendering this class unusable with Queue<>.
+	 */
+	struct RxItem: public uavcan::CanFrame {
+		const uavcan::MonotonicTime ts_mono;
+		const uavcan::UtcTime ts_utc;
+		const uavcan::CanIOFlags flags;
+		const uint8_t iface_index;
+
+		RxItem(const uavcan::CanRxFrame &arg_frame, uavcan::CanIOFlags arg_flags) :
+			uavcan::CanFrame(arg_frame),
+			ts_mono(arg_frame.ts_mono),
+			ts_utc(arg_frame.ts_utc),
+			flags(arg_flags),
+			iface_index(arg_frame.iface_index)
+		{
+			// Making sure it will fit into a pool block with a pointer prefix
+			static_assert(sizeof(RxItem) <= (uavcan::MemPoolBlockSize - 8), "Bad coder, no coffee");
+		}
+	};
+
+	pthread_mutex_t &common_driver_mutex_;
+
+	uavcan::CanTxQueue prioritized_tx_queue_;
+	Queue<RxItem> rx_queue_;
+
+	int16_t send(const uavcan::CanFrame &frame, uavcan::MonotonicTime tx_deadline, uavcan::CanIOFlags flags) override
+	{
+		Lock lock(common_driver_mutex_);
+		prioritized_tx_queue_.push(frame, tx_deadline, uavcan::CanTxQueue::Volatile, flags);
+		return 1;
+	}
+
+	int16_t receive(uavcan::CanFrame &out_frame, uavcan::MonotonicTime &out_ts_monotonic,
+			uavcan::UtcTime &out_ts_utc, uavcan::CanIOFlags &out_flags) override
+	{
+		Lock lock(common_driver_mutex_);
+
+		if (rx_queue_.isEmpty()) {
+			return 0;
+		}
+
+		const auto item = *rx_queue_.peek();
+		rx_queue_.pop();
+
+		out_frame = item;
+		out_ts_monotonic = item.ts_mono;
+		out_ts_utc = item.ts_utc;
+		out_flags = item.flags;
+
+		return 1;
+	}
+
+	int16_t configureFilters(const uavcan::CanFilterConfig *, std::uint16_t) override { return -uavcan::ErrDriver; }
+	uint16_t getNumFilters() const override { return 0; }
+	uint64_t getErrorCount() const override { return 0; }
+
+public:
+	VirtualCanIface(uavcan::IPoolAllocator &allocator, uavcan::ISystemClock &clock,
+			pthread_mutex_t &arg_mutex, unsigned quota_per_queue) :
+		common_driver_mutex_(arg_mutex),
+		prioritized_tx_queue_(allocator, clock, quota_per_queue),
+		rx_queue_(allocator, quota_per_queue)
+	{
+	}
+
+	~VirtualCanIface()
+	{
+	}
+
+	/**
+	 * Note that RX queue overwrites oldest items when overflowed.
+	 * Call this from the main thread only.
+	 * No additional locking is required.
+	 */
+	void addRxFrame(const uavcan::CanRxFrame &frame, uavcan::CanIOFlags flags)
+	{
+		Lock lock(common_driver_mutex_);
+
+		if (!rx_queue_.tryEmplace(frame, flags) && !rx_queue_.isEmpty()) {
+			rx_queue_.pop();
+			(void)rx_queue_.tryEmplace(frame, flags);
+		}
+	}
+
+	/**
+	 * Call this from the main thread only.
+	 * No additional locking is required.
+	 */
+	void flushTxQueueTo(uavcan::INode &main_node, std::uint8_t iface_index)
+	{
+		Lock lock(common_driver_mutex_);
+		const std::uint8_t iface_mask = static_cast<std::uint8_t>(1U << iface_index);
+
+		while (auto e = prioritized_tx_queue_.peek()) {
+			UAVCAN_TRACE("VirtualCanIface", "TX injection [iface=0x%02x]: %s",
+				     unsigned(iface_mask), e->toString().c_str());
+
+			const int res = main_node.injectTxFrame(e->frame, e->deadline, iface_mask,
+								uavcan::CanTxQueue::Qos(e->qos), e->flags);
+
+			prioritized_tx_queue_.remove(e);
+
+			if (res <= 0) {
+				break;
+			}
+
+		}
+	}
+
+	/**
+	 * Call this from the sub-node thread only.
+	 * No additional locking is required.
+	 */
+	bool hasDataInRxQueue()
+	{
+		Lock lock(common_driver_mutex_);
+		return !rx_queue_.isEmpty();
+	}
+};
+
+/**
+ * This interface defines one method that will be called by the main node thread periodically in order to
+ * transfer contents of TX queue of the sub-node into the TX queue of the main node.
+ */
+class ITxQueueInjector
+{
+public:
+	virtual ~ITxQueueInjector() { }
+
+	/**
+	 * Flush contents of TX queues into the main node.
+	 * @param main_node         Reference to the main node.
+	 */
+	virtual void injectTxFramesInto(uavcan::INode &main_node) = 0;
+};
+
+/**
+ * Objects of this class are owned by the sub-node thread.
+ * This class does not use heap memory.
+ */
+class VirtualCanDriver : public uavcan::ICanDriver,
+	public uavcan::IRxFrameListener,
+	public ITxQueueInjector,
+	uavcan::Noncopyable
+{
+	class Event
+	{
+		FAR px4_sem_t sem;
+
+
+	public:
+
+		int init()
+		{
+			int rv = px4_sem_init(&sem, 0, 0);
+
+			if (rv == 0) {
+				px4_sem_setprotocol(&sem, SEM_PRIO_NONE);
+			}
+
+			return rv;
+		}
+
+		int deinit()
+		{
+			return px4_sem_destroy(&sem);
+		}
+
+
+		Event()
+		{
+		}
+
+		~Event()
+		{
+		}
+
+
+		/**
+		 */
+
+		void waitFor(uavcan::MonotonicDuration duration)
+		{
+			static const int NsPerSec = 1000000000;
+
+			if (duration.isPositive()) {
+				auto abstime = ::timespec();
+
+				if (clock_gettime(CLOCK_REALTIME, &abstime) >= 0) {
+					abstime.tv_nsec += duration.toUSec() * 1000;
+
+					if (abstime.tv_nsec >= NsPerSec) {
+						abstime.tv_sec++;
+						abstime.tv_nsec -= NsPerSec;
+					}
+
+					(void)px4_sem_timedwait(&sem, &abstime);
+				}
+			}
+		}
+
+		void signal()
+		{
+			int count;
+			int rv = px4_sem_getvalue(&sem, &count);
+
+			if (rv > 0 && count <= 0) {
+				px4_sem_post(&sem);
+			}
+		}
+	};
+
+	Event event_;                     ///< Used to unblock the select() call when IO happens.
+	pthread_mutex_t driver_mutex_;    ///< Shared across all ifaces
+	uavcan::LazyConstructor<VirtualCanIface> ifaces_[uavcan::MaxCanIfaces];
+	const unsigned num_ifaces_;
+	uavcan::ISystemClock &clock_;
+
+	uavcan::ICanIface *getIface(uint8_t iface_index) override
+	{
+		return (iface_index < num_ifaces_) ? ifaces_[iface_index].operator VirtualCanIface * () : nullptr;
+	}
+
+	uint8_t getNumIfaces() const override { return num_ifaces_; }
+
+	/**
+	 * This and other methods of ICanDriver will be invoked by the sub-node thread.
+	 */
+	int16_t select(uavcan::CanSelectMasks &inout_masks,
+		       const uavcan::CanFrame * (&)[uavcan::MaxCanIfaces],
+		       uavcan::MonotonicTime blocking_deadline) override
+	{
+		bool need_block = (inout_masks.write == 0);    // Write queue is infinite
+
+		for (unsigned i = 0; need_block && (i < num_ifaces_); i++) {
+			const bool need_read = inout_masks.read & (1U << i);
+
+			if (need_read && ifaces_[i]->hasDataInRxQueue()) {
+				need_block = false;
+			}
+		}
+
+		if (need_block) {
+			event_.waitFor(blocking_deadline - clock_.getMonotonic());
+		}
+
+		inout_masks = uavcan::CanSelectMasks();
+
+		for (unsigned i = 0; i < num_ifaces_; i++) {
+			const std::uint8_t iface_mask = 1U << i;
+			inout_masks.write |= iface_mask;           // Always ready to write
+
+			if (ifaces_[i]->hasDataInRxQueue()) {
+				inout_masks.read |= iface_mask;
+			}
+		}
+
+		return num_ifaces_;       // We're always ready to write, hence > 0.
+	}
+
+	/**
+	 * This handler will be invoked by the main node thread.
+	 */
+	void handleRxFrame(const uavcan::CanRxFrame &frame, uavcan::CanIOFlags flags) override
+	{
+		UAVCAN_TRACE("VirtualCanDriver", "RX [flags=%u]: %s", unsigned(flags), frame.toString().c_str());
+
+		if (frame.iface_index < num_ifaces_) {
+			ifaces_[frame.iface_index]->addRxFrame(frame, flags);
+			event_.signal();
+
+		}
+	}
+
+	/**
+	 * This method will be invoked by the main node thread.
+	 */
+	void injectTxFramesInto(uavcan::INode &main_node) override
+	{
+		for (unsigned i = 0; i < num_ifaces_; i++) {
+			ifaces_[i]->flushTxQueueTo(main_node, i);
+		}
+
+		event_.signal();
+	}
+
+public:
+	VirtualCanDriver(unsigned arg_num_ifaces,
+			 uavcan::ISystemClock &system_clock,
+			 uavcan::IPoolAllocator &allocator,
+			 unsigned virtual_iface_block_allocation_quota) :
+		num_ifaces_(arg_num_ifaces),
+		clock_(system_clock)
+	{
+		Lock::init(driver_mutex_);
+		event_.init();
+
+		assert(num_ifaces_ > 0 && num_ifaces_ <= uavcan::MaxCanIfaces);
+
+		const unsigned quota_per_queue = virtual_iface_block_allocation_quota;             // 2x overcommit
+
+		for (unsigned i = 0; i < num_ifaces_; i++) {
+			ifaces_[i].construct<uavcan::IPoolAllocator &, uavcan::ISystemClock &, pthread_mutex_t &,
+				unsigned>(allocator, clock_, driver_mutex_, quota_per_queue);
+		}
+	}
+
+	~VirtualCanDriver()
+	{
+		Lock::deinit(driver_mutex_);
+		event_.deinit();
+	}
+
+};