Merge remote-tracking branch 'upstream/master' into dev_ros

Conflicts:
	src/lib/mathlib/math/Matrix.hpp
	src/modules/mc_att_control/mc_att_control_main.cpp
	src/modules/uORB/topics/vehicle_status.h
	src/platforms/px4_includes.h

src/drivers/batt_smbus/batt_smbus.cpp

@@ -0,0 +1,585 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file batt_smbus.cpp
+ *
+ * Driver for a battery monitor connected via SMBus (I2C).
+ *
+ * @author Randy Mackay <rmackay9@yahoo.com>
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <sched.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+#include <ctype.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+
+#include <board_config.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/subsystem_info.h>
+#include <uORB/topics/battery_status.h>
+
+#include <float.h>
+
+#include <drivers/device/i2c.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_batt_smbus.h>
+#include <drivers/device/ringbuffer.h>
+
+#define BATT_SMBUS_ADDR_MIN			0x08	///< lowest possible address
+#define BATT_SMBUS_ADDR_MAX			0x7F	///< highest possible address
+
+#define BATT_SMBUS_I2C_BUS			PX4_I2C_BUS_EXPANSION
+#define BATT_SMBUS_ADDR				0x0B	///< I2C address
+#define BATT_SMBUS_TEMP				0x08	///< temperature register
+#define BATT_SMBUS_VOLTAGE			0x09	///< voltage register
+#define BATT_SMBUS_DESIGN_CAPACITY		0x18	///< design capacity register
+#define BATT_SMBUS_DESIGN_VOLTAGE		0x19	///< design voltage register
+#define BATT_SMBUS_SERIALNUM			0x1c	///< serial number register
+#define BATT_SMBUS_MANUFACTURE_NAME		0x20	///< manufacturer name
+#define BATT_SMBUS_MANUFACTURE_INFO		0x25	///< cell voltage register
+#define BATT_SMBUS_CURRENT			0x2a	///< current register
+#define BATT_SMBUS_MEASUREMENT_INTERVAL_MS	(1000000 / 10)	///< time in microseconds, measure at 10hz
+
+#define BATT_SMBUS_PEC_POLYNOMIAL		0x07	///< Polynomial for calculating PEC
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+class BATT_SMBUS : public device::I2C
+{
+public:
+	BATT_SMBUS(int bus = PX4_I2C_BUS_EXPANSION, uint16_t batt_smbus_addr = BATT_SMBUS_ADDR);
+	virtual ~BATT_SMBUS();
+
+	/**
+	 * Initialize device
+	 *
+	 * Calls probe() to check for device on bus.
+	 *
+	 * @return 0 on success, error code on failure
+	 */
+	virtual int		init();
+
+	/**
+	 * Test device
+	 *
+	 * @return 0 on success, error code on failure
+	 */
+	virtual int		test();
+
+	/**
+	 * Search all possible slave addresses for a smart battery
+	 */
+	int			search();
+
+protected:
+	/**
+	 * Check if the device can be contacted
+	 */
+	virtual int		probe();
+
+private:
+
+	/**
+	 * Start periodic reads from the battery
+	 */
+	void			start();
+
+	/**
+	 * Stop periodic reads from the battery
+	 */
+	void			stop();
+
+	/**
+	 * static function that is called by worker queue
+	 */
+	static void		cycle_trampoline(void *arg);
+
+	/**
+	 * perform a read from the battery
+	 */
+	void			cycle();
+
+	/**
+	 * Read a word from specified register
+	 */
+	int			read_reg(uint8_t reg, uint16_t &val);
+
+	/**
+	 * Read block from bus
+	 * @return returns number of characters read if successful, zero if unsuccessful
+	 */
+	uint8_t			read_block(uint8_t reg, uint8_t *data, uint8_t max_len, bool append_zero);
+
+	/**
+	 * Calculate PEC for a read or write from the battery
+	 * @param buff is the data that was read or will be written
+	 */
+	uint8_t			get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const;
+
+	// internal variables
+	work_s			_work;		///< work queue for scheduling reads
+	RingBuffer		*_reports;	///< buffer of recorded voltages, currents
+	struct battery_status_s _last_report;	///< last published report, used for test()
+	orb_advert_t		_batt_topic;	///< uORB battery topic
+	orb_id_t		_batt_orb_id;	///< uORB battery topic ID
+};
+
+namespace
+{
+BATT_SMBUS *g_batt_smbus;	///< device handle. For now, we only support one BATT_SMBUS device
+}
+
+void batt_smbus_usage();
+
+extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]);
+
+BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) :
+	I2C("batt_smbus", BATT_SMBUS_DEVICE_PATH, bus, batt_smbus_addr, 400000),
+	_work{},
+	_reports(nullptr),
+	_batt_topic(-1),
+	_batt_orb_id(nullptr)
+{
+	// work_cancel in the dtor will explode if we don't do this...
+	memset(&_work, 0, sizeof(_work));
+}
+
+BATT_SMBUS::~BATT_SMBUS()
+{
+	// make sure we are truly inactive
+	stop();
+
+	if (_reports != nullptr) {
+		delete _reports;
+	}
+}
+
+int
+BATT_SMBUS::init()
+{
+	int ret = ENOTTY;
+
+	// attempt to initialise I2C bus
+	ret = I2C::init();
+
+	if (ret != OK) {
+		errx(1, "failed to init I2C");
+		return ret;
+
+	} else {
+		// allocate basic report buffers
+		_reports = new RingBuffer(2, sizeof(struct battery_status_s));
+
+		if (_reports == nullptr) {
+			ret = ENOTTY;
+
+		} else {
+			// start work queue
+			start();
+		}
+	}
+
+	// init orb id
+	_batt_orb_id = ORB_ID(battery_status);
+
+	return ret;
+}
+
+int
+BATT_SMBUS::test()
+{
+	int sub = orb_subscribe(ORB_ID(battery_status));
+	bool updated = false;
+	struct battery_status_s status;
+	uint64_t start_time = hrt_absolute_time();
+
+	// loop for 5 seconds
+	while ((hrt_absolute_time() - start_time) < 5000000) {
+
+		// display new info that has arrived from the orb
+		orb_check(sub, &updated);
+
+		if (updated) {
+			if (orb_copy(ORB_ID(battery_status), sub, &status) == OK) {
+				warnx("V=%4.2f C=%4.2f", status.voltage_v, status.current_a);
+			}
+		}
+
+		// sleep for 0.05 seconds
+		usleep(50000);
+	}
+
+	return OK;
+}
+
+int
+BATT_SMBUS::search()
+{
+	bool found_slave = false;
+	uint16_t tmp;
+
+	// search through all valid SMBus addresses
+	for (uint8_t i = BATT_SMBUS_ADDR_MIN; i <= BATT_SMBUS_ADDR_MAX; i++) {
+		set_address(i);
+
+		if (read_reg(BATT_SMBUS_VOLTAGE, tmp) == OK) {
+			warnx("battery found at 0x%x", (int)i);
+			found_slave = true;
+		}
+
+		// short sleep
+		usleep(1);
+	}
+
+	// display completion message
+	if (found_slave) {
+		warnx("Done.");
+
+	} else {
+		warnx("No smart batteries found.");
+	}
+
+	return OK;
+}
+
+int
+BATT_SMBUS::probe()
+{
+	// always return OK to ensure device starts
+	return OK;
+}
+
+void
+BATT_SMBUS::start()
+{
+	// reset the report ring and state machine
+	_reports->flush();
+
+	// schedule a cycle to start things
+	work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this, 1);
+}
+
+void
+BATT_SMBUS::stop()
+{
+	work_cancel(HPWORK, &_work);
+}
+
+void
+BATT_SMBUS::cycle_trampoline(void *arg)
+{
+	BATT_SMBUS *dev = (BATT_SMBUS *)arg;
+
+	dev->cycle();
+}
+
+void
+BATT_SMBUS::cycle()
+{
+	// read data from sensor
+	struct battery_status_s new_report;
+
+	// set time of reading
+	new_report.timestamp = hrt_absolute_time();
+
+	// read voltage
+	uint16_t tmp;
+
+	if (read_reg(BATT_SMBUS_VOLTAGE, tmp) == OK) {
+		// initialise new_report
+		memset(&new_report, 0, sizeof(new_report));
+
+		// convert millivolts to volts
+		new_report.voltage_v = ((float)tmp) / 1000.0f;
+
+		// read current
+		usleep(1);
+		uint8_t buff[4];
+
+		if (read_block(BATT_SMBUS_CURRENT, buff, 4, false) == 4) {
+			new_report.current_a = (float)((int32_t)((uint32_t)buff[3] << 24 | (uint32_t)buff[2] << 16 | (uint32_t)buff[1] << 8 |
+						       (uint32_t)buff[0])) / 1000.0f;
+		}
+
+		// publish to orb
+		if (_batt_topic != -1) {
+			orb_publish(_batt_orb_id, _batt_topic, &new_report);
+
+		} else {
+			_batt_topic = orb_advertise(_batt_orb_id, &new_report);
+
+			if (_batt_topic < 0) {
+				errx(1, "ADVERT FAIL");
+			}
+		}
+
+		// copy report for test()
+		_last_report = new_report;
+
+		// post a report to the ring
+		_reports->force(&new_report);
+
+		// notify anyone waiting for data
+		poll_notify(POLLIN);
+	}
+
+	// schedule a fresh cycle call when the measurement is done
+	work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this,
+		   USEC2TICK(BATT_SMBUS_MEASUREMENT_INTERVAL_MS));
+}
+
+int
+BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val)
+{
+	uint8_t buff[3];	// 2 bytes of data + PEC
+
+	// read from register
+	int ret = transfer(&reg, 1, buff, 3);
+
+	if (ret == OK) {
+		// check PEC
+		uint8_t pec = get_PEC(reg, true, buff, 2);
+
+		if (pec == buff[2]) {
+			val = (uint16_t)buff[1] << 8 | (uint16_t)buff[0];
+
+		} else {
+			ret = ENOTTY;
+		}
+	}
+
+	// return success or failure
+	return ret;
+}
+
+uint8_t
+BATT_SMBUS::read_block(uint8_t reg, uint8_t *data, uint8_t max_len, bool append_zero)
+{
+	uint8_t buff[max_len + 2];  // buffer to hold results
+
+	usleep(1);
+
+	// read bytes including PEC
+	int ret = transfer(&reg, 1, buff, max_len + 2);
+
+	// return zero on failure
+	if (ret != OK) {
+		return 0;
+	}
+
+	// get length
+	uint8_t bufflen = buff[0];
+
+	// sanity check length returned by smbus
+	if (bufflen == 0 || bufflen > max_len) {
+		return 0;
+	}
+
+	// check PEC
+	uint8_t pec = get_PEC(reg, true, buff, bufflen + 1);
+
+	if (pec != buff[bufflen + 1]) {
+		// debug
+		warnx("CurrPEC:%x vs MyPec:%x", (int)buff[bufflen + 1], (int)pec);
+		return 0;
+
+	}
+
+	// copy data
+	memcpy(data, &buff[1], bufflen);
+
+	// optionally add zero to end
+	if (append_zero) {
+		data[bufflen] = '\0';
+	}
+
+	// return success
+	return bufflen;
+}
+
+uint8_t
+BATT_SMBUS::get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const
+{
+	// exit immediately if no data
+	if (len <= 0) {
+		return 0;
+	}
+
+	// prepare temp buffer for calcing crc
+	uint8_t tmp_buff[len + 3];
+	tmp_buff[0] = (uint8_t)get_address() << 1;
+	tmp_buff[1] = cmd;
+	tmp_buff[2] = tmp_buff[0] | (uint8_t)reading;
+	memcpy(&tmp_buff[3], buff, len);
+
+	// initialise crc to zero
+	uint8_t crc = 0;
+	uint8_t shift_reg = 0;
+	bool do_invert;
+
+	// for each byte in the stream
+	for (uint8_t i = 0; i < sizeof(tmp_buff); i++) {
+		// load next data byte into the shift register
+		shift_reg = tmp_buff[i];
+
+		// for each bit in the current byte
+		for (uint8_t j = 0; j < 8; j++) {
+			do_invert = (crc ^ shift_reg) & 0x80;
+			crc <<= 1;
+			shift_reg <<= 1;
+
+			if (do_invert) {
+				crc ^= BATT_SMBUS_PEC_POLYNOMIAL;
+			}
+		}
+	}
+
+	// return result
+	return crc;
+}
+
+///////////////////////// shell functions ///////////////////////
+
+void
+batt_smbus_usage()
+{
+	warnx("missing command: try 'start', 'test', 'stop', 'search'");
+	warnx("options:");
+	warnx("    -b i2cbus (%d)", BATT_SMBUS_I2C_BUS);
+	warnx("    -a addr (0x%x)", BATT_SMBUS_ADDR);
+}
+
+int
+batt_smbus_main(int argc, char *argv[])
+{
+	int i2cdevice = BATT_SMBUS_I2C_BUS;
+	int batt_smbusadr = BATT_SMBUS_ADDR; // 7bit address
+
+	int ch;
+
+	// jump over start/off/etc and look at options first
+	while ((ch = getopt(argc, argv, "a:b:")) != EOF) {
+		switch (ch) {
+		case 'a':
+			batt_smbusadr = strtol(optarg, NULL, 0);
+			break;
+
+		case 'b':
+			i2cdevice = strtol(optarg, NULL, 0);
+			break;
+
+		default:
+			batt_smbus_usage();
+			exit(0);
+		}
+	}
+
+	if (optind >= argc) {
+		batt_smbus_usage();
+		exit(1);
+	}
+
+	const char *verb = argv[optind];
+
+	if (!strcmp(verb, "start")) {
+		if (g_batt_smbus != nullptr) {
+			errx(1, "already started");
+
+		} else {
+			// create new global object
+			g_batt_smbus = new BATT_SMBUS(i2cdevice, batt_smbusadr);
+
+			if (g_batt_smbus == nullptr) {
+				errx(1, "new failed");
+			}
+
+			if (OK != g_batt_smbus->init()) {
+				delete g_batt_smbus;
+				g_batt_smbus = nullptr;
+				errx(1, "init failed");
+			}
+		}
+
+		exit(0);
+	}
+
+	// need the driver past this point
+	if (g_batt_smbus == nullptr) {
+		warnx("not started");
+		batt_smbus_usage();
+		exit(1);
+	}
+
+	if (!strcmp(verb, "test")) {
+		g_batt_smbus->test();
+		exit(0);
+	}
+
+	if (!strcmp(verb, "stop")) {
+		delete g_batt_smbus;
+		g_batt_smbus = nullptr;
+		exit(0);
+	}
+
+	if (!strcmp(verb, "search")) {
+		g_batt_smbus->search();
+		exit(0);
+	}
+
+	batt_smbus_usage();
+	exit(0);
+}

src/drivers/batt_smbus/module.mk

@@ -0,0 +1,8 @@
+#
+# driver for SMBus smart batteries
+#
+
+MODULE_COMMAND	 = batt_smbus
+SRCS		 = batt_smbus.cpp
+
+MAXOPTIMIZATION	 = -Os

src/drivers/device/device.h

@@ -445,6 +445,13 @@ protected:
 	 */
 	virtual int unregister_class_devname(const char *class_devname, unsigned class_instance);
 
+	/**
+	 * Get the device name.
+	 *
+	 * @return the file system string of the device handle
+	 */
+	const char*	get_devname() { return _devname; }
+
 	bool		_pub_blocked;		/**< true if publishing should be blocked */
 
 private:

src/drivers/device/i2c.h

@@ -58,7 +58,7 @@ public:
 	/**
 	 * Get the address
 	 */
-	int16_t		get_address() { return _address; }
+	int16_t		get_address() const { return _address; }
 	
 protected:
 	/**
@@ -132,6 +132,7 @@ protected:
 	 */
 	void		set_address(uint16_t address) {
 		_address = address;
+		_device_id.devid_s.address = _address;
 	}
 
 private:

src/drivers/drv_baro.h

@@ -65,6 +65,7 @@ struct baro_report {
  */
 ORB_DECLARE(sensor_baro0);
 ORB_DECLARE(sensor_baro1);
+ORB_DECLARE(sensor_baro2);
 
 /*
  * ioctl() definitions

src/drivers/drv_batt_smbus.h

@@ -0,0 +1,47 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file drv_batt_smbus.h
+ *
+ * SMBus battery monitor device API
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+#include "drv_orb_dev.h"
+
+/* device path */
+#define BATT_SMBUS_DEVICE_PATH "/dev/batt_smbus"

src/drivers/drv_px4flow.h

@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file Rangefinder driver interface.
+ * @file PX4Flow driver interface.
  */
 
 #ifndef _DRV_PX4FLOW_H

src/drivers/drv_sensor.h

@@ -82,8 +82,19 @@
 #define SENSORIOCGQUEUEDEPTH	_SENSORIOC(3)
 
 /**
- * Reset the sensor to its default configuration.
+ * Reset the sensor to its default configuration
  */
 #define SENSORIOCRESET		_SENSORIOC(4)
 
-#endif /* _DRV_SENSOR_H */
+/**
+ * Set the sensor orientation
+ */
+#define SENSORIOCSROTATION	_SENSORIOC(5)
+
+/**
+ * Get the sensor orientation
+ */
+#define SENSORIOCGROTATION	_SENSORIOC(6)
+
+#endif /* _DRV_SENSOR_H */
+

src/drivers/frsky_telemetry/frsky_data.c

@@ -223,7 +223,7 @@ void frsky_send_frame2(int uart)
 	char lat_ns = 0, lon_ew = 0;
 	int sec = 0;
 	if (global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) {
-		time_t time_gps = global_pos.time_gps_usec / 1000000;
+		time_t time_gps = global_pos.time_utc_usec / 1000000ULL;
 		struct tm *tm_gps = gmtime(&time_gps);
 
 		course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f;
@@ -274,7 +274,7 @@ void frsky_send_frame3(int uart)
 	orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos);
 
 	/* send formatted frame */
-	time_t time_gps = global_pos.time_gps_usec / 1000000;
+	time_t time_gps = global_pos.time_utc_usec / 1000000ULL;
 	struct tm *tm_gps = gmtime(&time_gps);
 	uint16_t hour_min = (tm_gps->tm_min << 8) | (tm_gps->tm_hour & 0xff);
 	frsky_send_data(uart, FRSKY_ID_GPS_DAY_MONTH, tm_gps->tm_mday);

src/drivers/gps/ashtech.cpp

@@ -38,7 +38,7 @@ ASHTECH::~ASHTECH()
 int ASHTECH::handle_message(int len)
 {
 	char * endp;
-	
+
 	if (len < 7) { return 0; }
 
 	int uiCalcComma = 0;
@@ -99,8 +99,26 @@ int ASHTECH::handle_message(int len)
 		timeinfo.tm_sec = int(ashtech_sec);
 		time_t epoch = mktime(&timeinfo);
 
-		_gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
-		_gps_position->time_gps_usec += (uint64_t)((ashtech_sec - int(ashtech_sec)) * 1e6);
+		if (epoch > GPS_EPOCH_SECS) {
+			uint64_t usecs = static_cast<uint64_t>((ashtech_sec - static_cast<uint64_t>(ashtech_sec))) * 1e6;
+
+			// FMUv2+ boards have a hardware RTC, but GPS helps us to configure it
+			// and control its drift. Since we rely on the HRT for our monotonic
+			// clock, updating it from time to time is safe.
+
+			timespec ts;
+			ts.tv_sec = epoch;
+			ts.tv_nsec = usecs * 1000;
+			if (clock_settime(CLOCK_REALTIME, &ts)) {
+				warn("failed setting clock");
+			}
+
+			_gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL;
+			_gps_position->time_utc_usec += usecs;
+		} else {
+			_gps_position->time_utc_usec = 0;
+		}
+
 		_gps_position->timestamp_time = hrt_absolute_time();
 	}
 
@@ -611,8 +629,8 @@ void ASHTECH::decode_init(void)
 
 }
 
-/* 
- * ashtech board configuration script 
+/*
+ * ashtech board configuration script
  */
 
 const char comm[] = "$PASHS,POP,20\r\n"\

src/drivers/gps/gps.cpp

@@ -552,7 +552,7 @@ start(const char *uart_path, bool fake_gps, bool enable_sat_info)
 	fd = open(GPS_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0) {
-		errx(1, "Could not open device path: %s\n", GPS_DEVICE_PATH);
+		errx(1, "open: %s\n", GPS_DEVICE_PATH);
 		goto fail;
 	}
 
@@ -565,7 +565,7 @@ fail:
 		g_dev = nullptr;
 	}
 
-	errx(1, "driver start failed");
+	errx(1, "start failed");
 }
 
 /**
@@ -604,7 +604,7 @@ reset()
 		err(1, "failed ");
 
 	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
-		err(1, "driver reset failed");
+		err(1, "reset failed");
 
 	exit(0);
 }
@@ -616,7 +616,7 @@ void
 info()
 {
 	if (g_dev == nullptr)
-		errx(1, "driver not running");
+		errx(1, "not running");
 
 	g_dev->print_info();
 

src/drivers/gps/gps_helper.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -91,7 +91,7 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud)
 		warnx("try baudrate: %d\n", speed);
 
 	default:
-		warnx("ERROR: Unsupported baudrate: %d\n", baud);
+		warnx("ERR: baudrate: %d\n", baud);
 		return -EINVAL;
 	}
 
@@ -109,20 +109,19 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud)
 
 	/* set baud rate */
 	if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
-		warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state);
+		warnx("ERR: %d (cfsetispeed)\n", termios_state);
 		return -1;
 	}
 
 	if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
-		warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state);
+		warnx("ERR: %d (cfsetospeed)\n", termios_state);
 		return -1;
 	}
 
 	if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
-		warnx("ERROR setting baudrate (tcsetattr)\n");
+		warnx("ERR: %d (tcsetattr)\n", termios_state);
 		return -1;
 	}
 
-	/* XXX if resetting the parser here, ensure it does exist (check for null pointer) */
 	return 0;
 }

src/drivers/gps/gps_helper.h

@@ -43,6 +43,8 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_gps_position.h>
 
+#define GPS_EPOCH_SECS 1234567890ULL
+
 class GPS_Helper
 {
 public:

src/drivers/gps/mtk.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -73,39 +73,38 @@ MTK::configure(unsigned &baudrate)
 
 	/* Write config messages, don't wait for an answer */
 	if (strlen(MTK_OUTPUT_5HZ) != write(_fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) {
-		warnx("mtk: config write failed");
-		return -1;
+		goto errout;
 	}
 
 	usleep(10000);
 
 	if (strlen(MTK_SET_BINARY) != write(_fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) {
-		warnx("mtk: config write failed");
-		return -1;
+		goto errout;
 	}
 
 	usleep(10000);
 
 	if (strlen(SBAS_ON) != write(_fd, SBAS_ON, strlen(SBAS_ON))) {
-		warnx("mtk: config write failed");
-		return -1;
+		goto errout;
 	}
 
 	usleep(10000);
 
 	if (strlen(WAAS_ON) != write(_fd, WAAS_ON, strlen(WAAS_ON))) {
-		warnx("mtk: config write failed");
-		return -1;
+		goto errout;
 	}
 
 	usleep(10000);
 
 	if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) {
-		warnx("mtk: config write failed");
-		return -1;
+		goto errout;
 	}
 
 	return 0;
+
+errout:
+	warnx("mtk: config write failed");
+	return -1;
 }
 
 int
@@ -222,7 +221,6 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
 				ret = 1;
 
 			} else {
-				warnx("MTK Checksum invalid");
 				ret = -1;
 			}
 
@@ -282,8 +280,8 @@ MTK::handle_message(gps_mtk_packet_t &packet)
 	timeinfo_conversion_temp -= timeinfo.tm_sec * 1e3;
 	time_t epoch = mktime(&timeinfo);
 
-	_gps_position->time_gps_usec = epoch * 1e6; //TODO: test this
-	_gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3;
+	_gps_position->time_utc_usec = epoch * 1e6; //TODO: test this
+	_gps_position->time_utc_usec += timeinfo_conversion_temp * 1e3;
 	_gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time();
 
 	// Position and velocity update always at the same time

src/drivers/gps/ubx.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013, 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -748,17 +748,23 @@ UBX::payload_rx_done(void)
 			timeinfo.tm_sec		= _buf.payload_rx_nav_pvt.sec;
 			time_t epoch = mktime(&timeinfo);
 
-#ifndef CONFIG_RTC
-			//Since we lack a hardware RTC, set the system time clock based on GPS UTC
-			//TODO generalize this by moving into gps.cpp?
-			timespec ts;
-			ts.tv_sec = epoch;
-			ts.tv_nsec = _buf.payload_rx_nav_pvt.nano;
-			clock_settime(CLOCK_REALTIME, &ts);
-#endif
+			if (epoch > GPS_EPOCH_SECS) {
+				// FMUv2+ boards have a hardware RTC, but GPS helps us to configure it
+				// and control its drift. Since we rely on the HRT for our monotonic
+				// clock, updating it from time to time is safe.
 
-			_gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
-			_gps_position->time_gps_usec += (uint64_t)(_buf.payload_rx_nav_pvt.nano * 1e-3f);
+				timespec ts;
+				ts.tv_sec = epoch;
+				ts.tv_nsec = _buf.payload_rx_nav_pvt.nano;
+				if (clock_settime(CLOCK_REALTIME, &ts)) {
+					warn("failed setting clock");
+				}
+
+				_gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL;
+				_gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000;
+			} else {
+				_gps_position->time_utc_usec = 0;
+			}
 		}
 
 		_gps_position->timestamp_time		= hrt_absolute_time();
@@ -808,7 +814,7 @@ UBX::payload_rx_done(void)
 		UBX_TRACE_RXMSG("Rx NAV-TIMEUTC\n");
 
 		{
-			/* convert to unix timestamp */
+			// convert to unix timestamp
 			struct tm timeinfo;
 			timeinfo.tm_year	= _buf.payload_rx_nav_timeutc.year - 1900;
 			timeinfo.tm_mon		= _buf.payload_rx_nav_timeutc.month - 1;
@@ -818,17 +824,25 @@ UBX::payload_rx_done(void)
 			timeinfo.tm_sec		= _buf.payload_rx_nav_timeutc.sec;
 			time_t epoch = mktime(&timeinfo);
 
-#ifndef CONFIG_RTC
-			//Since we lack a hardware RTC, set the system time clock based on GPS UTC
-			//TODO generalize this by moving into gps.cpp?
-			timespec ts;
-			ts.tv_sec = epoch;
-			ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano;
-			clock_settime(CLOCK_REALTIME, &ts);
-#endif
+			// only set the time if it makes sense
 
-			_gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
-			_gps_position->time_gps_usec += (uint64_t)(_buf.payload_rx_nav_timeutc.nano * 1e-3f);
+			if (epoch > GPS_EPOCH_SECS) {
+				// FMUv2+ boards have a hardware RTC, but GPS helps us to configure it
+				// and control its drift. Since we rely on the HRT for our monotonic
+				// clock, updating it from time to time is safe.
+
+				timespec ts;
+				ts.tv_sec = epoch;
+				ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano;
+				if (clock_settime(CLOCK_REALTIME, &ts)) {
+					warn("failed setting clock");
+				}
+
+				_gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL;
+				_gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000;
+			} else {
+				_gps_position->time_utc_usec = 0;
+			}
 		}
 
 		_gps_position->timestamp_time = hrt_absolute_time();

src/drivers/hmc5883/hmc5883.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -34,7 +34,7 @@
 /**
  * @file hmc5883.cpp
  *
- * Driver for the HMC5883 magnetometer connected via I2C.
+ * Driver for the HMC5883 / HMC5983 magnetometer connected via I2C or SPI.
  */
 
 #include <nuttx/config.h>
