"Get newest update"Merge branch 'master' of https://github.com/PX4/Firmware

ROMFS/px4fmu_common/init.d/3035_viper

@@ -8,3 +8,5 @@
 sh /etc/init.d/rc.fw_defaults
 
 set MIXER Viper
+
+set FAILSAFE "-c567 -p 1000"

ROMFS/px4fmu_common/init.d/rc.interface

@@ -77,4 +77,9 @@ then
 			pwm max -c $PWM_OUTPUTS -p $PWM_MAX
 		fi
 	fi
+
+	if [ $FAILSAFE != none ]
+	then
+		pwm failsafe -d $OUTPUT_DEV $FAILSAFE
+	fi
 fi

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

@@ -66,6 +66,9 @@ then
 	#
 	sercon
 
+	# Try to get an USB console
+	nshterm /dev/ttyACM0 &
+
 	#
 	# Start the ORB (first app to start)
 	#
@@ -96,11 +99,9 @@ then
 	#
 	if rgbled start
 	then
-		echo "[init] RGB Led"
 	else
 		if blinkm start
 		then
-			echo "[init] BlinkM"
 			blinkm systemstate
 		fi
 	fi
@@ -129,6 +130,7 @@ then
 	set LOAD_DEFAULT_APPS yes
 	set GPS yes
 	set GPS_FAKE no
+	set FAILSAFE none
 
 	#
 	# Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts
@@ -279,9 +281,6 @@ then
 		fi
 	fi
 
-	# Try to get an USB console
-	nshterm /dev/ttyACM0 &
-
 	#
 	# Start the datamanager (and do not abort boot if it fails)
 	#
@@ -304,11 +303,10 @@ then
 	then
 		if [ $OUTPUT_MODE == uavcan_esc ]
 		then
-			if uavcan start 1
+			if param compare UAVCAN_ENABLE 0
 			then
-				echo "CAN UP"
+				echo "[init] OVERRIDING UAVCAN_ENABLE = 1"
-			else
+				param set UAVCAN_ENABLE 1
-				echo "CAN ERR"
 			fi
 		fi
 
@@ -447,6 +445,11 @@ then
 
 	mavlink start $MAVLINK_FLAGS
 
+	#
+	# UAVCAN
+	#
+	sh /etc/init.d/rc.uavcan
+
 	#
 	# Sensors, Logging, GPS
 	#

ROMFS/px4fmu_common/mixers/FMU_AERT.mix

@@ -64,21 +64,22 @@ O:      10000  10000      0 -10000  10000
 S: 0 3      0  20000 -10000 -10000  10000
 
 
-Gimbal / flaps / payload mixer for last four channels
+Gimbal / flaps / payload mixer for last four channels,
+using the payload control group
 -----------------------------------------------------
 
 M: 1
 O:      10000  10000      0 -10000  10000
-S: 0 4  10000  10000      0 -10000  10000
+S: 2 0  10000  10000      0 -10000  10000
 
 M: 1
 O:      10000  10000      0 -10000  10000
-S: 0 5  10000  10000      0 -10000  10000
+S: 2 1  10000  10000      0 -10000  10000
 
 M: 1
 O:      10000  10000      0 -10000  10000
-S: 0 6  10000  10000      0 -10000  10000
+S: 2 2  10000  10000      0 -10000  10000
 
 M: 1
 O:      10000  10000      0 -10000  10000
-S: 0 7  10000  10000      0 -10000  10000
+S: 2 3  10000  10000      0 -10000  10000

ROMFS/px4fmu_common/mixers/Viper.mix

@@ -52,21 +52,20 @@ M: 1
 O:      10000  10000      0 -10000  10000
 S: 0 3      0  20000 -10000 -10000  10000
 
-Gimbal / flaps / payload mixer for last four channels
+Inputs to the mixer come from channel group 2 (payload), channels 0
+(bay servo 1), 1 (bay servo 2) and 3 (drop release).
 -----------------------------------------------------
 
 M: 1
 O:      10000  10000      0 -10000  10000
-S: 0 4  10000  10000      0 -10000  10000
+S: 2 0  10000  10000      0 -10000  10000
 
 M: 1
 O:      10000  10000      0 -10000  10000
-S: 0 5  10000  10000      0 -10000  10000
+S: 2 1  10000  10000      0 -10000  10000
 
 M: 1
 O:      10000  10000      0 -10000  10000
-S: 0 6  10000  10000      0 -10000  10000
+S: 2 2 -10000 -10000      0 -10000  10000
+
 
-M: 1
-O:      10000  10000      0 -10000  10000
-S: 0 7  10000  10000      0 -10000  10000

Tools/px_uploader.py

@@ -178,9 +178,9 @@ class uploader(object):
         MAVLINK_REBOOT_ID1 = bytearray(b'\xfe\x21\x72\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x01\x00\x00\x48\xf0')
         MAVLINK_REBOOT_ID0 = bytearray(b'\xfe\x21\x45\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x00\x00\x00\xd7\xac')
 
-        def __init__(self, portname, baudrate):
+        def __init__(self, portname, baudrate, interCharTimeout=0.001, timeout=0.5):
                 # open the port, keep the default timeout short so we can poll quickly
-                self.port = serial.Serial(portname, baudrate, timeout=2.0)
+                self.port = serial.Serial(portname, baudrate)
                 self.otp = b''
                 self.sn = b''
 
@@ -195,7 +195,7 @@ class uploader(object):
         def __recv(self, count=1):
                 c = self.port.read(count)
                 if len(c) < 1:
-                        raise RuntimeError("timeout waiting for data")
+                        raise RuntimeError("timeout waiting for data (%u bytes)", count)
 #               print("recv " + binascii.hexlify(c))
                 return c
 

makefiles/config_px4fmu-v2_default.mk

@@ -41,6 +41,7 @@ MODULES		+= drivers/frsky_telemetry
 MODULES		+= modules/sensors
 MODULES		+= drivers/mkblctrl
 MODULES		+= drivers/pca8574
+MODULES		+= drivers/px4flow
 
 
 # Needs to be burned to the ground and re-written; for now,

src/drivers/drv_pwm_output.h

@@ -94,6 +94,11 @@ __BEGIN_DECLS
  */
 #define PWM_LOWEST_MAX 1700
 
