Merge pull request #1301 from PX4/uavcan_sensors

Generalizing support for UAVCAN-interfaced sensor nodes

ROMFS/px4fmu_common/init.d/rc.uavcan

@@ -0,0 +1,18 @@
+#!nsh
+#
+# UAVCAN initialization script.
+#
+
+if param compare UAVCAN_ENABLE 1
+then
+	if uavcan start
+	then
+		# First sensor publisher to initialize takes lowest instance ID
+		# This delay ensures that UAVCAN-interfaced sensors would be allocated on lowest instance IDs
+		sleep 1
+		echo "[init] UAVCAN started"
+	else
+		echo "[init] ERROR: Could not start UAVCAN"
+		tone_alarm $TUNE_OUT_ERROR
+	fi
+fi

ROMFS/px4fmu_common/init.d/rcS

@@ -304,11 +304,10 @@ then
 	then
 		if [ $OUTPUT_MODE == uavcan_esc ]
 		then
-			if uavcan start 1
+			if param compare UAVCAN_ENABLE 0
 			then
-				echo "CAN UP"
-			else
-				echo "CAN ERR"
+				echo "[init] OVERRIDING UAVCAN_ENABLE = 1"
+				param set UAVCAN_ENABLE 1
 			fi
 		fi
 
@@ -447,6 +446,11 @@ then
 
 	mavlink start $MAVLINK_FLAGS
 
+	#
+	# UAVCAN
+	#
+	sh /etc/init.d/rc.uavcan
+
 	#
 	# Sensors, Logging, GPS
 	#

src/modules/uavcan/actuators/esc.cpp

@@ -32,12 +32,12 @@
  ****************************************************************************/
 
 /**
- * @file esc_controller.cpp
+ * @file esc.cpp
  *
  * @author Pavel Kirienko <pavel.kirienko@gmail.com>
  */
 
-#include "esc_controller.hpp"
+#include "esc.hpp"
 #include <systemlib/err.h>
 
 UavcanEscController::UavcanEscController(uavcan::INode &node) :

src/modules/uavcan/actuators/esc.hpp

@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file esc_controller.hpp
+ * @file esc.hpp
  *
  * UAVCAN <--> ORB bridge for ESC messages:
  *     uavcan.equipment.esc.RawCommand

src/modules/uavcan/module.mk

@@ -40,10 +40,19 @@ MODULE_COMMAND = uavcan
 
 MAXOPTIMIZATION = -Os
 
+# Main
 SRCS += uavcan_main.cpp              \
         uavcan_clock.cpp             \
-        esc_controller.cpp      \
-        gnss_receiver.cpp
+        uavcan_params.c
+
+# Actuators
+SRCS += actuators/esc.cpp
+
+# Sensors
+SRCS += sensors/sensor_bridge.cpp   \
+        sensors/gnss.cpp            \
+        sensors/mag.cpp             \
+        sensors/baro.cpp
 
 #
 # libuavcan

src/modules/uavcan/sensors/baro.cpp

