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
2026-05-22 22:32:11 +08:00 · 2015-01-05 10:02:07 +01:00
parent e16c4ff76e c3ed35f5cc
commit 16b9f666e7
83 changed files with 3468 additions and 1194 deletions
@@ -41,3 +41,4 @@ tags
 *.orig
 src/modules/uORB/topics/*
 Firmware.zip
+unittests/build
@@ -10,7 +10,7 @@ before_script:
 # General toolchain dependencies
  - sudo apt-get install libc6:i386 libgcc1:i386 gcc-4.6-base:i386 libstdc++5:i386 libstdc++6:i386
  - sudo apt-get install python-serial python-argparse
-  - sudo apt-get install flex bison libncurses5-dev autoconf texinfo build-essential libtool zlib1g-dev genromfs git wget
+  - sudo apt-get install flex bison libncurses5-dev autoconf texinfo build-essential libtool zlib1g-dev genromfs git wget cmake
  - pushd .
  - cd ~
  - wget https://launchpadlibrarian.net/174121628/gcc-arm-none-eabi-4_7-2014q2-20140408-linux.tar.bz2
@@ -49,19 +49,19 @@ script:
  - zip Firmware.zip Images/*.px4

 # We use an encrypted env variable to ensure this only executes when artifacts are uploaded.
-after_script:
-  - echo "Branch $TRAVIS_BRANCH (pull request: $TRAVIS_PULL_REQUEST) ready for flight testing." >> $PX4_REPORT
-  - git log -n1 > $PX4_REPORT
-  - echo " " >> $PX4_REPORT
-  - echo "Files available at:" >> $PX4_REPORT
-  - echo "https://px4-travis.s3.amazonaws.com/PX4/Firmware/$TRAVIS_BUILD_NUMBER/$TRAVIS_BUILD_NUMBER.1/Firmware.zip" >> $PX4_REPORT
-  - echo "Description of desired tests is available at:" >> $PX4_REPORT
-  - echo "https://github.com/PX4/Firmware/pull/$TRAVIS_PULL_REQUEST" >> $PX4_REPORT
-  - echo " " >> $PX4_REPORT
-  - echo "Thanks for testing!" >> $PX4_REPORT
-  - echo " " >> $PX4_REPORT
-  - /usr/bin/mail -s "$SUBJECT ($TRAVIS_COMMIT)" "$PX4_EMAIL" < "$PX4_REPORT"
-  #- s3cmd put --acl-public --guess-mime-type --config=.s3cfg Firmware.zip s3://s3-website-us-east-1.amazonaws.com/#$TRAVIS_JOB_ID/
+#after_script:
+#  - echo "Branch $TRAVIS_BRANCH (pull request: $TRAVIS_PULL_REQUEST) ready for flight testing." >> $PX4_REPORT
+#  - git log -n1 > $PX4_REPORT
+#  - echo " " >> $PX4_REPORT
+#  - echo "Files available at:" >> $PX4_REPORT
+#  - echo "https://px4-travis.s3.amazonaws.com/PX4/Firmware/$TRAVIS_BUILD_NUMBER/$TRAVIS_BUILD_NUMBER.1/Firmware.zip" >> $PX4_REPORT
+#  - echo "Description of desired tests is available at:" >> $PX4_REPORT
+#  - echo "https://github.com/PX4/Firmware/pull/$TRAVIS_PULL_REQUEST" >> $PX4_REPORT
+#  - echo " " >> $PX4_REPORT
+#  - echo "Thanks for testing!" >> $PX4_REPORT
+#  - echo " " >> $PX4_REPORT
+#  - /usr/bin/mail -s "$SUBJECT ($TRAVIS_COMMIT)" "$PX4_EMAIL" < "$PX4_REPORT"
+#  - s3cmd put --acl-public --guess-mime-type --config=.s3cfg Firmware.zip s3://s3-website-us-east-1.amazonaws.com/#$TRAVIS_JOB_ID/

 deploy:
  provider: releases
@@ -178,9 +178,10 @@ define showtaskstack
 		printf "can't measure idle stack\n"
 	else
 		set $stack_free = 0
-		while ($stack_free < $task->adj_stack_size) && *(uint8_t *)($task->stack_alloc_ptr + $stack_free)
+		while ($stack_free < $task->adj_stack_size) && ((uint32_t *)($task->stack_alloc_ptr))[$stack_free] == 0xffffffff
 			set $stack_free = $stack_free + 1
 		end
+		set $stack_free = $stack_free * 4
 		printf"    stack 0x%08x-0x%08x (%d) %d free\n", $task->stack_alloc_ptr, $task->adj_stack_ptr, $task->adj_stack_size, $stack_free
 	end
 end
@@ -278,4 +279,4 @@ define my_mem
        set $cursor = $cursor - 4
      end
    end
-end
+end
@@ -11,9 +11,13 @@
  * [Fixed wing](http://px4.io/platforms/planes/start)
  * [VTOL](http://px4.io/platforms/vtol/start)
 *   Binaries (always up-to-date from master):
-  * [Downloads](https://pixhawk.org/downloads)
+  * [Downloads](http://px4.io/downloads)
 *   Mailing list: [Google Groups](http://groups.google.com/group/px4users)

+### Users ###
+
+Please refer to the [user documentation](https://pixhawk.org/users/start) for flying drones with PX4.
+
 ### Developers ###

 Contributing guide:
@@ -25,7 +29,7 @@ http://px4.io/dev/
 This repository contains code supporting these boards:
  * PX4FMUv1.x
  * PX4FMUv2.x
-  * AeroCore
+  * AeroCore (v1 and v2)

 ## NuttShell (NSH) ##

@@ -10,7 +10,7 @@ then
 	param set NAV_LAND_ALT 90
 	param set NAV_RTL_ALT 100
 	param set NAV_RTL_LAND_T -1
-	param set NAV_ACCEPT_RAD 50
+	param set NAV_ACC_RAD 50
 	param set FW_T_HRATE_P 0.01
 	param set FW_T_RLL2THR 15
 	param set FW_T_SRATE_P 0.01
@@ -41,7 +41,9 @@ then
 	param set PE_POSNE_NOISE 0.5
 	param set PE_POSD_NOISE 1.0

-	param set NAV_ACCEPT_RAD 2.0
+	param set NAV_ACC_RAD 2.0
+	param set RTL_RETURN_ALT 30.0
+	param set RTL_DESCEND_ALT 10.0
 fi

 set PWM_RATE 400
@@ -52,6 +52,10 @@ then
 	if lsm303d start
 	then
 	fi
+
+	if ms5611 -X start
+	then
+	fi
 fi

 if meas_airspeed start
@@ -26,6 +26,8 @@ usleep 100000
 mavlink stream -d /dev/ttyACM0 -s POSITION_TARGET_GLOBAL_INT -r 10
 usleep 100000
 mavlink stream -d /dev/ttyACM0 -s LOCAL_POSITION_NED -r 30
+usleep 100000
+mavlink stream -d /dev/ttyACM0 -s MANUAL_CONTROL -r 5

 # Exit shell to make it available to MAVLink
 exit
@@ -1,61 +1,12 @@
-PX4 mixer definitions
-=====================
+## PX4 mixer definitions ##

 Files in this directory implement example mixers that can be used as a basis
 for customisation, or for general testing purposes.

-Mixer basics
------------
+For a detailed description of the mixing architecture and examples see:
+http://px4.io/dev/mixing

-Mixers combine control values from various sources (control tasks, user inputs,
-etc.) and produce output values suitable for controlling actuators; servos,
-motors, switches and so on.
-
-An actuator derives its value from the combination of one or more control
-values. Each of the control values is scaled according to the actuator's
-configuration and then combined to produce the actuator value, which may then be
-further scaled to suit the specific output type.
-
-Internally, all scaling is performed using floating point values. Inputs and
-outputs are clamped to the range -1.0 to 1.0.
-
-control    control   control
-   |          |         |
-   v          v         v
- scale      scale     scale
-   |          |         |
-   |          v         |
-   +-------> mix <------+
-              |
-            scale
-              |
-              v
-             out
-
-Scaling
-------
-
-Basic scalers provide linear scaling of the input to the output.
-
-Each scaler allows the input value to be scaled independently for inputs
-greater/less than zero. An offset can be applied to the output, and lower and
-upper boundary constraints can be applied. Negative scaling factors cause the
-output to be inverted (negative input produces positive output).
-
-Scaler pseudocode:
-
-if (input < 0)
-    output = (input * NEGATIVE_SCALE) + OFFSET
-else
-    output = (input * POSITIVE_SCALE) + OFFSET
-
-if (output < LOWER_LIMIT)
-    output = LOWER_LIMIT
-if (output > UPPER_LIMIT)
-    output = UPPER_LIMIT
-
-Syntax
------
+### Syntax ###

 Mixer definitions are text files; lines beginning with a single capital letter
 followed by a colon are significant. All other lines are ignored, meaning that
@@ -65,6 +16,9 @@ Each file may define more than one mixer; the allocation of mixers to actuators
 is specific to the device reading the mixer definition, and the number of
 actuator outputs generated by a mixer is specific to the mixer.

+For example: each simple or null mixer is assigned to outputs 1 to x
+in the order they appear in the mixer file.
+
 A mixer begins with a line of the form

 	<tag>: <mixer arguments>
@@ -72,8 +26,7 @@ A mixer begins with a line of the form
 The tag selects the mixer type; 'M' for a simple summing mixer, 'R' for a 
 multirotor mixer, etc.

-Null Mixer
-..........
+#### Null Mixer ####

 A null mixer consumes no controls and generates a single actuator output whose
 value is always zero.  Typically a null mixer is used as a placeholder in a
@@ -83,8 +36,7 @@ The null mixer definition has the form:

  Z:

-Simple Mixer
-............
+#### Simple Mixer ####

 A simple mixer combines zero or more control inputs into a single actuator
 output.  Inputs are scaled, and the mixing function sums the result before
@@ -122,8 +74,7 @@ discussed above. Whilst the calculations are performed as floating-point
 operations, the values stored in the definition file are scaled by a factor of
 10000; i.e. an offset of -0.5 is encoded as -5000.

-Multirotor Mixer
-................
+#### Multirotor Mixer ####

 The multirotor mixer combines four control inputs (roll, pitch, yaw, thrust)
 into a set of actuator outputs intended to drive motor speed controllers.
@@ -97,6 +97,26 @@ else
 	set unit_test_failure_list "${unit_test_failure_list} commander_tests"
 fi

+if hmc5883 -I start
+then
+	# This is an FMUv3
+	echo "FMUv3"
+	ms5611 start
+	mpu6000 -X start
+	mpu6000 start
+	lsm303d -X start
+	l3gd20 -X start
+	echo "EVALUATION ONLY SENSORS (not used in production)"
+	ms5611 -X start
+else
+	# This is an FMUv1 or FMUv2
+	echo "FMUv2 (or FMUv3 where 'hmc5883 -I start' failed)"
+	ms5611 start
+	mpu6000 start
+	lsm303d start
+	l3gd20 start
+fi
+
 if [ $unit_test_failure == 0 ]
 then
 	echo
@@ -109,10 +109,10 @@ ARCHOPTIMIZATION	 = $(MAXOPTIMIZATION) \
 			   -fno-strict-aliasing \
 			   -fno-strength-reduce \
 			   -fomit-frame-pointer \
-   			   -funsafe-math-optimizations \
-   			   -fno-builtin-printf \
-   			   -ffunction-sections \
-   			   -fdata-sections
+			   -funsafe-math-optimizations \
+			   -fno-builtin-printf \
+			   -ffunction-sections \
+			   -fdata-sections

 # enable precise stack overflow tracking
 # note - requires corresponding support in NuttX
@@ -228,7 +228,7 @@ DEP_INCLUDES		 = $(subst .o,.d,$(OBJS))
 define COMPILE
 	@$(ECHO) "CC:      $1"
 	@$(MKDIR) -p $(dir $2)
-	$(Q) $(CC) -MD -c $(CFLAGS) $(abspath $1) -o $2
+	$(Q) $(CCACHE) $(CC) -MD -c $(CFLAGS) $(abspath $1) -o $2
 endef

 # Compile C++ source $1 to $2
@@ -237,7 +237,7 @@ endef
 define COMPILEXX
 	@$(ECHO) "CXX:     $1"
 	@$(MKDIR) -p $(dir $2)
-	$(Q) $(CXX) -MD -c $(CXXFLAGS) $(abspath $1) -o $2
+	$(Q) $(CCACHE) $(CXX) -MD -c $(CXXFLAGS) $(abspath $1) -o $2
 endef

 # Assemble $1 into $2
@@ -96,6 +96,7 @@ int32 component_id			# subsystem / component id, inspired by MAVLink's component

 bool is_rotary_wing			# True if system is in rotary wing configuration, so for a VTOL this is only true while flying as a multicopter
 bool is_vtol				# True if the system is VTOL capable
+bool vtol_fw_permanent_stab		# True if vtol should stabilize attitude for fw in manual mode

 bool condition_battery_voltage_valid
 bool condition_system_in_air_restore		# true if we can restore in mid air
@@ -323,7 +323,7 @@ CONFIG_STM32_USART_SINGLEWIRE=y
 #
 # CONFIG_STM32_I2C_DYNTIMEO is not set
 CONFIG_STM32_I2CTIMEOSEC=0
-CONFIG_STM32_I2CTIMEOMS=1
+CONFIG_STM32_I2CTIMEOMS=10
 # CONFIG_STM32_I2C_DUTY16_9 is not set

 #
@@ -477,7 +477,9 @@ CONFIG_SPI=y
 # CONFIG_SPI_OWNBUS is not set
 CONFIG_SPI_EXCHANGE=y
 # CONFIG_SPI_CMDDATA is not set
-# CONFIG_RTC is not set
+CONFIG_RTC=y
+CONFIG_RTC_DATETIME=y
+CONFIG_RTC_HSECLOCK=y
 CONFIG_WATCHDOG=y
 # CONFIG_ANALOG is not set
 # CONFIG_AUDIO_DEVICES is not set
@@ -0,0 +1,8 @@
+#
+# driver for SMBus smart batteries
+#
+
+MODULE_COMMAND	 = batt_smbus
+SRCS		 = batt_smbus.cpp
+
+MAXOPTIMIZATION	 = -Os
@@ -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:
@@ -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:
@@ -65,6 +65,7 @@ struct baro_report {
 */
 ORB_DECLARE(sensor_baro0);
 ORB_DECLARE(sensor_baro1);
+ORB_DECLARE(sensor_baro2);

 /*
 * ioctl() definitions
@@ -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"
@@ -32,7 +32,7 @@
 ****************************************************************************/

 /**
- * @file Rangefinder driver interface.
+ * @file PX4Flow driver interface.
 */

 #ifndef _DRV_PX4FLOW_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 */
+
@@ -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);
@@ -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"\
@@ -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();

@@ -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;
 }
@@ -43,6 +43,8 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_gps_position.h>

+#define GPS_EPOCH_SECS 1234567890ULL
+
 class GPS_Helper
 {
 public:
@@ -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
@@ -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();
@@ -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);
@@ -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;
@@ -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 */
@@ -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 */
@@ -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

@@ -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;
 	}

@@ -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'");
 }
@@ -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;
+			}
 		}
 	}

@@ -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'");
@@ -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);
@@ -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];
@@ -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];
 			}

@@ -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;
@@ -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;
 						}
@@ -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);
 			}

@@ -41,3 +41,5 @@ SRCS		= fw_att_control_main.cpp \
 		  fw_att_control_params.c

 MODULE_STACKSIZE = 1200
+
+MAXOPTIMIZATION = -Os
@@ -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:
@@ -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),
--- a/Show More
+++ b/Show More