+/**
+ * Do not output a channel with this value
+ */
+#define PWM_IGNORE_THIS_CHANNEL UINT16_MAX
+
 /**
  * Servo output signal type, value is actual servo output pulse
  * width in microseconds.

src/drivers/drv_px4flow.h

@@ -46,37 +46,6 @@
 
 #define PX4FLOW_DEVICE_PATH	"/dev/px4flow"
 
-/**
- * @addtogroup topics
- * @{
- */
-
-/**
- * Optical flow in NED body frame in SI units.
- *
- * @see http://en.wikipedia.org/wiki/International_System_of_Units
- *
- * @warning If possible the usage of the raw flow and performing rotation-compensation
- *		using the autopilot angular rate estimate is recommended.
- */
-struct px4flow_report {
-
-	uint64_t timestamp;		/**< in microseconds since system start          */
-
-	int16_t flow_raw_x;		/**< flow in pixels in X direction, not rotation-compensated */
-	int16_t flow_raw_y;		/**< flow in pixels in Y direction, not rotation-compensated */
-	float flow_comp_x_m;		/**< speed over ground in meters per second, rotation-compensated */
-	float flow_comp_y_m;		/**< speed over ground in meters per second, rotation-compensated */
-	float ground_distance_m;	/**< Altitude / distance to ground in meters */
-	uint8_t	quality;		/**< Quality of the measurement, 0: bad quality, 255: maximum quality */
-	uint8_t sensor_id;		/**< id of the sensor emitting the flow value */
-
-};
-
-/**
- * @}
- */
-
 /*
  * ObjDev tag for px4flow data.
  */

src/drivers/ets_airspeed/ets_airspeed.cpp

@@ -155,7 +155,6 @@ ETSAirspeed::collect()
 	}
 
 	uint16_t diff_pres_pa_raw = val[1] << 8 | val[0];
-	uint16_t diff_pres_pa;
         if (diff_pres_pa_raw == 0) {
 		// a zero value means the pressure sensor cannot give us a
 		// value. We need to return, and not report a value or the
@@ -166,28 +165,21 @@ ETSAirspeed::collect()
 		return -1;
         }
 
-	if (diff_pres_pa_raw < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
-		diff_pres_pa = 0;
-	} else {
-		diff_pres_pa = diff_pres_pa_raw - _diff_pres_offset;
-	}
-
 	// The raw value still should be compensated for the known offset
 	diff_pres_pa_raw -= _diff_pres_offset;
 
 	// Track maximum differential pressure measured (so we can work out top speed).
-	if (diff_pres_pa > _max_differential_pressure_pa) {
+	if (diff_pres_pa_raw > _max_differential_pressure_pa) {
-		_max_differential_pressure_pa = diff_pres_pa;
+		_max_differential_pressure_pa = diff_pres_pa_raw;
 	}
 
 	differential_pressure_s report;
 	report.timestamp = hrt_absolute_time();
         report.error_count = perf_event_count(_comms_errors);
-	report.differential_pressure_pa = (float)diff_pres_pa;
 
 	// XXX we may want to smooth out the readings to remove noise.
-	report.differential_pressure_filtered_pa = (float)diff_pres_pa;
+	report.differential_pressure_filtered_pa = diff_pres_pa_raw;
-	report.differential_pressure_raw_pa = (float)diff_pres_pa_raw;
+	report.differential_pressure_raw_pa = diff_pres_pa_raw;
 	report.temperature = -1000.0f;
 	report.max_differential_pressure_pa = _max_differential_pressure_pa;
 
@@ -369,7 +361,7 @@ test()
 		err(1, "immediate read failed");
 
 	warnx("single read");
-	warnx("diff pressure: %f pa", (double)report.differential_pressure_pa);
+	warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa);
 
 	/* start the sensor polling at 2Hz */
 	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
@@ -394,7 +386,7 @@ test()
 			err(1, "periodic read failed");
 
 		warnx("periodic read %u", i);
-		warnx("diff pressure: %f pa", (double)report.differential_pressure_pa);
+		warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa);
 	}
 
 	/* reset the sensor polling to its default rate */

src/drivers/meas_airspeed/meas_airspeed.cpp

@@ -228,44 +228,23 @@ MEASAirspeed::collect()
 	// the raw value still should be compensated for the known offset
 	diff_press_pa_raw -= _diff_pres_offset;
 
-	float diff_press_pa = fabsf(diff_press_pa_raw);
-	
 	/*
-	  note that we return both the absolute value with offset
-	  applied and a raw value without the offset applied. This
-	  makes it possible for higher level code to detect if the
-	  user has the tubes connected backwards, and also makes it
-	  possible to correctly use offsets calculated by a higher
-	  level airspeed driver.
-
 	  With the above calculation the MS4525 sensor will produce a
 	  positive number when the top port is used as a dynamic port
 	  and bottom port is used as the static port
-
-	  Also note that the _diff_pres_offset is applied before the
-	  fabsf() not afterwards. It needs to be done this way to
-	  prevent a bias at low speeds, but this also means that when
-	  setting a offset you must set it based on the raw value, not
-	  the offset value
 	 */
-	
+
 	struct differential_pressure_s report;
 
 	/* track maximum differential pressure measured (so we can work out top speed). */
-	if (diff_press_pa > _max_differential_pressure_pa) {
+	if (diff_press_pa_raw > _max_differential_pressure_pa) {
-		_max_differential_pressure_pa = diff_press_pa;
+		_max_differential_pressure_pa = diff_press_pa_raw;
 	}
 
 	report.timestamp = hrt_absolute_time();
 	report.error_count = perf_event_count(_comms_errors);
 	report.temperature = temperature;
-	report.differential_pressure_pa = diff_press_pa;
+	report.differential_pressure_filtered_pa =  _filter.apply(diff_press_pa_raw);
-	report.differential_pressure_filtered_pa =  _filter.apply(diff_press_pa);
-
-	/* the dynamics of the filter can make it overshoot into the negative range */
-	if (report.differential_pressure_filtered_pa < 0.0f) {
-		report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa);
-	}
 
 	report.differential_pressure_raw_pa = diff_press_pa_raw;
 	report.max_differential_pressure_pa = _max_differential_pressure_pa;
@@ -345,7 +324,7 @@ MEASAirspeed::cycle()
 /**
    correct for 5V rail voltage if the system_power ORB topic is
    available
-   
+
    See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of
    offset versus voltage for 3 sensors
  */
@@ -394,7 +373,7 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature)
 	if (voltage_diff < -1.0f) {
 		voltage_diff = -1.0f;
 	}
-	temperature -= voltage_diff * temp_slope;	
+	temperature -= voltage_diff * temp_slope;
 #endif // CONFIG_ARCH_BOARD_PX4FMU_V2
 }
 
@@ -523,7 +502,7 @@ test()
 	}
 
 	warnx("single read");
-	warnx("diff pressure: %d pa", (int)report.differential_pressure_pa);
+	warnx("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa);
 
 	/* start the sensor polling at 2Hz */
 	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
@@ -551,7 +530,7 @@ test()
 		}
 
 		warnx("periodic read %u", i);
-		warnx("diff pressure: %d pa", (int)report.differential_pressure_pa);
+		warnx("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa);
 		warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature);
 	}
 

src/drivers/px4flow/px4flow.cpp

@@ -37,7 +37,7 @@
  *
  * Driver for the PX4FLOW module connected via I2C.
  */
-	 
+
 #include <nuttx/config.h>
 
 #include <drivers/device/i2c.h>
@@ -68,7 +68,7 @@
 
 #include <uORB/uORB.h>
 #include <uORB/topics/subsystem_info.h>