@@ -0,0 +1,117 @@
+/****************************************************************************
+ *
+ *   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 "baro.hpp"
+#include <cmath>
+
+static const orb_id_t BARO_TOPICS[2] = {
+	ORB_ID(sensor_baro0),
+	ORB_ID(sensor_baro1)
+};
+
+const char *const UavcanBarometerBridge::NAME = "baro";
+
+UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode& node) :
+UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_DEVICE_PATH, BARO_TOPICS),
+_sub_air_data(node)
+{
+}
+
+int UavcanBarometerBridge::init()
+{
+	int res = device::CDev::init();
+	if (res < 0) {
+		return res;
+	}
+
+	res = _sub_air_data.start(AirDataCbBinder(this, &UavcanBarometerBridge::air_data_sub_cb));
+	if (res < 0) {
+		log("failed to start uavcan sub: %d", res);
+		return res;
+	}
+	return 0;
+}
+
+int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+	case BAROIOCSMSLPRESSURE: {
+		if ((arg < 80000) || (arg > 120000)) {
+			return -EINVAL;
+		} else {
+			log("new msl pressure %u", _msl_pressure);
+			_msl_pressure = arg;
+			return OK;
+		}
+	}
+	case BAROIOCGMSLPRESSURE: {
+		return _msl_pressure;
+	}
+	default: {
+		return CDev::ioctl(filp, cmd, arg);
+	}
+	}
+}
+
+void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticAirData> &msg)
+{
+	auto report = ::baro_report();
+
+	report.timestamp = msg.getUtcTimestamp().toUSec();
+	if (report.timestamp == 0) {
+		report.timestamp = msg.getMonotonicTimestamp().toUSec();
+	}
+
+	report.temperature = msg.static_temperature;
+	report.pressure    = msg.static_pressure / 100.0F;  // Convert to millibar
+
+	/*
+	 * Altitude computation
+	 * Refer to the MS5611 driver for details
+	 */
+	const double T1 = 15.0 + 273.15; // temperature at base height in Kelvin
+	const double a  = -6.5 / 1000;   // temperature gradient in degrees per metre
+	const double g  = 9.80665;       // gravity constant in m/s/s
+	const double R  = 287.05;        // ideal gas constant in J/kg/K
+
+	const double p1 = _msl_pressure / 1000.0;      // current pressure at MSL in kPa
+	const double p = double(msg.static_pressure) / 1000.0; // measured pressure in kPa
+
+	report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
+
+	publish(msg.getSrcNodeID().get(), &report);
+}

src/modules/uavcan/sensors/baro.hpp

@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ *   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>
+ */
+
+#pragma once
+
+#include "sensor_bridge.hpp"
+#include <drivers/drv_baro.h>
+
+#include <uavcan/equipment/air_data/StaticAirData.hpp>
+
+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:
+	int ioctl(struct file *filp, int cmd, unsigned long arg) override;
+
+	void air_data_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticAirData> &msg);
+
+	typedef uavcan::MethodBinder<UavcanBarometerBridge*,
+		void (UavcanBarometerBridge::*)
+			(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticAirData>&)>
+		AirDataCbBinder;
+
+	uavcan::Subscriber<uavcan::equipment::air_data::StaticAirData, AirDataCbBinder> _sub_air_data;
+	unsigned _msl_pressure = 101325;
+};

src/modules/uavcan/sensors/gnss.cpp

@@ -32,52 +32,64 @@
  ****************************************************************************/
 
 /**
- * @file gnss_receiver.cpp
+ * @file gnss.cpp
  *
  * @author Pavel Kirienko <pavel.kirienko@gmail.com>
  * @author Andrew Chambers <achamber@gmail.com>
  *
  */
 
-#include "gnss_receiver.hpp"
+#include "gnss.hpp"
 #include <systemlib/err.h>
 #include <mathlib/mathlib.h>
 
 #define MM_PER_CM 			10	// Millimeters per centimeter
 
-UavcanGnssReceiver::UavcanGnssReceiver(uavcan::INode &node) :
+const char *const UavcanGnssBridge::NAME = "gnss";
+
+UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
 _node(node),
-_uavcan_sub_status(node),
+_sub_fix(node),
 _report_pub(-1)
 {
 }
 
-int UavcanGnssReceiver::init()
+int UavcanGnssBridge::init()
 {
-	int res = -1;
-
-	// GNSS fix subscription
-	res = _uavcan_sub_status.start(FixCbBinder(this, &UavcanGnssReceiver::gnss_fix_sub_cb));
+	int res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb));
 	if (res < 0)
 	{
 		warnx("GNSS fix sub failed %i", res);
 		return res;
 	}
-
-	// Clear the uORB GPS report
-	memset(&_report, 0, sizeof(_report));
-
 	return res;
 }
 