@@ -74,11 +74,12 @@
 #include <getopt.h>
 #include <lib/conversion/rotation.h>
 
+#include "hmc5883.h"
+
 /*
  * HMC5883 internal constants and data structures.
  */
 
-#define HMC5883L_ADDRESS		PX4_I2C_OBDEV_HMC5883
 #define HMC5883L_DEVICE_PATH_INT	"/dev/hmc5883_int"
 #define HMC5883L_DEVICE_PATH_EXT	"/dev/hmc5883_ext"
 
@@ -95,9 +96,6 @@
 #define ADDR_DATA_OUT_Y_MSB		0x07
 #define ADDR_DATA_OUT_Y_LSB		0x08
 #define ADDR_STATUS			0x09
-#define ADDR_ID_A			0x0a
-#define ADDR_ID_B			0x0b
-#define ADDR_ID_C			0x0c
 
 /* modes not changeable outside of driver */
 #define HMC5883L_MODE_NORMAL		(0 << 0)  /* default */
@@ -115,10 +113,11 @@
 #define STATUS_REG_DATA_OUT_LOCK	(1 << 1) /* page 16: set if data is only partially read, read device to reset */
 #define STATUS_REG_DATA_READY		(1 << 0) /* page 16: set if all axes have valid measurements */
 
-#define ID_A_WHO_AM_I			'H'
-#define ID_B_WHO_AM_I			'4'
-#define ID_C_WHO_AM_I			'3'
-
+enum HMC5883_BUS {
+	HMC5883_BUS_ALL,
+	HMC5883_BUS_INTERNAL,
+	HMC5883_BUS_EXTERNAL
+};
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -130,10 +129,10 @@ static const int ERROR = -1;
 # error This requires CONFIG_SCHED_WORKQUEUE.
 #endif
 
-class HMC5883 : public device::I2C
+class HMC5883 : public device::CDev
 {
 public:
-	HMC5883(int bus, const char *path, enum Rotation rotation);
+	HMC5883(device::Device *interface, const char *path, enum Rotation rotation);
 	virtual ~HMC5883();
 
 	virtual int		init();
@@ -147,7 +146,7 @@ public:
 	void			print_info();
 
 protected:
-	virtual int		probe();
+	Device			*_interface;
 
 private:
 	work_s			_work;
@@ -174,7 +173,6 @@ private:
 	bool			_sensor_ok;		/**< sensor was found and reports ok */
 	bool			_calibrated;		/**< the calibration is valid */
 
-	int			_bus;			/**< the bus the device is connected to */
 	enum Rotation		_rotation;
 
 	struct mag_report	_last_report;           /**< used for info() */
@@ -182,15 +180,6 @@ private:
 	uint8_t			_range_bits;
 	uint8_t			_conf_reg;
 
-	/**
-	 * Test whether the device supported by the driver is present at a
-	 * specific address.
-	 *
-	 * @param address	The I2C bus address to probe.
-	 * @return		True if the device is present.
-	 */
-	int			probe_address(uint8_t address);
-
 	/**
 	 * Initialise the automatic measurement state machine and start it.
 	 *
@@ -349,8 +338,9 @@ private:
 extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]);
 
 
-HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) :
-	I2C("HMC5883", path, bus, HMC5883L_ADDRESS, 400000),
+HMC5883::HMC5883(device::Device *interface, const char *path, enum Rotation rotation) :
+	CDev("HMC5883", path),
+	_interface(interface),
 	_work{},
 	_measure_ticks(0),
 	_reports(nullptr),
@@ -369,7 +359,6 @@ HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) :
 	_conf_errors(perf_alloc(PC_COUNT, "hmc5883_conf_errors")),
 	_sensor_ok(false),
 	_calibrated(false),
-	_bus(bus),
 	_rotation(rotation),
 	_last_report{0},
 	_range_bits(0),
@@ -416,9 +405,11 @@ HMC5883::init()
 {
 	int ret = ERROR;
 
-	/* do I2C init (and probe) first */
-	if (I2C::init() != OK)
+	ret = CDev::init();
+	if (ret != OK) {
+		debug("CDev init failed");
 		goto out;
+	}
 
 	/* allocate basic report buffers */
 	_reports = new RingBuffer(2, sizeof(mag_report));
@@ -429,20 +420,7 @@ HMC5883::init()
 	reset();
 
 	_class_instance = register_class_devname(MAG_DEVICE_PATH);
-
-	switch (_class_instance) {
-		case CLASS_DEVICE_PRIMARY:
-			_mag_orb_id = ORB_ID(sensor_mag0);
-			break;
-
-		case CLASS_DEVICE_SECONDARY:
-			_mag_orb_id = ORB_ID(sensor_mag1);
-			break;
-
-		case CLASS_DEVICE_TERTIARY:
-			_mag_orb_id = ORB_ID(sensor_mag2);
-			break;
-	}
+	_mag_orb_id = ORB_ID_TRIPLE(sensor_mag, _class_instance);
 
 	ret = OK;
 	/* sensor is ok, but not calibrated */
@@ -559,30 +537,6 @@ void HMC5883::check_conf(void)
 	}
 }
 
-int
-HMC5883::probe()
-{
-	uint8_t data[3] = {0, 0, 0};
-
-	_retries = 10;
-
-	if (read_reg(ADDR_ID_A, data[0]) ||
-	    read_reg(ADDR_ID_B, data[1]) ||
-	    read_reg(ADDR_ID_C, data[2]))
-		debug("read_reg fail");
-
-	_retries = 2;
-
-	if ((data[0] != ID_A_WHO_AM_I) ||
-	    (data[1] != ID_B_WHO_AM_I) ||
-	    (data[2] != ID_C_WHO_AM_I)) {
-		debug("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]);
-		return -EIO;
-	}
-
-	return OK;
-}
-
 ssize_t
 HMC5883::read(struct file *filp, char *buffer, size_t buflen)
 {
@@ -643,6 +597,8 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen)
 int
 HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
+	unsigned dummy = arg;
+
 	switch (cmd) {
 	case SENSORIOCSPOLLRATE: {
 		switch (arg) {
@@ -768,14 +724,12 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return check_calibration();
 
 	case MAGIOCGEXTERNAL:
-		if (_bus == PX4_I2C_BUS_EXPANSION)
-			return 1;
-		else
-			return 0;
+		debug("MAGIOCGEXTERNAL in main driver");
+		return _interface->ioctl(cmd, dummy);
 
 	default:
 		/* give it to the superclass */
-		return I2C::ioctl(filp, cmd, arg);
+		return CDev::ioctl(filp, cmd, arg);
 	}
 }
 
@@ -890,7 +844,6 @@ HMC5883::collect()
 	} report;
 
 	int	ret;
-	uint8_t	cmd;
 	uint8_t check_counter;
 
 	perf_begin(_sample_perf);
@@ -908,8 +861,7 @@ HMC5883::collect()
 	 */
 
 	/* get measurements from the device */
-	cmd = ADDR_DATA_OUT_X_MSB;
-	ret = transfer(&cmd, 1, (uint8_t *)&hmc_report, sizeof(hmc_report));
+	ret = _interface->read(ADDR_DATA_OUT_X_MSB, (uint8_t *)&hmc_report, sizeof(hmc_report));
 
 	if (ret != OK) {
 		perf_count(_comms_errors);
@@ -946,14 +898,14 @@ HMC5883::collect()
 
 	/* scale values for output */
 
-#ifdef PX4_I2C_BUS_ONBOARD
-	if (_bus == PX4_I2C_BUS_ONBOARD) {
+	// XXX revisit for SPI part, might require a bus type IOCTL
+	unsigned dummy;
+	if (!_interface->ioctl(MAGIOCGEXTERNAL, dummy)) {
 		// convert onboard so it matches offboard for the
 		// scaling below
 		report.y = -report.y;
 		report.x = -report.x;
         }
-#endif
 
 	/* the standard external mag by 3DR has x pointing to the
 	 * right, y pointing backwards, and z down, therefore switch x
@@ -1291,15 +1243,17 @@ int HMC5883::set_excitement(unsigned enable)
 int
 HMC5883::write_reg(uint8_t reg, uint8_t val)
 {
-	uint8_t cmd[] = { reg, val };
-
-	return transfer(&cmd[0], 2, nullptr, 0);
+	uint8_t buf = val;
+	return _interface->write(reg, &buf, 1);
 }
 
 int
 HMC5883::read_reg(uint8_t reg, uint8_t &val)
 {
-	return transfer(&reg, 1, &val, 1);
+	uint8_t buf = val;
+	int ret = _interface->read(reg, &buf, 1);
+	val = buf;
+	return ret;
 }
 
 float
@@ -1351,6 +1305,7 @@ void	test(int bus);
 void	reset(int bus);
 int	info(int bus);
 int	calibrate(int bus);
+const char* get_path(int bus);
 void	usage();
 
 /**
@@ -1360,43 +1315,99 @@ void	usage();
  * is either successfully up and running or failed to start.
  */
 void
-start(int bus, enum Rotation rotation)
+start(int external_bus, enum Rotation rotation)
 {
 	int fd;
 
 	/* create the driver, attempt expansion bus first */
-	if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) {
-		if (g_dev_ext != nullptr)
-			errx(0, "already started external");
-		g_dev_ext = new HMC5883(PX4_I2C_BUS_EXPANSION, HMC5883L_DEVICE_PATH_EXT, rotation);
-		if (g_dev_ext != nullptr && OK != g_dev_ext->init()) {
-			delete g_dev_ext;
-			g_dev_ext = nullptr;
+	if (g_dev_ext != nullptr) {
+			warnx("already started external");
+	} else if (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_EXTERNAL) {
+
+		device::Device *interface = nullptr;
+
+		/* create the driver, only attempt I2C for the external bus */
+		if (interface == nullptr && (HMC5883_I2C_interface != nullptr)) {
+			interface = HMC5883_I2C_interface(PX4_I2C_BUS_EXPANSION);
+
+			if (interface->init() != OK) {
+				delete interface;
+				interface = nullptr;
+				warnx("no device on I2C bus #%u", PX4_I2C_BUS_EXPANSION);
+			}
+		}
+
+#ifdef PX4_I2C_BUS_ONBOARD
+		if (interface == nullptr && (HMC5883_I2C_interface != nullptr)) {
+			interface = HMC5883_I2C_interface(PX4_I2C_BUS_ONBOARD);
+
+			if (interface->init() != OK) {
+				delete interface;
+				interface = nullptr;
+				warnx("no device on I2C bus #%u", PX4_I2C_BUS_ONBOARD);
+			}
+		}
+#endif
+
+		/* interface will be null if init failed */
+		if (interface != nullptr) {
+
+			g_dev_ext = new HMC5883(interface, HMC5883L_DEVICE_PATH_EXT, rotation);
+			if (g_dev_ext != nullptr && OK != g_dev_ext->init()) {
+				delete g_dev_ext;
+				g_dev_ext = nullptr;
+			}
+
 		}
 	}
 			
 
-#ifdef PX4_I2C_BUS_ONBOARD
 	/* if this failed, attempt onboard sensor */
-	if (bus == -1 || bus == PX4_I2C_BUS_ONBOARD) {
-		if (g_dev_int != nullptr)
-			errx(0, "already started internal");
-		g_dev_int = new HMC5883(PX4_I2C_BUS_ONBOARD, HMC5883L_DEVICE_PATH_INT, rotation);
-		if (g_dev_int != nullptr && OK != g_dev_int->init()) {
+	if (g_dev_int != nullptr) {
+		warnx("already started internal");
+	} else if (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_INTERNAL) {
 
-			/* tear down the failing onboard instance */
-			delete g_dev_int;
-			g_dev_int = nullptr;
+		device::Device *interface = nullptr;
 
-			if (bus == PX4_I2C_BUS_ONBOARD) {
+		/* create the driver, try SPI first, fall back to I2C if unsuccessful */
+#ifdef PX4_SPIDEV_HMC
+		if (HMC5883_SPI_interface != nullptr) {
+			interface = HMC5883_SPI_interface(PX4_SPI_BUS_SENSORS);
+		}
+#endif
+
+#ifdef PX4_I2C_BUS_ONBOARD
+		/* this device is already connected as external if present above */
+		if (interface == nullptr && (HMC5883_I2C_interface != nullptr)) {
+			interface = HMC5883_I2C_interface(PX4_I2C_BUS_ONBOARD);
+		}
+#endif
+		if (interface == nullptr) {
+			warnx("no internal bus scanned");
+			goto fail;
+		}
+
+		if (interface->init() != OK) {
+			delete interface;
+			warnx("no device on internal bus");
+		} else {
+
+			g_dev_int = new HMC5883(interface, HMC5883L_DEVICE_PATH_INT, rotation);
+			if (g_dev_int != nullptr && OK != g_dev_int->init()) {
+
+				/* tear down the failing onboard instance */
+				delete g_dev_int;
+				g_dev_int = nullptr;
+
+				if (external_bus == HMC5883_BUS_INTERNAL) {
+					goto fail;
+				}
+			}
+			if (g_dev_int == nullptr && external_bus == HMC5883_BUS_INTERNAL) {
 				goto fail;
 			}
 		}
-		if (g_dev_int == nullptr && bus == PX4_I2C_BUS_ONBOARD) {
-			goto fail;
-		}
 	}
-#endif
 
 	if (g_dev_int == nullptr && g_dev_ext == nullptr)
 		goto fail;
@@ -1425,11 +1436,11 @@ start(int bus, enum Rotation rotation)
 	exit(0);
 
 fail:
-	if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) {
+	if (g_dev_int != nullptr && (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_INTERNAL)) {
 		delete g_dev_int;
 		g_dev_int = nullptr;
 	}
-	if (g_dev_ext != nullptr && (bus == -1 || bus == PX4_I2C_BUS_EXPANSION)) {
+	if (g_dev_ext != nullptr && (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_EXTERNAL)) {
 		delete g_dev_ext;
 		g_dev_ext = nullptr;
 	}
@@ -1448,7 +1459,7 @@ test(int bus)
 	struct mag_report report;
 	ssize_t sz;
 	int ret;
-	const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT);
+	const char *path = get_path(bus);
 
 	int fd = open(path, O_RDONLY);
 
@@ -1549,7 +1560,7 @@ test(int bus)
 int calibrate(int bus)
 {
 	int ret;
-	const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT);
+	const char *path = get_path(bus);
 
 	int fd = open(path, O_RDONLY);
 
@@ -1576,7 +1587,7 @@ int calibrate(int bus)
 void
 reset(int bus)
 {
-	const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT);
+	const char *path = get_path(bus);
 
 	int fd = open(path, O_RDONLY);
 
@@ -1600,12 +1611,12 @@ info(int bus)
 {
 	int ret = 1;
 
-	HMC5883 *g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext);
+	HMC5883 *g_dev = (bus == HMC5883_BUS_INTERNAL ? g_dev_int : g_dev_ext);
 	if (g_dev == nullptr) {
 		warnx("not running on bus %d", bus);
 	} else {
 
-		warnx("running on bus: %d (%s)\n", bus, ((PX4_I2C_BUS_ONBOARD) ? "onboard" : "offboard"));
+		warnx("running on bus: %d (%s)\n", bus, ((HMC5883_BUS_INTERNAL) ? "onboard" : "offboard"));
 
 		g_dev->print_info();
 		ret = 0;
@@ -1614,6 +1625,12 @@ info(int bus)
 	return ret;
 }
 
+const char*
+get_path(int bus)
+{
+	return ((bus == HMC5883_BUS_INTERNAL) ? HMC5883L_DEVICE_PATH_INT : HMC5883L_DEVICE_PATH_EXT);
+}
+
 void
 usage()
 {
@@ -1622,7 +1639,7 @@ usage()
 	warnx("    -R rotation");
 	warnx("    -C calibrate on start");
 	warnx("    -X only external bus");
-#ifdef PX4_I2C_BUS_ONBOARD
+#if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC)
 	warnx("    -I only internal bus");
 #endif
 }
@@ -1633,7 +1650,7 @@ int
 hmc5883_main(int argc, char *argv[])
 {
 	int ch;
-	int bus = -1;
+	int bus = HMC5883_BUS_ALL;
 	enum Rotation rotation = ROTATION_NONE;
         bool calibrate = false;
 
@@ -1642,13 +1659,13 @@ hmc5883_main(int argc, char *argv[])
 		case 'R':
 			rotation = (enum Rotation)atoi(optarg);
 			break;
-#ifdef PX4_I2C_BUS_ONBOARD
+#if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC)
 		case 'I':
-			bus = PX4_I2C_BUS_ONBOARD;
+			bus = HMC5883_BUS_INTERNAL;
 			break;
 #endif
 		case 'X':
-			bus = PX4_I2C_BUS_EXPANSION;
+			bus = HMC5883_BUS_EXTERNAL;
 			break;
 		case 'C':
 			calibrate = true;
@@ -1692,13 +1709,13 @@ hmc5883_main(int argc, char *argv[])
 	 * Print driver information.
 	 */
 	if (!strcmp(verb, "info") || !strcmp(verb, "status")) {
-		if (bus == -1) {
+		if (bus == HMC5883_BUS_ALL) {
 			int ret = 0;
-			if (hmc5883::info(PX4_I2C_BUS_ONBOARD)) {
+			if (hmc5883::info(HMC5883_BUS_INTERNAL)) {
 				ret = 1;
 			}
 
-			if (hmc5883::info(PX4_I2C_BUS_EXPANSION)) {
+			if (hmc5883::info(HMC5883_BUS_EXTERNAL)) {
 				ret = 1;
 			}
 			exit(ret);

src/drivers/hmc5883/hmc5883.h

@@ -0,0 +1,52 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file hmc5883.h
+ *
+ * Shared defines for the hmc5883 driver.
+ */
+
+#pragma once
+
+#define ADDR_ID_A			0x0a
+#define ADDR_ID_B			0x0b
+#define ADDR_ID_C			0x0c
+
+#define ID_A_WHO_AM_I			'H'
+#define ID_B_WHO_AM_I			'4'
+#define ID_C_WHO_AM_I			'3'
+
+/* interface factories */
+extern device::Device *HMC5883_SPI_interface(int bus) weak_function;
+extern device::Device *HMC5883_I2C_interface(int bus) weak_function;

src/drivers/hmc5883/hmc5883_i2c.cpp

@@ -0,0 +1,175 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+ /**
+  * @file HMC5883_I2C.cpp
+  *
+  * I2C interface for HMC5883 / HMC 5983
+  */
+
+/* XXX trim includes */
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <string.h>
+#include <assert.h>
+#include <debug.h>
+#include <errno.h>
+#include <unistd.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/device/i2c.h>
+#include <drivers/drv_mag.h>
+
+#include "hmc5883.h"
+
+#include "board_config.h"
+
+#ifdef PX4_I2C_OBDEV_HMC5883
+
+#define HMC5883L_ADDRESS		PX4_I2C_OBDEV_HMC5883
+
+device::Device *HMC5883_I2C_interface(int bus);
+
+class HMC5883_I2C : public device::I2C
+{
+public:
+	HMC5883_I2C(int bus);
+	virtual ~HMC5883_I2C();
+
+	virtual int	init();
+	virtual int	read(unsigned address, void *data, unsigned count); 
+	virtual int	write(unsigned address, void *data, unsigned count);
+
+	virtual int	ioctl(unsigned operation, unsigned &arg);
+
+protected:
+	virtual int	probe();
+
+};
+
+device::Device *
+HMC5883_I2C_interface(int bus)
+{
+	return new HMC5883_I2C(bus);
+}
+
+HMC5883_I2C::HMC5883_I2C(int bus) :
+	I2C("HMC5883_I2C", nullptr, bus, HMC5883L_ADDRESS, 400000)
+{
+}
+
+HMC5883_I2C::~HMC5883_I2C()
+{
+}
+
+int
+HMC5883_I2C::init()
+{
+	/* this will call probe() */
+	return I2C::init();
+}
+
+int
+HMC5883_I2C::ioctl(unsigned operation, unsigned &arg)
+{
+	int ret;
+
+	switch (operation) {
+
+	case MAGIOCGEXTERNAL:
+		if (_bus == PX4_I2C_BUS_EXPANSION) {
+			return 1;
+		} else {
+			return 0;
+		}
+
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+int
+HMC5883_I2C::probe()
+{
+	uint8_t data[3] = {0, 0, 0};
+
+	_retries = 10;
+
+	if (read(ADDR_ID_A, &data[0], 1) ||
+	    read(ADDR_ID_B, &data[1], 1) ||
+	    read(ADDR_ID_C, &data[2], 1)) {
+		debug("read_reg fail");
+		return -EIO;
+	}
+
+	_retries = 2;
+
+	if ((data[0] != ID_A_WHO_AM_I) ||
+	    (data[1] != ID_B_WHO_AM_I) ||
+	    (data[2] != ID_C_WHO_AM_I)) {
+		debug("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]);
+		return -EIO;
+	}
+
+	return OK;
+}
+
+int
+HMC5883_I2C::write(unsigned address, void *data, unsigned count)
+{
+	uint8_t buf[32];
+
+	if (sizeof(buf) < (count + 1)) {
+		return -EIO;
+	}
+
+	buf[0] = address;
+	memcpy(&buf[1], data, count);
+
+	return transfer(&buf[0], count + 1, nullptr, 0);
+}
+
+int
+HMC5883_I2C::read(unsigned address, void *data, unsigned count)
+{
+	uint8_t cmd = address;
+	return transfer(&cmd, 1, (uint8_t *)data, count);
+}
+
+#endif /* PX4_I2C_OBDEV_HMC5883 */

src/drivers/hmc5883/hmc5883_spi.cpp

@@ -0,0 +1,189 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+ /**
+  * @file HMC5883_SPI.cpp
+  *
+  * SPI interface for HMC5983
+  */
+
+/* XXX trim includes */
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <string.h>
+#include <assert.h>
+#include <debug.h>
+#include <errno.h>
+#include <unistd.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/device/spi.h>
+#include <drivers/drv_mag.h>
+
+#include "hmc5883.h"
+#include <board_config.h>
+
+#ifdef PX4_SPIDEV_HMC
+
+/* SPI protocol address bits */
+#define DIR_READ			(1<<7)
+#define DIR_WRITE			(0<<7)
+#define ADDR_INCREMENT			(1<<6)
+
+#define HMC_MAX_SEND_LEN		4
+#define HMC_MAX_RCV_LEN			8
+
+device::Device *HMC5883_SPI_interface(int bus);
+
+class HMC5883_SPI : public device::SPI
+{
+public:
+	HMC5883_SPI(int bus, spi_dev_e device);
+	virtual ~HMC5883_SPI();
+
+	virtual int	init();
+	virtual int	read(unsigned address, void *data, unsigned count); 
+	virtual int	write(unsigned address, void *data, unsigned count);
+
+	virtual int	ioctl(unsigned operation, unsigned &arg);
+
+};
+
+device::Device *
+HMC5883_SPI_interface(int bus)
+{
+	return new HMC5883_SPI(bus, (spi_dev_e)PX4_SPIDEV_HMC);
+}
+
+HMC5883_SPI::HMC5883_SPI(int bus, spi_dev_e device) :
+	SPI("HMC5883_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */)
+{
+}
+
+HMC5883_SPI::~HMC5883_SPI()
+{
+}
+
+int
+HMC5883_SPI::init()
+{
+	int ret;
+
+	ret = SPI::init();
+	if (ret != OK) {
+		debug("SPI init failed");
+		return -EIO;
+	}
+
+	// read WHO_AM_I value
+	uint8_t data[3] = {0, 0, 0};
+
+	if (read(ADDR_ID_A, &data[0], 1) ||
+	    read(ADDR_ID_B, &data[1], 1) ||
+	    read(ADDR_ID_C, &data[2], 1)) {
+		debug("read_reg fail");
+	}
+
+	if ((data[0] != ID_A_WHO_AM_I) ||
+	    (data[1] != ID_B_WHO_AM_I) ||
+	    (data[2] != ID_C_WHO_AM_I)) {
+		debug("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]);
+		return -EIO;
+	}
+
+	return OK;
+}
+
+int
+HMC5883_SPI::ioctl(unsigned operation, unsigned &arg)
+{
+	int ret;
+
+	switch (operation) {
+
+	case MAGIOCGEXTERNAL:
+
+#ifdef PX4_SPI_BUS_EXT
+		if (_bus == PX4_SPI_BUS_EXT) {
+			return 1;
+		} else 
+#endif
+		{
+			return 0;
+		}
+
+	default:
+	{
+		ret = -EINVAL;
+	}
+	}
+
+	return ret;
+}
+
+int
+HMC5883_SPI::write(unsigned address, void *data, unsigned count)
+{
+	uint8_t buf[32];
+
+	if (sizeof(buf) < (count + 1)) {
+		return -EIO;
+	}
+
+	buf[0] = address | DIR_WRITE;
+	memcpy(&buf[1], data, count);
+
+	return transfer(&buf[0], &buf[0], count + 1);
+}
+
+int
+HMC5883_SPI::read(unsigned address, void *data, unsigned count)
+{
+	uint8_t buf[32];
+
+	if (sizeof(buf) < (count + 1)) {
+		return -EIO;
+	}
+
+	buf[0] = address | DIR_READ | ADDR_INCREMENT;
+
+	int ret = transfer(&buf[0], &buf[0], count + 1);
+	memcpy(data, &buf[1], count);
+	return ret;
+}
+
+#endif /* PX4_SPIDEV_HMC */

src/drivers/hmc5883/module.mk

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -37,7 +37,7 @@
 
 MODULE_COMMAND	= hmc5883
 
-SRCS		= hmc5883.cpp
+SRCS		= hmc5883_i2c.cpp hmc5883_spi.cpp hmc5883.cpp
 
 MODULE_STACKSIZE	= 1200
 

src/drivers/hott/hott_sensors/hott_sensors.cpp

@@ -157,7 +157,7 @@ hott_sensors_thread_main(int argc, char *argv[])
 	/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
 	const int uart = open_uart(device);
 	if (uart < 0) {
-		errx(1, "Failed opening HoTT UART, exiting.");
+		errx(1, "Open fail, exiting.");
 		thread_running = false;
 	}
 

src/drivers/l3gd20/l3gd20.cpp

@@ -142,6 +142,7 @@ static const int ERROR = -1;
 #define ADDR_INT1_TSH_ZH		0x36
 #define ADDR_INT1_TSH_ZL		0x37
 #define ADDR_INT1_DURATION		0x38
+#define ADDR_LOW_ODR			0x39
 
 
 /* Internal configuration values */
@@ -200,6 +201,12 @@ public:
 	 */
 	void			print_info();
 
+	// print register dump
+	void			print_registers();
+
+	// trigger an error
+	void			test_error();
+
 protected:
 	virtual int		probe();
 
@@ -225,6 +232,9 @@ private:
 	perf_counter_t		_sample_perf;
 	perf_counter_t		_reschedules;
 	perf_counter_t		_errors;
+	perf_counter_t		_bad_registers;
+
+	uint8_t			_register_wait;
 
 	math::LowPassFilter2p	_gyro_filter_x;
 	math::LowPassFilter2p	_gyro_filter_y;
@@ -235,6 +245,14 @@ private:
 
 	enum Rotation		_rotation;
 
+	// this is used to support runtime checking of key
+	// configuration registers to detect SPI bus errors and sensor
+	// reset
+#define L3GD20_NUM_CHECKED_REGISTERS 8
+	static const uint8_t	_checked_registers[L3GD20_NUM_CHECKED_REGISTERS];
+	uint8_t			_checked_values[L3GD20_NUM_CHECKED_REGISTERS];
+	uint8_t			_checked_next;
+
 	/**
 	 * Start automatic measurement.
 	 */
@@ -266,6 +284,11 @@ private:
 	 */
 	static void		measure_trampoline(void *arg);
 
+	/**
+	 * check key registers for correct values
+	 */
+	void			check_registers(void);
+
 	/**
 	 * Fetch measurements from the sensor and update the report ring.
 	 */
@@ -298,6 +321,14 @@ private:
 	 */
 	void			modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
 
+	/**
+	 * Write a register in the L3GD20, updating _checked_values
+	 *
+	 * @param reg		The register to write.
+	 * @param value		The new value to write.
+	 */
+	void			write_checked_reg(unsigned reg, uint8_t value);
+
 	/**
 	 * Set the L3GD20 measurement range.
 	 *
@@ -338,6 +369,19 @@ private:
 	L3GD20 operator=(const L3GD20&);
 };
 
+/*
+  list of registers that will be checked in check_registers(). Note
+  that ADDR_WHO_AM_I must be first in the list.
+ */
+const uint8_t L3GD20::_checked_registers[L3GD20_NUM_CHECKED_REGISTERS] = { ADDR_WHO_AM_I,
+                                                                           ADDR_CTRL_REG1,
+                                                                           ADDR_CTRL_REG2,
+                                                                           ADDR_CTRL_REG3,
+                                                                           ADDR_CTRL_REG4,
+                                                                           ADDR_CTRL_REG5,
+                                                                           ADDR_FIFO_CTRL_REG,
+									   ADDR_LOW_ODR };
+
 L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation) :
 	SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */),
 	_call{},
@@ -355,11 +399,14 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati
 	_sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")),
 	_reschedules(perf_alloc(PC_COUNT, "l3gd20_reschedules")),
 	_errors(perf_alloc(PC_COUNT, "l3gd20_errors")),
+	_bad_registers(perf_alloc(PC_COUNT, "l3gd20_bad_registers")),
+	_register_wait(0),
 	_gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
 	_gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
 	_gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
 	_is_l3g4200d(false),
-        _rotation(rotation)                                            
+	_rotation(rotation),
+	_checked_next(0)
 {
 	// enable debug() calls
 	_debug_enabled = true;
@@ -389,6 +436,7 @@ L3GD20::~L3GD20()
 	perf_free(_sample_perf);
 	perf_free(_reschedules);
 	perf_free(_errors);
+	perf_free(_bad_registers);
 }
 
 int
@@ -448,29 +496,27 @@ L3GD20::probe()
 	(void)read_reg(ADDR_WHO_AM_I);
 
 	bool success = false;
+	uint8_t v = 0;
 
-	/* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */
-	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) {
-
+	/* verify that the device is attached and functioning, accept
+	 * L3GD20, L3GD20H and L3G4200D */
+	if ((v=read_reg(ADDR_WHO_AM_I)) == WHO_I_AM) {
 		_orientation = SENSOR_BOARD_ROTATION_DEFAULT;
 		success = true;
-	}
-
-
-	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) {
+	} else if ((v=read_reg(ADDR_WHO_AM_I)) == WHO_I_AM_H) {
 		_orientation = SENSOR_BOARD_ROTATION_180_DEG;
 		success = true;
-	}
-
-	/* Detect the L3G4200D used on AeroCore */
-	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_L3G4200D) {
+	} else if ((v=read_reg(ADDR_WHO_AM_I)) == WHO_I_AM_L3G4200D) {
+		/* Detect the L3G4200D used on AeroCore */
 		_is_l3g4200d = true;
 		_orientation = SENSOR_BOARD_ROTATION_DEFAULT;
 		success = true;
 	}
 
-	if (success)
+	if (success) {
+		_checked_values[0] = v;
 		return OK;
+	}
 
 	return -EIO;
 }
@@ -672,6 +718,18 @@ L3GD20::write_reg(unsigned reg, uint8_t value)
 	transfer(cmd, nullptr, sizeof(cmd));
 }
 
+void
+L3GD20::write_checked_reg(unsigned reg, uint8_t value)
+{
+	write_reg(reg, value);
+	for (uint8_t i=0; i<L3GD20_NUM_CHECKED_REGISTERS; i++) {
+		if (reg == _checked_registers[i]) {
+			_checked_values[i] = value;
+		}
+	}
+}
+
+
 void
 L3GD20::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
 {
@@ -680,7 +738,7 @@ L3GD20::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
 	val = read_reg(reg);
 	val &= ~clearbits;
 	val |= setbits;
-	write_reg(reg, val);
+	write_checked_reg(reg, val);
 }
 
 int
@@ -714,7 +772,7 @@ L3GD20::set_range(unsigned max_dps)
 
 	_gyro_range_rad_s = new_range / 180.0f * M_PI_F;
 	_gyro_range_scale = new_range_scale_dps_digit / 180.0f * M_PI_F;
-	write_reg(ADDR_CTRL_REG4, bits);
+	write_checked_reg(ADDR_CTRL_REG4, bits);
 
 	return OK;
 }
@@ -750,7 +808,7 @@ L3GD20::set_samplerate(unsigned frequency)
 		return -EINVAL;
 	}
 
-	write_reg(ADDR_CTRL_REG1, bits);
+	write_checked_reg(ADDR_CTRL_REG1, bits);
 
 	return OK;
 }
@@ -791,6 +849,11 @@ L3GD20::disable_i2c(void)
 		uint8_t a = read_reg(0x05);
 		write_reg(0x05, (0x20 | a));
 		if (read_reg(0x05) == (a | 0x20)) {
+			// this sets the I2C_DIS bit on the
+			// L3GD20H. The l3gd20 datasheet doesn't
+			// mention this register, but it does seem to
+			// accept it.
+			write_checked_reg(ADDR_LOW_ODR, 0x08);
 			return;
 		}
 	}
@@ -804,18 +867,18 @@ L3GD20::reset()
 	disable_i2c();
 
 	/* set default configuration */
-	write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
-	write_reg(ADDR_CTRL_REG2, 0);		/* disable high-pass filters */
-	write_reg(ADDR_CTRL_REG3, 0x08);        /* DRDY enable */
-	write_reg(ADDR_CTRL_REG4, REG4_BDU);
-	write_reg(ADDR_CTRL_REG5, 0);
-
-	write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE);		/* disable wake-on-interrupt */
+	write_checked_reg(ADDR_CTRL_REG1, 
+                          REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
+	write_checked_reg(ADDR_CTRL_REG2, 0);		/* disable high-pass filters */
+	write_checked_reg(ADDR_CTRL_REG3, 0x08);        /* DRDY enable */
+	write_checked_reg(ADDR_CTRL_REG4, REG4_BDU);
+	write_checked_reg(ADDR_CTRL_REG5, 0);
+	write_checked_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE);		/* disable wake-on-interrupt */
 
 	/* disable FIFO. This makes things simpler and ensures we
 	 * aren't getting stale data. It means we must run the hrt
 	 * callback fast enough to not miss data. */
-	write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);
+	write_checked_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);
 
 	set_samplerate(0); // 760Hz or 800Hz
 	set_range(L3GD20_DEFAULT_RANGE_DPS);
@@ -839,20 +902,36 @@ L3GD20::measure_trampoline(void *arg)
 # define L3GD20_USE_DRDY 0
 #endif
 
+void
+L3GD20::check_registers(void)
+{
+	uint8_t v;
+	if ((v=read_reg(_checked_registers[_checked_next])) != _checked_values[_checked_next]) {
+		/*
+		  if we get the wrong value then we know the SPI bus
+		  or sensor is very sick. We set _register_wait to 20
+		  and wait until we have seen 20 good values in a row
+		  before we consider the sensor to be OK again. 
+		 */
+		perf_count(_bad_registers);
+
+		/*
+		  try to fix the bad register value. We only try to
+		  fix one per loop to prevent a bad sensor hogging the
+		  bus. We skip zero as that is the WHO_AM_I, which
+		  is not writeable
+		 */
+		if (_checked_next != 0) {
+			write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]);
+		}
+		_register_wait = 20;
+        }
+        _checked_next = (_checked_next+1) % L3GD20_NUM_CHECKED_REGISTERS;
+}
+
 void
 L3GD20::measure()
 {
-#if L3GD20_USE_DRDY
-	// if the gyro doesn't have any data ready then re-schedule
-	// for 100 microseconds later. This ensures we don't double
-	// read a value and then miss the next value
-	if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) {
-		perf_count(_reschedules);
-		hrt_call_delay(&_call, 100);
-		return;
-	}
-#endif
-
 	/* status register and data as read back from the device */
 #pragma pack(push, 1)
 	struct {
@@ -870,6 +949,20 @@ L3GD20::measure()
 	/* start the performance counter */
 	perf_begin(_sample_perf);
 
+        check_registers();
+
+#if L3GD20_USE_DRDY
+	// if the gyro doesn't have any data ready then re-schedule
+	// for 100 microseconds later. This ensures we don't double
+	// read a value and then miss the next value
+	if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) {
+		perf_count(_reschedules);
+		hrt_call_delay(&_call, 100);
+                perf_end(_sample_perf);
+		return;
+	}
+#endif
+
 	/* fetch data from the sensor */
 	memset(&raw_report, 0, sizeof(raw_report));
 	raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT;
@@ -900,7 +993,7 @@ L3GD20::measure()
 	 *		  74 from all measurements centers them around zero.
 	 */
 	report.timestamp = hrt_absolute_time();
-        report.error_count = 0; // not recorded
+        report.error_count = perf_event_count(_bad_registers);
 	
 	switch (_orientation) {
 
@@ -973,7 +1066,39 @@ L3GD20::print_info()
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_reschedules);
 	perf_print_counter(_errors);
+	perf_print_counter(_bad_registers);
 	_reports->print_info("report queue");
+        ::printf("checked_next: %u\n", _checked_next);
+        for (uint8_t i=0; i<L3GD20_NUM_CHECKED_REGISTERS; i++) {
+            uint8_t v = read_reg(_checked_registers[i]);
+            if (v != _checked_values[i]) {
+                ::printf("reg %02x:%02x should be %02x\n", 
+                         (unsigned)_checked_registers[i],
+                         (unsigned)v,
+                         (unsigned)_checked_values[i]);
+            }
+        }
+}
+
+void
+L3GD20::print_registers()
+{
+	printf("L3GD20 registers\n");
+	for (uint8_t reg=0; reg<=0x40; reg++) {
+		uint8_t v = read_reg(reg);
+		printf("%02x:%02x ",(unsigned)reg, (unsigned)v);
+		if ((reg+1) % 16 == 0) {
+			printf("\n");
+		}
+	}
+	printf("\n");
+}
+
+void
+L3GD20::test_error()
+{
+	// trigger a deliberate error
+        write_reg(ADDR_CTRL_REG3, 0);
 }
 
 int
@@ -1011,6 +1136,8 @@ void	start(bool external_bus, enum Rotation rotation);
 void	test();
 void	reset();
 void	info();
+void	regdump();
+void	test_error();
 
 /**
  * Start the driver.
@@ -1149,10 +1276,40 @@ info()
 	exit(0);
 }
 
+/**
+ * Dump the register information
+ */
+void
+regdump(void)
+{
+	if (g_dev == nullptr)
+		errx(1, "driver not running");
+
+	printf("regdump @ %p\n", g_dev);
+	g_dev->print_registers();
+
+	exit(0);
+}
+
+/**
+ * trigger an error
+ */
+void
+test_error(void)
+{
+	if (g_dev == nullptr)
+		errx(1, "driver not running");
+
+	printf("regdump @ %p\n", g_dev);
+	g_dev->test_error();
+
+	exit(0);
+}
+
 void
 usage()
 {
-	warnx("missing command: try 'start', 'info', 'test', 'reset'");
+	warnx("missing command: try 'start', 'info', 'test', 'reset', 'testerror' or 'regdump'");
 	warnx("options:");
 	warnx("    -X    (external bus)");
 	warnx("    -R rotation");
@@ -1209,5 +1366,17 @@ l3gd20_main(int argc, char *argv[])
 	if (!strcmp(verb, "info"))
 		l3gd20::info();
 
-	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
+	/*
+	 * Print register information.
+	 */
+	if (!strcmp(verb, "regdump"))
+		l3gd20::regdump();
+
+	/*
+	 * trigger an error
+	 */
+	if (!strcmp(verb, "testerror"))
+		l3gd20::test_error();
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'testerror' or 'regdump'");
 }

src/drivers/ll40ls/ll40ls.cpp

@@ -746,6 +746,9 @@ start(int bus)
 		if (g_dev_ext != nullptr && OK != g_dev_ext->init()) {
 			delete g_dev_ext;
 			g_dev_ext = nullptr;
+			if (bus == PX4_I2C_BUS_EXPANSION) {
+				goto fail;
+			}
 		}
 	}
 

src/drivers/lsm303d/lsm303d.cpp

@@ -77,10 +77,6 @@
 #endif
 static const int ERROR = -1;
 
-// enable this to debug the buggy lsm303d sensor in very early
-// prototype pixhawk boards
-#define CHECK_EXTREMES 0
-
 /* SPI protocol address bits */
 #define DIR_READ				(1<<7)
 #define DIR_WRITE				(0<<7)
@@ -242,14 +238,9 @@ public:
 	void			print_registers();
 
 	/**
-	 * toggle logging
+	 * deliberately trigger an error
 	 */
-	void			toggle_logging();
-
-	/**
-	 * check for extreme accel values
-	 */
-	void			check_extremes(const accel_report *arb);
+	void			test_error();
 
 protected:
 	virtual int		probe();
@@ -292,30 +283,30 @@ private:
 
 	perf_counter_t		_accel_sample_perf;
 	perf_counter_t		_mag_sample_perf;
-	perf_counter_t		_reg1_resets;
-	perf_counter_t		_reg7_resets;
-	perf_counter_t		_extreme_values;
 	perf_counter_t		_accel_reschedules;
+	perf_counter_t		_bad_registers;
+	perf_counter_t		_bad_values;
+
+	uint8_t			_register_wait;
 
 	math::LowPassFilter2p	_accel_filter_x;
 	math::LowPassFilter2p	_accel_filter_y;
 	math::LowPassFilter2p	_accel_filter_z;
 
-	// expceted values of reg1 and reg7 to catch in-flight
-	// brownouts of the sensor
-	uint8_t			_reg1_expected;
-	uint8_t			_reg7_expected;
-
-	// accel logging
-	int			_accel_log_fd;
-	bool			_accel_logging_enabled;
-	uint64_t		_last_extreme_us;	
-	uint64_t		_last_log_us;	
-	uint64_t		_last_log_sync_us;	
-	uint64_t		_last_log_reg_us;	
-	uint64_t		_last_log_alarm_us;
 	enum Rotation		_rotation;
 
+	// values used to 
+	float			_last_accel[3];
+	uint8_t			_constant_accel_count;
+
+	// this is used to support runtime checking of key
+	// configuration registers to detect SPI bus errors and sensor
+	// reset
+#define LSM303D_NUM_CHECKED_REGISTERS 8
+	static const uint8_t	_checked_registers[LSM303D_NUM_CHECKED_REGISTERS];
+	uint8_t			_checked_values[LSM303D_NUM_CHECKED_REGISTERS];
+	uint8_t			_checked_next;
+
 	/**
 	 * Start automatic measurement.
 	 */
@@ -356,6 +347,11 @@ private:
 	 */
 	static void		mag_measure_trampoline(void *arg);
 
+	/**
+	 * check key registers for correct values
+	 */
+	void			check_registers(void);
+
 	/**
 	 * Fetch accel measurements from the sensor and update the report ring.
 	 */
@@ -407,6 +403,14 @@ private:
 	 */
 	void			modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
 
+	/**
+	 * Write a register in the LSM303D, updating _checked_values
+	 *
+	 * @param reg		The register to write.
+	 * @param value		The new value to write.
+	 */
+	void			write_checked_reg(unsigned reg, uint8_t value);
+
 	/**
 	 * Set the LSM303D accel measurement range.
 	 *
@@ -468,6 +472,19 @@ private:
 	LSM303D operator=(const LSM303D&);
 };
 
+/*
+  list of registers that will be checked in check_registers(). Note
+  that ADDR_WHO_AM_I must be first in the list.
+ */
+const uint8_t LSM303D::_checked_registers[LSM303D_NUM_CHECKED_REGISTERS] = { ADDR_WHO_AM_I,
+									     ADDR_CTRL_REG1,
+									     ADDR_CTRL_REG2,
+									     ADDR_CTRL_REG3,
+									     ADDR_CTRL_REG4,
+									     ADDR_CTRL_REG5,
+									     ADDR_CTRL_REG6,
+                                                                             ADDR_CTRL_REG7 };
+
 /**
  * Helper class implementing the mag driver node.
  */
@@ -528,23 +545,16 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota
 	_mag_read(0),
 	_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
 	_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")),
-	_reg1_resets(perf_alloc(PC_COUNT, "lsm303d_reg1_resets")),
-	_reg7_resets(perf_alloc(PC_COUNT, "lsm303d_reg7_resets")),
-	_extreme_values(perf_alloc(PC_COUNT, "lsm303d_extremes")),
 	_accel_reschedules(perf_alloc(PC_COUNT, "lsm303d_accel_resched")),
+	_bad_registers(perf_alloc(PC_COUNT, "lsm303d_bad_registers")),
+	_bad_values(perf_alloc(PC_COUNT, "lsm303d_bad_values")),
+	_register_wait(0),
 	_accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
 	_accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
 	_accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
-	_reg1_expected(0),
-	_reg7_expected(0),
-	_accel_log_fd(-1),
-	_accel_logging_enabled(false),
-	_last_extreme_us(0),
-	_last_log_us(0),
-	_last_log_sync_us(0),
-	_last_log_reg_us(0),
-	_last_log_alarm_us(0),
-	_rotation(rotation)
+	_rotation(rotation),
+	_constant_accel_count(0),
+	_checked_next(0)
 {
 	_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D;
 
@@ -586,9 +596,8 @@ LSM303D::~LSM303D()
 	/* delete the perf counter */
 	perf_free(_accel_sample_perf);
 	perf_free(_mag_sample_perf);
-	perf_free(_reg1_resets);
-	perf_free(_reg7_resets);
-	perf_free(_extreme_values);
+	perf_free(_bad_registers);
+	perf_free(_bad_values);
 	perf_free(_accel_reschedules);
 }
 
@@ -703,15 +712,14 @@ LSM303D::reset()
 	disable_i2c();
 
 	/* enable accel*/
-	_reg1_expected = REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A;
-	write_reg(ADDR_CTRL_REG1, _reg1_expected);
+	write_checked_reg(ADDR_CTRL_REG1, 
+			  REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A);
 
 	/* enable mag */
-	_reg7_expected = REG7_CONT_MODE_M;
-	write_reg(ADDR_CTRL_REG7, _reg7_expected);
-	write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
-	write_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1
-	write_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2
+	write_checked_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
+	write_checked_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
+	write_checked_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1
+	write_checked_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2
 
 	accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G);
 	accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE);