-//#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/optical_flow.h>
 
 #include <board_config.h>
 
@@ -80,7 +80,7 @@
 /* PX4FLOW Registers addresses */
 #define PX4FLOW_REG	0x00		/* Measure Register */
 
-#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz
+#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz */
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -115,17 +115,17 @@ class PX4FLOW : public device::I2C
 public:
 	PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS);
 	virtual ~PX4FLOW();
-	
+
 	virtual int 		init();
-	
+
 	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
 	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
-	
+
 	/**
 	* Diagnostics - print some basic information about the driver.
 	*/
 	void				print_info();
-	
+
 protected:
 	virtual int			probe();
 
@@ -136,13 +136,13 @@ private:
 	bool				_sensor_ok;
 	int					_measure_ticks;
 	bool				_collect_phase;
-	
+
 	orb_advert_t		_px4flow_topic;
 
 	perf_counter_t		_sample_perf;
 	perf_counter_t		_comms_errors;
 	perf_counter_t		_buffer_overflows;
-	
+
 	/**
 	* Test whether the device supported by the driver is present at a
 	* specific address.
@@ -151,7 +151,7 @@ private:
 	* @return		True if the device is present.
 	*/
 	int					probe_address(uint8_t address);
-	
+
 	/**
 	* Initialise the automatic measurement state machine and start it.
 	*
@@ -159,12 +159,12 @@ private:
 	*       to make it more aggressive about resetting the bus in case of errors.
 	*/
 	void				start();
-	
+
 	/**
 	* Stop the automatic measurement state machine.
 	*/
 	void				stop();
-	
+
 	/**
 	* Perform a poll cycle; collect from the previous measurement
 	* and start a new one.
@@ -179,8 +179,8 @@ private:
 	* @param arg		Instance pointer for the driver that is polling.
 	*/
 	static void		cycle_trampoline(void *arg);
-	
+
-	
+
 };
 
 /*
@@ -201,7 +201,7 @@ PX4FLOW::PX4FLOW(int bus, int address) :
 {
 	// enable debug() calls
 	_debug_enabled = true;
-	
+
 	// work_cancel in the dtor will explode if we don't do this...
 	memset(&_work, 0, sizeof(_work));
 }
@@ -226,13 +226,13 @@ PX4FLOW::init()
 		goto out;
 
 	/* allocate basic report buffers */
-	_reports = new RingBuffer(2, sizeof(px4flow_report));
+	_reports = new RingBuffer(2, sizeof(struct optical_flow_s));
 
 	if (_reports == nullptr)
 		goto out;
 
 	/* get a publish handle on the px4flow topic */
-	struct px4flow_report zero_report;
+	struct optical_flow_s zero_report;
 	memset(&zero_report, 0, sizeof(zero_report));
 	_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report);
 
@@ -323,24 +323,24 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
 		/* lower bound is mandatory, upper bound is a sanity check */
 		if ((arg < 1) || (arg > 100))
 			return -EINVAL;
-		
+
 		irqstate_t flags = irqsave();
 		if (!_reports->resize(arg)) {
 			irqrestore(flags);
 			return -ENOMEM;
 		}
 		irqrestore(flags);
-		
+
 		return OK;
 	}
 
 	case SENSORIOCGQUEUEDEPTH:
 		return _reports->size();
-		
+
 	case SENSORIOCRESET:
 		/* XXX implement this */
 		return -EINVAL;
-	
+
 	default:
 		/* give it to the superclass */
 		return I2C::ioctl(filp, cmd, arg);
@@ -350,8 +350,8 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
 ssize_t
 PX4FLOW::read(struct file *filp, char *buffer, size_t buflen)
 {
-	unsigned count = buflen / sizeof(struct px4flow_report);
+	unsigned count = buflen / sizeof(struct optical_flow_s);
-	struct px4flow_report *rbuf = reinterpret_cast<struct px4flow_report *>(buffer);
+	struct optical_flow_s *rbuf = reinterpret_cast<struct optical_flow_s *>(buffer);
 	int ret = 0;
 
 	/* buffer must be large enough */
@@ -425,7 +425,7 @@ PX4FLOW::measure()
 		return ret;
 	}
 	ret = OK;
-	
+
 	return ret;
 }
 
@@ -433,14 +433,14 @@ int
 PX4FLOW::collect()
 {
 	int	ret = -EIO;
-	
+
 	/* read from the sensor */
 	uint8_t val[22] = {0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0};
-	
+
 	perf_begin(_sample_perf);
-	
+
 	ret = transfer(nullptr, 0, &val[0], 22);
-	
+
 	if (ret < 0)
 	{
 		log("error reading from sensor: %d", ret);
@@ -448,7 +448,7 @@ PX4FLOW::collect()
 		perf_end(_sample_perf);
 		return ret;
 	}
-	
+
 //	f.frame_count = val[1] << 8 | val[0];
 //	f.pixel_flow_x_sum= val[3] << 8 | val[2];
 //	f.pixel_flow_y_sum= val[5] << 8 | val[4];
@@ -466,7 +466,7 @@ PX4FLOW::collect()
 	int16_t flowcy = val[9] << 8 | val[8];
 	int16_t gdist = val[21] << 8 | val[20];
 
-	struct px4flow_report report;
+	struct optical_flow_s report;
 	report.flow_comp_x_m = float(flowcx)/1000.0f;
 	report.flow_comp_y_m = float(flowcy)/1000.0f;
 	report.flow_raw_x= val[3] << 8 | val[2];
@@ -503,7 +503,7 @@ PX4FLOW::start()
 
 	/* schedule a cycle to start things */
 	work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1);
