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-28 10:46:33 +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:
+#after_script:
-  - echo "Branch $TRAVIS_BRANCH (pull request: $TRAVIS_PULL_REQUEST) ready for flight testing." >> $PX4_REPORT
+#  - echo "Branch $TRAVIS_BRANCH (pull request: $TRAVIS_PULL_REQUEST) ready for flight testing." >> $PX4_REPORT
-  - git log -n1 > $PX4_REPORT
+#  - git log -n1 > $PX4_REPORT
-  - echo " " >> $PX4_REPORT
+#  - echo " " >> $PX4_REPORT
-  - echo "Files available at:" >> $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 "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 "Description of desired tests is available at:" >> $PX4_REPORT
-  - echo "https://github.com/PX4/Firmware/pull/$TRAVIS_PULL_REQUEST" >> $PX4_REPORT
+#  - echo "https://github.com/PX4/Firmware/pull/$TRAVIS_PULL_REQUEST" >> $PX4_REPORT
-  - echo " " >> $PX4_REPORT
+#  - echo " " >> $PX4_REPORT
-  - echo "Thanks for testing!" >> $PX4_REPORT
+#  - echo "Thanks for testing!" >> $PX4_REPORT
-  - echo " " >> $PX4_REPORT
+#  - echo " " >> $PX4_REPORT
-  - /usr/bin/mail -s "$SUBJECT ($TRAVIS_COMMIT)" "$PX4_EMAIL" < "$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/
+#  - 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,
+### Syntax ###
 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
 ------
 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 \
+			   -funsafe-math-optimizations \
-   			   -fno-builtin-printf \
+			   -fno-builtin-printf \
-   			   -ffunction-sections \
+			   -ffunction-sections \
-   			   -fdata-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
+		if (epoch > GPS_EPOCH_SECS) {
-		_gps_position->time_gps_usec += (uint64_t)((ashtech_sec - int(ashtech_sec)) * 1e6);
+			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");
+		goto errout;
 		return -1;
 	}
 	usleep(10000);
 	if (strlen(MTK_SET_BINARY) != write(_fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) {
-		warnx("mtk: config write failed");
+		goto errout;
 		return -1;
 	}
 	usleep(10000);
 	if (strlen(SBAS_ON) != write(_fd, SBAS_ON, strlen(SBAS_ON))) {
-		warnx("mtk: config write failed");
+		goto errout;
 		return -1;
 	}
 	usleep(10000);
 	if (strlen(WAAS_ON) != write(_fd, WAAS_ON, strlen(WAAS_ON))) {
-		warnx("mtk: config write failed");
+		goto errout;
 		return -1;
 	}
 	usleep(10000);
 	if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) {
-		warnx("mtk: config write failed");
+		goto errout;
 		return -1;
 	}
 	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_utc_usec = epoch * 1e6; //TODO: test this
-	_gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3;
+	_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
+			if (epoch > GPS_EPOCH_SECS) {
-			//Since we lack a hardware RTC, set the system time clock based on GPS UTC
+				// FMUv2+ boards have a hardware RTC, but GPS helps us to configure it
-			//TODO generalize this by moving into gps.cpp?
+				// and control its drift. Since we rely on the HRT for our monotonic
-			timespec ts;
+				// clock, updating it from time to time is safe.
 			ts.tv_sec = epoch;
 			ts.tv_nsec = _buf.payload_rx_nav_pvt.nano;
 			clock_settime(CLOCK_REALTIME, &ts);
 #endif
-			_gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
+				timespec ts;
-			_gps_position->time_gps_usec += (uint64_t)(_buf.payload_rx_nav_pvt.nano * 1e-3f);
+				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
+			// only set the time if it makes sense
 			//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
-			_gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
+			if (epoch > GPS_EPOCH_SECS) {
-			_gps_position->time_gps_usec += (uint64_t)(_buf.payload_rx_nav_timeutc.nano * 1e-3f);
+				// 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'
+enum HMC5883_BUS {
-#define ID_B_WHO_AM_I			'4'
+	HMC5883_BUS_ALL,
-#define ID_C_WHO_AM_I			'3'
+	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) :
+HMC5883::HMC5883(device::Device *interface, const char *path, enum Rotation rotation) :
-	I2C("HMC5883", path, bus, HMC5883L_ADDRESS, 400000),
+	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 */
+	ret = CDev::init();
-	if (I2C::init() != OK)
+	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);
-
+	_mag_orb_id = ORB_ID_TRIPLE(sensor_mag, _class_instance);
 	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;
 	}
 	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)