@@ -739,128 +747,14 @@ LSM303D::probe()
 	/* verify that the device is attached and functioning */
 	bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM);
 	
-	if (success)
+	if (success) {
+		_checked_values[0] = WHO_I_AM;
 		return OK;
+        }
 
 	return -EIO;
 }
 
-#define ACCEL_LOGFILE "/fs/microsd/lsm303d.log"
-
-/**
-   check for extreme accelerometer values and log to a file on the SD card
- */
-void
-LSM303D::check_extremes(const accel_report *arb)
-{
-	const float extreme_threshold = 30;
-        static bool boot_ok = false;
-	bool is_extreme = (fabsf(arb->x) > extreme_threshold && 
-			   fabsf(arb->y) > extreme_threshold && 
-			   fabsf(arb->z) > extreme_threshold);
-	if (is_extreme) {
-		perf_count(_extreme_values);
-		// force accel logging on if we see extreme values
-		_accel_logging_enabled = true;
-	} else {
-            boot_ok = true;
-        }
-
-	if (! _accel_logging_enabled) {
-		// logging has been disabled by user, close
-		if (_accel_log_fd != -1) {
-			::close(_accel_log_fd);
-			_accel_log_fd = -1;
-		}
-		return;
-	}
-	if (_accel_log_fd == -1) {
-		// keep last 10 logs
-		::unlink(ACCEL_LOGFILE ".9");
-		for (uint8_t i=8; i>0; i--) {
-			uint8_t len = strlen(ACCEL_LOGFILE)+3;
-			char log1[len], log2[len];
-			snprintf(log1, sizeof(log1), "%s.%u", ACCEL_LOGFILE, (unsigned)i);
-			snprintf(log2, sizeof(log2), "%s.%u", ACCEL_LOGFILE, (unsigned)(i+1));
-			::rename(log1, log2);
-		}
-		::rename(ACCEL_LOGFILE, ACCEL_LOGFILE ".1");
-
-		// open the new logfile
-		_accel_log_fd = ::open(ACCEL_LOGFILE, O_WRONLY|O_CREAT|O_TRUNC, 0666);
-		if (_accel_log_fd == -1) {
-			return;
-		}
-	}
-
-	uint64_t now = hrt_absolute_time();
-	// log accels at 1Hz
-	if (_last_log_us == 0 ||
-	    now - _last_log_us > 1000*1000) {
-		_last_log_us = now;
-		::dprintf(_accel_log_fd, "ARB %llu %.3f %.3f %.3f %d %d %d boot_ok=%u\r\n",
-			  (unsigned long long)arb->timestamp, 
-			  (double)arb->x, (double)arb->y, (double)arb->z,
-			  (int)arb->x_raw,
-			  (int)arb->y_raw,
-			  (int)arb->z_raw,
-			  (unsigned)boot_ok);
-	}
-
-        const uint8_t reglist[] = { ADDR_WHO_AM_I, 0x02, 0x15, ADDR_STATUS_A, ADDR_STATUS_M, ADDR_CTRL_REG0, ADDR_CTRL_REG1, 
-                                    ADDR_CTRL_REG2, ADDR_CTRL_REG3, ADDR_CTRL_REG4, ADDR_CTRL_REG5, ADDR_CTRL_REG6, 
-                                    ADDR_CTRL_REG7, ADDR_OUT_TEMP_L, ADDR_OUT_TEMP_H, ADDR_INT_CTRL_M, ADDR_INT_SRC_M, 
-                                    ADDR_REFERENCE_X, ADDR_REFERENCE_Y, ADDR_REFERENCE_Z, ADDR_OUT_X_L_A, ADDR_OUT_X_H_A, 
-                                    ADDR_OUT_Y_L_A, ADDR_OUT_Y_H_A, ADDR_OUT_Z_L_A, ADDR_OUT_Z_H_A, ADDR_FIFO_CTRL, 
-                                    ADDR_FIFO_SRC, ADDR_IG_CFG1, ADDR_IG_SRC1, ADDR_IG_THS1, ADDR_IG_DUR1, ADDR_IG_CFG2, 
-                                    ADDR_IG_SRC2, ADDR_IG_THS2, ADDR_IG_DUR2, ADDR_CLICK_CFG, ADDR_CLICK_SRC, 
-                                    ADDR_CLICK_THS, ADDR_TIME_LIMIT, ADDR_TIME_LATENCY, ADDR_TIME_WINDOW, 
-                                    ADDR_ACT_THS, ADDR_ACT_DUR,
-                                    ADDR_OUT_X_L_M, ADDR_OUT_X_H_M, 
-                                    ADDR_OUT_Y_L_M, ADDR_OUT_Y_H_M, ADDR_OUT_Z_L_M, ADDR_OUT_Z_H_M, 0x02, 0x15, ADDR_WHO_AM_I};
-        uint8_t regval[sizeof(reglist)];
-        for (uint8_t i=0; i<sizeof(reglist); i++) {
-            regval[i] = read_reg(reglist[i]);
-        }
-
-	// log registers at 10Hz when we have extreme values, or 0.5 Hz without
-	if (_last_log_reg_us == 0 ||
-	    (is_extreme && (now - _last_log_reg_us > 250*1000)) ||
-	    (now - _last_log_reg_us > 10*1000*1000)) {
-		_last_log_reg_us = now;
-		::dprintf(_accel_log_fd, "XREG %llu", (unsigned long long)hrt_absolute_time());
-		for (uint8_t i=0; i<sizeof(reglist); i++) {
-			::dprintf(_accel_log_fd, " %02x:%02x", (unsigned)reglist[i], (unsigned)regval[i]);
-		}
-		::dprintf(_accel_log_fd, "\n");
-	}
-
-	// fsync at 0.1Hz
-	if (now - _last_log_sync_us > 10*1000*1000) {
-		_last_log_sync_us = now;
-		::fsync(_accel_log_fd);
-	}
-
-	// play alarm every 10s if we have had an extreme value
-	if (perf_event_count(_extreme_values) != 0 && 
-	    (now - _last_log_alarm_us > 10*1000*1000)) {
-		_last_log_alarm_us = now;
-		int tfd = ::open(TONEALARM_DEVICE_PATH, 0);
-		if (tfd != -1) {
-			uint8_t tone = 3;
-			if (!is_extreme) {
-				tone = 3;
-			} else if (boot_ok) {
-				tone = 4;
-			} else {
-				tone = 5;
-			}
-			::ioctl(tfd, TONE_SET_ALARM, tone);
-			::close(tfd);
-		}		
-	}
-}
-
 ssize_t
 LSM303D::read(struct file *filp, char *buffer, size_t buflen)
 {
@@ -879,9 +773,6 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen)
 		 */
 		while (count--) {
 			if (_accel_reports->get(arb)) {
-#if CHECK_EXTREMES
-				check_extremes(arb);
-#endif
 				ret += sizeof(*arb);
 				arb++;
 			}
@@ -1262,6 +1153,17 @@ LSM303D::write_reg(unsigned reg, uint8_t value)
 	transfer(cmd, nullptr, sizeof(cmd));
 }
 
+void
+LSM303D::write_checked_reg(unsigned reg, uint8_t value)
+{
+	write_reg(reg, value);
+	for (uint8_t i=0; i<LSM303D_NUM_CHECKED_REGISTERS; i++) {
+		if (reg == _checked_registers[i]) {
+			_checked_values[i] = value;
+		}
+	}
+}
+
 void
 LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
 {
@@ -1270,7 +1172,7 @@ LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
 	val = read_reg(reg);
 	val &= ~clearbits;
 	val |= setbits;
-	write_reg(reg, val);
+	write_checked_reg(reg, val);
 }
 
 int
@@ -1439,7 +1341,6 @@ LSM303D::accel_set_samplerate(unsigned frequency)
 	}
 
 	modify_reg(ADDR_CTRL_REG1, clearbits, setbits);
-	_reg1_expected = (_reg1_expected & ~clearbits) | setbits;
 
 	return OK;
 }
@@ -1514,25 +1415,36 @@ LSM303D::mag_measure_trampoline(void *arg)
 	dev->mag_measure();
 }
 
+void
+LSM303D::check_registers(void)
+{
+	uint8_t v;
+	if ((v=read_reg(_checked_registers[_checked_next])) != _checked_values[_checked_next]) {
+		/*
+		  if we get the wrong value then we know the SPI bus
+		  or sensor is very sick. We set _register_wait to 20
+		  and wait until we have seen 20 good values in a row
+		  before we consider the sensor to be OK again. 
+		 */
+		perf_count(_bad_registers);
+
+		/*
+		  try to fix the bad register value. We only try to
+		  fix one per loop to prevent a bad sensor hogging the
+		  bus. We skip zero as that is the WHO_AM_I, which
+		  is not writeable
+		 */
+		if (_checked_next != 0) {
+			write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]);
+		}
+		_register_wait = 20;
+        }
+        _checked_next = (_checked_next+1) % LSM303D_NUM_CHECKED_REGISTERS;
+}
+
 void
 LSM303D::measure()
 {
-	// if the accel doesn't have any data ready then re-schedule
-	// for 100 microseconds later. This ensures we don't double
-	// read a value and then miss the next value.
-	// Note that DRDY is not available when the lsm303d is
-	// connected on the external bus
-	if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) {
-		perf_count(_accel_reschedules);
-		hrt_call_delay(&_accel_call, 100);
-		return;
-	}
-	if (read_reg(ADDR_CTRL_REG1) != _reg1_expected) {
-		perf_count(_reg1_resets);
-		reset();
-		return;
-	}
-
 	/* status register and data as read back from the device */
 
 #pragma pack(push, 1)