-	
+
 	/* notify about state change */
 	struct subsystem_info_s info = {
 		true,
@@ -644,7 +644,7 @@ start()
 
 fail:
 
-	if (g_dev != nullptr) 
+	if (g_dev != nullptr)
 	{
 		delete g_dev;
 		g_dev = nullptr;
@@ -678,7 +678,7 @@ void stop()
 void
 test()
 {
-	struct px4flow_report report;
+	struct optical_flow_s report;
 	ssize_t sz;
 	int ret;
 
@@ -777,7 +777,7 @@ px4flow_main(int argc, char *argv[])
 	 */
 	if (!strcmp(argv[1], "start"))
 		px4flow::start();
-	
+
 	 /*
 	  * Stop the driver
 	  */

src/drivers/px4fmu/fmu.cpp

@@ -1272,7 +1272,9 @@ PX4FMU::write(file *filp, const char *buffer, size_t len)
 	memcpy(values, buffer, count * 2);
 
 	for (uint8_t i = 0; i < count; i++) {
-		up_pwm_servo_set(i, values[i]);
+		if (values[i] != PWM_IGNORE_THIS_CHANNEL) {
+			up_pwm_servo_set(i, values[i]);
+		}
 	}
 
 	return count * 2;

src/lib/launchdetection/CatapultLaunchMethod.cpp

@@ -66,10 +66,10 @@ void CatapultLaunchMethod::update(float accel_x)
 	last_timestamp = hrt_absolute_time();
 
 	if (accel_x > threshold_accel.get()) {
-		integrator += accel_x * dt;
+		integrator += dt;
 //		warnx("*** integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f",
 //				(double)integrator, (double)threshold_accel.get(),  (double)threshold_time.get(), (double)accel_x, (double)dt);
-		if (integrator > threshold_accel.get() * threshold_time.get()) {
+		if (integrator > threshold_time.get()) {
 			launchDetected = true;
 		}
 

src/modules/commander/airspeed_calibration.cpp

@@ -180,11 +180,13 @@ int do_airspeed_calibration(int mavlink_fd)
 		return ERROR;
 	}
 
-	mavlink_log_critical(mavlink_fd, "Offset of %d Pa, create airflow now!", (int)diff_pres_offset);
+	mavlink_log_critical(mavlink_fd, "Offset of %d Pascal", (int)diff_pres_offset);
 
 	/* wait 500 ms to ensure parameter propagated through the system */
 	usleep(500 * 1000);
 
+	mavlink_log_critical(mavlink_fd, "Create airflow now");
+
 	calibration_counter = 0;
 	const unsigned maxcount = 3000;
 
@@ -204,18 +206,18 @@ int do_airspeed_calibration(int mavlink_fd)
 			calibration_counter++;
 
 			if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
-				if (calibration_counter % 100 == 0) {
+				if (calibration_counter % 500 == 0) {
-					mavlink_log_critical(mavlink_fd, "Missing airflow! (%d, wanted: 50 Pa, #h101)",
+					mavlink_log_info(mavlink_fd, "Create airflow! (%d, wanted: 50 Pa)",
-					(int)diff_pres.differential_pressure_raw_pa);
+						(int)diff_pres.differential_pressure_raw_pa);
 				}
 				continue;
 			}
 
 			/* do not allow negative values */
 			if (diff_pres.differential_pressure_raw_pa < 0.0f) {
-				mavlink_log_info(mavlink_fd, "negative pressure: ERROR (%d Pa)",
+				mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!");
-					(int)diff_pres.differential_pressure_raw_pa);
+				mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)",
-				mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa);
+						(int)diff_pres.differential_pressure_raw_pa);
 				close(diff_pres_sub);
 
 				/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
@@ -236,7 +238,7 @@ int do_airspeed_calibration(int mavlink_fd)
 				mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
 				return ERROR;
 			} else {
-				mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)",
+				mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)",
 					(int)diff_pres.differential_pressure_raw_pa);
 				break;
 			}

src/modules/commander/commander.cpp

@@ -615,6 +615,11 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 	case VEHICLE_CMD_PREFLIGHT_CALIBRATION:
 	case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
 	case VEHICLE_CMD_PREFLIGHT_STORAGE:
+	case VEHICLE_CMD_CUSTOM_0:
+	case VEHICLE_CMD_CUSTOM_1:
+	case VEHICLE_CMD_CUSTOM_2:
+	case VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY:
+	case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY:
 		/* ignore commands that handled in low prio loop */
 		break;
 

src/modules/commander/state_machine_helper.cpp

@@ -182,12 +182,19 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 
 						// Fail transition if power levels on the avionics rail
 						// are measured but are insufficient
-						if (status->condition_power_input_valid && ((status->avionics_power_rail_voltage > 0.0f) &&
+						if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) {
-							(status->avionics_power_rail_voltage < 4.9f))) {
+							// Check avionics rail voltages
-
+							if (status->avionics_power_rail_voltage < 4.75f) {
-							mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage);
+								mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
-							feedback_provided = true;
+								feedback_provided = true;
-							valid_transition = false;
+								valid_transition = false;
+							} else if (status->avionics_power_rail_voltage < 4.9f) {
+								mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
+								feedback_provided = true;
+							} else if (status->avionics_power_rail_voltage > 5.4f) {
+								mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage);
+								feedback_provided = true;
+							}
 						}
 					}
 

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -145,6 +145,7 @@ private:
 	perf_counter_t	_nonfinite_output_perf;		/**< performance counter for non finite output */
 
 	bool		_setpoint_valid;		/**< flag if the position control setpoint is valid */
+	bool		_debug;				/**< if set to true, print debug output */
 
 	struct {
 		float tconst;
@@ -324,7 +325,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")),
 	_nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")),
 /* states */
-	_setpoint_valid(false)
+	_setpoint_valid(false),
+	_debug(false)
 {
 	/* safely initialize structs */
 	_att = {};
@@ -700,7 +702,8 @@ FixedwingAttitudeControl::task_main()
 						perf_count(_nonfinite_input_perf);
 					}
 				} else {
-					airspeed = _airspeed.true_airspeed_m_s;
+					/* prevent numerical drama by requiring 0.5 m/s minimal speed */
+					airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s);
 				}
 
 				/*
@@ -785,7 +788,7 @@ FixedwingAttitudeControl::task_main()
 					speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d;
 					speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d;
 				} else	{
-					if (loop_counter % 10 == 0) {
+					if (_debug && loop_counter % 10 == 0) {
 						warnx("Did not get a valid R\n");
 					}
 				}
@@ -808,7 +811,7 @@ FixedwingAttitudeControl::task_main()
 						_roll_ctrl.reset_integrator();
 						perf_count(_nonfinite_output_perf);
 
-						if (loop_counter % 10 == 0) {
+						if (_debug && loop_counter % 10 == 0) {
 							warnx("roll_u %.4f", (double)roll_u);
 						}
 					}
@@ -821,7 +824,7 @@ FixedwingAttitudeControl::task_main()
 					if (!isfinite(pitch_u)) {
 						_pitch_ctrl.reset_integrator();
 						perf_count(_nonfinite_output_perf);
-						if (loop_counter % 10 == 0) {
+						if (_debug && loop_counter % 10 == 0) {
 							warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f,"
 								" airspeed %.4f, airspeed_scaling %.4f,"
 								" roll_sp %.4f, pitch_sp %.4f,"
@@ -845,7 +848,7 @@ FixedwingAttitudeControl::task_main()
 					if (!isfinite(yaw_u)) {
 						_yaw_ctrl.reset_integrator();
 						perf_count(_nonfinite_output_perf);
-						if (loop_counter % 10 == 0) {
+						if (_debug && loop_counter % 10 == 0) {
 							warnx("yaw_u %.4f", (double)yaw_u);
 						}
 					}
@@ -853,13 +856,13 @@ FixedwingAttitudeControl::task_main()
 					/* throttle passed through */
 					_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
 					if (!isfinite(throttle_sp)) {
-						if (loop_counter % 10 == 0) {
+						if (_debug && loop_counter % 10 == 0) {
 							warnx("throttle_sp %.4f", (double)throttle_sp);
 						}
 					}
 				} else {
 					perf_count(_nonfinite_input_perf);
-					if (loop_counter % 10 == 0) {
+					if (_debug && loop_counter % 10 == 0) {
 						warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp);
 					}
 				}