+		debug("MAGIOCGEXTERNAL in main driver");
-			return 1;
+		return _interface->ioctl(cmd, dummy);
 		else
 			return 0;
 	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 = _interface->read(ADDR_DATA_OUT_X_MSB, (uint8_t *)&hmc_report, sizeof(hmc_report));
 	ret = transfer(&cmd, 1, (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
+	// XXX revisit for SPI part, might require a bus type IOCTL
-	if (_bus == PX4_I2C_BUS_ONBOARD) {
+	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 };
+	uint8_t buf = val;
-
+	return _interface->write(reg, &buf, 1);
 	return transfer(&cmd[0], 2, nullptr, 0);
 }
 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) {
-		if (g_dev_ext != nullptr)
+			warnx("already started external");
-			errx(0, "already started external");
+	} else if (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_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()) {
+		device::Device *interface = nullptr;
-			delete g_dev_ext;
+
-			g_dev_ext = 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) {
-		if (g_dev_int != nullptr)
+		warnx("already started internal");
-			errx(0, "already started internal");
+	} else if (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_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()) {
-			/* tear down the failing onboard instance */
+		device::Device *interface = nullptr;
 			delete g_dev_int;
 			g_dev_int = 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 */
+	/* verify that the device is attached and functioning, accept
-	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) {
+	 * L3GD20, L3GD20H and L3G4200D */
-
+	if ((v=read_reg(ADDR_WHO_AM_I)) == WHO_I_AM) {
 		_orientation = SENSOR_BOARD_ROTATION_DEFAULT;
 		success = true;
-	}
+	} else if ((v=read_reg(ADDR_WHO_AM_I)) == WHO_I_AM_H) {
 	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) {
 		_orientation = SENSOR_BOARD_ROTATION_180_DEG;
 		success = true;
-	}
+	} else if ((v=read_reg(ADDR_WHO_AM_I)) == WHO_I_AM_L3G4200D) {
-
+		/* Detect the L3G4200D used on AeroCore */
 	/* Detect the L3G4200D used on AeroCore */
 	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_L3G4200D) {
 		_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_checked_reg(ADDR_CTRL_REG1, 
-	write_reg(ADDR_CTRL_REG2, 0);		/* disable high-pass filters */
+                          REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
-	write_reg(ADDR_CTRL_REG3, 0x08);        /* DRDY enable */
+	write_checked_reg(ADDR_CTRL_REG2, 0);		/* disable high-pass filters */
-	write_reg(ADDR_CTRL_REG4, REG4_BDU);
+	write_checked_reg(ADDR_CTRL_REG3, 0x08);        /* DRDY enable */
-	write_reg(ADDR_CTRL_REG5, 0);
+	write_checked_reg(ADDR_CTRL_REG4, REG4_BDU);
-
+	write_checked_reg(ADDR_CTRL_REG5, 0);
-	write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE);		/* disable wake-on-interrupt */
+	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) :
+MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char* path) :
-	CDev("MS5611", MS5611_BARO_DEVICE_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) {
+		_baro_topic = orb_advertise(_orb_id, &brp);
 			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;
 		}
 		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) {
+			orb_publish(_orb_id, _baro_topic, &report);
 				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;
 			}
 		}
 		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	test(bool external_bus);
-void	reset();
+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 (external_bus) {
-	if (g_dev == nullptr) {
+		g_dev_ext = new MS5611(interface, prom_buf, MS5611_BARO_DEVICE_PATH_EXT);
-		delete interface;
+		if (g_dev_ext == nullptr) {
-		errx(1, "failed to allocate driver");
+			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) {
+	if (g_dev_int != nullptr) {
-		delete g_dev;
+		delete g_dev_int;
-		g_dev = nullptr;
+		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);
+	if (g_dev_ext) {
-	g_dev->print_info();
+		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
+#define I2C_FLOW_ADDRESS 		0x42	///< 7-bit address. 8-bit address is 0x84, range 0x42 - 0x49
 //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) :
+PX4FLOW::PX4FLOW(int bus, int address, enum Rotation rotation) :
-	I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000), //400khz
+	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 i = 0; i < M; i++)
-			for (unsigned int j = 0; j < M; j++) {
+			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 i = 0; i < M; i++)
-			for (unsigned int j = 0; j < M; j++) {
+			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 i = 0; i < M; i++)
-			for (unsigned int j = 0; j < M; j++) {
+			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 i = 0; i < M; i++)
-			for (unsigned int j = 0; j < M; j++) {
+			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_rates_enabled = (status.is_rotary_wing || status.vtol_fw_permanent_stab);
-		control_mode.flag_control_attitude_enabled = status.is_rotary_wing;
+		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)),
+	_rates_sp_id(0),
-	_actuators_id(ORB_ID(actuator_controls_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) {
+		if (!_rates_sp_id) {
-			_rates_sp_id = ORB_ID(fw_virtual_rates_setpoint);
+			if (_vehicle_status.is_vtol) {
-			_actuators_id = ORB_ID(actuator_controls_virtual_fw);
+				_rates_sp_id = ORB_ID(fw_virtual_rates_setpoint);
-		} else {
+				_actuators_id = ORB_ID(actuator_controls_virtual_fw);
-			_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
+			} else {
-			_actuators_id = ORB_ID(actuator_controls_0);
+				_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);
+						if (EOF == fputs(msg.text, fp)) {
-						fputs("\n", fp);
+							write_err_count++;
-						fsync(fileno(fp));
+						} else {
-					} 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