@@ -1550,6 +1462,30 @@ LSM303D::measure()
 	/* start the performance counter */
 	perf_begin(_accel_sample_perf);
 
+	check_registers();
+
+	// if the accel doesn't have any data ready then re-schedule
+	// for 100 microseconds later. This ensures we don't double
+	// read a value and then miss the next value.
+	// Note that DRDY is not available when the lsm303d is
+	// connected on the external bus
+#ifdef GPIO_EXTI_ACCEL_DRDY
+	if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) {
+		perf_count(_accel_reschedules);
+		hrt_call_delay(&_accel_call, 100);
+                perf_end(_accel_sample_perf);
+		return;
+	}
+#endif
+
+	if (_register_wait != 0) {
+		// we are waiting for some good transfers before using
+		// the sensor again.
+		_register_wait--;
+		perf_end(_accel_sample_perf);
+		return;
+	}
+
 	/* fetch data from the sensor */
 	memset(&raw_accel_report, 0, sizeof(raw_accel_report));
 	raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT;
@@ -1572,7 +1508,12 @@ LSM303D::measure()
 
 
 	accel_report.timestamp = hrt_absolute_time();
-        accel_report.error_count = 0; // not reported
+
+	// report the error count as the sum of the number of bad
+	// register reads and bad values. This allows the higher level
+	// code to decide if it should use this sensor based on
+	// whether it has had failures
+        accel_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values);
 
 	accel_report.x_raw = raw_accel_report.x;
 	accel_report.y_raw = raw_accel_report.y;
@@ -1582,6 +1523,36 @@ LSM303D::measure()
 	float y_in_new = ((accel_report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
 	float z_in_new = ((accel_report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
 
+	/*
+	  we have logs where the accelerometers get stuck at a fixed
+	  large value. We want to detect this and mark the sensor as
+	  being faulty
+	 */
+	if (fabsf(_last_accel[0] - x_in_new) < 0.001f &&
+	    fabsf(_last_accel[1] - y_in_new) < 0.001f &&
+	    fabsf(_last_accel[2] - z_in_new) < 0.001f &&
+	    fabsf(x_in_new) > 20 &&
+	    fabsf(y_in_new) > 20 &&
+	    fabsf(z_in_new) > 20) {
+		_constant_accel_count += 1;
+	} else {
+		_constant_accel_count = 0;
+	}
+	if (_constant_accel_count > 100) {
+		// we've had 100 constant accel readings with large
+		// values. The sensor is almost certainly dead. We
+		// will raise the error_count so that the top level
+		// flight code will know to avoid this sensor, but
+		// we'll still give the data so that it can be logged
+		// and viewed
+		perf_count(_bad_values);
+		_constant_accel_count = 0;
+	}
+	    
+	_last_accel[0] = x_in_new;
+	_last_accel[1] = y_in_new;
+	_last_accel[2] = z_in_new;
+
 	accel_report.x = _accel_filter_x.apply(x_in_new);
 	accel_report.y = _accel_filter_y.apply(y_in_new);
 	accel_report.z = _accel_filter_z.apply(z_in_new);
@@ -1611,12 +1582,6 @@ LSM303D::measure()
 void
 LSM303D::mag_measure()
 {
-	if (read_reg(ADDR_CTRL_REG7) != _reg7_expected) {
-		perf_count(_reg7_resets);
-		reset();
-		return;
-	}
-
 	/* status register and data as read back from the device */
 #pragma pack(push, 1)
 	struct {
@@ -1691,8 +1656,22 @@ LSM303D::print_info()
 	printf("accel reads:          %u\n", _accel_read);
 	printf("mag reads:            %u\n", _mag_read);
 	perf_print_counter(_accel_sample_perf);
+	perf_print_counter(_mag_sample_perf);
+	perf_print_counter(_bad_registers);
+	perf_print_counter(_bad_values);
+	perf_print_counter(_accel_reschedules);
 	_accel_reports->print_info("accel reports");
 	_mag_reports->print_info("mag reports");
+        ::printf("checked_next: %u\n", _checked_next);
+        for (uint8_t i=0; i<LSM303D_NUM_CHECKED_REGISTERS; i++) {
+            uint8_t v = read_reg(_checked_registers[i]);
+            if (v != _checked_values[i]) {
+                ::printf("reg %02x:%02x should be %02x\n", 
+                         (unsigned)_checked_registers[i],
+                         (unsigned)v,
+                         (unsigned)_checked_values[i]);
+            }
+        }
 }
 
 void
@@ -1750,20 +1729,13 @@ LSM303D::print_registers()
 	for (uint8_t i=0; i<sizeof(regmap)/sizeof(regmap[0]); i++) {
 		printf("0x%02x %s\n", read_reg(regmap[i].reg), regmap[i].name);
 	}
-	printf("_reg1_expected=0x%02x\n", _reg1_expected);
-	printf("_reg7_expected=0x%02x\n", _reg7_expected);
 }
 
 void
-LSM303D::toggle_logging()
+LSM303D::test_error()
 {
-	if (! _accel_logging_enabled) {
-		_accel_logging_enabled = true;
-		printf("Started logging to %s\n", ACCEL_LOGFILE);
-	} else {
-		_accel_logging_enabled = false;
-		printf("Stopped logging\n");
-	}
+	// trigger an error
+        write_reg(ADDR_CTRL_REG3, 0);
 }
 
 LSM303D_mag::LSM303D_mag(LSM303D *parent) :
@@ -1839,8 +1811,8 @@ void	test();
 void	reset();
 void	info();
 void	regdump();
-void	logging();
 void	usage();
+void	test_error();
 
 /**
  * Start the driver.
@@ -2047,15 +2019,15 @@ regdump()
 }
 
 /**
- * toggle logging
+ * trigger an error
  */
 void
-logging()
+test_error()
 {
 	if (g_dev == nullptr)
 		errx(1, "driver not running\n");
 
-	g_dev->toggle_logging();
+	g_dev->test_error();
 
 	exit(0);
 }
@@ -2063,7 +2035,7 @@ logging()
 void
 usage()
 {
-	warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump', 'logging'");
+	warnx("missing command: try 'start', 'info', 'test', 'reset', 'testerror' or 'regdump'");
 	warnx("options:");
 	warnx("    -X    (external bus)");
 	warnx("    -R rotation");
@@ -2127,10 +2099,10 @@ lsm303d_main(int argc, char *argv[])
 		lsm303d::regdump();
 
 	/*
-	 * dump device registers
+	 * trigger an error
 	 */
-	if (!strcmp(verb, "logging"))
-		lsm303d::logging();
+	if (!strcmp(verb, "testerror"))
+		lsm303d::test_error();
 
-	errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'logging' or 'regdump'");
+	errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'testerror' or 'regdump'");
 }

src/drivers/mpu6000/mpu6000.cpp

@@ -113,6 +113,10 @@
 #define MPUREG_FIFO_COUNTL		0x73
 #define MPUREG_FIFO_R_W			0x74
 #define MPUREG_PRODUCT_ID		0x0C
+#define MPUREG_TRIM1			0x0D
+#define MPUREG_TRIM2			0x0E
+#define MPUREG_TRIM3			0x0F
+#define MPUREG_TRIM4			0x10
 
 // Configuration bits MPU 3000 and MPU 6000 (not revised)?
 #define BIT_SLEEP			0x40
@@ -121,6 +125,9 @@
 #define MPU_CLK_SEL_PLLGYROX		0x01
 #define MPU_CLK_SEL_PLLGYROZ		0x03
 #define MPU_EXT_SYNC_GYROX		0x02
+#define BITS_GYRO_ST_X			0x80
+#define BITS_GYRO_ST_Y			0x40
+#define BITS_GYRO_ST_Z			0x20
 #define BITS_FS_250DPS			0x00
 #define BITS_FS_500DPS			0x08
 #define BITS_FS_1000DPS			0x10
@@ -196,6 +203,16 @@ public:
 
 	void			print_registers();
 
+	/**
+	 * Test behaviour against factory offsets
+	 *
+	 * @return 0 on success, 1 on failure
+	 */
+	int 			factory_self_test();
+
+	// deliberately cause a sensor error
+	void 			test_error();
+
 protected:
 	virtual int		probe();
 
@@ -231,7 +248,12 @@ private:
 	perf_counter_t		_gyro_reads;
 	perf_counter_t		_sample_perf;
 	perf_counter_t		_bad_transfers;
+	perf_counter_t		_bad_registers;
 	perf_counter_t		_good_transfers;
+	perf_counter_t		_reset_retries;
+
+	uint8_t			_register_wait;
+	uint64_t		_reset_wait;
 
 	math::LowPassFilter2p	_accel_filter_x;
 	math::LowPassFilter2p	_accel_filter_y;
@@ -242,6 +264,18 @@ private:
 
 	enum Rotation		_rotation;
 
+	// this is used to support runtime checking of key
+	// configuration registers to detect SPI bus errors and sensor
+	// reset
+#define MPU6000_NUM_CHECKED_REGISTERS 9
+	static const uint8_t	_checked_registers[MPU6000_NUM_CHECKED_REGISTERS];
+	uint8_t			_checked_values[MPU6000_NUM_CHECKED_REGISTERS];
+	uint8_t			_checked_next;
+
+	// use this to avoid processing measurements when in factory
+	// self test
+	volatile bool		_in_factory_test;
+
 	/**
 	 * Start automatic measurement.
 	 */
@@ -257,7 +291,7 @@ private:
 	 *
 	 * Resets the chip and measurements ranges, but not scale and offset.
 	 */
-	void			reset();
+	int			reset();
 
 	/**
 	 * Static trampoline from the hrt_call context; because we don't have a
@@ -281,7 +315,7 @@ private:
 	 * @param		The register to read.
 	 * @return		The value that was read.
 	 */
-	uint8_t			read_reg(unsigned reg);
+	uint8_t			read_reg(unsigned reg, uint32_t speed=MPU6000_LOW_BUS_SPEED);
 	uint16_t		read_reg16(unsigned reg);
 
 	/**
@@ -303,6 +337,14 @@ private:
 	 */
 	void			modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
 
+	/**
+	 * Write a register in the MPU6000, updating _checked_values
+	 *
+	 * @param reg		The register to write.
+	 * @param value		The new value to write.
+	 */
+	void			write_checked_reg(unsigned reg, uint8_t value);
+
 	/**
 	 * Set the MPU6000 measurement range.
 	 *
@@ -347,11 +389,50 @@ private:
 	*/
 	void _set_sample_rate(uint16_t desired_sample_rate_hz);
 
+	/*
+	  check that key registers still have the right value
+	 */
+	void check_registers(void);
+
 	/* do not allow to copy this class due to pointer data members */
 	MPU6000(const MPU6000&);
 	MPU6000 operator=(const MPU6000&);
+
+#pragma pack(push, 1)
+	/**
+	 * Report conversation within the MPU6000, including command byte and
+	 * interrupt status.
+	 */
+	struct MPUReport {
+		uint8_t		cmd;
+		uint8_t		status;
+		uint8_t		accel_x[2];
+		uint8_t		accel_y[2];
+		uint8_t		accel_z[2];
+		uint8_t		temp[2];
+		uint8_t		gyro_x[2];
+		uint8_t		gyro_y[2];
+		uint8_t		gyro_z[2];
+	};
+#pragma pack(pop)
 };
 
+/*
+  list of registers that will be checked in check_registers(). Note
+  that MPUREG_PRODUCT_ID must be first in the list.
+ */
+const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPUREG_PRODUCT_ID,
+									     MPUREG_PWR_MGMT_1,
+									     MPUREG_USER_CTRL,
+									     MPUREG_SMPLRT_DIV,
+                                                                             MPUREG_CONFIG,
+									     MPUREG_GYRO_CONFIG,
+									     MPUREG_ACCEL_CONFIG,
+									     MPUREG_INT_ENABLE,
+									     MPUREG_INT_PIN_CFG };
+
+					   
+
 /**
  * Helper class implementing the gyro driver node.
  */
@@ -407,14 +488,20 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
 	_gyro_reads(perf_alloc(PC_COUNT, "mpu6000_gyro_read")),
 	_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")),
 	_bad_transfers(perf_alloc(PC_COUNT, "mpu6000_bad_transfers")),
+	_bad_registers(perf_alloc(PC_COUNT, "mpu6000_bad_registers")),
 	_good_transfers(perf_alloc(PC_COUNT, "mpu6000_good_transfers")),
+	_reset_retries(perf_alloc(PC_COUNT, "mpu6000_reset_retries")),
+	_register_wait(0),
+	_reset_wait(0),
 	_accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
 	_accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
 	_accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
 	_gyro_filter_x(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
 	_gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
 	_gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
-	_rotation(rotation)
+	_rotation(rotation),
+	_checked_next(0),
+	_in_factory_test(false)
 {
 	// disable debug() calls
 	_debug_enabled = false;
@@ -460,6 +547,7 @@ MPU6000::~MPU6000()
 	perf_free(_accel_reads);
 	perf_free(_gyro_reads);
 	perf_free(_bad_transfers);
+	perf_free(_bad_registers);
 	perf_free(_good_transfers);
 }
 
@@ -486,7 +574,8 @@ MPU6000::init()
 	if (_gyro_reports == nullptr)
 		goto out;
 
-	reset();
+	if (reset() != OK)
+		goto out;
 
 	/* Initialize offsets and scales */
 	_accel_scale.x_offset = 0;
@@ -571,27 +660,39 @@ out:
 	return ret;
 }
 
-void MPU6000::reset()
+int MPU6000::reset()
 {
 	// if the mpu6000 is initialised after the l3gd20 and lsm303d
 	// then if we don't do an irqsave/irqrestore here the mpu6000
 	// frequenctly comes up in a bad state where all transfers
 	// come as zero
-	irqstate_t state;
-	state = irqsave();
+	uint8_t tries = 5;
+	while (--tries != 0) {
+		irqstate_t state;
+		state = irqsave();
 
-	write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
-	up_udelay(10000);
+		write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
+		up_udelay(10000);
 
-	// Wake up device and select GyroZ clock. Note that the
-	// MPU6000 starts up in sleep mode, and it can take some time
-	// for it to come out of sleep
-	write_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
-	up_udelay(1000);
+		// Wake up device and select GyroZ clock. Note that the
+		// MPU6000 starts up in sleep mode, and it can take some time
+		// for it to come out of sleep
+		write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
+		up_udelay(1000);
+		
+		// Disable I2C bus (recommended on datasheet)
+		write_checked_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
+		irqrestore(state);
 
-	// Disable I2C bus (recommended on datasheet)
-	write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
-        irqrestore(state);
+		if (read_reg(MPUREG_PWR_MGMT_1) == MPU_CLK_SEL_PLLGYROZ) {
+			break;
+		}
+		perf_count(_reset_retries);
+		usleep(2000);
+	}
+	if (read_reg(MPUREG_PWR_MGMT_1) != MPU_CLK_SEL_PLLGYROZ) {
+		return -EIO;
+	}
 
 	usleep(1000);
 
@@ -605,7 +706,7 @@ void MPU6000::reset()
 	_set_dlpf_filter(MPU6000_DEFAULT_ONCHIP_FILTER_FREQ);
 	usleep(1000);
 	// Gyro scale 2000 deg/s ()
-	write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
+	write_checked_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
 	usleep(1000);
 
 	// correct gyro scale factors
@@ -624,7 +725,7 @@ void MPU6000::reset()
 	case MPU6000_REV_C5:
 		// Accel scale 8g (4096 LSB/g)
 		// Rev C has different scaling than rev D
-		write_reg(MPUREG_ACCEL_CONFIG, 1 << 3);
+		write_checked_reg(MPUREG_ACCEL_CONFIG, 1 << 3);
 		break;
 
 	case MPU6000ES_REV_D6:
@@ -639,7 +740,7 @@ void MPU6000::reset()
 	// presumably won't have the accel scaling bug		
 	default:
 		// Accel scale 8g (4096 LSB/g)
-		write_reg(MPUREG_ACCEL_CONFIG, 2 << 3);
+		write_checked_reg(MPUREG_ACCEL_CONFIG, 2 << 3);
 		break;
 	}
 
@@ -651,15 +752,16 @@ void MPU6000::reset()
 	usleep(1000);
 
 	// INT CFG => Interrupt on Data Ready
-	write_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN);        // INT: Raw data ready
+	write_checked_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN);        // INT: Raw data ready
 	usleep(1000);
-	write_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read
+	write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read
 	usleep(1000);
 
 	// Oscillator set
 	// write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ);
 	usleep(1000);
 
+	return OK;
 }
 
 int
@@ -684,6 +786,7 @@ MPU6000::probe()
 	case MPU6000_REV_D9:
 	case MPU6000_REV_D10:
 		debug("ID 0x%02x", _product);
+		_checked_values[0] = _product;
 		return OK;
 	}
 
@@ -700,7 +803,7 @@ MPU6000::_set_sample_rate(uint16_t desired_sample_rate_hz)
   uint8_t div = 1000 / desired_sample_rate_hz;
   if(div>200) div=200;
   if(div<1) div=1;
-  write_reg(MPUREG_SMPLRT_DIV, div-1);
+  write_checked_reg(MPUREG_SMPLRT_DIV, div-1);
   _sample_rate = 1000 / div;
 }
 
@@ -734,7 +837,7 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz)
 	} else {
 		filter = BITS_DLPF_CFG_2100HZ_NOLPF;
 	}
-	write_reg(MPUREG_CONFIG, filter);
+	write_checked_reg(MPUREG_CONFIG, filter);
 }
 
 ssize_t
@@ -833,6 +936,173 @@ MPU6000::gyro_self_test()
 	return 0;
 }
 
+
+/*
+  perform a self-test comparison to factory trim values. This takes
+  about 200ms and will return OK if the current values are within 14%
+  of the expected values (as per datasheet)
+ */
+int
+MPU6000::factory_self_test()
+{
+	_in_factory_test = true;
+	uint8_t saved_gyro_config = read_reg(MPUREG_GYRO_CONFIG);
+	uint8_t saved_accel_config = read_reg(MPUREG_ACCEL_CONFIG);
+	const uint16_t repeats = 100;
+	int ret = OK;
+
+	// gyro self test has to be done at 250DPS
+	write_reg(MPUREG_GYRO_CONFIG, BITS_FS_250DPS);
+
+	struct MPUReport mpu_report;	
+	float accel_baseline[3];
+	float gyro_baseline[3];
+	float accel[3];
+	float gyro[3];
+	float accel_ftrim[3];
+	float gyro_ftrim[3];
+
+	// get baseline values without self-test enabled
+        set_frequency(MPU6000_HIGH_BUS_SPEED);
+
+	memset(accel_baseline, 0, sizeof(accel_baseline));
+	memset(gyro_baseline, 0, sizeof(gyro_baseline));
+	memset(accel, 0, sizeof(accel));
+	memset(gyro, 0, sizeof(gyro));
+
+	for (uint8_t i=0; i<repeats; i++) {
+		up_udelay(1000);
+		mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS;
+		transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report));
+
+		accel_baseline[0] += int16_t_from_bytes(mpu_report.accel_x);
+		accel_baseline[1] += int16_t_from_bytes(mpu_report.accel_y);
+		accel_baseline[2] += int16_t_from_bytes(mpu_report.accel_z);
+		gyro_baseline[0] += int16_t_from_bytes(mpu_report.gyro_x);
+		gyro_baseline[1] += int16_t_from_bytes(mpu_report.gyro_y);
+		gyro_baseline[2] += int16_t_from_bytes(mpu_report.gyro_z);
+	}
+
+#if 1
+	write_reg(MPUREG_GYRO_CONFIG, 
+		  BITS_FS_250DPS | 
+		  BITS_GYRO_ST_X | 
+		  BITS_GYRO_ST_Y | 
+		  BITS_GYRO_ST_Z);
+
+	// accel 8g, self-test enabled all axes
+	write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config | 0xE0);
+#endif
+
+	up_udelay(20000);
+
+	// get values with self-test enabled
+        set_frequency(MPU6000_HIGH_BUS_SPEED);
+
+
+	for (uint8_t i=0; i<repeats; i++) {
+		up_udelay(1000);
+		mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS;
+		transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report));
+		accel[0] += int16_t_from_bytes(mpu_report.accel_x);
+		accel[1] += int16_t_from_bytes(mpu_report.accel_y);
+		accel[2] += int16_t_from_bytes(mpu_report.accel_z);
+		gyro[0] += int16_t_from_bytes(mpu_report.gyro_x);
+		gyro[1] += int16_t_from_bytes(mpu_report.gyro_y);
+		gyro[2] += int16_t_from_bytes(mpu_report.gyro_z);
+	}
+
+	for (uint8_t i=0; i<3; i++) {
+		accel_baseline[i] /= repeats;
+		gyro_baseline[i] /= repeats;
+		accel[i] /= repeats;
+		gyro[i] /= repeats;
+	}
+
+	// extract factory trim values
+	uint8_t trims[4];
+	trims[0] = read_reg(MPUREG_TRIM1);
+	trims[1] = read_reg(MPUREG_TRIM2);
+	trims[2] = read_reg(MPUREG_TRIM3);
+	trims[3] = read_reg(MPUREG_TRIM4);
+	uint8_t atrim[3];
+	uint8_t gtrim[3];
+
+	atrim[0] = ((trims[0]>>3)&0x1C) | ((trims[3]>>4)&0x03);
+	atrim[1] = ((trims[1]>>3)&0x1C) | ((trims[3]>>2)&0x03);
+	atrim[2] = ((trims[2]>>3)&0x1C) | ((trims[3]>>0)&0x03);
+	gtrim[0] = trims[0] & 0x1F;
+	gtrim[1] = trims[1] & 0x1F;
+	gtrim[2] = trims[2] & 0x1F;
+
+	// convert factory trims to right units
+	for (uint8_t i=0; i<3; i++) {
+		accel_ftrim[i] = 4096 * 0.34f * powf(0.92f/0.34f, (atrim[i]-1)/30.0f);
+		gyro_ftrim[i] = 25 * 131.0f * powf(1.046f, gtrim[i]-1);
+	}
+	// Y gyro trim is negative
+	gyro_ftrim[1] *= -1;
+
+	for (uint8_t i=0; i<3; i++) {
+		float diff = accel[i]-accel_baseline[i];
+		float err = 100*(diff - accel_ftrim[i]) / accel_ftrim[i];
+		::printf("ACCEL[%u] baseline=%d accel=%d diff=%d ftrim=%d err=%d\n",
+			 (unsigned)i,
+			 (int)(1000*accel_baseline[i]),
+			 (int)(1000*accel[i]),
+			 (int)(1000*diff),
+			 (int)(1000*accel_ftrim[i]),
+			 (int)err);
+		if (fabsf(err) > 14) {
+			::printf("FAIL\n");
+			ret = -EIO;
+		}
+	}	
+	for (uint8_t i=0; i<3; i++) {
+		float diff = gyro[i]-gyro_baseline[i];
+		float err = 100*(diff - gyro_ftrim[i]) / gyro_ftrim[i];
+		::printf("GYRO[%u] baseline=%d gyro=%d diff=%d ftrim=%d err=%d\n",
+			 (unsigned)i,
+			 (int)(1000*gyro_baseline[i]),
+			 (int)(1000*gyro[i]),
+			 (int)(1000*(gyro[i]-gyro_baseline[i])),
+			 (int)(1000*gyro_ftrim[i]),
+			 (int)err);
+		if (fabsf(err) > 14) {
+			::printf("FAIL\n");
+			ret = -EIO;
+		}
+	}	
+
+	write_reg(MPUREG_GYRO_CONFIG, saved_gyro_config);
+	write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config);
+
+	_in_factory_test = false;
+	if (ret == OK) {
+		::printf("PASSED\n");
+	}
+
+	return ret;
+}
+
+
+/*
+  deliberately trigger an error in the sensor to trigger recovery
+ */
+void
+MPU6000::test_error()
+{
+	_in_factory_test = true;
+	// deliberately trigger an error. This was noticed during
+	// development as a handy way to test the reset logic
+	uint8_t data[16];
+	memset(data, 0, sizeof(data));
+	transfer(data, data, sizeof(data));
+	::printf("error triggered\n");
+	print_registers();
+	_in_factory_test = false;
+}
+
 ssize_t
 MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen)
 {
@@ -874,8 +1144,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 	switch (cmd) {
 
 	case SENSORIOCRESET:
-		reset();
-		return OK;
+		return reset();
 
 	case SENSORIOCSPOLLRATE: {
 			switch (arg) {
@@ -1094,12 +1363,12 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 }
 
 uint8_t
-MPU6000::read_reg(unsigned reg)
+MPU6000::read_reg(unsigned reg, uint32_t speed)
 {
 	uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0};
 
         // general register transfer at low clock speed
-        set_frequency(MPU6000_LOW_BUS_SPEED);
+        set_frequency(speed);
 
 	transfer(cmd, cmd, sizeof(cmd));
 
@@ -1144,6 +1413,17 @@ MPU6000::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
 	write_reg(reg, val);
 }
 
+void
+MPU6000::write_checked_reg(unsigned reg, uint8_t value)
+{
+	write_reg(reg, value);
+	for (uint8_t i=0; i<MPU6000_NUM_CHECKED_REGISTERS; i++) {
+		if (reg == _checked_registers[i]) {
+			_checked_values[i] = value;
+		}
+	}
+}
+
 int
 MPU6000::set_range(unsigned max_g)
 {
@@ -1215,27 +1495,72 @@ MPU6000::measure_trampoline(void *arg)
 	dev->measure();
 }
 
+void
+MPU6000::check_registers(void)
+{
+	/*
+	  we read the register at full speed, even though it isn't
+	  listed as a high speed register. The low speed requirement
+	  for some registers seems to be a propgation delay
+	  requirement for changing sensor configuration, which should
+	  not apply to reading a single register. It is also a better
+	  test of SPI bus health to read at the same speed as we read
+	  the data registers.
+	*/
+	uint8_t v;
+	if ((v=read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) != 
+	    _checked_values[_checked_next]) {
+		/*
+		  if we get the wrong value then we know the SPI bus
+		  or sensor is very sick. We set _register_wait to 20
+		  and wait until we have seen 20 good values in a row
+		  before we consider the sensor to be OK again. 
+		 */
+		perf_count(_bad_registers);
+
+		/*
+		  try to fix the bad register value. We only try to
+		  fix one per loop to prevent a bad sensor hogging the
+		  bus.
+		 */
+		if (_register_wait == 0 || _checked_next == 0) {
+			// if the product_id is wrong then reset the
+			// sensor completely
+			write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
+			// after doing a reset we need to wait a long
+			// time before we do any other register writes
+			// or we will end up with the mpu6000 in a
+			// bizarre state where it has all correct
+			// register values but large offsets on the
+			// accel axes
+			_reset_wait = hrt_absolute_time() + 10000;
+                        _checked_next = 0;
+		} else {
+			write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]);
+			// waiting 3ms between register writes seems
+			// to raise the chance of the sensor
+			// recovering considerably
+			_reset_wait = hrt_absolute_time() + 3000;
+		}
+		_register_wait = 20;
+	}
+	_checked_next = (_checked_next+1) % MPU6000_NUM_CHECKED_REGISTERS;
+}
+
 void
 MPU6000::measure()
 {
-#pragma pack(push, 1)
-	/**
-	 * Report conversation within the MPU6000, including command byte and
-	 * interrupt status.
-	 */
-	struct MPUReport {
-		uint8_t		cmd;
-		uint8_t		status;
-		uint8_t		accel_x[2];
-		uint8_t		accel_y[2];
-		uint8_t		accel_z[2];
-		uint8_t		temp[2];
-		uint8_t		gyro_x[2];
-		uint8_t		gyro_y[2];
-		uint8_t		gyro_z[2];
-	} mpu_report;
-#pragma pack(pop)
+	if (_in_factory_test) {
+		// don't publish any data while in factory test mode
+		return;
+	}
 
+	if (hrt_absolute_time() < _reset_wait) {
+		// we're waiting for a reset to complete
+		return;
+	}
+
+	struct MPUReport mpu_report;
 	struct Report {
 		int16_t		accel_x;
 		int16_t		accel_y;
@@ -1254,6 +1579,8 @@ MPU6000::measure()
 	 */
 	mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS;
 
+	check_registers();
+
         // sensor transfer at high clock speed
         set_frequency(MPU6000_HIGH_BUS_SPEED);
 
@@ -1292,6 +1619,14 @@ MPU6000::measure()
 	}
 
 	perf_count(_good_transfers);
+
+	if (_register_wait != 0) {
+		// we are waiting for some good transfers before using
+		// the sensor again. We still increment
+		// _good_transfers, but don't return any data yet
+		_register_wait--;
+		return;
+	}
 	    
 
 	/*
@@ -1321,7 +1656,12 @@ MPU6000::measure()
 	 * Adjust and scale results to m/s^2.
 	 */
 	grb.timestamp = arb.timestamp = hrt_absolute_time();
-        grb.error_count = arb.error_count = 0; // not reported
+
+	// report the error count as the sum of the number of bad
+	// transfers and bad register reads. This allows the higher
+	// level code to decide if it should use this sensor based on
+	// whether it has had failures
+        grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);
 
 	/*
 	 * 1) Scale raw value to SI units using scaling from datasheet.
@@ -1411,9 +1751,21 @@ MPU6000::print_info()
 	perf_print_counter(_accel_reads);
 	perf_print_counter(_gyro_reads);
 	perf_print_counter(_bad_transfers);
+	perf_print_counter(_bad_registers);
 	perf_print_counter(_good_transfers);
+	perf_print_counter(_reset_retries);
 	_accel_reports->print_info("accel queue");
 	_gyro_reports->print_info("gyro queue");
+        ::printf("checked_next: %u\n", _checked_next);
+        for (uint8_t i=0; i<MPU6000_NUM_CHECKED_REGISTERS; i++) {
+            uint8_t v = read_reg(_checked_registers[i], MPU6000_HIGH_BUS_SPEED);
+            if (v != _checked_values[i]) {
+                ::printf("reg %02x:%02x should be %02x\n", 
+                         (unsigned)_checked_registers[i],
+                         (unsigned)v,
+                         (unsigned)_checked_values[i]);
+            }
+        }
 }
 
 void
@@ -1497,6 +1849,8 @@ void	test(bool);
 void	reset(bool);
 void	info(bool);
 void	regdump(bool);
+void	testerror(bool);
+void	factorytest(bool);
 void	usage();
 
 /**
@@ -1688,10 +2042,40 @@ regdump(bool external_bus)
 	exit(0);
 }
 
+/**
+ * deliberately produce an error to test recovery
+ */
+void
+testerror(bool external_bus)
+{
+	MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int;
+	if (*g_dev_ptr == nullptr)
+		errx(1, "driver not running");
+
+	(*g_dev_ptr)->test_error();
+
+	exit(0);
+}
+
+/**
+ * Dump the register information
+ */
+void
+factorytest(bool external_bus)
+{
+	MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int;
+	if (*g_dev_ptr == nullptr)
+		errx(1, "driver not running");
+
+	(*g_dev_ptr)->factory_self_test();
+
+	exit(0);
+}
+
 void
 usage()
 {
-	warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump'");
+	warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump', 'factorytest', 'testerror'");
 	warnx("options:");
 	warnx("    -X    (external bus)");
 	warnx("    -R rotation");
@@ -1754,5 +2138,11 @@ mpu6000_main(int argc, char *argv[])
 	if (!strcmp(verb, "regdump"))
 		mpu6000::regdump(external_bus);
 
-	errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'");
+	if (!strcmp(verb, "factorytest"))
+		mpu6000::factorytest(external_bus);
+
+	if (!strcmp(verb, "testerror"))
+		mpu6000::testerror(external_bus);
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'regdump', 'factorytest' or 'testerror'");
 }