src/modules/mavlink/mavlink_ftp.cpp

@@ -114,7 +114,7 @@ MavlinkFTP::_worker(Request *req)
 	uint32_t messageCRC;
 
 	// basic sanity checks; must validate length before use
-	if ((hdr->magic != kProtocolMagic) || (hdr->size > kMaxDataLength)) {
+	if (hdr->size > kMaxDataLength) {
 		errorCode = kErrNoRequest;
 		goto out;
 	}
@@ -122,6 +122,9 @@ MavlinkFTP::_worker(Request *req)
 	// check request CRC to make sure this is one of ours
 	messageCRC = hdr->crc32;
 	hdr->crc32 = 0;
+	hdr->padding[0] = 0;
+	hdr->padding[1] = 0;
+	hdr->padding[2] = 0;
 	if (crc32(req->rawData(), req->dataSize()) != messageCRC) {
 		errorCode = kErrNoRequest;
 		goto out;
@@ -199,10 +202,13 @@ MavlinkFTP::_reply(Request *req)
 {
 	auto hdr = req->header();
 	
-	hdr->magic = kProtocolMagic;
+	hdr->seqNumber = req->header()->seqNumber + 1;
 
 	// message is assumed to be already constructed in the request buffer, so generate the CRC
 	hdr->crc32 = 0;
+	hdr->padding[0] = 0;
+	hdr->padding[1] = 0;
+	hdr->padding[2] = 0;
 	hdr->crc32 = crc32(req->rawData(), req->dataSize());
 
 	// then pack and send the reply back to the request source

src/modules/mavlink/mavlink_ftp.h

@@ -38,9 +38,6 @@
  *
  * MAVLink remote file server.
  *
- * Messages are wrapped in ENCAPSULATED_DATA messages. Every message includes
- * a session ID and sequence number.
- *
  * A limited number of requests (currently 2) may be outstanding at a time.
  * Additional messages will be discarded.
  *
@@ -74,16 +71,19 @@ private:
 
 	static MavlinkFTP	*_server;
 
+	/// @brief Trying to pack structures across differing compilers is questionable for Clients, so we pad the
+	/// structure ourselves to 32 bit alignment which should get us what we want.
 	struct RequestHeader
-	{
+        {
-		uint8_t		magic;
+		uint16_t	seqNumber;  ///< sequence number for message
-		uint8_t		session;
+		uint8_t		session;    ///< Session id for read and write commands
-		uint8_t		opcode;
+		uint8_t		opcode;     ///< Command opcode
-		uint8_t		size;
+		uint8_t		size;       ///< Size of data
-		uint32_t	crc32;
+		uint8_t		padding[3];
-		uint32_t	offset;
+		uint32_t	crc32;      ///< CRC for entire Request structure, with crc32 and padding set to 0
+		uint32_t	offset;     ///< Offsets for List and Read commands
 		uint8_t		data[];
-	};
+        };
 
 	enum Opcode : uint8_t
 	{
@@ -131,10 +131,11 @@ private:
 		};
 
 		bool		decode(Mavlink *mavlink, mavlink_message_t *fromMessage) {
-			if (fromMessage->msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA) {
+			if (fromMessage->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
+				_systemId = fromMessage->sysid;
 				_mavlink = mavlink;
-				mavlink_msg_encapsulated_data_decode(fromMessage, &_message);
+				mavlink_msg_file_transfer_protocol_decode(fromMessage, &_message);
-				return true;
+				return _message.target_system == _mavlink->get_system_id();
 			}
 			return false;
 		}
@@ -145,8 +146,14 @@ private:
 			// flat memory architecture, as we're operating between threads here.
 			mavlink_message_t msg;
 			msg.checksum = 0;
-			unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(),
+			unsigned len = mavlink_msg_file_transfer_protocol_pack_chan(_mavlink->get_system_id(),		// Sender system id
-				_mavlink->get_channel(), &msg, sequence()+1, rawData());
+										    _mavlink->get_component_id(),	// Sender component id
+										    _mavlink->get_channel(),		// Channel to send on
+										    &msg,				// Message to pack payload into
+										    0,					// Target network
+										    _systemId,				// Target system id
+										    0,					// Target component id
+										    rawData());				// Payload to pack into message
 
 			_mavlink->lockMessageBufferMutex();
 			bool success = _mavlink->message_buffer_write(&msg, len);
@@ -167,26 +174,25 @@ private:
 #endif
 		}
 
-		uint8_t		*rawData() { return &_message.data[0]; }
+		uint8_t		*rawData() { return &_message.payload[0]; }
-		RequestHeader 	*header()  { return reinterpret_cast<RequestHeader *>(&_message.data[0]); }
+		RequestHeader 	*header()  { return reinterpret_cast<RequestHeader *>(&_message.payload[0]); }
 		uint8_t         *requestData() { return &(header()->data[0]); }
 		unsigned	dataSize() { return header()->size + sizeof(RequestHeader); }
-		uint16_t	sequence() const { return _message.seqnr; }
 		mavlink_channel_t channel() { return _mavlink->get_channel(); }
 
 		char		*dataAsCString();
 
 	private:
 		Mavlink			*_mavlink;
-		mavlink_encapsulated_data_t _message;
+		mavlink_file_transfer_protocol_t _message;
+		uint8_t _systemId;
 
 	};
 
-	static const uint8_t	kProtocolMagic = 'f';
 	static const char	kDirentFile = 'F';
 	static const char	kDirentDir = 'D';
 	static const char	kDirentUnknown = 'U';
-	static const uint8_t	kMaxDataLength = MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN - sizeof(RequestHeader);
+	static const uint8_t	kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(RequestHeader);
 
 	/// Request worker; runs on the low-priority work queue to service
 	/// remote requests.

src/modules/mavlink/mavlink_main.cpp

@@ -1396,14 +1396,14 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
 		configure_stream("ATTITUDE_TARGET", 3.0f);
 		configure_stream("DISTANCE_SENSOR", 0.5f);
-		configure_stream("OPTICAL_FLOW", 0.5f);
+		configure_stream("OPTICAL_FLOW", 20.0f);
 		break;
 
 	case MAVLINK_MODE_ONBOARD:
 		configure_stream("SYS_STATUS", 1.0f);
-		configure_stream("ATTITUDE", 15.0f);
+		configure_stream("ATTITUDE", 50.0f);
-		configure_stream("GLOBAL_POSITION_INT", 15.0f);
+		configure_stream("GLOBAL_POSITION_INT", 50.0f);
-		configure_stream("CAMERA_CAPTURE", 1.0f);
+		configure_stream("CAMERA_CAPTURE", 2.0f);
 		break;
 
 	default:

src/modules/mavlink/mavlink_messages.cpp

@@ -774,6 +774,9 @@ private:
 	MavlinkOrbSubscription *_airspeed_sub;
 	uint64_t _airspeed_time;
 
+	MavlinkOrbSubscription *_sensor_combined_sub;
+	uint64_t _sensor_combined_time;
+
 	/* do not allow top copying this class */
 	MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &);
 	MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &);