-void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg)
+unsigned UavcanGnssBridge::get_num_redundant_channels() const
+{
+	return (_receiver_node_id < 0) ? 0 : 1;
+}
+
+void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg)
 {
-	_report.timestamp_position = hrt_absolute_time();
-	_report.lat = msg.lat_1e7;
-	_report.lon = msg.lon_1e7;
-	_report.alt = msg.alt_1e2 * MM_PER_CM;	// Convert from centimeter (1e2) to millimeters (1e3)
+	// This bridge does not support redundant GNSS receivers yet.
+	if (_receiver_node_id < 0) {
+		_receiver_node_id = msg.getSrcNodeID().get();
+		warnx("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();
+
+	report.timestamp_position = hrt_absolute_time();
+	report.lat = msg.lat_1e7;
+	report.lon = msg.lon_1e7;
+	report.alt = msg.alt_1e2 * MM_PER_CM;	// Convert from centimeter (1e2) to millimeters (1e3)
 
-	_report.timestamp_variance = _report.timestamp_position;
+	report.timestamp_variance = report.timestamp_position;
 
 
 	// Check if the msg contains valid covariance information
@@ -90,19 +102,19 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uav
 
 		// 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;
+		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;
+		report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F;
 	} else {
-		_report.eph = -1.0F;
-		_report.epv = -1.0F;
+		report.eph = -1.0F;
+		report.epv = -1.0F;
 	}
 
 	if (valid_velocity_covariance) {
 	    float vel_cov[9];
 	    msg.velocity_covariance.unpackSquareMatrix(vel_cov);
-		_report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]);
+		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
@@ -118,36 +130,36 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uav
 		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 =
+		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.s_variance_m_s = -1.0F;
+		report.c_variance_rad = -1.0F;
 	}
 
-	_report.fix_type = msg.status;
+	report.fix_type = msg.status;
 
-	_report.timestamp_velocity = _report.timestamp_position;
-	_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_velocity = report.timestamp_position;
+	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 = _report.timestamp_position;
-	_report.time_gps_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec();	// Convert to microseconds
+	report.timestamp_time = report.timestamp_position;
+	report.time_gps_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec();	// Convert to microseconds
 
-	_report.satellites_used = msg.sats_used;
+	report.satellites_used = msg.sats_used;
 
 	if (_report_pub > 0) {
-		orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
+		orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &report);
 
 	} else {
-		_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
+		_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &report);
 	}
 
 }

src/modules/uavcan/sensors/gnss.hpp

@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file gnss_receiver.hpp
+ * @file gnss.hpp
  *
  * UAVCAN --> ORB bridge for GNSS messages:
  *     uavcan.equipment.gnss.Fix
@@ -51,12 +51,20 @@
 #include <uavcan/uavcan.hpp>
 #include <uavcan/equipment/gnss/Fix.hpp>
 
-class UavcanGnssReceiver
+#include "sensor_bridge.hpp"
+
+class UavcanGnssBridge : public IUavcanSensorBridge
 {
 public:
-	UavcanGnssReceiver(uavcan::INode& node);
+	static const char *const NAME;
+
+	UavcanGnssBridge(uavcan::INode& node);
+
+	const char *get_name() const override { return NAME; }
+
+	int init() override;
 
-	int init();
+	unsigned get_num_redundant_channels() const override;
 
 private:
 	/**
@@ -64,21 +72,14 @@ private:
 	 */
 	void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg);
 
-
-	typedef uavcan::MethodBinder<UavcanGnssReceiver*,
-		void (UavcanGnssReceiver::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>&)>
+	typedef uavcan::MethodBinder<UavcanGnssBridge*,
+		void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>&)>
 		FixCbBinder;
 
-	/*
-	 * libuavcan related things
-	 */
 	uavcan::INode &_node;
-	uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder>	_uavcan_sub_status;
+	uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder> _sub_fix;
+	int _receiver_node_id = -1;
 
-	/*
-	 * uORB
-	 */
-	struct vehicle_gps_position_s 	_report;					///< uORB topic for gnss position
 	orb_advert_t _report_pub;                ///< uORB pub for gnss position
 
 };

src/modules/uavcan/sensors/mag.cpp