src/drivers/ms5611/ms5611.cpp

@@ -91,12 +91,13 @@ static const int ERROR = -1;
 /* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */
 #define MS5611_CONVERSION_INTERVAL	10000	/* microseconds */
 #define MS5611_MEASUREMENT_RATIO	3	/* pressure measurements per temperature measurement */
-#define MS5611_BARO_DEVICE_PATH		"/dev/ms5611"
+#define MS5611_BARO_DEVICE_PATH_EXT	"/dev/ms5611_ext"
+#define MS5611_BARO_DEVICE_PATH_INT	"/dev/ms5611_int"
 
 class MS5611 : public device::CDev
 {
 public:
-	MS5611(device::Device *interface, ms5611::prom_u &prom_buf);
+	MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char* path);
 	~MS5611();
 
 	virtual int		init();
@@ -133,6 +134,7 @@ protected:
 	unsigned		_msl_pressure;	/* in Pa */
 
 	orb_advert_t		_baro_topic;
+	orb_id_t		_orb_id;
 
 	int			_class_instance;
 
@@ -195,8 +197,8 @@ protected:
  */
 extern "C" __EXPORT int ms5611_main(int argc, char *argv[]);
 
-MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) :
-	CDev("MS5611", MS5611_BARO_DEVICE_PATH),
+MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char* path) :
+	CDev("MS5611", path),
 	_interface(interface),
 	_prom(prom_buf.s),
 	_measure_ticks(0),
@@ -224,7 +226,7 @@ MS5611::~MS5611()
 	stop_cycle();
 
 	if (_class_instance != -1)
-		unregister_class_devname(MS5611_BARO_DEVICE_PATH, _class_instance);
+		unregister_class_devname(get_devname(), _class_instance);
 
 	/* free any existing reports */
 	if (_reports != nullptr)
@@ -261,6 +263,7 @@ MS5611::init()
 
 	/* register alternate interfaces if we have to */
 	_class_instance = register_class_devname(BARO_DEVICE_PATH);
+	_orb_id = ORB_ID_TRIPLE(sensor_baro, _class_instance);
 
 	struct baro_report brp;
 	/* do a first measurement cycle to populate reports with valid data */
@@ -300,14 +303,7 @@ MS5611::init()
 
 		ret = OK;
 
-		switch (_class_instance) {
-			case CLASS_DEVICE_PRIMARY:
-				_baro_topic = orb_advertise(ORB_ID(sensor_baro0), &brp);
-				break;
-			case CLASS_DEVICE_SECONDARY:
-				_baro_topic = orb_advertise(ORB_ID(sensor_baro1), &brp);
-				break;
-		}
+		_baro_topic = orb_advertise(_orb_id, &brp);
 
 		if (_baro_topic < 0) {
 			warnx("failed to create sensor_baro publication");
@@ -729,15 +725,7 @@ MS5611::collect()
 		/* publish it */
 		if (!(_pub_blocked)) {
 			/* publish it */
-			switch (_class_instance) {
-				case CLASS_DEVICE_PRIMARY:
-					orb_publish(ORB_ID(sensor_baro0), _baro_topic, &report);
-					break;
-
-				case CLASS_DEVICE_SECONDARY:
-					orb_publish(ORB_ID(sensor_baro1), _baro_topic, &report);
-					break;
-			}
+			orb_publish(_orb_id, _baro_topic, &report);
 		}
 
 		if (_reports->force(&report)) {
@@ -787,13 +775,15 @@ MS5611::print_info()
 namespace ms5611
 {
 
-MS5611	*g_dev;
+/* initialize explicitely for clarity */
+MS5611	*g_dev_ext = nullptr;
+MS5611	*g_dev_int = nullptr;
 
 void	start(bool external_bus);
-void	test();
-void	reset();
+void	test(bool external_bus);
+void	reset(bool external_bus);
 void	info();
-void	calibrate(unsigned altitude);
+void	calibrate(unsigned altitude, bool external_bus);
 void	usage();
 
 /**
@@ -852,9 +842,13 @@ start(bool external_bus)
 	int fd;
 	prom_u prom_buf;
 
-	if (g_dev != nullptr)
+	if (external_bus && (g_dev_ext != nullptr)) {
 		/* if already started, the still command succeeded */
-		errx(0, "already started");
+		errx(0, "ext already started");
+	} else if (!external_bus && (g_dev_int != nullptr)) {
+		/* if already started, the still command succeeded */
+		errx(0, "int already started");
+	}
 
 	device::Device *interface = nullptr;
 
@@ -872,16 +866,35 @@ start(bool external_bus)
 		errx(1, "interface init failed");
 	}
 
-	g_dev = new MS5611(interface, prom_buf);
-	if (g_dev == nullptr) {
-		delete interface;
-		errx(1, "failed to allocate driver");
+	if (external_bus) {
+		g_dev_ext = new MS5611(interface, prom_buf, MS5611_BARO_DEVICE_PATH_EXT);
+		if (g_dev_ext == nullptr) {
+			delete interface;
+			errx(1, "failed to allocate driver");
+		}
+
+		if (g_dev_ext->init() != OK)
+			goto fail;
+
+		fd = open(MS5611_BARO_DEVICE_PATH_EXT, O_RDONLY);
+
+	} else {
+
+		g_dev_int = new MS5611(interface, prom_buf, MS5611_BARO_DEVICE_PATH_INT);
+		if (g_dev_int == nullptr) {
+			delete interface;
+			errx(1, "failed to allocate driver");
+		}
+
+		if (g_dev_int->init() != OK)
+			goto fail;
+
+		fd = open(MS5611_BARO_DEVICE_PATH_INT, O_RDONLY);
+
 	}
-	if (g_dev->init() != OK)
-		goto fail;
 
 	/* set the poll rate to default, starts automatic data collection */
-	fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY);
+
 	if (fd < 0) {
 		warnx("can't open baro device");
 		goto fail;
@@ -895,9 +908,14 @@ start(bool external_bus)
 
 fail:
 
-	if (g_dev != nullptr) {
-		delete g_dev;
-		g_dev = nullptr;
+	if (g_dev_int != nullptr) {
+		delete g_dev_int;
+		g_dev_int = nullptr;
+	}
+
+	if (g_dev_ext != nullptr) {
+		delete g_dev_ext;
+		g_dev_ext = nullptr;
 	}
 
 	errx(1, "driver start failed");
@@ -909,16 +927,22 @@ fail:
  * and automatic modes.
  */
 void
-test()
+test(bool external_bus)
 {
 	struct baro_report report;
 	ssize_t sz;
 	int ret;
 
-	int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY);
+	int fd;
+
+	if (external_bus) {
+		fd = open(MS5611_BARO_DEVICE_PATH_EXT, O_RDONLY);
+	} else {
+		fd = open(MS5611_BARO_DEVICE_PATH_INT, O_RDONLY);
+	}
 
 	if (fd < 0)
-		err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", MS5611_BARO_DEVICE_PATH);
+		err(1, "open failed (try 'ms5611 start' if the driver is not running)");
 
 	/* do a simple demand read */
 	sz = read(fd, &report, sizeof(report));
@@ -972,9 +996,15 @@ test()
  * Reset the driver.
  */
 void
-reset()
+reset(bool external_bus)
 {
-	int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY);
+	int fd;
+
+	if (external_bus) {
+		fd = open(MS5611_BARO_DEVICE_PATH_EXT, O_RDONLY);
+	} else {
+		fd = open(MS5611_BARO_DEVICE_PATH_INT, O_RDONLY);
+	}
 
 	if (fd < 0)
 		err(1, "failed ");
@@ -994,11 +1024,18 @@ reset()
 void
 info()
 {
-	if (g_dev == nullptr)
+	if (g_dev_ext == nullptr && g_dev_int == nullptr)
 		errx(1, "driver not running");
 
-	printf("state @ %p\n", g_dev);
-	g_dev->print_info();
+	if (g_dev_ext) {
+		warnx("ext:");
+		g_dev_ext->print_info();
+	}
+
+	if (g_dev_int) {
+		warnx("int:");
+		g_dev_int->print_info();
+	}
 
 	exit(0);
 }
@@ -1007,16 +1044,22 @@ info()
  * Calculate actual MSL pressure given current altitude
  */
 void
-calibrate(unsigned altitude)
+calibrate(unsigned altitude, bool external_bus)
 {
 	struct baro_report report;
 	float	pressure;
 	float	p1;
 
-	int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY);
+	int fd;
+
+	if (external_bus) {
+		fd = open(MS5611_BARO_DEVICE_PATH_EXT, O_RDONLY);
+	} else {
+		fd = open(MS5611_BARO_DEVICE_PATH_INT, O_RDONLY);
+	}
 
 	if (fd < 0)
-		err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", MS5611_BARO_DEVICE_PATH);
+		err(1, "open failed (try 'ms5611 start' if the driver is not running)");
 
 	/* start the sensor polling at max */
 	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX))
@@ -1101,6 +1144,12 @@ ms5611_main(int argc, char *argv[])
 
 	const char *verb = argv[optind];
 
+	if (argc > optind+1) {
+		if (!strcmp(argv[optind+1], "-X")) {
+			external_bus = true;
+		}
+	}
+
 	/*
 	 * Start/load the driver.
 	 */
@@ -1111,13 +1160,13 @@ ms5611_main(int argc, char *argv[])
 	 * Test the driver/device.
 	 */
 	if (!strcmp(verb, "test"))
-		ms5611::test();
+		ms5611::test(external_bus);
 
 	/*
 	 * Reset the driver.
 	 */
 	if (!strcmp(verb, "reset"))
-		ms5611::reset();
+		ms5611::reset(external_bus);
 
 	/*
 	 * Print driver information.
@@ -1134,7 +1183,7 @@ ms5611_main(int argc, char *argv[])
 
 		long altitude = strtol(argv[optind+1], nullptr, 10);
 
-		ms5611::calibrate(altitude);
+		ms5611::calibrate(altitude, external_bus);
 	}
 
 	errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'");

src/drivers/px4flow/px4flow.cpp

@@ -62,6 +62,8 @@
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
 
+#include <conversion/rotation.h>
+
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_px4flow.h>
 #include <drivers/device/ringbuffer.h>
@@ -73,13 +75,14 @@
 #include <board_config.h>
 
 /* Configuration Constants */
-#define I2C_FLOW_ADDRESS 		0x42 //* 7-bit address. 8-bit address is 0x84
-//range 0x42 - 0x49
+#define I2C_FLOW_ADDRESS 		0x42	///< 7-bit address. 8-bit address is 0x84, range 0x42 - 0x49
 
 /* PX4FLOW Registers addresses */
-#define PX4FLOW_REG		0x16	/* Measure Register 22*/
+#define PX4FLOW_REG			0x16	///< Measure Register 22
+
+#define PX4FLOW_CONVERSION_INTERVAL	20000	///< in microseconds! 20000 = 50 Hz 100000 = 10Hz
+#define PX4FLOW_I2C_MAX_BUS_SPEED	400000	///< 400 KHz maximum speed
 
-#define PX4FLOW_CONVERSION_INTERVAL 20000 //in microseconds! 20000 = 50 Hz 100000 = 10Hz
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
 # undef ERROR
@@ -125,7 +128,7 @@ struct i2c_integral_frame f_integral;
 class PX4FLOW: public device::I2C
 {
 public:
-	PX4FLOW(int bus, int address = I2C_FLOW_ADDRESS);
+	PX4FLOW(int bus, int address = I2C_FLOW_ADDRESS, enum Rotation rotation = (enum Rotation)0);
 	virtual ~PX4FLOW();
 
 	virtual int 		init();
@@ -154,6 +157,8 @@ private:
 	perf_counter_t		_sample_perf;
 	perf_counter_t		_comms_errors;
 	perf_counter_t		_buffer_overflows;
+	
+	enum Rotation				_sensor_rotation;
 
 	/**
 	 * Test whether the device supported by the driver is present at a
@@ -199,8 +204,8 @@ private:
  */
 extern "C" __EXPORT int px4flow_main(int argc, char *argv[]);
 
-PX4FLOW::PX4FLOW(int bus, int address) :
-	I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000), //400khz
+PX4FLOW::PX4FLOW(int bus, int address, enum Rotation rotation) :
+	I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, PX4FLOW_I2C_MAX_BUS_SPEED), /* 100-400 KHz */
 	_reports(nullptr),
 	_sensor_ok(false),
 	_measure_ticks(0),
@@ -208,7 +213,8 @@ PX4FLOW::PX4FLOW(int bus, int address) :
 	_px4flow_topic(-1),
 	_sample_perf(perf_alloc(PC_ELAPSED, "px4flow_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "px4flow_comms_errors")),
-	_buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows"))
+	_buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")),
+	_sensor_rotation(rotation)
 {
 	// enable debug() calls
 	_debug_enabled = false;
@@ -361,6 +367,13 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case SENSORIOCGQUEUEDEPTH:
 		return _reports->size();
 
+	case SENSORIOCSROTATION:
+		_sensor_rotation = (enum Rotation)arg;
+		return OK;
+
+	case SENSORIOCGROTATION:
+		return _sensor_rotation;
+
 	case SENSORIOCRESET:
 		/* XXX implement this */
 		return -EINVAL;
@@ -535,6 +548,10 @@ PX4FLOW::collect()
 	report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius
 
 	report.sensor_id = 0;
+	
+	/* rotate measurements according to parameter */
+	float zeroval = 0.0f;
+	rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval); 
 
 	if (_px4flow_topic < 0) {
 		_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report);

src/include/mavlink/mavlink_log.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,7 +35,7 @@
  * @file mavlink_log.h
  * MAVLink text logging.
  *
- * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Lorenz Meier <lorenz@px4.io>
  */
 
 #ifndef MAVLINK_LOG
@@ -99,6 +99,38 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...);
  */
 #define mavlink_log_info(_fd, _text, ...)		mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__);
 
+/**
+ * Send a mavlink emergency message and print to console.
+ *
+ * @param _fd		A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
+ * @param _text		The text to log;
+ */
+#define mavlink_and_console_log_emergency(_fd, _text, ...)		mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__); \
+									fprintf(stderr, "telem> "); \
+									fprintf(stderr, _text, ##__VA_ARGS__); \
+									fprintf(stderr, "\n");
+
+/**
+ * Send a mavlink critical message and print to console.
+ *
+ * @param _fd		A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
+ * @param _text		The text to log;
+ */
+#define mavlink_and_console_log_critical(_fd, _text, ...)		mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__); \
+									fprintf(stderr, "telem> "); \
+									fprintf(stderr, _text, ##__VA_ARGS__); \
+									fprintf(stderr, "\n");
+
+/**
+ * Send a mavlink emergency message and print to console.
+ *
+ * @param _fd		A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
+ * @param _text		The text to log;
+ */
+#define mavlink_and_console_log_info(_fd, _text, ...)			mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); \
+									fprintf(stderr, "telem> "); \
+									fprintf(stderr, _text, ##__VA_ARGS__); \
+									fprintf(stderr, "\n");
 
 struct mavlink_logmessage {
 	char text[MAVLINK_LOG_MAXLEN + 1];

src/lib/mathlib/math/Matrix.hpp

@@ -199,8 +199,8 @@ public:
 	Matrix<M, N> operator -(void) const {
 		Matrix<M, N> res;
 
-		for (unsigned int i = 0; i < N; i++)
-			for (unsigned int j = 0; j < M; j++) {
+		for (unsigned int i = 0; i < M; i++)
+			for (unsigned int j = 0; j < N; j++)
 				res.data[i][j] = -data[i][j];
 			}
 
@@ -213,8 +213,8 @@ public:
 	Matrix<M, N> operator +(const Matrix<M, N> &m) const {
 		Matrix<M, N> res;
 
-		for (unsigned int i = 0; i < N; i++)
-			for (unsigned int j = 0; j < M; j++) {
+		for (unsigned int i = 0; i < M; i++)
+			for (unsigned int j = 0; j < N; j++)
 				res.data[i][j] = data[i][j] + m.data[i][j];
 			}
 
@@ -222,8 +222,8 @@ public:
 	}
 
 	Matrix<M, N> &operator +=(const Matrix<M, N> &m) {
-		for (unsigned int i = 0; i < N; i++)
-			for (unsigned int j = 0; j < M; j++) {
+		for (unsigned int i = 0; i < M; i++)
+			for (unsigned int j = 0; j < N; j++)
 				data[i][j] += m.data[i][j];
 			}
 
@@ -245,8 +245,8 @@ public:
 	}
 
 	Matrix<M, N> &operator -=(const Matrix<M, N> &m) {
-		for (unsigned int i = 0; i < N; i++)
-			for (unsigned int j = 0; j < M; j++) {
+		for (unsigned int i = 0; i < M; i++)
+			for (unsigned int j = 0; j < N; j++)
 				data[i][j] -= m.data[i][j];
 			}
 

src/modules/commander/commander.cpp

@@ -1267,7 +1267,7 @@ int commander_thread_main(int argc, char *argv[])
 		if (updated) {
 			/* vtol status changed */
 			orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status);
-
+			status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab;
 			/* Make sure that this is only adjusted if vehicle realy is of type vtol*/
 			if (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR || VEHICLE_TYPE_VTOL_QUADROTOR) {
 				status.is_rotary_wing = vtol_status.vtol_in_rw_mode;
@@ -2246,8 +2246,8 @@ set_control_mode()
 	case NAVIGATION_STATE_MANUAL:
 		control_mode.flag_control_manual_enabled = true;
 		control_mode.flag_control_auto_enabled = false;
-		control_mode.flag_control_rates_enabled = status.is_rotary_wing;
-		control_mode.flag_control_attitude_enabled = status.is_rotary_wing;
+		control_mode.flag_control_rates_enabled = (status.is_rotary_wing || status.vtol_fw_permanent_stab);
+		control_mode.flag_control_attitude_enabled = (status.is_rotary_wing || status.vtol_fw_permanent_stab);
 		control_mode.flag_control_altitude_enabled = false;
 		control_mode.flag_control_climb_rate_enabled = false;
 		control_mode.flag_control_position_enabled = false;

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -1471,7 +1471,7 @@ FixedwingEstimator::task_main()
 							map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon);
 							_global_pos.lat = est_lat;
 							_global_pos.lon = est_lon;
-							_global_pos.time_gps_usec = _gps.time_gps_usec;
+							_global_pos.time_utc_usec = _gps.time_utc_usec;
 							_global_pos.eph = _gps.eph;
 							_global_pos.epv = _gps.epv;
 						}

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -336,8 +336,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_actuators_0_pub(-1),
 	_actuators_2_pub(-1),
 
-	_rates_sp_id(ORB_ID(vehicle_rates_setpoint)),
-	_actuators_id(ORB_ID(actuator_controls_0)),
+	_rates_sp_id(0),
+	_actuators_id(0),
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
@@ -589,12 +589,14 @@ FixedwingAttitudeControl::vehicle_status_poll()
 	if (vehicle_status_updated) {
 		orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
 		/* set correct uORB ID, depending on if vehicle is VTOL or not */
-		if (_vehicle_status.is_vtol) {
-			_rates_sp_id = ORB_ID(fw_virtual_rates_setpoint);
-			_actuators_id = ORB_ID(actuator_controls_virtual_fw);
-		} else {
-			_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
-			_actuators_id = ORB_ID(actuator_controls_0);
+		if (!_rates_sp_id) {
+			if (_vehicle_status.is_vtol) {
+				_rates_sp_id = ORB_ID(fw_virtual_rates_setpoint);
+				_actuators_id = ORB_ID(actuator_controls_virtual_fw);
+			} else {
+				_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
+				_actuators_id = ORB_ID(actuator_controls_0);
+			}
 		}
 	}
 }
@@ -720,22 +722,24 @@ FixedwingAttitudeControl::task_main()
 				R.set(_att.R);
 				R_adapted.set(_att.R);
 
-				//move z to x
+				/* move z to x */
 				R_adapted(0, 0) = R(0, 2);
 				R_adapted(1, 0) = R(1, 2);
 				R_adapted(2, 0) = R(2, 2);
-				//move x to z
+
+				/* move x to z */
 				R_adapted(0, 2) = R(0, 0);
 				R_adapted(1, 2) = R(1, 0);
 				R_adapted(2, 2) = R(2, 0);
 
-				//change direction of pitch (convert to right handed system)
+				/* change direction of pitch (convert to right handed system) */
 				R_adapted(0, 0) = -R_adapted(0, 0);
 				R_adapted(1, 0) = -R_adapted(1, 0);
 				R_adapted(2, 0) = -R_adapted(2, 0);
 				math::Vector<3> euler_angles;		//adapted euler angles for fixed wing operation
 				euler_angles = R_adapted.to_euler();
-				//fill in new attitude data
+
+				/* fill in new attitude data */
 				_att.roll    = euler_angles(0);
 				_att.pitch   = euler_angles(1);
 				_att.yaw     = euler_angles(2);
@@ -749,7 +753,7 @@ FixedwingAttitudeControl::task_main()
 				PX4_R(_att.R, 2, 1) = R_adapted(2, 1);
 				PX4_R(_att.R, 2, 2) = R_adapted(2, 2);
 
-				// lastly, roll- and yawspeed have to be swaped
+				/* lastly, roll- and yawspeed have to be swaped */
 				float helper = _att.rollspeed;
 				_att.rollspeed = -_att.yawspeed;
 				_att.yawspeed = helper;
@@ -820,6 +824,7 @@ FixedwingAttitudeControl::task_main()
 
 				float roll_sp = _parameters.rollsp_offset_rad;
 				float pitch_sp = _parameters.pitchsp_offset_rad;
+				float yaw_manual = 0.0f;
 				float throttle_sp = 0.0f;
 
 				/* Read attitude setpoint from uorb if
@@ -880,6 +885,8 @@ FixedwingAttitudeControl::task_main()
 						+ _parameters.rollsp_offset_rad;
 					pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch)
 						+ _parameters.pitchsp_offset_rad;
+					/* allow manual control of rudder deflection */
+					yaw_manual = _manual.r;
 					throttle_sp = _manual.z;
 					_actuators.control[4] = _manual.flaps;
 
@@ -979,6 +986,9 @@ FixedwingAttitudeControl::task_main()
 							_pitch_ctrl.get_desired_rate(),
 							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
 					_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
+
+					/* add in manual rudder control */
+					_actuators.control[2] += yaw_manual;
 					if (!isfinite(yaw_u)) {
 						_yaw_ctrl.reset_integrator();
 						perf_count(_nonfinite_output_perf);
@@ -1018,7 +1028,7 @@ FixedwingAttitudeControl::task_main()
 				if (_rate_sp_pub > 0) {
 					/* publish the attitude rates setpoint */
 					orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp);
-				} else {
+				} else if (_rates_sp_id) {
 					/* advertise the attitude rates setpoint */
 					_rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp);
 				}
@@ -1043,7 +1053,7 @@ FixedwingAttitudeControl::task_main()
 			/* publish the actuator controls */
 			if (_actuators_0_pub > 0) {
 				orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
-			} else {
+			} else if (_actuators_id) {
 				_actuators_0_pub= orb_advertise(_actuators_id, &_actuators);
 			}
 

src/modules/fw_att_control/module.mk

@@ -41,3 +41,5 @@ SRCS		= fw_att_control_main.cpp \
 		  fw_att_control_params.c
 
 MODULE_STACKSIZE = 1200
+
+MAXOPTIMIZATION = -Os

src/modules/mavlink/mavlink_main.cpp

@@ -1419,6 +1419,8 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("DISTANCE_SENSOR", 10.0f);
 		configure_stream("OPTICAL_FLOW_RAD", 10.0f);
 		configure_stream("VFR_HUD", 10.0f);
+		configure_stream("SYSTEM_TIME", 1.0f);
+		configure_stream("TIMESYNC", 10.0f);
 		break;
 
 	default:

src/modules/mavlink/mavlink_messages.cpp

@@ -342,6 +342,8 @@ private:
 	MavlinkStreamStatustext(MavlinkStreamStatustext &);
 	MavlinkStreamStatustext& operator = (const MavlinkStreamStatustext &);
 	FILE *fp = nullptr;
+	unsigned write_err_count = 0;
+	static const unsigned write_err_threshold = 5;
 
 protected:
 	explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink)
@@ -370,10 +372,21 @@ protected:
 				/* write log messages in first instance to disk */
 				if (_mavlink->get_instance_id() == 0) {
 					if (fp) {
-						fputs(msg.text, fp);
-						fputs("\n", fp);
-						fsync(fileno(fp));
-					} else {
+						if (EOF == fputs(msg.text, fp)) {
+							write_err_count++;
+						} else {
+							write_err_count = 0;
+						}
+
+						if (write_err_count >= write_err_threshold) {
+							(void)fclose(fp);
+							fp = nullptr;
+						} else {
+							(void)fputs("\n", fp);
+							(void)fsync(fileno(fp));
+						}
+
+					} else if (write_err_count < write_err_threshold) {
 						/* string to hold the path to the log */
 						char log_file_name[32] = "";
 						char log_file_path[64] = "";
@@ -389,6 +402,10 @@ protected:
 						strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &tt);
 						snprintf(log_file_path, sizeof(log_file_path), "/fs/microsd/%s", log_file_name);
 						fp = fopen(log_file_path, "ab");
+
+						/* write first message */
+						fputs(msg.text, fp);
+						fputs("\n", fp);
 					}
 				}
 			}
@@ -923,6 +940,92 @@ protected:
 	}
 };
 