@@ -789,7 +792,9 @@ protected:
 		_act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))),
 		_act_time(0),
 		_airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))),
-		_airspeed_time(0)
+		_airspeed_time(0),
+		_sensor_combined_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))),
+		_sensor_combined_time(0)
 	{}
 
 	void send(const hrt_abstime t)
@@ -799,12 +804,14 @@ protected:
 		struct actuator_armed_s armed;
 		struct actuator_controls_s act;
 		struct airspeed_s airspeed;
+		struct sensor_combined_s sensor_combined;
 
 		bool updated = _att_sub->update(&_att_time, &att);
 		updated |= _pos_sub->update(&_pos_time, &pos);
 		updated |= _armed_sub->update(&_armed_time, &armed);
 		updated |= _act_sub->update(&_act_time, &act);
 		updated |= _airspeed_sub->update(&_airspeed_time, &airspeed);
+		updated |= _sensor_combined_sub->update(&_sensor_combined_time, &sensor_combined);
 
 		if (updated) {
 			mavlink_vfr_hud_t msg;
@@ -813,7 +820,7 @@ protected:
 			msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e);
 			msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
 			msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f;
-			msg.alt = pos.alt;
+			msg.alt = sensor_combined.baro_alt_meter;
 			msg.climb = -pos.vel_d;
 
 			_mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg);
@@ -879,8 +886,8 @@ protected:
 			msg.eph = cm_uint16_from_m_float(gps.eph);
 			msg.epv = cm_uint16_from_m_float(gps.epv);
 			msg.vel = cm_uint16_from_m_float(gps.vel_m_s),
-		    msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
+			msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
-		    msg.satellites_visible = gps.satellites_used;
+			msg.satellites_visible = gps.satellites_used;
 
 			_mavlink->send_message(MAVLINK_MSG_ID_GPS_RAW_INT, &msg);
 		}
@@ -950,11 +957,11 @@ protected:
 			msg.lat = pos.lat * 1e7;
 			msg.lon = pos.lon * 1e7;
 			msg.alt = pos.alt * 1000.0f;
-		    msg.relative_alt = (pos.alt - home.alt) * 1000.0f;
+			msg.relative_alt = (pos.alt - home.alt) * 1000.0f;
-		    msg.vx = pos.vel_n * 100.0f;
+			msg.vx = pos.vel_n * 100.0f;
-		    msg.vy = pos.vel_e * 100.0f;
+			msg.vy = pos.vel_e * 100.0f;
-		    msg.vz = pos.vel_d * 100.0f;
+			msg.vz = pos.vel_d * 100.0f;
-		    msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f;
+			msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f;
 
 			_mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, &msg);
 		}
@@ -1015,9 +1022,9 @@ protected:
 			msg.x = pos.x;
 			msg.y = pos.y;
 			msg.z = pos.z;
-		    msg.vx = pos.vx;
+			msg.vx = pos.vx;
-		    msg.vy = pos.vy;
+			msg.vy = pos.vy;
-		    msg.vz = pos.vz;
+			msg.vz = pos.vz;
 
 			_mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED, &msg);
 		}
@@ -1078,9 +1085,9 @@ protected:
 			msg.x = pos.x;
 			msg.y = pos.y;
 			msg.z = pos.z;
-		    msg.roll = pos.roll;
+			msg.roll = pos.roll;
-		    msg.pitch = pos.pitch;
+			msg.pitch = pos.pitch;
-		    msg.yaw = pos.yaw;
+			msg.yaw = pos.yaw;
 
 			_mavlink->send_message(MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, &msg);
 		}

src/modules/mavlink/mavlink_receiver.cpp

@@ -172,7 +172,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_request_data_stream(msg);
 		break;
 
-	case MAVLINK_MSG_ID_ENCAPSULATED_DATA:
+	case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL:
 		MavlinkFTP::getServer()->handle_message(_mavlink, msg);
 		break;
 

src/modules/px4iofirmware/registers.c

@@ -285,7 +285,9 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 		while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
 
 			/* XXX range-check value? */
-			r_page_servos[offset] = *values;
+			if (*values != PWM_IGNORE_THIS_CHANNEL) {
+				r_page_servos[offset] = *values;
+			}
 
 			offset++;
 			num_values--;

src/modules/sdlog2/sdlog2.c

@@ -76,6 +76,7 @@
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/satellite_info.h>
 #include <uORB/topics/vehicle_vicon_position.h>
+#include <uORB/topics/vision_position_estimate.h>
 #include <uORB/topics/vehicle_global_velocity_setpoint.h>
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/battery_status.h>
@@ -934,6 +935,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct vehicle_global_position_s global_pos;
 		struct position_setpoint_triplet_s triplet;
 		struct vehicle_vicon_position_s vicon_pos;
+		struct vision_position_estimate vision_pos;
 		struct optical_flow_s flow;
 		struct rc_channels_s rc;
 		struct differential_pressure_s diff_pres;
@@ -984,6 +986,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_EST1_s log_EST1;
 			struct log_PWR_s log_PWR;
 			struct log_VICN_s log_VICN;
+			struct log_VISN_s log_VISN;
 			struct log_GS0A_s log_GS0A;
 			struct log_GS0B_s log_GS0B;
 			struct log_GS1A_s log_GS1A;
@@ -1013,6 +1016,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int gps_pos_sub;
 		int sat_info_sub;
 		int vicon_pos_sub;
+		int vision_pos_sub;
 		int flow_sub;
 		int rc_sub;
 		int airspeed_sub;
@@ -1043,6 +1047,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
 	subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
+	subs.vision_pos_sub = orb_subscribe(ORB_ID(vision_position_estimate));
 	subs.flow_sub = orb_subscribe(ORB_ID(optical_flow));
 	subs.rc_sub = orb_subscribe(ORB_ID(rc_channels));
 	subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
@@ -1459,6 +1464,22 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_VICN.yaw = buf.vicon_pos.yaw;
 			LOGBUFFER_WRITE_AND_COUNT(VICN);
 		}