@@ -0,0 +1,123 @@
+/****************************************************************************
+ *
+ *   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 "mag.hpp"
+
+static const orb_id_t MAG_TOPICS[3] = {
+	ORB_ID(sensor_mag0),
+	ORB_ID(sensor_mag1),
+	ORB_ID(sensor_mag2)
+};
+
+const char *const UavcanMagnetometerBridge::NAME = "mag";
+
+UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) :
+UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS),
+_sub_mag(node)
+{
+	_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) {
+		log("failed to start uavcan sub: %d", res);
+		return res;
+	}
+	return 0;
+}
+
+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 MAGIOCSELFTEST: {
+		return 0;           // Nothing to do
+	}
+	case MAGIOCGEXTERNAL: {
+		return 0;           // We don't want anyone to transform the coordinate frame, so we declare it onboard
+	}
+	case MAGIOCSSAMPLERATE: {
+		return 0;           // Pretend that this stuff is supported to keep the sensor app happy
+	}
+	case MAGIOCCALIBRATE:
+	case MAGIOCGSAMPLERATE:
+	case MAGIOCSRANGE:
+	case MAGIOCGRANGE:
+	case MAGIOCSLOWPASS:
+	case MAGIOCEXSTRAP:
+	case MAGIOCGLOWPASS: {
+		return -EINVAL;
+	}
+	default: {
+		return CDev::ioctl(filp, cmd, arg);
+	}
+	}
+}
+
+void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg)
+{
+	auto report = ::mag_report();
+
+	report.range_ga = 1.3F;   // Arbitrary number, doesn't really mean anything
+
+	report.timestamp = msg.getUtcTimestamp().toUSec();
+	if (report.timestamp == 0) {
+		report.timestamp = msg.getMonotonicTimestamp().toUSec();
+	}
+
+	report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale;
+	report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale;
+	report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale;
+
+	publish(msg.getSrcNodeID().get(), &report);
+}

src/modules/uavcan/sensors/mag.hpp

@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ *   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>
+ */
+
+#pragma once
+
+#include "sensor_bridge.hpp"
+#include <drivers/drv_mag.h>
+
+#include <uavcan/equipment/ahrs/Magnetometer.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:
+	int ioctl(struct file *filp, int cmd, unsigned long arg) override;
+
+	void mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg);
+
+	typedef uavcan::MethodBinder<UavcanMagnetometerBridge*,
+		void (UavcanMagnetometerBridge::*)
+			(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer>&)>
+		MagCbBinder;
+
+	uavcan::Subscriber<uavcan::equipment::ahrs::Magnetometer, MagCbBinder> _sub_mag;
+	mag_scale _scale = {};
+};

src/modules/uavcan/sensors/sensor_bridge.cpp