+class MavlinkStreamSystemTime : public MavlinkStream
+{
+public:
+	const char *get_name() const {
+		return MavlinkStreamSystemTime::get_name_static();
+	}
+
+	static const char *get_name_static() {
+		return "SYSTEM_TIME";
+	}
+
+	uint8_t get_id() {
+		return MAVLINK_MSG_ID_SYSTEM_TIME;
+	}
+
+	static MavlinkStream *new_instance(Mavlink *mavlink) {
+		return new MavlinkStreamSystemTime(mavlink);
+	}
+
+	unsigned get_size() {
+		return MAVLINK_MSG_ID_SYSTEM_TIME_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+	}
+
+private:
+	/* do not allow top copying this class */
+	MavlinkStreamSystemTime(MavlinkStreamSystemTime &);
+	MavlinkStreamSystemTime &operator = (const MavlinkStreamSystemTime &);
+
+protected:
+	explicit MavlinkStreamSystemTime(Mavlink *mavlink) : MavlinkStream(mavlink)
+	{}
+
+	void send(const hrt_abstime t) {
+		mavlink_system_time_t msg;
+		timespec tv;
+
+		clock_gettime(CLOCK_REALTIME, &tv);
+
+		msg.time_boot_ms = hrt_absolute_time() / 1000;
+		msg.time_unix_usec = (uint64_t)tv.tv_sec * 1000000 + tv.tv_nsec / 1000;
+
+		_mavlink->send_message(MAVLINK_MSG_ID_SYSTEM_TIME, &msg);
+	}
+};
+
+class MavlinkStreamTimesync : public MavlinkStream
+{
+public:
+	const char *get_name() const {
+		return MavlinkStreamTimesync::get_name_static();
+	}
+
+	static const char *get_name_static() {
+		return "TIMESYNC";
+	}
+
+	uint8_t get_id() {
+		return MAVLINK_MSG_ID_TIMESYNC;
+	}
+
+	static MavlinkStream *new_instance(Mavlink *mavlink) {
+		return new MavlinkStreamTimesync(mavlink);
+	}
+
+	unsigned get_size() {
+		return MAVLINK_MSG_ID_TIMESYNC_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+	}
+
+private:
+	/* do not allow top copying this class */
+	MavlinkStreamTimesync(MavlinkStreamTimesync &);
+	MavlinkStreamTimesync &operator = (const MavlinkStreamTimesync &);
+
+protected:
+	explicit MavlinkStreamTimesync(Mavlink *mavlink) : MavlinkStream(mavlink)
+	{}
+
+	void send(const hrt_abstime t) {
+		mavlink_timesync_t msg;
+
+		msg.tc1 = 0;
+		msg.ts1 = hrt_absolute_time() * 1000; // boot time in nanoseconds
+
+		_mavlink->send_message(MAVLINK_MSG_ID_TIMESYNC, &msg);
+	}
+};
 
 class MavlinkStreamGlobalPositionInt : public MavlinkStream
 {
@@ -2197,6 +2300,8 @@ StreamListItem *streams_list[] = {
 	new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static),
 	new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static),
 	new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static),
+	new StreamListItem(&MavlinkStreamSystemTime::new_instance, &MavlinkStreamSystemTime::get_name_static),
+	new StreamListItem(&MavlinkStreamTimesync::new_instance, &MavlinkStreamTimesync::get_name_static),
 	new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static),
 	new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static),
 	new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static),

src/modules/mavlink/mavlink_parameters.cpp

@@ -44,7 +44,9 @@
 #include "mavlink_main.h"
 
 MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink),
-	_send_all_index(-1)
+	_send_all_index(-1),
+	_rc_param_map_pub(-1),
+	_rc_param_map()
 {
 }
 
@@ -135,6 +137,43 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 			break;
 		}
 
+	case MAVLINK_MSG_ID_PARAM_MAP_RC: {
+			/* map a rc channel to a parameter */
+			mavlink_param_map_rc_t map_rc;
+			mavlink_msg_param_map_rc_decode(msg, &map_rc);
+
+			if (map_rc.target_system == mavlink_system.sysid &&
+			    (map_rc.target_component == mavlink_system.compid ||
+			     map_rc.target_component == MAV_COMP_ID_ALL)) {
+
+				/* Copy values from msg to uorb using the parameter_rc_channel_index as index */
+				size_t i = map_rc.parameter_rc_channel_index;
+				_rc_param_map.param_index[i] = map_rc.param_index;
+				strncpy(&(_rc_param_map.param_id[i][0]), map_rc.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+				/* enforce null termination */
+				_rc_param_map.param_id[i][MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = '\0';
+				_rc_param_map.scale[i] = map_rc.scale;
+				_rc_param_map.value0[i] = map_rc.param_value0;
+				_rc_param_map.value_min[i] = map_rc.param_value_min;
+				_rc_param_map.value_max[i] = map_rc.param_value_max;
+				if (map_rc.param_index == -2) { // -2 means unset map
+					_rc_param_map.valid[i] = false;
+				} else {
+					_rc_param_map.valid[i] = true;
+				}
+				_rc_param_map.timestamp = hrt_absolute_time();
+
+				if (_rc_param_map_pub < 0) {
+					_rc_param_map_pub = orb_advertise(ORB_ID(rc_parameter_map), &_rc_param_map);
+
+				} else {
+					orb_publish(ORB_ID(rc_parameter_map), _rc_param_map_pub, &_rc_param_map);
+				}
+
+			}
+			break;
+		}
+
 	default:
 		break;
 	}

src/modules/mavlink/mavlink_parameters.h

@@ -44,6 +44,8 @@
 
 #include "mavlink_bridge_header.h"
 #include "mavlink_stream.h"
+#include <uORB/uORB.h>
+#include <uORB/topics/rc_parameter_map.h>
 
 class MavlinkParametersManager : public MavlinkStream
 {
@@ -112,4 +114,7 @@ protected:
 	void send(const hrt_abstime t);
 
 	void send_param(param_t param);
+
+	orb_advert_t _rc_param_map_pub;
+	struct rc_parameter_map_s _rc_param_map;
 };

src/modules/mavlink/mavlink_receiver.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -68,6 +68,8 @@
 
 #include <mathlib/mathlib.h>
 
+#include <conversion/rotation.h>
+
 #include <systemlib/param/param.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/err.h>
@@ -121,7 +123,9 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_old_timestamp(0),
 	_hil_local_proj_inited(0),
 	_hil_local_alt0(0.0f),
-	_hil_local_proj_ref{}
+	_hil_local_proj_ref{},
+	_time_offset_avg_alpha(0.6),
+	_time_offset(0)
 {
 
 	// make sure the FTP server is started
@@ -188,6 +192,14 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		MavlinkFTP::get_server()->handle_message(_mavlink, msg);
 		break;
 
+	case MAVLINK_MSG_ID_SYSTEM_TIME:
+		handle_message_system_time(msg);
+		break;
+
+	case MAVLINK_MSG_ID_TIMESYNC:
+		handle_message_timesync(msg);
+		break;
+
 	default:
 		break;
 	}
@@ -251,7 +263,7 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
 		//check for MAVLINK terminate command
 		if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
 			/* This is the link shutdown command, terminate mavlink */
-			warnx("terminated by remote command");
+			warnx("terminated by remote");
 			fflush(stdout);
 			usleep(50000);
 
@@ -261,7 +273,7 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
 		} else {
 
 			if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
-				warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID",
+				warnx("ignoring CMD with same SYS/COMP (%d/%d) ID",
 				      mavlink_system.sysid, mavlink_system.compid);
 				return;
 			}
@@ -307,7 +319,7 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
 		//check for MAVLINK terminate command
 		if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
 			/* This is the link shutdown command, terminate mavlink */
-			warnx("terminated by remote command");
+			warnx("terminated by remote");
 			fflush(stdout);
 			usleep(50000);
 
@@ -317,7 +329,7 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
 		} else {
 
 			if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
-				warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID",
+				warnx("ignoring CMD with same SYS/COMP (%d/%d) ID",
 				      mavlink_system.sysid, mavlink_system.compid);
 				return;
 			}
@@ -358,6 +370,9 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
 	mavlink_optical_flow_rad_t flow;
 	mavlink_msg_optical_flow_rad_decode(msg, &flow);
 
+	enum Rotation flow_rot;
+	param_get(param_find("SENS_FLOW_ROT"),&flow_rot);
+
 	struct optical_flow_s f;
 	memset(&f, 0, sizeof(f));
 
@@ -374,6 +389,10 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
 	f.sensor_id = flow.sensor_id;
 	f.gyro_temperature = flow.temperature;
 
+	/* rotate measurements according to parameter */
+	float zeroval = 0.0f;
+	rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, zeroval);
+
 	if (_flow_pub < 0) {
 		_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
 
@@ -689,7 +708,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
 	// Use the component ID to identify the vision sensor
 	vision_position.id = msg->compid;
 
-	vision_position.timestamp_boot = hrt_absolute_time();
+	vision_position.timestamp_boot = to_hrt(pos.usec); // Synced time
 	vision_position.timestamp_computer = pos.usec;
 	vision_position.x = pos.x;
 	vision_position.y = pos.y;
@@ -861,7 +880,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 	manual.r = man.r / 1000.0f;
 	manual.z = man.z / 1000.0f;
 
-	warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z);
+	// warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z);
 
 	if (_manual_pub < 0) {
 		_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
@@ -918,6 +937,66 @@ MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg)
 	}
 }
 
+void
+MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg)
+{
+	mavlink_system_time_t time;
+	mavlink_msg_system_time_decode(msg, &time);
+
+	timespec tv;
+	clock_gettime(CLOCK_REALTIME, &tv);
+
+	// date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009
+	bool onb_unix_valid = tv.tv_sec > PX4_EPOCH_SECS;
+	bool ofb_unix_valid = time.time_unix_usec > PX4_EPOCH_SECS * 1000ULL;
+
+	if (!onb_unix_valid && ofb_unix_valid) {
+		tv.tv_sec = time.time_unix_usec / 1000000ULL;
+		tv.tv_nsec = (time.time_unix_usec % 1000000ULL) * 1000ULL;
+		if(clock_settime(CLOCK_REALTIME, &tv)) {
+			warn("failed setting clock");
+		}
+		else {
+		warnx("[timesync] UTC time synced.");
+		}
+	}
+
+}
+
+void
+MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg)
+{
+	mavlink_timesync_t tsync;
+	mavlink_msg_timesync_decode(msg, &tsync);
+
+	uint64_t now_ns = hrt_absolute_time() * 1000LL ;
+
+	if (tsync.tc1 == 0) {
+
+		mavlink_timesync_t rsync; // return timestamped sync message
+
+		rsync.tc1 = now_ns;
+		rsync.ts1 = tsync.ts1;
+
+		_mavlink->send_message(MAVLINK_MSG_ID_TIMESYNC, &rsync);
+
+		return;
+
+	} else if (tsync.tc1 > 0) {
+
+		int64_t offset_ns = (tsync.ts1 + now_ns - tsync.tc1*2)/2 ; 
+		int64_t dt = _time_offset - offset_ns;
+
+		if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew 
+			_time_offset = offset_ns;
+			warnx("[timesync] Hard setting offset.");
+		} else {
+			smooth_time_offset(offset_ns);
+		}
+	}
+
+}
+
 void
 MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 {
@@ -1110,7 +1189,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 
 	/* print HIL sensors rate */
 	if ((timestamp - _old_timestamp) > 10000000) {
-		printf("receiving HIL sensors at %d hz\n", _hil_frames / 10);
+		// printf("receiving HIL sensors at %d hz\n", _hil_frames / 10);
 		_old_timestamp = timestamp;
 		_hil_frames = 0;
 	}
@@ -1128,7 +1207,7 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
 	memset(&hil_gps, 0, sizeof(hil_gps));
 
 	hil_gps.timestamp_time = timestamp;
-	hil_gps.time_gps_usec = gps.time_usec;
+	hil_gps.time_utc_usec = gps.time_usec;
 
 	hil_gps.timestamp_position = timestamp;
 	hil_gps.lat = gps.lat;
@@ -1385,6 +1464,23 @@ void MavlinkReceiver::print_status()
 
 }
 
+uint64_t MavlinkReceiver::to_hrt(uint64_t usec)
+{
+	return usec - (_time_offset / 1000) ;
+}
+
+
+void MavlinkReceiver::smooth_time_offset(uint64_t offset_ns)
+{
+	/* alpha = 0.6 fixed for now. The closer alpha is to 1.0,
+         * the faster the moving average updates in response to
+	 * new offset samples.
+	 */
+
+ 	_time_offset = (_time_offset_avg_alpha * offset_ns) + (1.0 - _time_offset_avg_alpha) * _time_offset;
+}
+
+
 void *MavlinkReceiver::start_helper(void *context)
 {
 	MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink *)context);

src/modules/mavlink/mavlink_receiver.h

@@ -75,6 +75,8 @@
 
 #include "mavlink_ftp.h"
 
+#define PX4_EPOCH_SECS 1234567890ULL
+
 class Mavlink;
 
 class MavlinkReceiver
@@ -124,12 +126,23 @@ private:
 	void handle_message_manual_control(mavlink_message_t *msg);
 	void handle_message_heartbeat(mavlink_message_t *msg);
 	void handle_message_request_data_stream(mavlink_message_t *msg);
+	void handle_message_system_time(mavlink_message_t *msg);
+	void handle_message_timesync(mavlink_message_t *msg);
 	void handle_message_hil_sensor(mavlink_message_t *msg);
 	void handle_message_hil_gps(mavlink_message_t *msg);
 	void handle_message_hil_state_quaternion(mavlink_message_t *msg);
 
 	void *receive_thread(void *arg);
 
+	/**
+	* Convert remote nsec timestamp to local hrt time (usec)
+	*/
+	uint64_t to_hrt(uint64_t nsec);
+	/**
+	* Exponential moving average filter to smooth time offset
+	*/
+	void smooth_time_offset(uint64_t offset_ns);	
+
 	mavlink_status_t status;
 	struct vehicle_local_position_s hil_local_pos;
 	struct vehicle_control_mode_s _control_mode;
@@ -164,6 +177,8 @@ private:
 	bool _hil_local_proj_inited;
 	float _hil_local_alt0;
 	struct map_projection_reference_s _hil_local_proj_ref;
+	double _time_offset_avg_alpha;
+	uint64_t _time_offset;
 
 	/* do not allow copying this class */
 	MavlinkReceiver(const MavlinkReceiver&);

src/modules/mavlink/mavlink_tests/module.mk

@@ -47,4 +47,6 @@ MODULE_STACKSIZE = 5000
 
 MAXOPTIMIZATION	 = -Os
 
-EXTRACXXFLAGS	= -Weffc++ -DMAVLINK_FTP_UNIT_TEST
+EXTRACXXFLAGS	= -Weffc++ -DMAVLINK_FTP_UNIT_TEST -Wno-attributes -Wno-packed
+
+EXTRACFLAGS	= -Wno-packed

src/modules/navigator/navigator_params.c

@@ -50,7 +50,8 @@
  * Default value of loiter radius for missions, loiter, RTL, etc. (fixedwing only).
  *
  * @unit meters
- * @min 0.0
+ * @min 20
+ * @max 200
  * @group Mission
  */
 PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
@@ -61,10 +62,11 @@ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
  * Default acceptance radius, overridden by acceptance radius of waypoint if set.
  *
  * @unit meters
- * @min 1.0
+ * @min 0.05
+ * @max 200
  * @group Mission
  */
-PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f);
+PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 10.0f);
 
 /**
  * Set OBC mode for data link loss

src/modules/navigator/rtl_params.c

@@ -53,7 +53,8 @@
  * Default value of loiter radius after RTL (fixedwing only).
  *
  * @unit meters
- * @min 0.0
+ * @min 20
+ * @max 200
  * @group RTL
  */
 PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f);
@@ -65,10 +66,10 @@ PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f);
  *
  * @unit meters
  * @min 0
- * @max 1
+ * @max 150
  * @group RTL
  */
-PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 100);
+PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 60);
 
 
 /**
@@ -78,7 +79,7 @@ PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 100);
  * Land (i.e. slowly descend) from this altitude if autolanding allowed.
  *
  * @unit meters
- * @min 0
+ * @min 2
  * @max 100
  * @group RTL
  */
@@ -92,7 +93,7 @@ PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20);
  *
  * @unit seconds
  * @min -1
- * @max
+ * @max 300
  * @group RTL
  */
 PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f);

src/modules/position_estimator_inav/position_estimator_inav_main.c

@@ -1167,7 +1167,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			if (local_pos.xy_global && local_pos.z_global) {
 				/* publish global position */
 				global_pos.timestamp = t;
-				global_pos.time_gps_usec = gps.time_gps_usec;
+				global_pos.time_utc_usec = gps.time_utc_usec;
 
 				double est_lat, est_lon;
 				map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon);

src/modules/sdlog2/module.mk

@@ -46,5 +46,5 @@ MODULE_STACKSIZE = 1200
 
 MAXOPTIMIZATION	 = -Os
 
-EXTRACFLAGS = -Wframe-larger-than=1200
+EXTRACFLAGS = -Wframe-larger-than=1300
 

src/modules/sdlog2/sdlog2.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -39,12 +39,14 @@
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
  * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Ban Siesta <bansiesta@gmail.com>
  */
 
 #include <nuttx/config.h>
 #include <sys/types.h>
 #include <sys/stat.h>
 #include <sys/prctl.h>
+#include <sys/statfs.h>
 #include <fcntl.h>
 #include <errno.h>
 #include <unistd.h>
@@ -57,6 +59,7 @@
 #include <unistd.h>
 #include <drivers/drv_hrt.h>
 #include <math.h>
+#include <time.h>
 
 #include <drivers/drv_range_finder.h>
 
@@ -103,6 +106,8 @@
 #include "sdlog2_format.h"
 #include "sdlog2_messages.h"
 
+#define PX4_EPOCH_SECS 1234567890ULL
+
 /**
  * Logging rate.
  *
@@ -158,7 +163,9 @@ static const int MIN_BYTES_TO_WRITE = 512;
 
 static bool _extended_logging = false;
 
-static const char *log_root = "/fs/microsd/log";
+#define MOUNTPOINT "/fs/microsd"
+static const char *mountpoint = MOUNTPOINT;
+static const char *log_root = MOUNTPOINT "/log";
 static int mavlink_fd = -1;
 struct logbuffer_s lb;
 
@@ -171,6 +178,7 @@ static char log_dir[32];
 /* statistics counters */
 static uint64_t start_time = 0;
 static unsigned long log_bytes_written = 0;
+static unsigned long last_checked_bytes_written = 0;
 static unsigned long log_msgs_written = 0;
 static unsigned long log_msgs_skipped = 0;
 
@@ -185,6 +193,9 @@ static bool log_name_timestamp = false;
 /* helper flag to track system state changes */
 static bool flag_system_armed = false;
 
+/* flag if warning about MicroSD card being almost full has already been sent */
+static bool space_warning_sent = false;
+
 static pthread_t logwriter_pthread = 0;
 static pthread_attr_t logwriter_attr;
 
@@ -244,6 +255,11 @@ static bool file_exist(const char *filename);
 
 static int file_copy(const char *file_old, const char *file_new);
 
+/**
+ * Check if there is still free space available
+ */
+static int check_free_space(void);
+
 static void handle_command(struct vehicle_command_s *cmd);
 
 static void handle_status(struct vehicle_status_s *cmd);
@@ -338,13 +354,16 @@ int create_log_dir()
 	uint16_t dir_number = 1; // start with dir sess001
 	int mkdir_ret;
 
-	if (log_name_timestamp && gps_time != 0) {
-		/* use GPS date for log dir naming: e.g. /fs/microsd/2014-01-19 */
-		time_t gps_time_sec = gps_time / 1000000;
-		struct tm t;
-		gmtime_r(&gps_time_sec, &t);
+	struct timespec ts;
+	clock_gettime(CLOCK_REALTIME, &ts);
+	/* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.px4log */
+	time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9);
+	struct tm tt;
+	struct tm *ttp = gmtime_r(&utc_time_sec, &tt);
+
+	if (log_name_timestamp && ttp && (utc_time_sec > PX4_EPOCH_SECS)) {
 		int n = snprintf(log_dir, sizeof(log_dir), "%s/", log_root);
-		strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &t);
+		strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &tt);
 		mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO);
 
 		if (mkdir_ret == OK) {
@@ -384,8 +403,7 @@ int create_log_dir()
 	}
 
 	/* print logging path, important to find log file later */
-	warnx("log dir: %s", log_dir);
-	mavlink_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir);
+	mavlink_and_console_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir);
 	return 0;
 }
 
@@ -395,12 +413,16 @@ int open_log_file()
 	char log_file_name[32] = "";
 	char log_file_path[64] = "";
 
-	if (log_name_timestamp && gps_time != 0) {
-		/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
-		time_t gps_time_sec = gps_time / 1000000;
-		struct tm t;
-		gmtime_r(&gps_time_sec, &t);
-		strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.bin", &t);
+	struct timespec ts;
+	clock_gettime(CLOCK_REALTIME, &ts);
+	/* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.px4log */
+	time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9);
+	struct tm tt;
+	struct tm *ttp = gmtime_r(&utc_time_sec, &tt);
+
+	/* start logging if we have a valid time and the time is not in the past */
+	if (log_name_timestamp && ttp && (utc_time_sec > PX4_EPOCH_SECS)) {
+		strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.px4log", &tt);
 		snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name);
 
 	} else {
@@ -408,8 +430,8 @@ int open_log_file()
 
 		/* look for the next file that does not exist */
 		while (file_number <= MAX_NO_LOGFILE) {
-			/* format log file path: e.g. /fs/microsd/sess001/log001.bin */
-			snprintf(log_file_name, sizeof(log_file_name), "log%03u.bin", file_number);
+			/* format log file path: e.g. /fs/microsd/sess001/log001.px4log */
+			snprintf(log_file_name, sizeof(log_file_name), "log%03u.px4log", file_number);
 			snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name);
 
 			if (!file_exist(log_file_path)) {
@@ -421,7 +443,7 @@ int open_log_file()
 
 		if (file_number > MAX_NO_LOGFILE) {
 			/* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */
-			mavlink_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE);
+			mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE);
 			return -1;
 		}
 	}
@@ -429,12 +451,10 @@ int open_log_file()
 	int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC);
 
 	if (fd < 0) {
-		warn("failed opening log: %s", log_file_name);
-		mavlink_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name);
+		mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name);
 
 	} else {
-		warnx("log file: %s", log_file_name);
-		mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name);
+		mavlink_and_console_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name);
 	}
 
 	return fd;
@@ -446,12 +466,15 @@ int open_perf_file(const char* str)
 	char log_file_name[32] = "";
 	char log_file_path[64] = "";
 
-	if (log_name_timestamp && gps_time != 0) {
-		/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
-		time_t gps_time_sec = gps_time / 1000000;
-		struct tm t;
-		gmtime_r(&gps_time_sec, &t);
-		strftime(log_file_name, sizeof(log_file_name), "perf%H_%M_%S.txt", &t);
+	struct timespec ts;
+	clock_gettime(CLOCK_REALTIME, &ts);
+	/* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.txt */
+	time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9);
+	struct tm tt;
+	struct tm *ttp = gmtime_r(&utc_time_sec, &tt);
+
+	if (log_name_timestamp && ttp && (utc_time_sec > PX4_EPOCH_SECS)) {
+		strftime(log_file_name, sizeof(log_file_name), "perf%H_%M_%S.txt", &tt);
 		snprintf(log_file_path, sizeof(log_file_path), "%s/%s_%s", log_dir, str, log_file_name);
 
 	} else {
@@ -459,7 +482,7 @@ int open_perf_file(const char* str)
 
 		/* look for the next file that does not exist */
 		while (file_number <= MAX_NO_LOGFILE) {
-			/* format log file path: e.g. /fs/microsd/sess001/log001.bin */
+			/* format log file path: e.g. /fs/microsd/sess001/log001.txt */
 			snprintf(log_file_name, sizeof(log_file_name), "perf%03u.txt", file_number);
 			snprintf(log_file_path, sizeof(log_file_path), "%s/%s_%s", log_dir, str, log_file_name);
 
@@ -472,7 +495,7 @@ int open_perf_file(const char* str)
 
 		if (file_number > MAX_NO_LOGFILE) {
 			/* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */
-			mavlink_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE);
+			mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE);
 			return -1;
 		}
 	}
@@ -480,12 +503,8 @@ int open_perf_file(const char* str)
 	int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC);
 
 	if (fd < 0) {
-		warn("failed opening log: %s", log_file_name);
-		mavlink_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name);
+		mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening: %s", log_file_name);
 
-	} else {
-		warnx("log file: %s", log_file_name);
-		mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name);
 	}
 
 	return fd;