+		
+		/* --- VISION POSITION --- */
+		if (copy_if_updated(ORB_ID(vision_position_estimate), subs.vision_pos_sub, &buf.vision_pos)) {
+			log_msg.msg_type = LOG_VISN_MSG;
+			log_msg.body.log_VISN.x = buf.vision_pos.x;
+			log_msg.body.log_VISN.y = buf.vision_pos.y;
+			log_msg.body.log_VISN.z = buf.vision_pos.z;
+			log_msg.body.log_VISN.vx = buf.vision_pos.vx;
+			log_msg.body.log_VISN.vy = buf.vision_pos.vy;
+			log_msg.body.log_VISN.vz = buf.vision_pos.vz;
+			log_msg.body.log_VISN.qx = buf.vision_pos.q[0];
+			log_msg.body.log_VISN.qy = buf.vision_pos.q[1];
+			log_msg.body.log_VISN.qz = buf.vision_pos.q[2];
+			log_msg.body.log_VISN.qw = buf.vision_pos.q[3];
+			LOGBUFFER_WRITE_AND_COUNT(VISN);
+		}
 
 		/* --- FLOW --- */
 		if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) {

src/modules/sdlog2/sdlog2_messages.h

@@ -391,6 +391,20 @@ struct log_TEL_s {
 	uint64_t heartbeat_time;
 };
 
+/* --- VISN - VISION POSITION --- */
+#define LOG_VISN_MSG 38
+struct log_VISN_s {
+	float x;
+	float y;
+	float z;
+	float vx;
+	float vy;
+	float vz;
+	float qx;
+	float qy;
+	float qz;
+	float qw;
+};
 
 /********** SYSTEM MESSAGES, ID > 0x80 **********/
 
@@ -449,6 +463,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(EST1, "ffffffffffffffff",	"s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"),
 	LOG_FORMAT(PWR, "fffBBBBB",		"Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
 	LOG_FORMAT(VICN, "ffffff",		"X,Y,Z,Roll,Pitch,Yaw"),
+	LOG_FORMAT(VISN, "ffffffffff",		"X,Y,Z,VX,VY,VZ,QuatX,QuatY,QuatZ,QuatW"),
 	LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),

src/modules/sensors/sensors.cpp

@@ -1229,16 +1229,18 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
 	if (updated) {
 		orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres);
 
-		raw.differential_pressure_pa = _diff_pres.differential_pressure_pa;
+		raw.differential_pressure_pa = _diff_pres.differential_pressure_raw_pa;
 		raw.differential_pressure_timestamp = _diff_pres.timestamp;
 		raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa;
 
 		float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
 
 		_airspeed.timestamp = _diff_pres.timestamp;
-		_airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa);
+
-		_airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f,
+		/* don't risk to feed negative airspeed into the system */
-					      raw.baro_pres_mbar * 1e2f, air_temperature_celsius);
+		_airspeed.indicated_airspeed_m_s = math::max(0.0f, calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa));
+		_airspeed.true_airspeed_m_s = math::max(0.0f, calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f,
+					      raw.baro_pres_mbar * 1e2f, air_temperature_celsius));
 		_airspeed.air_temperature_celsius = air_temperature_celsius;
 
 		/* announce the airspeed if needed, just publish else */
@@ -1457,12 +1459,10 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 					if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0.0f)) {
 
 						float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa;
-						float diff_pres_pa = (diff_pres_pa_raw > 0.0f) ? diff_pres_pa_raw : 0.0f;
 
 						_diff_pres.timestamp = t;
-						_diff_pres.differential_pressure_pa = diff_pres_pa;
 						_diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw;
-						_diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa * 0.1f);
+						_diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa_raw * 0.1f);
 						_diff_pres.temperature = -1000.0f;
 
 						/* announce the airspeed if needed, just publish else */

src/modules/uORB/topics/differential_pressure.h

@@ -54,7 +54,6 @@
 struct differential_pressure_s {
 	uint64_t	timestamp;			/**< Microseconds since system boot, needed to integrate */
 	uint64_t	error_count;			/**< Number of errors detected by driver */
-	float	differential_pressure_pa;		/**< Differential pressure reading */
 	float	differential_pressure_raw_pa;		/**< Raw differential pressure reading (may be negative) */
 	float	differential_pressure_filtered_pa;	/**< Low pass filtered differential pressure reading */
 	float	max_differential_pressure_pa;		/**< Maximum differential pressure reading */

src/modules/uORB/topics/vehicle_command.h

@@ -1,9 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012-2014 PX4 Development Team. All rights reserved.
- *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
- *           @author Julian Oes <joes@student.ethz.ch>
- *           @author Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -37,6 +34,10 @@
 /**
  * @file vehicle_command.h
  * Definition of the vehicle command uORB topic.
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
 #ifndef TOPIC_VEHICLE_COMMAND_H_
@@ -52,6 +53,9 @@
  * but can contain additional ones.
  */
 enum VEHICLE_CMD {
+	VEHICLE_CMD_CUSTOM_0 = 0, /* test command */
+	VEHICLE_CMD_CUSTOM_1 = 1, /* test command */
+	VEHICLE_CMD_CUSTOM_2 = 2, /* test command */
 	VEHICLE_CMD_NAV_WAYPOINT = 16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude|  */
 	VEHICLE_CMD_NAV_LOITER_UNLIM = 17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude|  */
 	VEHICLE_CMD_NAV_LOITER_TURNS = 18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude|  */
@@ -87,7 +91,8 @@ enum VEHICLE_CMD {
 	VEHICLE_CMD_MISSION_START = 300, /* start running a mission |first_item: the first mission item to run| last_item:  the last mission item to run (after this item is run, the mission ends)|  */
 	VEHICLE_CMD_COMPONENT_ARM_DISARM = 400, /* Arms / Disarms a component |1 to arm, 0 to disarm|  */
 	VEHICLE_CMD_START_RX_PAIR = 500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX|  */
-	VEHICLE_CMD_ENUM_END = 501, /*  | */
+	VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001, /**< Prepare a payload deployment in the flight plan */
+	VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 /**< Control a pre-programmed payload deployment */
 };
 
 /**
@@ -115,8 +120,8 @@ struct vehicle_command_s {
 	float param2;			/**< Parameter 2, as defined by MAVLink VEHICLE_CMD enum.   */
 	float param3;			/**< Parameter 3, as defined by MAVLink VEHICLE_CMD enum.   */
 	float param4;			/**< Parameter 4, as defined by MAVLink VEHICLE_CMD enum.   */
-	float param5;			/**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum.   */
+	double param5;			/**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum.   */
-	float param6;			/**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum.   */
+	double param6;			/**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum.   */
 	float param7;			/**< Parameter 7, as defined by MAVLink VEHICLE_CMD enum.   */
 	enum VEHICLE_CMD command;		/**< Command ID, as defined MAVLink by VEHICLE_CMD enum.   */
 	uint8_t target_system;		/**< System which should execute the command  */

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
 
-SRCS += uavcan_main.cpp         \
+# Main
-        uavcan_clock.cpp        \
+SRCS += uavcan_main.cpp              \
-        esc_controller.cpp      \
+        uavcan_clock.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,74 @@
  ****************************************************************************/
 
 /**
- * @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;
+	int res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb));
-
-	// GNSS fix subscription
-	res = _uavcan_sub_status.start(FixCbBinder(this, &UavcanGnssReceiver::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
 {
-	_report.timestamp_position = hrt_absolute_time();
+	return (_receiver_node_id < 0) ? 0 : 1;
-	_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;
+void UavcanGnssBridge::print_status() const
+{
+	printf("RX errors: %d, receiver node id: ", _sub_fix.getFailureCount());
+	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)
+{
+	// 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;
 
 
 	// Check if the msg contains valid covariance information
@@ -90,19 +112,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.eph = -1.0F;
-		_report.epv = -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 +140,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.s_variance_m_s = -1.0F;
-		_report.c_variance_rad = -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.timestamp_velocity = report.timestamp_position;
-	_report.vel_n_m_s = msg.ned_velocity[0];
+	report.vel_n_m_s = msg.ned_velocity[0];
-	_report.vel_e_m_s = msg.ned_velocity[1];
+	report.vel_e_m_s = msg.ned_velocity[1];
-	_report.vel_d_m_s = msg.ned_velocity[2];
+	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.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.cog_rad = atan2f(report.vel_e_m_s, report.vel_n_m_s);
-	_report.vel_ned_valid = true;
+	report.vel_ned_valid = true;
 
-	_report.timestamp_time = _report.timestamp_position;
+	report.timestamp_time = report.timestamp_position;
-	_report.time_gps_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec();	// Convert to microseconds
+	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,22 @@
 #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;
 
-	int init();
+	UavcanGnssBridge(uavcan::INode& node);
+
+	const char *get_name() const override { return NAME; }
+
+	int init() override;
+
+	unsigned get_num_redundant_channels() const override;
+
+	void print_status() const override;
 
 private:
 	/**
@@ -64,21 +74,14 @@ private:
 	 */
 	void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg);
 
-
+	typedef uavcan::MethodBinder<UavcanGnssBridge*,
-	typedef uavcan::MethodBinder<UavcanGnssReceiver*,
+		void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>&)>
-		void (UavcanGnssReceiver::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>&)>
 		FixCbBinder;
 