@@ -0,0 +1,142 @@
+/****************************************************************************
+ *
+ *   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].redunancy_channel_id >= 0) {
+			(void)unregister_class_devname(_class_devname, _channels[i].class_instance);
+		}
+	}
+	delete [] _orb_topics;
+	delete [] _channels;
+}
+
+void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const void *report)
+{
+	Channel *channel = nullptr;
+
+	// Checking if such channel already exists
+	for (unsigned i = 0; i < _max_channels; i++) {
+		if (_channels[i].redunancy_channel_id == redundancy_channel_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
+		}
+
+		log("adding channel %d...", redundancy_channel_id);
+
+		// Search for the first free channel
+		for (unsigned i = 0; i < _max_channels; i++) {
+			if (_channels[i].redunancy_channel_id < 0) {
+				channel = _channels + i;
+				break;
+			}
+		}
+
+		// No free channels left
+		if (channel == nullptr) {
+			_out_of_channels = true;
+			log("out of channels");
+			return;
+		}
+
+		// 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;
+			log("out of class instances");
+			(void)unregister_class_devname(_class_devname, class_instance);
+			return;
+		}
+
+		// Publish to the appropriate topic, abort on failure
+		channel->orb_id               = _orb_topics[class_instance];
+		channel->redunancy_channel_id = redundancy_channel_id;
+		channel->class_instance       = class_instance;
+
+		channel->orb_advert = orb_advertise(channel->orb_id, report);
+		if (channel->orb_advert < 0) {
+			log("ADVERTISE FAILED");
+			(void)unregister_class_devname(_class_devname, class_instance);
+			*channel = Channel();
+			return;
+		}
+
+		log("channel %d class instance %d ok", channel->redunancy_channel_id, channel->class_instance);
+	}
+	assert(channel != nullptr);
+
+	(void)orb_publish(channel->orb_id, 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].redunancy_channel_id >= 0) {
+			out += 1;
+		}
+	}
+	return out;
+}

src/modules/uavcan/sensors/sensor_bridge.hpp

@@ -0,0 +1,124 @@
+/****************************************************************************
+ *
+ *   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>
+ */
+
+#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;
+
+	/**
+	 * Sensor bridge factory.
+	 * Creates a bridge object by its ASCII name, e.g. "gnss", "mag".
+	 * @return nullptr if such bridge can't be created.
+	 */
+	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 redunancy_channel_id = -1;
+		orb_id_t orb_id          = nullptr;
+		orb_advert_t orb_advert  = -1;
+		int class_instance       = -1;
+	};
+
+	const unsigned _max_channels;
+	const char *const _class_devname;
+	orb_id_t *const _orb_topics;
+	Channel *const _channels;
+	bool _out_of_channels = false;
+
+protected:
+	template <unsigned MaxChannels>
+	UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname,
+	                           const orb_id_t (&orb_topics)[MaxChannels]) :
+	device::CDev(name, devname),
+	_max_channels(MaxChannels),
+	_class_devname(class_devname),
+	_orb_topics(new orb_id_t[MaxChannels]),
+	_channels(new Channel[MaxChannels])
+	{
+		memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels);
+	}
+
+	/**
+	 * Sends one measurement into appropriate ORB topic.
+	 * New redundancy channels will be registered automatically.
+	 * @param redundancy_channel_id Redundant sensor identifier (e.g. UAVCAN Node ID)
+	 * @param report                ORB message object
+	 */
+	void publish(const int redundancy_channel_id, const void *report);
+
+public:
+	virtual ~UavcanCDevSensorBridgeBase();
+
+	unsigned get_num_redundant_channels() const override;
+};

src/modules/uavcan/uavcan_main.cpp

@@ -38,6 +38,7 @@
 #include <fcntl.h>
 #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
+#include <systemlib/param/param.h>
 #include <systemlib/mixer/mixer.h>
 #include <systemlib/board_serial.h>
 #include <version/version.h>
@@ -65,16 +66,18 @@ UavcanNode *UavcanNode::_instance;
 UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
 	CDev("uavcan", UAVCAN_DEVICE_PATH),
 	_node(can_driver, system_clock),
-	_esc_controller(_node),
-	_gnss_receiver(_node)
+	_node_mutex(),
+	_esc_controller(_node)
 {
 	_control_topics[0] = ORB_ID(actuator_controls_0);
 	_control_topics[1] = ORB_ID(actuator_controls_1);
 	_control_topics[2] = ORB_ID(actuator_controls_2);
 	_control_topics[3] = ORB_ID(actuator_controls_3);
 
-	// memset(_controls, 0, sizeof(_controls));
-	// memset(_poll_fds, 0, sizeof(_poll_fds));
+	const int res = pthread_mutex_init(&_node_mutex, nullptr);
+	if (res < 0) {
+		std::abort();
+	}
 }
 
 UavcanNode::~UavcanNode()
@@ -103,6 +106,14 @@ UavcanNode::~UavcanNode()
 
 	::close(_armed_sub);
 
+	// Removing the sensor bridges
+	auto br = _sensor_bridges.getHead();
+	while (br != nullptr) {
+		auto next = br->getSibling();
+		delete br;
+		br = next;
+	}
+
 	_instance = nullptr;
 }
 