@@ -547,6 +566,7 @@ static void *logwriter_thread(void *arg)
 		pthread_mutex_unlock(&logbuffer_mutex);
 
 		if (available > 0) {
+
 			/* do heavy IO here */
 			if (available > MAX_WRITE_CHUNK) {
 				n = MAX_WRITE_CHUNK;
@@ -584,6 +604,16 @@ static void *logwriter_thread(void *arg)
 		if (++poll_count == 10) {
 			fsync(log_fd);
 			poll_count = 0;
+
+		}
+
+		if (log_bytes_written - last_checked_bytes_written > 20*1024*1024) {
+			/* check if space is available, if not stop everything */
+			if (check_free_space() != OK) {
+				logwriter_should_exit = true;
+				main_thread_should_exit = true;
+			}
+			last_checked_bytes_written = log_bytes_written;
 		}
 	}
 
@@ -598,15 +628,15 @@ static void *logwriter_thread(void *arg)
 
 void sdlog2_start_log()
 {
-	warnx("start logging");
-	mavlink_log_info(mavlink_fd, "[sdlog2] start logging");
+	mavlink_and_console_log_info(mavlink_fd, "[sdlog2] start logging");
 
 	/* create log dir if needed */
 	if (create_log_dir() != 0) {
-		mavlink_log_critical(mavlink_fd, "[sdlog2] error creating log dir");
-		errx(1, "error creating log dir");
+		mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] error creating log dir");
+		exit(1);
 	}
 
+
 	/* initialize statistics counter */
 	log_bytes_written = 0;
 	start_time = hrt_absolute_time();
@@ -644,8 +674,7 @@ void sdlog2_start_log()
 
 void sdlog2_stop_log()
 {
-	warnx("stop logging");
-	mavlink_log_info(mavlink_fd, "[sdlog2] stop logging");
+	mavlink_and_console_log_info(mavlink_fd, "[sdlog2] stop logging");
 
 	logging_enabled = false;
 
@@ -771,7 +800,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 
 	if (mavlink_fd < 0) {
-		warnx("failed to open MAVLink log stream, start mavlink app first");
+		warnx("ERR: log stream, start mavlink app first");
 	}
 
 	/* delay = 1 / rate (rate defined by -r option), default log rate: 50 Hz */
@@ -899,11 +928,17 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 	}
 
+
+	if (check_free_space() != OK) {
+		errx(1, "ERR: MicroSD almost full");
+	}
+
+
 	/* create log root dir */
 	int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO);
 
 	if (mkdir_ret != 0 && errno != EEXIST) {
-		err(1, "failed creating log root dir: %s", log_root);
+		err(1, "ERR: failed creating log dir: %s", log_root);
 	}
 
 	/* copy conversion scripts */
@@ -1112,6 +1147,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	hrt_abstime magnetometer_timestamp = 0;
 	hrt_abstime barometer_timestamp = 0;
 	hrt_abstime differential_pressure_timestamp = 0;
+	hrt_abstime barometer1_timestamp = 0;
 	hrt_abstime gyro1_timestamp = 0;
 	hrt_abstime accelerometer1_timestamp = 0;
 	hrt_abstime magnetometer1_timestamp = 0;
@@ -1127,7 +1163,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		/* check GPS topic to get GPS time */
 		if (log_name_timestamp) {
 			if (!orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) {
-				gps_time = buf_gps_pos.time_gps_usec;
+				gps_time = buf_gps_pos.time_utc_usec;
 			}
 		}
 
@@ -1155,7 +1191,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos);
 
 		if (gps_pos_updated && log_name_timestamp) {
-			gps_time = buf_gps_pos.time_gps_usec;
+			gps_time = buf_gps_pos.time_utc_usec;
 		}
 
 		if (!logging_enabled) {
@@ -1185,7 +1221,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		if (gps_pos_updated) {
 
 			log_msg.msg_type = LOG_GPS_MSG;
-			log_msg.body.log_GPS.gps_time = buf_gps_pos.time_gps_usec;
+			log_msg.body.log_GPS.gps_time = buf_gps_pos.time_utc_usec;
 			log_msg.body.log_GPS.fix_type = buf_gps_pos.fix_type;
 			log_msg.body.log_GPS.eph = buf_gps_pos.eph;
 			log_msg.body.log_GPS.epv = buf_gps_pos.epv;
@@ -1257,6 +1293,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			bool write_IMU1 = false;
 			bool write_IMU2 = false;
 			bool write_SENS = false;
+			bool write_SENS1 = false;
 
 			if (buf.sensor.timestamp != gyro_timestamp) {
 				gyro_timestamp = buf.sensor.timestamp;
@@ -1307,6 +1344,22 @@ int sdlog2_thread_main(int argc, char *argv[])
 				LOGBUFFER_WRITE_AND_COUNT(SENS);
 			}
 
+			if (buf.sensor.baro1_timestamp != barometer1_timestamp) {
+				barometer1_timestamp = buf.sensor.baro1_timestamp;
+				write_SENS1 = true;
+			}
+
+			if (write_SENS1) {
+				log_msg.msg_type = LOG_AIR1_MSG;
+				log_msg.body.log_SENS.baro_pres = buf.sensor.baro1_pres_mbar;
+				log_msg.body.log_SENS.baro_alt = buf.sensor.baro1_alt_meter;
+				log_msg.body.log_SENS.baro_temp = buf.sensor.baro1_temp_celcius;
+				log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure1_pa;
+				log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure1_filtered_pa;
+				// XXX moving to AIR0-AIR2 instead of SENS
+				LOGBUFFER_WRITE_AND_COUNT(SENS);
+			}
+
 			if (buf.sensor.accelerometer1_timestamp != accelerometer1_timestamp) {
 				accelerometer1_timestamp = buf.sensor.accelerometer1_timestamp;
 				write_IMU1 = true;
@@ -1737,8 +1790,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 	free(lb.data);
 
-	warnx("exiting");
-
 	thread_running = false;
 
 	return 0;
@@ -1771,7 +1822,7 @@ int file_copy(const char *file_old, const char *file_new)
 	int ret = 0;
 
 	if (source == NULL) {
-		warnx("failed opening input file to copy");
+		warnx("ERR: open in");
 		return 1;
 	}
 
@@ -1779,7 +1830,7 @@ int file_copy(const char *file_old, const char *file_new)
 
 	if (target == NULL) {
 		fclose(source);
-		warnx("failed to open output file to copy");
+		warnx("ERR: open out");
 		return 1;
 	}
 
@@ -1804,6 +1855,34 @@ int file_copy(const char *file_old, const char *file_new)
 	return OK;
 }
 
+int check_free_space()
+{
+	/* use statfs to determine the number of blocks left */
+	FAR struct statfs statfs_buf;
+	if (statfs(mountpoint, &statfs_buf) != OK) {
+		errx(ERROR, "ERR: statfs");
+	}
+
+	/* use a threshold of 50 MiB */
+	if (statfs_buf.f_bavail < (int)(50 * 1024 * 1024 / statfs_buf.f_bsize)) {
+		mavlink_and_console_log_critical(mavlink_fd,
+			"[sdlog2] no space on MicroSD: %u MiB",
+			(unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U));
+		/* we do not need a flag to remember that we sent this warning because we will exit anyway */
+		return ERROR;
+
+	/* use a threshold of 100 MiB to send a warning */
+	} else if (!space_warning_sent && statfs_buf.f_bavail < (int)(100 * 1024 * 1024 / statfs_buf.f_bsize)) {
+		mavlink_and_console_log_critical(mavlink_fd,
+			"[sdlog2] space on MicroSD low: %u MiB",
+			(unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U));
+		/* we don't want to flood the user with warnings */
+		space_warning_sent = true;
+	}
+
+	return OK;
+}
+
 void handle_command(struct vehicle_command_s *cmd)
 {
 	int param;

src/modules/sdlog2/sdlog2_messages.h

@@ -424,6 +424,9 @@ struct log_ENCD_s {
 	float vel1;
 };
 
+/* --- AIR SPEED SENSORS - DIFF. PRESSURE --- */
+#define LOG_AIR1_MSG 40
+
 /********** SYSTEM MESSAGES, ID > 0x80 **********/
 
 /* --- TIME - TIME STAMP --- */
@@ -457,7 +460,8 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT_S(IMU, IMU, "fffffffff",		"AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
 	LOG_FORMAT_S(IMU1, IMU, "fffffffff",		"AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
 	LOG_FORMAT_S(IMU2, IMU, "fffffffff",		"AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
-	LOG_FORMAT(SENS, "fffff",		"BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"),
+	LOG_FORMAT_S(SENS, SENS, "fffff",		"BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"),
+	LOG_FORMAT_S(AIR1, SENS, "fffff",	"BaroPa,BaroAlt,BaroTmp,DiffPres,DiffPresF"),
 	LOG_FORMAT(LPOS, "ffffffffLLfBBBff",	"X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"),
 	LOG_FORMAT(LPSP, "ffff",		"X,Y,Z,Yaw"),
 	LOG_FORMAT(GPS, "QBffLLfffffBHHH",	"GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"),

src/modules/sensors/sensor_params.c

@@ -261,6 +261,25 @@ PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f);
  */
 PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
 
+/**
+ * PX4Flow board rotation
+ *
+ * This parameter defines the rotation of the PX4FLOW board relative to the platform. 
+ * Zero rotation is defined as Y on flow board pointing towards front of vehicle
+ * Possible values are:
+ *    0 = No rotation
+ *    1 = Yaw 45°
+ *    2 = Yaw 90°
+ *    3 = Yaw 135°
+ *    4 = Yaw 180°
+ *    5 = Yaw 225°
+ *    6 = Yaw 270°
+ *    7 = Yaw 315°
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(SENS_FLOW_ROT, 0);
+
 /**
  * Board rotation Y (Pitch) offset
  *
@@ -747,6 +766,41 @@ PARAM_DEFINE_INT32(RC_MAP_AUX2, 0);	/**< default function: camera roll */
  */
 PARAM_DEFINE_INT32(RC_MAP_AUX3, 0);
 
+/**
+ * Channel which changes a parameter
+ *
+ * Can be used for parameter tuning with the RC. This one is further referenced as the 1st parameter channel.
+ * Set to 0 to deactivate *
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_MAP_PARAM1, 0);
+
+/**
+ * Channel which changes a parameter
+ *
+ * Can be used for parameter tuning with the RC. This one is further referenced as the 2nd parameter channel.
+ * Set to 0 to deactivate *
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_MAP_PARAM2, 0);
+
+/**
+ * Channel which changes a parameter
+ *
+ * Can be used for parameter tuning with the RC. This one is further referenced as the 3th parameter channel.
+ * Set to 0 to deactivate *
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_MAP_PARAM3, 0);
 
 /**
  * Failsafe channel PWM threshold.

src/modules/sensors/sensors.cpp

@@ -37,7 +37,7 @@
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
  * @author Julian Oes <joes@student.ethz.ch>
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
 #include <nuttx/config.h>
@@ -63,6 +63,7 @@
 #include <drivers/drv_rc_input.h>
 #include <drivers/drv_adc.h>
 #include <drivers/drv_airspeed.h>
+#include <drivers/drv_px4flow.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
@@ -82,6 +83,7 @@
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/airspeed.h>
+#include <uORB/topics/rc_parameter_map.h>
 
 #define GYRO_HEALTH_COUNTER_LIMIT_ERROR 20   /* 40 ms downtime at 500 Hz update rate   */
 #define ACC_HEALTH_COUNTER_LIMIT_ERROR  20   /* 40 ms downtime at 500 Hz update rate   */
@@ -190,6 +192,14 @@ private:
 	uint8_t	get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv);
 	uint8_t	get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv);
 
+	/**
+	 * Update paramters from RC channels if the functionality is activated and the
+	 * input has changed since the last update
+	 *
+	 * @param
+	 */
+	void set_params_from_rc();
+
 	/**
 	 * Gather and publish RC input data.
 	 */
@@ -216,10 +226,12 @@ private:
 	int		_mag2_sub;			/**< raw mag2 data subscription */
 	int 		_rc_sub;			/**< raw rc channels data subscription */
 	int		_baro_sub;			/**< raw baro data subscription */
+	int		_baro1_sub;			/**< raw baro data subscription */
 	int		_airspeed_sub;			/**< airspeed subscription */
 	int		_diff_pres_sub;			/**< raw differential pressure subscription */
 	int		_vcontrol_mode_sub;			/**< vehicle control mode subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
+	int		_rc_parameter_map_sub;			/**< rc parameter map subscription */
 	int 		_manual_control_sub;			/**< notification of manual control updates */
 
 	orb_advert_t	_sensor_pub;			/**< combined sensor data topic */
@@ -237,6 +249,7 @@ private:
 	struct baro_report _barometer;			/**< barometer data */
 	struct differential_pressure_s _diff_pres;
 	struct airspeed_s _airspeed;
+	struct rc_parameter_map_s _rc_parameter_map;
 
 	math::Matrix<3, 3>	_board_rotation;		/**< rotation matrix for the orientation that the board is mounted */
 	math::Matrix<3, 3>	_external_mag_rotation;		/**< rotation matrix for the orientation that an external mag is mounted */
@@ -263,6 +276,7 @@ private:
 		float diff_pres_analog_scale;
 
 		int board_rotation;
+		int flow_rotation;
 		int external_mag_rotation;
 
 		float board_offset[3];
@@ -288,6 +302,8 @@ private:
 		int rc_map_aux4;
 		int rc_map_aux5;
 
+		int rc_map_param[RC_PARAM_MAP_NCHAN];
+
 		int32_t rc_fails_thr;
 		float rc_assist_th;
 		float rc_auto_th;
@@ -348,6 +364,12 @@ private:
 		param_t rc_map_aux4;
 		param_t rc_map_aux5;
 
+		param_t rc_map_param[RC_PARAM_MAP_NCHAN];
+		param_t rc_param[RC_PARAM_MAP_NCHAN];	/**< param handles for the paramters which are bound
+							  to a RC channel, equivalent float values in the
+							  _parameters struct are not existing
+							  because these parameters are never read. */
+
 		param_t rc_fails_thr;
 		param_t rc_assist_th;
 		param_t rc_auto_th;
@@ -361,6 +383,7 @@ private:
 		param_t battery_current_scaling;
 
 		param_t board_rotation;
+		param_t flow_rotation;
 		param_t external_mag_rotation;
 
 		param_t board_offset[3];
@@ -378,27 +401,27 @@ private:
 	/**
 	 * Do accel-related initialisation.
 	 */
-	void		accel_init();
+	int		accel_init();
 
 	/**
 	 * Do gyro-related initialisation.
 	 */
-	void		gyro_init();
+	int		gyro_init();
 
 	/**
 	 * Do mag-related initialisation.
 	 */
-	void		mag_init();
+	int		mag_init();
 
 	/**
 	 * Do baro-related initialisation.
 	 */
-	void		baro_init();
+	int		baro_init();
 
 	/**
 	 * Do adc-related initialisation.
 	 */
-	void		adc_init();
+	int		adc_init();
 
 	/**
 	 * Poll the accelerometer for updated data.
@@ -450,6 +473,11 @@ private:
 	 */
 	void 		parameter_update_poll(bool forced = false);
 
+	/**
+	 * Check for changes in rc_parameter_map
+	 */
+	void 		rc_parameter_map_poll(bool forced = false);
+
 	/**
 	 * Poll the ADC and update readings to suit.
 	 *
@@ -479,7 +507,7 @@ Sensors::Sensors() :
 	_fd_adc(-1),
 	_last_adc(0),
 
-	_task_should_exit(false),
+	_task_should_exit(true),
 	_sensors_task(-1),
 	_hil_enabled(false),
 	_publishing(true),
@@ -496,8 +524,10 @@ Sensors::Sensors() :
 	_mag2_sub(-1),
 	_rc_sub(-1),
 	_baro_sub(-1),
+	_baro1_sub(-1),
 	_vcontrol_mode_sub(-1),
 	_params_sub(-1),
+	_rc_parameter_map_sub(-1),
 	_manual_control_sub(-1),
 
 /* publications */
@@ -518,6 +548,7 @@ Sensors::Sensors() :
 {
 	memset(&_rc, 0, sizeof(_rc));
 	memset(&_diff_pres, 0, sizeof(_diff_pres));
+	memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map));
 
 	/* basic r/c parameters */
 	for (unsigned i = 0; i < _rc_max_chan_count; i++) {
@@ -570,6 +601,13 @@ Sensors::Sensors() :
 	_parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4");
 	_parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5");
 
+	/* RC to parameter mapping for changing parameters with RC */
+	for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
+		char name[PARAM_ID_LEN];
+		snprintf(name, PARAM_ID_LEN, "RC_MAP_PARAM%d", i + 1); // shifted by 1 because param name starts at 1
+		_parameter_handles.rc_map_param[i] = param_find(name);
+	}
+
 	/* RC thresholds */
 	_parameter_handles.rc_fails_thr = param_find("RC_FAILS_THR");
 	_parameter_handles.rc_assist_th = param_find("RC_ASSIST_TH");
@@ -614,6 +652,7 @@ Sensors::Sensors() :
 
 	/* rotations */
 	_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
+	_parameter_handles.flow_rotation = param_find("SENS_FLOW_ROT");
 	_parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT");
 
 	/* rotation offsets */
@@ -747,6 +786,9 @@ Sensors::parameters_update()
 	param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3));
 	param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4));
 	param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5));
+	for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
+		param_get(_parameter_handles.rc_map_param[i], &(_parameters.rc_map_param[i]));
+	}
 	param_get(_parameter_handles.rc_fails_thr, &(_parameters.rc_fails_thr));
 	param_get(_parameter_handles.rc_assist_th, &(_parameters.rc_assist_th));
 	_parameters.rc_assist_inv = (_parameters.rc_assist_th < 0);
@@ -791,6 +833,10 @@ Sensors::parameters_update()
 	_rc.function[RC_CHANNELS_FUNCTION_AUX_4] = _parameters.rc_map_aux4 - 1;
 	_rc.function[RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1;
 
+	for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
+		_rc.function[PARAM_1 + i] = _parameters.rc_map_param[i] - 1;
+	}
+
 	/* gyro offsets */
 	param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0]));
 	param_get(_parameter_handles.gyro_offset[1], &(_parameters.gyro_offset[1]));
@@ -831,8 +877,22 @@ Sensors::parameters_update()
 	}
 
 	param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
+	param_get(_parameter_handles.flow_rotation, &(_parameters.flow_rotation));
 	param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation));
 
+	/* set px4flow rotation */
+	int	flowfd;
+	flowfd = open(PX4FLOW_DEVICE_PATH, 0);
+	if (flowfd >= 0) {
+ 		int flowret = ioctl(flowfd, SENSORIOCSROTATION, _parameters.flow_rotation);
+ 		if (flowret) {
+ 			warnx("flow rotation could not be set");
+			close(flowfd);
+			return ERROR;
+ 		}
+		close(flowfd);
+ 	}	
+	
 	get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
 	get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation);
 
@@ -850,26 +910,26 @@ Sensors::parameters_update()
 
 	/* update barometer qnh setting */
 	param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh));
-	int	fd;
-	fd = open(BARO_DEVICE_PATH, 0);
-	if (fd < 0) {
-		warn("%s", BARO_DEVICE_PATH);
-		errx(1, "FATAL: no barometer found");
+	int	barofd;
+	barofd = open(BARO_DEVICE_PATH, 0);
+	if (barofd < 0) {
+		warnx("ERROR: no barometer foundon %s", BARO_DEVICE_PATH);
+		return ERROR;
 
 	} else {
-		int ret = ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100));
-		if (ret) {
+		int baroret = ioctl(barofd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100));
+		if (baroret) {
 			warnx("qnh could not be set");
-			close(fd);
+			close(barofd);
 			return ERROR;
 		}
-		close(fd);
+		close(barofd);
 	}
 
 	return OK;
 }
 
-void
+int
 Sensors::accel_init()
 {
 	int	fd;
@@ -877,8 +937,8 @@ Sensors::accel_init()
 	fd = open(ACCEL_DEVICE_PATH, 0);
 
 	if (fd < 0) {
-		warn("%s", ACCEL_DEVICE_PATH);
-		errx(1, "FATAL: no accelerometer found");
+		warnx("FATAL: no accelerometer found: %s", ACCEL_DEVICE_PATH);
+		return ERROR;
 
 	} else {
 
@@ -907,9 +967,11 @@ Sensors::accel_init()
 
 		close(fd);
 	}
+
+	return OK;
 }
 
-void
+int
 Sensors::gyro_init()
 {
 	int	fd;
@@ -917,8 +979,8 @@ Sensors::gyro_init()
 	fd = open(GYRO_DEVICE_PATH, 0);
 
 	if (fd < 0) {
-		warn("%s", GYRO_DEVICE_PATH);
-		errx(1, "FATAL: no gyro found");
+		warnx("FATAL: no gyro found: %s", GYRO_DEVICE_PATH);
+		return ERROR;
 
 	} else {
 
@@ -948,9 +1010,11 @@ Sensors::gyro_init()
 
 		close(fd);
 	}
+
+	return OK;
 }
 
-void
+int
 Sensors::mag_init()
 {
 	int	fd;
@@ -959,8 +1023,8 @@ Sensors::mag_init()
 	fd = open(MAG_DEVICE_PATH, 0);
 
 	if (fd < 0) {
-		warn("%s", MAG_DEVICE_PATH);
-		errx(1, "FATAL: no magnetometer found");
+		warnx("FATAL: no magnetometer found: %s", MAG_DEVICE_PATH);
+		return ERROR;
 	}
 
 	/* try different mag sampling rates */
@@ -981,7 +1045,8 @@ Sensors::mag_init()
 			ioctl(fd, SENSORIOCSPOLLRATE, 100);
 
 		} else {
-			errx(1, "FATAL: mag sampling rate could not be set");
+			warnx("FATAL: mag sampling rate could not be set");
+			return ERROR;
 		}
 	}
 
@@ -990,7 +1055,8 @@ Sensors::mag_init()
 	ret = ioctl(fd, MAGIOCGEXTERNAL, 0);
 
 	if (ret < 0) {
-		errx(1, "FATAL: unknown if magnetometer is external or onboard");
+		warnx("FATAL: unknown if magnetometer is external or onboard");
+		return ERROR;
 
 	} else if (ret == 1) {
 		_mag_is_external = true;
@@ -1000,9 +1066,11 @@ Sensors::mag_init()
 	}
 
 	close(fd);
+
+	return OK;
 }
 
-void
+int
 Sensors::baro_init()
 {
 	int	fd;
@@ -1010,26 +1078,30 @@ Sensors::baro_init()
 	fd = open(BARO_DEVICE_PATH, 0);
 
 	if (fd < 0) {
-		warn("%s", BARO_DEVICE_PATH);
-		errx(1, "FATAL: No barometer found");
+		warnx("FATAL: No barometer found: %s", BARO_DEVICE_PATH);
+		return ERROR;
 	}
 
 	/* set the driver to poll at 150Hz */
 	ioctl(fd, SENSORIOCSPOLLRATE, 150);
 
 	close(fd);
+
+	return OK;
 }
 
-void
+int
 Sensors::adc_init()
 {
 
 	_fd_adc = open(ADC_DEVICE_PATH, O_RDONLY | O_NONBLOCK);
 
 	if (_fd_adc < 0) {
-		warn(ADC_DEVICE_PATH);
-		warnx("FATAL: no ADC found");
+		warnx("FATAL: no ADC found: %s", ADC_DEVICE_PATH);
+		return ERROR;
 	}
+
+	return OK;
 }
 
 void
@@ -1200,6 +1272,34 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 
 		raw.magnetometer_timestamp = mag_report.timestamp;
 	}
+
+	orb_check(_mag1_sub, &mag_updated);
+
+	if (mag_updated) {
+		struct mag_report	mag_report;
+
+		orb_copy(ORB_ID(sensor_mag1), _mag1_sub, &mag_report);
+
+		raw.magnetometer1_raw[0] = mag_report.x_raw;
+		raw.magnetometer1_raw[1] = mag_report.y_raw;
+		raw.magnetometer1_raw[2] = mag_report.z_raw;
+
+		raw.magnetometer1_timestamp = mag_report.timestamp;
+	}
+
+	orb_check(_mag2_sub, &mag_updated);
+
+	if (mag_updated) {
+		struct mag_report	mag_report;
+
+		orb_copy(ORB_ID(sensor_mag2), _mag2_sub, &mag_report);
+
+		raw.magnetometer2_raw[0] = mag_report.x_raw;
+		raw.magnetometer2_raw[1] = mag_report.y_raw;
+		raw.magnetometer2_raw[2] = mag_report.z_raw;
+
+		raw.magnetometer2_timestamp = mag_report.timestamp;
+	}
 }
 
 void
@@ -1218,6 +1318,21 @@ Sensors::baro_poll(struct sensor_combined_s &raw)
 
 		raw.baro_timestamp = _barometer.timestamp;
 	}
+
+	orb_check(_baro1_sub, &baro_updated);
+
+	if (baro_updated) {
+
+		struct baro_report	baro_report;
+
+		orb_copy(ORB_ID(sensor_baro1), _baro1_sub, &baro_report);
+
+		raw.baro1_pres_mbar = baro_report.pressure; // Pressure in mbar
+		raw.baro1_alt_meter = baro_report.altitude; // Altitude in meters
+		raw.baro1_temp_celcius = baro_report.temperature; // Temperature in degrees celcius
+
+		raw.baro1_timestamp = baro_report.timestamp;
+	}
 }
 
 void
@@ -1373,6 +1488,45 @@ Sensors::parameter_update_poll(bool forced)
 	}
 }
 
+void
+Sensors::rc_parameter_map_poll(bool forced)
+{
+	bool map_updated;
+	orb_check(_rc_parameter_map_sub, &map_updated);
+
+	if (map_updated) {
+		orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map);
+		/* update paramter handles to which the RC channels are mapped */
+		for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
+			if (_rc.function[PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
+				/* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0)
+				 * or no request to map this channel to a param has been sent via mavlink
+				 */
+				continue;
+			}
+
+			/* Set the handle by index if the index is set, otherwise use the id */
+			if (_rc_parameter_map.param_index[i] >= 0) {
+				_parameter_handles.rc_param[i] = param_for_index((unsigned)_rc_parameter_map.param_index[i]);
+			} else {
+				_parameter_handles.rc_param[i] = param_find(_rc_parameter_map.param_id[i]);
+			}
+
+		}
+		warnx("rc to parameter map updated");
+		for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
+			warnx("\ti %d param_id %s scale %.3f value0 %.3f, min %.3f, max %.3f",
+					i,
+					_rc_parameter_map.param_id[i],
+					(double)_rc_parameter_map.scale[i],
+					(double)_rc_parameter_map.value0[i],
+					(double)_rc_parameter_map.value_min[i],
+					(double)_rc_parameter_map.value_max[i]
+					);
+		}
+	}
+}
+
 void
 Sensors::adc_poll(struct sensor_combined_s &raw)
 {
@@ -1551,6 +1705,31 @@ Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv)
 	}
 }
 
+void
+Sensors::set_params_from_rc()
+{
+	static float param_rc_values[RC_PARAM_MAP_NCHAN] = {};
+	for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
+		if (_rc.function[PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
+			/* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0)
+			 * or no request to map this channel to a param has been sent via mavlink
+			 */
+			continue;
+		}
+
+		float rc_val = get_rc_value((enum RC_CHANNELS_FUNCTION)(PARAM_1 + i), -1.0, 1.0);
+		/* Check if the value has changed,
+		 * maybe we need to introduce a more aggressive limit here */
+		if (rc_val > param_rc_values[i] + FLT_EPSILON || rc_val < param_rc_values[i] - FLT_EPSILON) {
+			param_rc_values[i] = rc_val;
+			float param_val = math::constrain(
+					_rc_parameter_map.value0[i] + _rc_parameter_map.scale[i] * rc_val,
+					_rc_parameter_map.value_min[i], _rc_parameter_map.value_max[i]);
+			param_set(_parameter_handles.rc_param[i], &param_val);
+		}
+	}
+}
+
 void
 Sensors::rc_poll()
 {
@@ -1717,6 +1896,13 @@ Sensors::rc_poll()
 			} else {
 				_actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3);
 			}