-	/*
+	uavcan::INode &_node;
-	 * libuavcan related things
+	uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder> _sub_fix;
-	 */
+	int _receiver_node_id = -1;
-	uavcan::INode													&_node;
-	uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder>	_uavcan_sub_status;
 
-	/*
+	orb_advert_t _report_pub;                ///< uORB pub for gnss position
-	 * 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,158 @@
+/****************************************************************************
+ *
+ *   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 [] _orb_topics;
+	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
+		}
+
+		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;
+			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->node_id        = node_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->node_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].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/modules/uavcan/sensors/sensor_bridge.hpp

@@ -0,0 +1,131 @@
+/****************************************************************************
+ *
+ *   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;
+
+	/**
+	 * Prints current status in a human readable format to stdout.
+	 */
+	virtual void print_status() 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 node_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 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/modules/uavcan/uavcan_main.cpp

@@ -38,8 +38,10 @@
 #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 <systemlib/scheduling_priorities.h>
 #include <version/version.h>
 #include <arch/board/board.h>
 #include <arch/chip/chip.h>
@@ -65,16 +67,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),
+	_node_mutex(),
-	_gnss_receiver(_node)
+	_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));
+	const int res = pthread_mutex_init(&_node_mutex, nullptr);
-	// memset(_poll_fds, 0, sizeof(_poll_fds));
+	if (res < 0) {
+		std::abort();
+	}
 }
 
 UavcanNode::~UavcanNode()
@@ -99,10 +103,18 @@ UavcanNode::~UavcanNode()
 	}
 
 	/* clean up the alternate device node */
-		// unregister_driver(PWM_OUTPUT_DEVICE_PATH);
+	// unregister_driver(PWM_OUTPUT_DEVICE_PATH);
 
 	::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;
 }
 
@@ -164,7 +176,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
 	 * Start the task. Normally it should never exit.
 	 */
 	static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();};
-	_instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, StackSize,
+	_instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize,
 			      static_cast<main_t>(run_trampoline), nullptr);
 
 	if (_instance->_task < 0) {
@@ -214,11 +226,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 +239,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();
+	// Sensor bridges
-	if (ret < 0)
+	IUavcanSensorBridge::make_all(_node, _sensor_bridges);
-		return ret;
+	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 +271,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 +316,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 +382,6 @@ int UavcanNode::run()
 				// Output to the bus
 				_esc_controller.update_outputs(outputs.output, outputs.noutputs);
 			}
-
 		}
 
 		// Check arming state
@@ -376,10 +405,7 @@ int UavcanNode::run()
 }
 
 int
-UavcanNode::control_callback(uintptr_t handle,
+UavcanNode::control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input)
-			 uint8_t control_group,
-			 uint8_t control_index,
-			 float &input)
 {
 	const actuator_controls_s *controls = (actuator_controls_s *)handle;
 
@@ -520,8 +546,23 @@ 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);
+	(void)pthread_mutex_lock(&_node_mutex);
-	warnx("mixer: %s", (_mixers == nullptr) ? "NONE" : "OK");
+
+	// ESC mixer status
+	printf("ESC actuators control groups: sub: %u / req: %u / fds: %u\n",
+	       (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
+	printf("ESC mixer: %s\n", (_mixers == nullptr) ? "NONE" : "OK");
+
+	// Sensor bridges
+	auto br = _sensor_bridges.getHead();
+	while (br != nullptr) {
+		printf("Sensor '%s':\n", br->get_name());
+		br->print_status();
+		printf("\n");
+		br = br->getSibling();
+	}
+
+	(void)pthread_mutex_unlock(&_node_mutex);
 }
 
 /*
@@ -529,79 +570,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) {
+		if (UavcanNode::instance()) {
-			print_usage();
+			errx(1, "already started");
-			::exit(1);
 		}
 
-		/*
+		// Node ID
-		 * Node ID
+		int32_t node_id = 0;
-		 */
+		(void)param_get(param_find("UAVCAN_NODE_ID"), &node_id);
-		const int node_id = atoi(argv[2]);
 
 		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
-		 * CAN bitrate
+		int32_t bitrate = 0;
-		 */
+		(void)param_get(param_find("UAVCAN_BITRATE"), &bitrate);
-		unsigned bitrate = 0;
 
-		if (argc > 3) {
+		// Start
-			bitrate = atol(argv[3]);
-		}
-
-		if (bitrate <= 0) {
-			bitrate = DEFAULT_CAN_BITRATE;
-		}
-
-		if (UavcanNode::instance()) {
-			errx(1, "already started");
-		}
-
-		/*
-		 * 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();
-			inst->print_info();
+		::exit(0);
-			return OK;
 	}
 
 	if (!std::strcmp(argv[1], "stop")) {
-		
+		delete inst;
-			delete inst;
+		::exit(0);
-			inst = nullptr;
-			return OK;
 	}
 
 	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 "actuators/esc.hpp"
-#include "gnss_receiver.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,
+	// TODO: move the actuator mixing stuff into the ESC controller class
-					 uint8_t control_group,
+	static int	control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input);
-					 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);
+
+
+