@@ -214,11 +225,12 @@ int UavcanNode::init(uavcan::NodeID node_id)
 {
 	int ret = -1;
 
-	/* do regular cdev init */
+	// Do regular cdev init
 	ret = CDev::init();
 
-	if (ret != OK)
+	if (ret != OK) {
 		return ret;
+	}
 
 	_node.setName("org.pixhawk.pixhawk");
 
@@ -226,14 +238,24 @@ int UavcanNode::init(uavcan::NodeID node_id)
 
 	fill_node_info();
 
-	/* initializing the bridges UAVCAN <--> uORB */
+	// Actuators
 	ret = _esc_controller.init();
-	if (ret < 0)
+	if (ret < 0) {
 		return ret;
+	}
 
-	ret = _gnss_receiver.init();
-	if (ret < 0)
+	// Sensor bridges
+	IUavcanSensorBridge::make_all(_node, _sensor_bridges);
+	auto br = _sensor_bridges.getHead();
+	while (br != nullptr) {
+		ret = br->init();
+		if (ret < 0) {
+			warnx("cannot init sensor bridge '%s' (%d)", br->get_name(), ret);
 			return ret;
+		}
+		warnx("sensor bridge '%s' init ok", br->get_name());
+		br = br->getSibling();
+	}
 
 	return _node.start();
 }
@@ -248,6 +270,8 @@ void UavcanNode::node_spin_once()
 
 int UavcanNode::run()
 {
+	(void)pthread_mutex_lock(&_node_mutex);
+
 	const unsigned PollTimeoutMs = 50;
 
 	// XXX figure out the output count
@@ -291,8 +315,13 @@ int UavcanNode::run()
 			_groups_subscribed = _groups_required;
 		}
 
+		// Mutex is unlocked while the thread is blocked on IO multiplexing
+		(void)pthread_mutex_unlock(&_node_mutex);
+
 		const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs);
 
+		(void)pthread_mutex_lock(&_node_mutex);
+
 		node_spin_once();  // Non-blocking
 
 		// this would be bad...
@@ -352,7 +381,6 @@ int UavcanNode::run()
 				// Output to the bus
 				_esc_controller.update_outputs(outputs.output, outputs.noutputs);
 			}
-
 		}
 
 		// Check arming state
@@ -376,10 +404,7 @@ int UavcanNode::run()
 }
 
 int
-UavcanNode::control_callback(uintptr_t handle,
-			 uint8_t control_group,
-			 uint8_t control_index,
-			 float &input)
+UavcanNode::control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input)
 {
 	const actuator_controls_s *controls = (actuator_controls_s *)handle;
 
@@ -520,8 +545,21 @@ UavcanNode::print_info()
 		warnx("not running, start first");
 	}
 