+
+			/* Update parameters from RC Channels (tuning with RC) if activated */
+			static hrt_abstime last_rc_to_param_map_time = 0;
+			if (hrt_elapsed_time(&last_rc_to_param_map_time) > 1e6) {
+				set_params_from_rc();
+				last_rc_to_param_map_time = hrt_absolute_time();
+			}
 		}
 	}
 }
@@ -1732,11 +1918,27 @@ Sensors::task_main()
 {
 
 	/* start individual sensors */
-	accel_init();
-	gyro_init();
-	mag_init();
-	baro_init();
-	adc_init();
+	int ret;
+	ret = accel_init();
+	if (ret) {
+		goto exit_immediate;
+	}
+	ret = gyro_init();
+	if (ret) {
+		goto exit_immediate;
+	}
+	ret = mag_init();
+	if (ret) {
+		goto exit_immediate;
+	}
+	ret = baro_init();
+	if (ret) {
+		goto exit_immediate;
+	}
+	ret = adc_init();
+	if (ret) {
+		goto exit_immediate;
+	}
 
 	/*
 	 * do subscriptions
@@ -1752,9 +1954,11 @@ Sensors::task_main()
 	_mag2_sub = orb_subscribe(ORB_ID(sensor_mag2));
 	_rc_sub = orb_subscribe(ORB_ID(input_rc));
 	_baro_sub = orb_subscribe(ORB_ID(sensor_baro0));
+	_baro1_sub = orb_subscribe(ORB_ID(sensor_baro1));
 	_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
 	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
+	_rc_parameter_map_sub = orb_subscribe(ORB_ID(rc_parameter_map));
 	_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 
 	/* rate limit vehicle status updates to 5Hz */
@@ -1788,6 +1992,7 @@ Sensors::task_main()
 	diff_pres_poll(raw);
 
 	parameter_update_poll(true /* forced */);
+	rc_parameter_map_poll(true /* forced */);
 
 	/* advertise the sensor_combined topic and make the initial publication */
 	_sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
@@ -1799,6 +2004,8 @@ Sensors::task_main()
 	fds[0].fd = _gyro_sub;
 	fds[0].events = POLLIN;
 
+	_task_should_exit = false;
+
 	while (!_task_should_exit) {
 
 		/* wait for up to 50ms for data */
@@ -1820,6 +2027,9 @@ Sensors::task_main()
 		/* check parameters for updates */
 		parameter_update_poll();
 
+		/* check rc parameter map for updates */
+		rc_parameter_map_poll();
+
 		/* the timestamp of the raw struct is updated by the gyro_poll() method */
 
 		/* copy most recent sensor data */
@@ -1846,8 +2056,9 @@ Sensors::task_main()
 
 	warnx("[sensors] exiting.");
 
+exit_immediate:
 	_sensors_task = -1;
-	_exit(0);
+	_exit(ret);
 }
 
 int
@@ -1863,9 +2074,13 @@ Sensors::start()
 				       (main_t)&Sensors::task_main_trampoline,
 				       nullptr);
 
+	/* wait until the task is up and running or has failed */
+	while(_sensors_task > 0 && _task_should_exit) {
+		usleep(100);
+	}
+
 	if (_sensors_task < 0) {
-		warn("task start failed");
-		return -errno;
+		return -ERROR;
 	}
 
 	return OK;

src/modules/systemlib/mixer/mixer_multirotor.cpp

@@ -78,89 +78,87 @@ float constrain(float val, float min, float max)
  */
 
 const MultirotorMixer::Rotor _config_quad_x[] = {
-	{ -0.707107,  0.707107,  1.00 },
-	{  0.707107, -0.707107,  1.00 },
-	{  0.707107,  0.707107, -1.00 },
-	{ -0.707107, -0.707107, -1.00 },
+	{ -0.707107,  0.707107,  1.000000 },
+	{  0.707107, -0.707107,  1.000000 },
+	{  0.707107,  0.707107, -1.000000 },
+	{ -0.707107, -0.707107, -1.000000 },
 };
 const MultirotorMixer::Rotor _config_quad_plus[] = {
-	{ -1.000000,  0.000000,  1.00 },
-	{  1.000000,  0.000000,  1.00 },
-	{  0.000000,  1.000000, -1.00 },
-	{ -0.000000, -1.000000, -1.00 },
+	{ -1.000000,  0.000000,  1.000000 },
+	{  1.000000,  0.000000,  1.000000 },
+	{  0.000000,  1.000000, -1.000000 },
+	{ -0.000000, -1.000000, -1.000000 },
 };
-//Add table for quad in V configuration, which is not generated by multi_tables!
 const MultirotorMixer::Rotor _config_quad_v[] = {
-	{ -0.3223,  0.9466,   0.4242 },
-	{  0.3223, -0.9466,   1.0000 },
-	{  0.3223,  0.9466,  -0.4242 },
-	{ -0.3223, -0.9466,  -1.0000 },
+	{ -0.322266,  0.946649,  0.424200 },
+	{  0.322266,  0.946649,  1.000000 },
+	{  0.322266,  0.946649, -0.424200 },
+	{ -0.322266,  0.946649, -1.000000 },
 };
 const MultirotorMixer::Rotor _config_quad_wide[] = {
-	{ -0.927184,  0.374607,  1.00 },
-	{  0.777146, -0.629320,  1.00 },
-	{  0.927184,  0.374607, -1.00 },
-	{ -0.777146, -0.629320, -1.00 },
+	{ -0.927184,  0.374607,  1.000000 },
+	{  0.777146, -0.629320,  1.000000 },
+	{  0.927184,  0.374607, -1.000000 },
+	{ -0.777146, -0.629320, -1.000000 },
 };
 const MultirotorMixer::Rotor _config_hex_x[] = {
-	{ -1.000000,  0.000000, -1.00 },
-	{  1.000000,  0.000000,  1.00 },
-	{  0.500000,  0.866025, -1.00 },
-	{ -0.500000, -0.866025,  1.00 },
-	{ -0.500000,  0.866025,  1.00 },
-	{  0.500000, -0.866025, -1.00 },
+	{ -1.000000,  0.000000, -1.000000 },
+	{  1.000000,  0.000000,  1.000000 },
+	{  0.500000,  0.866025, -1.000000 },
+	{ -0.500000, -0.866025,  1.000000 },
+	{ -0.500000,  0.866025,  1.000000 },
+	{  0.500000, -0.866025, -1.000000 },
 };
 const MultirotorMixer::Rotor _config_hex_plus[] = {
-	{  0.000000,  1.000000, -1.00 },
-	{ -0.000000, -1.000000,  1.00 },
-	{  0.866025, -0.500000, -1.00 },
-	{ -0.866025,  0.500000,  1.00 },
-	{  0.866025,  0.500000,  1.00 },
-	{ -0.866025, -0.500000, -1.00 },
+	{  0.000000,  1.000000, -1.000000 },
+	{ -0.000000, -1.000000,  1.000000 },
+	{  0.866025, -0.500000, -1.000000 },
+	{ -0.866025,  0.500000,  1.000000 },
+	{  0.866025,  0.500000,  1.000000 },
+	{ -0.866025, -0.500000, -1.000000 },
 };
 const MultirotorMixer::Rotor _config_hex_cox[] = {
-	{ -0.866025,  0.500000, -1.00 },
-	{ -0.866025,  0.500000,  1.00 },
-	{ -0.000000, -1.000000, -1.00 },
-	{ -0.000000, -1.000000,  1.00 },
-	{  0.866025,  0.500000, -1.00 },
-	{  0.866025,  0.500000,  1.00 },
+	{ -0.866025,  0.500000, -1.000000 },
+	{ -0.866025,  0.500000,  1.000000 },
+	{ -0.000000, -1.000000, -1.000000 },
+	{ -0.000000, -1.000000,  1.000000 },
+	{  0.866025,  0.500000, -1.000000 },
+	{  0.866025,  0.500000,  1.000000 },
 };
 const MultirotorMixer::Rotor _config_octa_x[] = {
-	{ -0.382683,  0.923880, -1.00 },
-	{  0.382683, -0.923880, -1.00 },
-	{ -0.923880,  0.382683,  1.00 },
-	{ -0.382683, -0.923880,  1.00 },
-	{  0.382683,  0.923880,  1.00 },
-	{  0.923880, -0.382683,  1.00 },
-	{  0.923880,  0.382683, -1.00 },
-	{ -0.923880, -0.382683, -1.00 },
+	{ -0.382683,  0.923880, -1.000000 },
+	{  0.382683, -0.923880, -1.000000 },
+	{ -0.923880,  0.382683,  1.000000 },
+	{ -0.382683, -0.923880,  1.000000 },
+	{  0.382683,  0.923880,  1.000000 },
+	{  0.923880, -0.382683,  1.000000 },
+	{  0.923880,  0.382683, -1.000000 },
+	{ -0.923880, -0.382683, -1.000000 },
 };
 const MultirotorMixer::Rotor _config_octa_plus[] = {
-	{  0.000000,  1.000000, -1.00 },
-	{ -0.000000, -1.000000, -1.00 },
-	{ -0.707107,  0.707107,  1.00 },
-	{ -0.707107, -0.707107,  1.00 },
-	{  0.707107,  0.707107,  1.00 },
-	{  0.707107, -0.707107,  1.00 },
-	{  1.000000,  0.000000, -1.00 },
-	{ -1.000000,  0.000000, -1.00 },
+	{  0.000000,  1.000000, -1.000000 },
+	{ -0.000000, -1.000000, -1.000000 },
+	{ -0.707107,  0.707107,  1.000000 },
+	{ -0.707107, -0.707107,  1.000000 },
+	{  0.707107,  0.707107,  1.000000 },
+	{  0.707107, -0.707107,  1.000000 },
+	{  1.000000,  0.000000, -1.000000 },
+	{ -1.000000,  0.000000, -1.000000 },
 };
 const MultirotorMixer::Rotor _config_octa_cox[] = {
-	{ -0.707107,  0.707107,  1.00 },
-	{  0.707107,  0.707107, -1.00 },
-	{  0.707107, -0.707107,  1.00 },
-	{ -0.707107, -0.707107, -1.00 },
-	{  0.707107,  0.707107,  1.00 },
-	{ -0.707107,  0.707107, -1.00 },
-	{ -0.707107, -0.707107,  1.00 },
-	{  0.707107, -0.707107, -1.00 },
+	{ -0.707107,  0.707107,  1.000000 },
+	{  0.707107,  0.707107, -1.000000 },
+	{  0.707107, -0.707107,  1.000000 },
+	{ -0.707107, -0.707107, -1.000000 },
+	{  0.707107,  0.707107,  1.000000 },
+	{ -0.707107,  0.707107, -1.000000 },
+	{ -0.707107, -0.707107,  1.000000 },
+	{  0.707107, -0.707107, -1.000000 },
 };
-const MultirotorMixer::Rotor _config_duorotor[] = {
-	{ -1.000000,  0.000000,  0.00 },
-	{  1.000000,  0.000000,  0.00 },
+const MultirotorMixer::Rotor _config_twin_engine[] = {
+	{ -1.000000,  0.000000,  0.000000 },
+	{  1.000000,  0.000000,  0.000000 },
 };
-
 const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = {
 	&_config_quad_x[0],
 	&_config_quad_plus[0],
@@ -172,7 +170,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = {
 	&_config_octa_x[0],
 	&_config_octa_plus[0],
 	&_config_octa_cox[0],
-	&_config_duorotor[0],
+	&_config_twin_engine[0],
 };
 const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = {
 	4, /* quad_x */

src/modules/systemlib/mixer/multi_tables

@@ -21,6 +21,12 @@ set quad_plus {
 	 180 CW
 }
 
+set quad_v {
+      18.8 0.4242
+     -18.8 1.0
+     -18.8 -0.4242
+      18.8 -1.0
+}
 
 set quad_wide {
 	  68 CCW
@@ -89,11 +95,14 @@ set octa_cox {
 	-135 CW
 }
 
+set twin_engine {
+      90 0.0
+     -90 0.0
+}
 
-set tables {quad_x quad_plus quad_wide hex_x hex_plus hex_cox octa_x octa_plus octa_cox}
+set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus hex_cox octa_x octa_plus octa_cox twin_engine}
 
-
-proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]}
+proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %9.6f }," [rcos [expr $a + 90]] [rcos $a] [expr $d]]}
 
 foreach table $tables {
 	puts [format "const MultirotorMixer::Rotor _config_%s\[\] = {" $table]
@@ -101,9 +110,11 @@ foreach table $tables {
 	upvar #0 $table angles
 	foreach {angle dir} $angles {
 		if {$dir == "CW"} {
+			set dd -1.0
+		} elseif {$dir == "CCW"} {
 			set dd 1.0
 		} else {
-			set dd -1.0
+			set dd $dir
 		}
 		factors $angle $dd
 	}

src/modules/uORB/objects_common.cpp

@@ -63,6 +63,7 @@ ORB_DEFINE(sensor_gyro2, struct gyro_report);
 #include <drivers/drv_baro.h>
 ORB_DEFINE(sensor_baro0, struct baro_report);
 ORB_DEFINE(sensor_baro1, struct baro_report);
+ORB_DEFINE(sensor_baro2, struct baro_report);
 
 #include <drivers/drv_range_finder.h>
 ORB_DEFINE(sensor_range_finder, struct range_finder_report);
@@ -246,3 +247,6 @@ ORB_DEFINE(tecs_status, struct tecs_status_s);
 
 #include "topics/wind_estimate.h"
 ORB_DEFINE(wind_estimate, struct wind_estimate_s);
+
+#include "topics/rc_parameter_map.h"
+ORB_DEFINE(rc_parameter_map, struct rc_parameter_map_s);

src/modules/uORB/topics/rc_parameter_map.h

@@ -0,0 +1,76 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT ,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file rc_parameter_map.h
+ * Maps RC channels to parameters
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#ifndef TOPIC_RC_PARAMETER_MAP_H
+#define TOPIC_RC_PARAMETER_MAP_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+#define RC_PARAM_MAP_NCHAN 3 // This limit is also hardcoded in the enum RC_CHANNELS_FUNCTION in rc_channels.h
+#define PARAM_ID_LEN 16 // corresponds to MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+struct rc_parameter_map_s {
+	uint64_t timestamp;			/**< time at which the map was updated */
+
+	bool valid[RC_PARAM_MAP_NCHAN];		/**< true for RC-Param channels which are mapped to a param */
+
+	int param_index[RC_PARAM_MAP_NCHAN];	/**< corresponding param index, this
+						  this field is ignored if set to -1, in this case param_id will
+						  be used*/
+	char param_id[RC_PARAM_MAP_NCHAN][PARAM_ID_LEN + 1];	/**< corresponding param id, null terminated */
+	float scale[RC_PARAM_MAP_NCHAN];	/** scale to map the RC input [-1, 1] to a parameter value */
+	float value0[RC_PARAM_MAP_NCHAN];	/** inital value around which the parameter value is changed */
+	float value_min[RC_PARAM_MAP_NCHAN];	/** minimal parameter value */
+	float value_max[RC_PARAM_MAP_NCHAN];	/** minimal parameter value */
+};
+
+/**
+ * @}
+ */
+
+ORB_DECLARE(rc_parameter_map);
+
+#endif

src/modules/uORB/topics/sensor_combined.h

@@ -122,15 +122,25 @@ struct sensor_combined_s {
 	float baro_pres_mbar;			/**< Barometric pressure, already temp. comp.     */
 	float baro_alt_meter;			/**< Altitude, already temp. comp.                */
 	float baro_temp_celcius;		/**< Temperature in degrees celsius               */
-	float adc_voltage_v[10];		/**< ADC voltages of ADC Chan 10/11/12/13 or -1      */
-	unsigned adc_mapping[10];		/**< Channel indices of each of these values */
-	float mcu_temp_celcius;			/**< Internal temperature measurement of MCU */
 	uint64_t baro_timestamp;		/**< Barometer timestamp        */
 
-	float differential_pressure_pa;				/**< Airspeed sensor differential pressure                  */
-	uint64_t differential_pressure_timestamp;		/**< Last measurement timestamp */
+	float baro1_pres_mbar;			/**< Barometric pressure, already temp. comp.     */
+	float baro1_alt_meter;			/**< Altitude, already temp. comp.                */
+	float baro1_temp_celcius;		/**< Temperature in degrees celsius               */
+	uint64_t baro1_timestamp;		/**< Barometer timestamp        */
+
+	float adc_voltage_v[10];		/**< ADC voltages of ADC Chan 10/11/12/13 or -1   */
+	unsigned adc_mapping[10];		/**< Channel indices of each of these values */
+	float mcu_temp_celcius;			/**< Internal temperature measurement of MCU */
+
+	float differential_pressure_pa;			/**< Airspeed sensor differential pressure */
+	uint64_t differential_pressure_timestamp;	/**< Last measurement timestamp */
 	float differential_pressure_filtered_pa;	/**< Low pass filtered airspeed sensor differential pressure reading */
 
+	float differential_pressure1_pa;			/**< Airspeed sensor differential pressure */
+	uint64_t differential_pressure1_timestamp;	/**< Last measurement timestamp */
+	float differential_pressure1_filtered_pa;	/**< Low pass filtered airspeed sensor differential pressure reading */
+
 };
 
 /**

src/modules/uORB/topics/vehicle_global_position.h

@@ -62,7 +62,7 @@
  */
 struct vehicle_global_position_s {
 	uint64_t timestamp;		/**< Time of this estimate, in microseconds since system start		*/
-	uint64_t time_gps_usec;		/**< GPS timestamp in microseconds					   */
+	uint64_t time_utc_usec;		/**< GPS UTC timestamp in microseconds					   */
 	double lat;			/**< Latitude in degrees							 	   */
 	double lon;			/**< Longitude in degrees							 	   */
 	float alt;			/**< Altitude AMSL in meters						 	   */

src/modules/uORB/topics/vehicle_gps_position.h

@@ -79,7 +79,7 @@ struct vehicle_gps_position_s {
 	bool vel_ned_valid;				/**< Flag to indicate if NED speed is valid */
 
 	uint64_t timestamp_time;			/**< Timestamp for time information */
-	uint64_t time_gps_usec;				/**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 */
+	uint64_t time_utc_usec;				/**< Timestamp (microseconds, UTC), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 */
 
 	uint8_t satellites_used;			/**< Number of satellites used */
 };

src/modules/uORB/topics/vtol_vehicle_status.h

@@ -54,6 +54,7 @@ struct vtol_vehicle_status_s {
 
 	uint64_t	timestamp;	/**< Microseconds since system boot */
 	bool vtol_in_rw_mode;	/*true: vtol vehicle is in rotating wing mode */
+	bool fw_permanent_stab;	/**< In fw mode stabilize attitude even if in manual mode*/
 };
 
 /**

src/modules/uavcan/sensors/gnss.cpp

@@ -159,7 +159,7 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
 	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.time_utc_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec();	// Convert to microseconds
 
 	report.satellites_used = msg.sats_used;
 

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -134,17 +134,21 @@ private:
 	struct {
 		param_t idle_pwm_mc;	//pwm value for idle in mc mode
 		param_t vtol_motor_count;
+		param_t vtol_fw_permanent_stab;	// in fw mode stabilize attitude also in manual mode
 		float mc_airspeed_min;		// min airspeed in multicoper mode (including prop-wash)
 		float mc_airspeed_trim;		// trim airspeed in multicopter mode
 		float mc_airspeed_max;		// max airpseed in multicopter mode
+		float fw_pitch_trim;		// trim for neutral elevon position in fw mode
 	} _params;
 
 	struct {
 		param_t idle_pwm_mc;
 		param_t vtol_motor_count;
+		param_t vtol_fw_permanent_stab;
 		param_t mc_airspeed_min;
 		param_t mc_airspeed_trim;
 		param_t mc_airspeed_max;
+		param_t fw_pitch_trim;
 	} _params_handles;
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
@@ -234,12 +238,15 @@ VtolAttitudeControl::VtolAttitudeControl() :
 
 	_params.idle_pwm_mc = PWM_LOWEST_MIN;
 	_params.vtol_motor_count = 0;
+	_params.vtol_fw_permanent_stab = 0;
 
 	_params_handles.idle_pwm_mc = param_find("VT_IDLE_PWM_MC");
 	_params_handles.vtol_motor_count = param_find("VT_MOT_COUNT");
+	_params_handles.vtol_fw_permanent_stab = param_find("VT_FW_PERM_STAB");
 	_params_handles.mc_airspeed_min = param_find("VT_MC_ARSPD_MIN");
 	_params_handles.mc_airspeed_max = param_find("VT_MC_ARSPD_MAX");
 	_params_handles.mc_airspeed_trim = param_find("VT_MC_ARSPD_TRIM");
+	_params_handles.fw_pitch_trim = param_find("VT_FW_PITCH_TRIM");
 
 	/* fetch initial parameter values */
 	parameters_update();
@@ -411,6 +418,9 @@ VtolAttitudeControl::parameters_update()
 	/* vtol motor count */
 	param_get(_params_handles.vtol_motor_count, &_params.vtol_motor_count);
 
+	/* vtol fw permanent stabilization */
+	param_get(_params_handles.vtol_fw_permanent_stab, &_params.vtol_fw_permanent_stab);
+
 	/* vtol mc mode min airspeed */
 	param_get(_params_handles.mc_airspeed_min, &v);
 	_params.mc_airspeed_min = v;
@@ -423,6 +433,10 @@ VtolAttitudeControl::parameters_update()
 	param_get(_params_handles.mc_airspeed_trim, &v);
 	_params.mc_airspeed_trim = v;
 
+	/* vtol pitch trim for fw mode */
+	param_get(_params_handles.fw_pitch_trim, &v);
+	_params.fw_pitch_trim = v;
+
 	return OK;
 }
 
@@ -452,7 +466,7 @@ void VtolAttitudeControl::fill_fw_att_control_output()
 	_actuators_out_0.control[3] = _actuators_fw_in.control[3];
 	/*controls for the elevons */
 	_actuators_out_1.control[0] = -_actuators_fw_in.control[0];	// roll elevon
-	_actuators_out_1.control[1] = _actuators_fw_in.control[1];	// pitch elevon
+	_actuators_out_1.control[1] = _actuators_fw_in.control[1] + _params.fw_pitch_trim;	// pitch elevon
 	// unused now but still logged
 	_actuators_out_1.control[2] = _actuators_fw_in.control[2];	// yaw
 	_actuators_out_1.control[3] = _actuators_fw_in.control[3];	// throttle
@@ -597,6 +611,9 @@ void VtolAttitudeControl::task_main()
 
 	parameters_update();  // initialize parameter cache
 
+	/* update vtol vehicle status*/
+	_vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false;
+
 	// make sure we start with idle in mc mode
 	set_idle_mc();
 	flag_idle_mc = true;
@@ -647,6 +664,8 @@ void VtolAttitudeControl::task_main()
 			parameters_update();
 		}
 
+		_vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false;
+
 		vehicle_control_mode_poll();	//Check for changes in vehicle control mode.
 		vehicle_manual_poll();			//Check for changes in manual inputs.
 		arming_status_poll();			//Check for arming status updates.

src/modules/vtol_att_control/vtol_att_control_params.c

@@ -86,3 +86,26 @@ PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MAX,30.0f);
  */
 PARAM_DEFINE_FLOAT(VT_MC_ARSPD_TRIM,10.0f);
 
+/**
+ * Permanent stabilization in fw mode
+ *
+ * If set to one this parameter will cause permanent attitude stabilization in fw mode.
+ * This parameter has been introduced for pure convenience sake.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_INT32(VT_FW_PERM_STAB,0);
+
+/**
+ * Fixed wing pitch trim
+ *
+ * This parameter allows to adjust the neutral elevon position in fixed wing mode.
+ *
+ * @min -1
+ * @max 1
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_FW_PITCH_TRIM,0.0f);
+

src/systemcmds/nshterm/nshterm.c

@@ -66,20 +66,18 @@ nshterm_main(int argc, char *argv[])
     int fd = -1;
     int armed_fd = orb_subscribe(ORB_ID(actuator_armed));
     struct actuator_armed_s armed;
-    /* we assume the system does not provide arming status feedback */
-    bool armed_updated = false;
 
-    /* try the first 30 seconds or if arming system is ready */
-    while ((retries < 300) || armed_updated) {
+    /* try to bring up the console - stop doing so if the system gets armed */
+    while (true) {
 
         /* abort if an arming topic is published and system is armed */
         bool updated = false;
-        if (orb_check(armed_fd, &updated)) {
+        orb_check(armed_fd, &updated);
+        if (updated) {
             /* the system is now providing arming status feedback.
              * instead of timing out, we resort to abort bringing
              * up the terminal.
              */
-            armed_updated = true;
             orb_copy(ORB_ID(actuator_armed), armed_fd, &armed);
 
             if (armed.armed) {
@@ -92,6 +90,7 @@ nshterm_main(int argc, char *argv[])
         /* which may not be ready immediately. */
         fd = open(argv[1], O_RDWR);
         if (fd != -1) {
+            close(armed_fd);
             break;
         }
         usleep(100000);
@@ -116,7 +115,7 @@ nshterm_main(int argc, char *argv[])
     }
 
     /* Set ONLCR flag (which appends a CR for every LF) */
-    uart_config.c_oflag |= (ONLCR | OPOST/* | OCRNL*/);
+    uart_config.c_oflag |= (ONLCR | OPOST);
 
     if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
         warnx("ERR set config %s\n", argv[1]);

src/systemcmds/tests/module.mk

@@ -35,5 +35,5 @@ SRCS			 = test_adc.c \
 			   test_mount.c \
 			   test_mtd.c
 
-EXTRACXXFLAGS = -Wframe-larger-than=2500
+EXTRACXXFLAGS = -Wframe-larger-than=2500 -Wno-float-equal -Wno-double-promotion
 

src/systemcmds/tests/test_mathlib.cpp

@@ -54,6 +54,7 @@ using namespace math;
 
 int test_mathlib(int argc, char *argv[])
 {
+	int rc = 0;
 	warnx("testing mathlib");
 
 	{
@@ -156,5 +157,53 @@ int test_mathlib(int argc, char *argv[])
 		TEST_OP("Matrix<10, 10> * Matrix<10, 10>", m1 * m2);
 	}
 
-	return 0;
+	{
+		// test nonsymmetric +, -, +=, -=
+
+		float data1[2][3] = {{1,2,3},{4,5,6}};
+		float data2[2][3] = {{2,4,6},{8,10,12}};
+		float data3[2][3] = {{3,6,9},{12,15,18}};
+		
+		Matrix<2, 3> m1(data1);
+		Matrix<2, 3> m2(data2);
+		Matrix<2, 3> m3(data3);
+
+		if (m1 + m2 != m3) {
+			warnx("Matrix<2, 3> + Matrix<2, 3> failed!");
+			(m1 + m2).print();
+			printf("!=\n");
+			m3.print();
+			rc = 1;
+		}
+
+		if (m3 - m2 != m1) {
+			warnx("Matrix<2, 3> - Matrix<2, 3> failed!");
+			(m3 - m2).print();
+			printf("!=\n");
+			m1.print();
+			rc = 1;
+		}
+
+		m1 += m2;
+		if (m1 != m3) {
+			warnx("Matrix<2, 3> += Matrix<2, 3> failed!");
+			m1.print();
+			printf("!=\n");
+			m3.print();
+			rc = 1;
+		}
+
+		m1 -= m2;
+		Matrix<2, 3> m1_orig(data1);
+		if (m1 != m1_orig) {
+			warnx("Matrix<2, 3> -= Matrix<2, 3> failed!");
+			m1.print();
+			printf("!=\n");
+			m1_orig.print();
+			rc = 1;
+		}
+	
+	}
+
+	return rc;
 }