-	warnx("actuators control groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
-	warnx("mixer: %s", (_mixers == nullptr) ? "NONE" : "OK");
+	(void)pthread_mutex_lock(&_node_mutex);
+
+	// ESC mixer status
+	warnx("ESC actuators control groups: sub: %u / req: %u / fds: %u",
+	      (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
+	warnx("ESC mixer: %s", (_mixers == nullptr) ? "NONE" : "OK");
+
+	// Sensor bridges
+	auto br = _sensor_bridges.getHead();
+	while (br != nullptr) {
+		warnx("Sensor '%s': channels: %u", br->get_name(), br->get_num_redundant_channels());
+		br = br->getSibling();
+	}
+
+	(void)pthread_mutex_unlock(&_node_mutex);
 }
 
 /*
@@ -529,79 +567,57 @@ UavcanNode::print_info()
  */
 static void print_usage()
 {
-	warnx("usage: uavcan start <node_id> [can_bitrate]");
+	warnx("usage: \n"
+	      "\tuavcan {start|status|stop}");
 }
 
 extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
 
 int uavcan_main(int argc, char *argv[])
 {
-	constexpr unsigned DEFAULT_CAN_BITRATE = 1000000;
-
 	if (argc < 2) {
 		print_usage();
 		::exit(1);
 	}
 
 	if (!std::strcmp(argv[1], "start")) {
-		if (argc < 3) {
-			print_usage();
-			::exit(1);
+		if (UavcanNode::instance()) {
+			errx(1, "already started");
 		}
 
-		/*
-		 * Node ID
-		 */
-		const int node_id = atoi(argv[2]);
+		// Node ID
+		int32_t node_id = 0;
+		(void)param_get(param_find("UAVCAN_NODE_ID"), &node_id);
 
 		if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) {
 			warnx("Invalid Node ID %i", node_id);
 			::exit(1);
 		}
 
-		/*
-		 * CAN bitrate
-		 */
-		unsigned bitrate = 0;
-
-		if (argc > 3) {
-			bitrate = atol(argv[3]);
-		}
-
-		if (bitrate <= 0) {
-			bitrate = DEFAULT_CAN_BITRATE;
-		}
+		// CAN bitrate
+		int32_t bitrate = 0;
+		(void)param_get(param_find("UAVCAN_BITRATE"), &bitrate);
 
-		if (UavcanNode::instance()) {
-			errx(1, "already started");
-		}
-
-		/*
-		 * Start
-		 */
+		// Start
 		warnx("Node ID %u, bitrate %u", node_id, bitrate);
 		return UavcanNode::start(node_id, bitrate);
-
 	}
 
 	/* commands below require the app to be started */
-	UavcanNode *inst = UavcanNode::instance();
+	UavcanNode *const inst = UavcanNode::instance();
 
 	if (!inst) {
 		errx(1, "application not running");
 	}
 
 	if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) {
-		
 		inst->print_info();
-			return OK;
+		::exit(0);
 	}
 
 	if (!std::strcmp(argv[1], "stop")) {
-		
 		delete inst;
-			inst = nullptr;
-			return OK;
+		::exit(0);
 	}
 
 	print_usage();

src/modules/uavcan/uavcan_main.hpp

@@ -42,8 +42,8 @@
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
 
-#include "esc_controller.hpp"
-#include "gnss_receiver.hpp"
+#include "actuators/esc.hpp"
+#include "sensors/sensor_bridge.hpp"
 
 /**
  * @file uavcan_main.hpp
@@ -77,12 +77,10 @@ public:
 
 	static int	start(uavcan::NodeID node_id, uint32_t bitrate);
 
-	Node&		getNode() { return _node; }
+	Node&		get_node() { return _node; }
 
-	static int	control_callback(uintptr_t handle,
-					 uint8_t control_group,
-					 uint8_t control_index,
-					 float &input);
+	// 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();
 
@@ -109,8 +107,11 @@ private:
 
 	static UavcanNode	*_instance;			///< singleton pointer
 	Node			_node;				///< library instance
+	pthread_mutex_t		_node_mutex;
+
 	UavcanEscController	_esc_controller;
-	UavcanGnssReceiver 	_gnss_receiver;
+
+	List<IUavcanSensorBridge*> _sensor_bridges;		///< List of active sensor bridges
 
 	MixerGroup		*_mixers = nullptr;
 

src/modules/uavcan/uavcan_params.c

@@ -0,0 +1,73 @@
+/****************************************************************************
+ *
+ *   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 <nuttx/config.h>
+#include <systemlib/param/param.h>
+
+/**
+ * Enable UAVCAN.
+ *
+ * Enables support for UAVCAN-interfaced actuators and sensors.
+ *
+ * @min 0
+ * @max 1
+ * @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
+ * @group UAVCAN
+ */
+PARAM_DEFINE_INT32(UAVCAN_NODE_ID, 1);
+
+/**
+ * UAVCAN CAN bus bitrate.
+ *
+ * @min 20000
+ * @max 1000000
+ * @group UAVCAN
+ */
+PARAM_DEFINE_INT32(UAVCAN_BITRATE, 1000000);
+
+
+

uavcan

@@ -1 +1 @@
-Subproject commit 6980ee824074aa2f7a62221bf6040ee411119520
+Subproject commit 75153eb6436d0cc00679056ff8e916c6d99057ad