MPU9250 I2C mode

Fixes for other boards

Functioning sensors

ROMFS/px4fmu_common/init.d/4900_crazyflie

@@ -10,5 +10,12 @@
 sh /etc/init.d/4001_quad_x
 
 
+if [ $AUTOCNF == yes ]
+then
+	param set COM_RC_IN_MODE 2
+	param set BAT_N_CELLS 1
+	param set BAT_CAPACITY 240
+	param set BAT_SOURCE 1
+fi
+
 set OUTPUT_MODE crazyflie
-set USE_IO no

ROMFS/px4fmu_common/init.d/rc.interface

@@ -109,6 +109,11 @@ then
         set MIXER_AUX none
 fi
 
+if ver hwcmp CRAZYFLIE
+then
+	set MIXER_AUX none
+fi
+
 if [ $MIXER_AUX != none -a $AUX_MODE != none ]
 then
 	#

ROMFS/px4fmu_common/init.d/rc.sensors

@@ -8,12 +8,20 @@ then
 	if ms5611 start
 	then
 	fi
+
 else
+	if ver hwcmp CRAZYFLIE
+	then
+	# Crazyflie uses separate driver
+	else
+
 	# Configure all I2C buses to 100 KHz as they
 	# are all external or slow
 	fmu i2c 1 100000
 	fmu i2c 2 100000
 
+	fi
+
 	if ver hwcmp PX4FMU_V4
 	then
 		# We know there are sketchy boards out there
@@ -198,6 +206,19 @@ then
 	fi
 fi
 
+if ver hwcmp CRAZYFLIE
+then
+	# Onboard I2C
+	if mpu9250 -R 12 start
+	then
+	fi
+
+	# I2C bypass of mpu 
+	if lps25h start
+	then
+	fi
+fi
+
 if meas_airspeed start
 then
 else

ROMFS/px4fmu_common/init.d/rcS

@@ -164,10 +164,16 @@ then
 		then
 			set USE_IO no
 		fi
+
+		if ver hwcmp CRAZYFLIE
+		then
+			set USE_IO no
+		fi
 	else
 		set USE_IO no
 	fi
 
+
 	# should set to 0.8 for mindpx-v2 borad.
 	if param compare INAV_LIDAR_ERR 0.5
 	then
@@ -675,6 +681,15 @@ then
 		ardrone_interface start -d /dev/ttyS1
 	fi
 
+
+	#
+	# Start Crazyflie driver
+	#
+	if ver hwcmp CRAZYFLIE
+	then
+		crazyflie start
+	fi
+
 	#
 	# Fixed wing setup
 	#

cmake/configs/nuttx_crazyflie_default.cmake

@@ -11,7 +11,7 @@ set(config_module_list
 	drivers/led
 	drivers/boards/crazyflie
 	drivers/crazyflie
-	#drivers/mpu9250
+	drivers/mpu9250
 	#drivers/ak8963
 	drivers/lps25h
 	drivers/gps

src/drivers/mpu9250/CMakeLists.txt

@@ -44,6 +44,7 @@ px4_add_module(
 		main.cpp
 		gyro.cpp
 		mag.cpp
+		mag_i2c.cpp
 	DEPENDS
 		platforms__common
 	)

src/drivers/mpu9250/gyro.h

@@ -31,6 +31,8 @@
  *
  ****************************************************************************/
 
+#pragma once
+
 class MPU9250;
 
 /**

src/drivers/mpu9250/mag.cpp

@@ -149,8 +149,11 @@
 #define AK8963_RESET            0x01
 
 
-MPU9250_mag::MPU9250_mag(MPU9250 *parent, const char *path) :
+// If interface is non-null, then it will used for interacting with the device.
+// Otherwise, it will passthrough the parent MPU9250
+MPU9250_mag::MPU9250_mag(MPU9250 *parent, device::Device *interface, const char *path) :
 	CDev("MPU9250_mag", path),
+	_interface(interface),
 	_parent(parent),
 	_mag_topic(nullptr),
 	_mag_orb_class_instance(-1),
@@ -249,7 +252,16 @@ bool MPU9250_mag::check_duplicate(uint8_t *mag_data)
 }
 
 void
-MPU9250_mag::measure(struct ak8963_regs data)
+MPU9250_mag::measure()
+{
+	struct ak8963_regs data;
+	if(OK == _interface->read(AK8963REG_ST1, &data, sizeof(struct ak8963_regs))){
+		_measure(data);
+	}
+}
+
+void
+MPU9250_mag::_measure(struct ak8963_regs data)
 {
 	bool mag_notify = true;
 
@@ -508,12 +520,7 @@ MPU9250_mag::set_passthrough(uint8_t reg, uint8_t size, uint8_t *out)
 void
 MPU9250_mag::read_block(uint8_t reg, uint8_t *val, uint8_t count)
 {
-	uint8_t addr = reg | 0x80;
+	_parent->_interface->read(reg, val, count);
-	uint8_t tx[32] = { addr, };
-	uint8_t rx[32];
-
-	_parent->transfer(tx, rx, count + 1);
-	memcpy(val, rx + 1, count);
 }
 
 void
@@ -525,12 +532,26 @@ MPU9250_mag::passthrough_read(uint8_t reg, uint8_t *buf, uint8_t size)
 	_parent->write_reg(MPUREG_I2C_SLV0_CTRL, 0); // disable new reads
 }
 
+uint8_t
+MPU9250_mag::read_reg(unsigned int reg)
+{
+	uint8_t buf;
+	if (_interface == nullptr) {
+		passthrough_read(reg, &buf, 0x01);
+	}
+	else {
+		_interface->read(reg, &buf, 1);
+	}
+	return buf;
+}
+
+
 bool
 MPU9250_mag::ak8963_check_id(void)
 {
 	for (int i = 0; i < 5; i++) {
-		uint8_t deviceid = 0;
+
-		passthrough_read(AK8963REG_WIA, &deviceid, 0x01);
+		uint8_t deviceid = read_reg(AK8963REG_WIA);
 
 		if (AK8963_DEVICE_ID == deviceid) {
 			return true;
@@ -551,10 +572,27 @@ MPU9250_mag::passthrough_write(uint8_t reg, uint8_t val)
 	_parent->write_reg(MPUREG_I2C_SLV0_CTRL, 0); // disable new writes
 }
 
+
+
+void
+MPU9250_mag::write_reg(unsigned reg, uint8_t value)
+{
+	// general register transfer at low clock speed
+	if (_interface == nullptr) {
+		passthrough_write(reg, value);
+	}
+	else {
+		_interface->write(MPU9250_LOW_SPEED_OP(reg), &value, 1);
+	}
+}
+
+
+
+
 void
 MPU9250_mag::ak8963_reset(void)
 {
-	passthrough_write(AK8963REG_CNTL2, AK8963_RESET);
+	write_reg(AK8963REG_CNTL2, AK8963_RESET);
 }
 
 bool
@@ -563,10 +601,15 @@ MPU9250_mag::ak8963_read_adjustments(void)
 	uint8_t response[3];
 	float ak8963_ASA[3];
 
-	passthrough_write(AK8963REG_CNTL1, AK8963_FUZE_MODE | AK8963_16BIT_ADC);
+	write_reg(AK8963REG_CNTL1, AK8963_FUZE_MODE | AK8963_16BIT_ADC);
 	usleep(50);
+	if (_interface != nullptr) {
+		_interface->read(AK8963REG_ASAX, response, 3);
+	}
+	else {
 		passthrough_read(AK8963REG_ASAX, response, 3);
-	passthrough_write(AK8963REG_CNTL1, AK8963_POWERDOWN_MODE);
+	}
+	write_reg(AK8963REG_CNTL1, AK8963_POWERDOWN_MODE);
 
 	for (int i = 0; i < 3; i++) {
 		if (0 != response[i] && 0xff != response[i]) {
@@ -589,10 +632,18 @@ MPU9250_mag::ak8963_setup(void)
 {
 	int retries = 10;
 
-	// enable the I2C master to slaves on the aux bus
+	/* Configures the parent to act in master mode */
+	if(_interface == nullptr) {
 		uint8_t user_ctrl = _parent->read_reg(MPUREG_USER_CTRL);
 		_parent->write_checked_reg(MPUREG_USER_CTRL, user_ctrl | BIT_I2C_MST_EN);
 		_parent->write_reg(MPUREG_I2C_MST_CTRL, BIT_I2C_MST_P_NSR | BIT_I2C_MST_WAIT_FOR_ES | BITS_I2C_MST_CLOCK_400HZ);
+	}
+	else {
+		//	uint8_t user_ctrl = _parent->read_reg(MPUREG_USER_CTRL);
+
+		//	// Passthrough mode
+		//	_parent->write_checked_reg(MPUREG_USER_CTRL, user_ctrl & (~BIT_I2C_MST_EN));
+	}
 
 	if (!ak8963_check_id()) {
 		::printf("AK8963: bad id\n");
@@ -605,9 +656,13 @@ MPU9250_mag::ak8963_setup(void)
 		}
 	}
 
-	passthrough_write(AK8963REG_CNTL1, AK8963_CONTINUOUS_MODE2 | AK8963_16BIT_ADC);
+	write_reg(AK8963REG_CNTL1, AK8963_CONTINUOUS_MODE2 | AK8963_16BIT_ADC);
 
+
+	if(_interface == NULL) {
+		/* Configure mpu to internally read ak8963 data  */
 		set_passthrough(AK8963REG_ST1, sizeof(struct ak8963_regs));
+	}
 
 	return true;
 }

src/drivers/mpu9250/mag.h

@@ -31,6 +31,8 @@
  *
  ****************************************************************************/
 
+#pragma once
+
 class MPU9250;
 
 #pragma pack(push, 1)
@@ -43,13 +45,18 @@ struct ak8963_regs {
 };
 #pragma pack(pop)
 
+extern device::Device *AK8963_I2C_interface(int bus, bool external_bus);
+
+typedef device::Device *(*MPU9250_mag_constructor)(int, bool);
+
+
 /**
  * Helper class implementing the magnetometer driver node.
  */
 class MPU9250_mag : public device::CDev
 {
 public:
-	MPU9250_mag(MPU9250 *parent, const char *path);
+	MPU9250_mag(MPU9250 *parent, device::Device *interface, const char *path);
 	~MPU9250_mag();
 
 	virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
@@ -67,9 +74,24 @@ public:
 	bool ak8963_read_adjustments(void);
 
 protected:
+	Device			*_interface;
+
 	friend class MPU9250;
 
-	void measure(struct ak8963_regs data);
+	/* Directly measure from the _interface if possible */
+	void measure();
+
+	/* Update the state with prefetched data (internally called by the regular measure() )*/
+	void _measure(struct ak8963_regs data);
+
+
+	uint8_t read_reg(unsigned reg);
+	void write_reg(unsigned reg, uint8_t value);
+
+
+	bool is_passthrough() { return _interface == nullptr; }
+
+
 	int self_test(void);
 
 private:

src/drivers/mpu9250/mag_i2c.cpp

@@ -0,0 +1,179 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 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 mag_i2c.cpp
+ *
+ * I2C interface for AK8963
+ */
+
+/* XXX trim includes */
+#include <px4_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_accel.h>
+#include <drivers/drv_device.h>
+
+#include "MPU9250.h"
+#include "mag.h"
+
+#include "board_config.h"
+
+
+// TODO: This is redundant with mag.cpp
+#define AK8963_I2C_ADDR         0x0C
+#define ADDR_ID			0x00
+
+#define ID_WHO_AM_I		0x48
+
+
+
+#ifdef USE_I2C
+
+device::Device *AK8963_I2C_interface(int bus, bool external_bus);
+
+class AK8963_I2C : public device::I2C
+{
+public:
+	AK8963_I2C(int bus);
+	virtual ~AK8963_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 *
+AK8963_I2C_interface(int bus, bool external_bus)
+{
+	return new AK8963_I2C(bus);
+}
+
+AK8963_I2C::AK8963_I2C(int bus) :
+	I2C("AK8963_I2C", nullptr, bus, AK8963_I2C_ADDR, 400000)
+{
+	_device_id.devid_s.devtype =  DRV_MAG_DEVTYPE_MPU9250;
+}
+
+AK8963_I2C::~AK8963_I2C()
+{
+}
+
+int
+AK8963_I2C::init()
+{
+	/* this will call probe() */
+	return I2C::init();
+}
+
+int
+AK8963_I2C::ioctl(unsigned operation, unsigned &arg)
+{
+	int ret;
+
+	switch (operation) {
+
+	case ACCELIOCGEXTERNAL:
+		return _bus == PX4_I2C_BUS_EXPANSION ? 1 : 0;
+
+	case DEVIOCGDEVICEID:
+		return CDev::ioctl(nullptr, operation, arg);
+
+	case MPUIOCGIS_I2C:
+		return 1;
+
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+int
+AK8963_I2C::write(unsigned reg_speed, void *data, unsigned count)
+{
+	uint8_t cmd[MPU_MAX_WRITE_BUFFER_SIZE];
+
+	if (sizeof(cmd) < (count + 1)) {
+		return -EIO;
+	}
+
+	cmd[0] = MPU9250_REG(reg_speed);
+	cmd[1] = *(uint8_t *)data;
+	return transfer(&cmd[0], count + 1, nullptr, 0);
+}
+
+int
+AK8963_I2C::read(unsigned reg_speed, void *data, unsigned count)
+{
+	uint8_t cmd = MPU9250_REG(reg_speed);
+	return transfer(&cmd, 1, (uint8_t *)data, count);
+}
+
+
+int
+AK8963_I2C::probe()
+{
+	uint8_t whoami = 0;
+	uint8_t expected = ID_WHO_AM_I;
+
+	if (read(ADDR_ID, &whoami, 1)) {
+		return -EIO;
+	}
+
+	if (whoami != expected) {
+		return -EIO;
+	}
+
+	return OK;
+}
+
+#endif

src/drivers/mpu9250/main.cpp

@@ -34,7 +34,7 @@
 /**
  * @file main.cpp
  *
- * Driver for the Invensense mpu9250 connected via SPI.
+ * Driver for the Invensense mpu9250 connected via I2C or SPI.
  *
  * @authors Andrew Tridgell
  *          Robert Dickenson
@@ -83,11 +83,6 @@
 /** driver 'main' command */
 extern "C" { __EXPORT int mpu9250_main(int argc, char *argv[]); }
 
-/**
- * Local functions in support of the shell command.
- */
-namespace mpu9250
-{
 
 enum MPU9250_BUS {
 	MPU9250_BUS_ALL = 0,
@@ -97,6 +92,13 @@ enum MPU9250_BUS {
 	MPU9250_BUS_SPI_EXTERNAL
 };
 
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace mpu9250
+{
+
 /*
   list of supported bus configurations
  */
@@ -107,30 +109,31 @@ struct mpu9250_bus_option {
 	const char *gyropath;
 	const char *magpath;
 	MPU9250_constructor interface_constructor;
+	bool magpassthrough;
 	uint8_t busnum;
-	MPU6000	*dev;
+	MPU9250	*dev;
 } bus_options[] = {
 #if defined (USE_I2C)
 #  if defined(PX4_I2C_BUS_ONBOARD)
-	{ MPU9250_BUS_I2C_INTERNAL, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, MPU_DEVICE_PATH_MAG,  &MPU9250_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL },
+	{ MPU9250_BUS_I2C_INTERNAL, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, MPU_DEVICE_PATH_MAG,  &MPU9250_I2C_interface, false, PX4_I2C_BUS_ONBOARD, NULL },
 #  endif
 #  if defined(PX4_I2C_BUS_EXPANSION)
-	{ MPU9250_BUS_I2C_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, MPU_DEVICE_PATH_MAG_EXT, &MPU9250_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL },
+	{ MPU9250_BUS_I2C_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, MPU_DEVICE_PATH_MAG_EXT, &MPU9250_I2C_interface, false, PX4_I2C_BUS_EXPANSION, NULL },
 #  endif
 #endif
 #ifdef PX4_SPIDEV_MPU
-	{ MPU9250_BUS_SPI_INTERNAL, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, MPU_DEVICE_PATH_MAG, &MPU9250_SPI_interface, PX4_SPI_BUS_SENSORS, NULL },
+	{ MPU9250_BUS_SPI_INTERNAL, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, MPU_DEVICE_PATH_MAG, &MPU9250_SPI_interface, true, PX4_SPI_BUS_SENSORS, NULL },
 #endif
 #if defined(PX4_SPI_BUS_EXT)
-	{ MPU9250_BUS_SPI_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, MPU_DEVICE_PATH_MAG_EXT &MPU9250_SPI_interface, PX4_SPI_BUS_EXT, NULL },
+	{ MPU9250_BUS_SPI_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, MPU_DEVICE_PATH_MAG_EXT &MPU9250_SPI_interface, true, PX4_SPI_BUS_EXT, NULL },
 #endif
 };
 
 #define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
 
 
-void	start(enum MPU9250_BUS busid, enum Rotation rotation);
+void	start(enum MPU9250_BUS busid, enum Rotation rotation, bool external_bus);
-bool	start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation);
+bool	start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, bool external_bus);
 struct mpu9250_bus_option &find_bus(enum MPU9250_BUS busid);
 void	stop(enum MPU9250_BUS busid);
 void	test(enum MPU9250_BUS busid);
@@ -159,7 +162,7 @@ struct mpu9250_bus_option &find_bus(enum MPU9250_BUS busid)
  * start driver for a specific bus option
  */
 bool
-start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, int range, int device_type, bool external)
+start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, bool external)
 {
 	int fd = -1;
 
@@ -168,7 +171,7 @@ start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, int range, int
 		return false;
 	}
 
-	device::Device *interface = bus.interface_constructor(bus.busnum, device_type, external);
+	device::Device *interface = bus.interface_constructor(bus.busnum, external);
 
 	if (interface == nullptr) {
 		warnx("no device on bus %u", (unsigned)bus.busid);
@@ -181,7 +184,17 @@ start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, int range, int
 		return false;
 	}
 
-	bus.dev = new MPU9250(interface, bus.accelpath, bus.gyropath, bus.magpath, rotation, device_type);
+	device::Device *mag_interface = nullptr;
+
+#ifdef USE_I2C
+	/* For i2c interfaces, connect to the magnetomer directly */
+	bool is_i2c = bus.busid == MPU9250_BUS_I2C_INTERNAL || bus.busid == MPU9250_BUS_I2C_EXTERNAL;
+	if (is_i2c) {
+		mag_interface = AK8963_I2C_interface(bus.busnum, external);
+	}
+#endif
+
+	bus.dev = new MPU9250(interface, mag_interface, bus.accelpath, bus.gyropath, bus.magpath, rotation);
 
 	if (bus.dev == nullptr) {
 		delete interface;
@@ -203,9 +216,6 @@ start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, int range, int
 		goto fail;
 	}
 
-	if (ioctl(fd, ACCELIOCSRANGE, range) < 0) {
-		goto fail;
-	}
 
 	close(fd);
 
@@ -232,7 +242,7 @@ fail:
  * or failed to detect the sensor.
  */
 void
-start(enum MPU9250_BUS busid, enum Rotation rotation, int range, int device_type, bool external)
+start(enum MPU9250_BUS busid, enum Rotation rotation, bool external)
 {
 
 	bool started = false;
@@ -248,7 +258,7 @@ start(enum MPU9250_BUS busid, enum Rotation rotation, int range, int device_type
 			continue;
 		}
 
-		started |= start_bus(bus_options[i], rotation, range, device_type, external);
+		started |= start_bus(bus_options[i], rotation, external);
 	}
 
 	exit(started ? 0 : 1);
@@ -473,7 +483,6 @@ usage()
 	warnx("options:");
 	warnx("    -X    (external bus)");
 	warnx("    -R rotation");
-	warnx("    -a accel range (in g)");
 }
 
 } // namespace
@@ -485,13 +494,13 @@ mpu9250_main(int argc, char *argv[])
 	int ch;
 	bool external = false;
 	enum Rotation rotation = ROTATION_NONE;
-	int accel_range = 8;
+	//int accel_range = 8;
 
 	/* jump over start/off/etc and look at options first */
-	while ((ch = getopt(argc, argv, "XISsR:a:")) != EOF) {
+	while ((ch = getopt(argc, argv, "XISsR:")) != EOF) {
 		switch (ch) {
 		case 'X':
-			busid = MPU9260_BUS_I2C_EXTERNAL;
+			busid = MPU9250_BUS_I2C_EXTERNAL;
 			break;
 
 		case 'I':
@@ -503,17 +512,13 @@ mpu9250_main(int argc, char *argv[])
 			break;
 
 		case 's':
-			busid = MPU9260_BUS_SPI_INTERNAL;
+			busid = MPU9250_BUS_SPI_INTERNAL;
 			break;
 
 		case 'R':
 			rotation = (enum Rotation)atoi(optarg);
 			break;
 
-		case 'a':
-			accel_range = atoi(optarg);
-			break;
-
 		default:
 			mpu9250::usage();
 			exit(0);
@@ -529,7 +534,7 @@ mpu9250_main(int argc, char *argv[])
 
 	 */
 	if (!strcmp(verb, "start")) {
-		mpu9250::start(busid, rotation, accel_range, device_type, external);
+		mpu9250::start(busid, rotation, external);
 	}
 
 	if (!strcmp(verb, "stop")) {

src/drivers/mpu9250/mpu9250.cpp

@@ -61,6 +61,7 @@
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
 #include <systemlib/conversions.h>
+#include <systemlib/px4_macros.h>
 
 #include <nuttx/arch.h>
 #include <nuttx/clock.h>
@@ -81,113 +82,6 @@
 #include "gyro.h"
 #include "mpu9250.h"
 
-#define DIR_READ			0x80
-#define DIR_WRITE			0x00
-
-// MPU 9250 registers
-#define MPUREG_WHOAMI			0x75
-#define MPUREG_SMPLRT_DIV		0x19
-#define MPUREG_CONFIG			0x1A
-#define MPUREG_GYRO_CONFIG		0x1B
-#define MPUREG_ACCEL_CONFIG		0x1C
-#define MPUREG_ACCEL_CONFIG2		0x1D
-#define MPUREG_LPACCEL_ODR		0x1E
-#define MPUREG_WOM_THRESH		0x1F
-#define MPUREG_FIFO_EN			0x23
-#define MPUREG_I2C_MST_CTRL		0x24
-#define MPUREG_I2C_SLV0_ADDR		0x25
-#define MPUREG_I2C_SLV0_REG		0x26
-#define MPUREG_I2C_SLV0_CTRL		0x27
-#define MPUREG_I2C_SLV1_ADDR		0x28
-#define MPUREG_I2C_SLV1_REG		0x29
-#define MPUREG_I2C_SLV1_CTRL		0x2A
-#define MPUREG_I2C_SLV2_ADDR		0x2B
-#define MPUREG_I2C_SLV2_REG		0x2C
-#define MPUREG_I2C_SLV2_CTRL		0x2D
-#define MPUREG_I2C_SLV3_ADDR		0x2E
-#define MPUREG_I2C_SLV3_REG		0x2F
-#define MPUREG_I2C_SLV3_CTRL		0x30
-#define MPUREG_I2C_SLV4_ADDR		0x31
-#define MPUREG_I2C_SLV4_REG		0x32
-#define MPUREG_I2C_SLV4_DO		0x33
-#define MPUREG_I2C_SLV4_CTRL		0x34
-#define MPUREG_I2C_SLV4_DI		0x35
-#define MPUREG_I2C_MST_STATUS		0x36
-#define MPUREG_INT_PIN_CFG		0x37
-#define MPUREG_INT_ENABLE		0x38
-#define MPUREG_INT_STATUS		0x3A
-#define MPUREG_ACCEL_XOUT_H		0x3B
-#define MPUREG_ACCEL_XOUT_L		0x3C
-#define MPUREG_ACCEL_YOUT_H		0x3D
-#define MPUREG_ACCEL_YOUT_L		0x3E
-#define MPUREG_ACCEL_ZOUT_H		0x3F
-#define MPUREG_ACCEL_ZOUT_L		0x40
-#define MPUREG_TEMP_OUT_H		0x41
-#define MPUREG_TEMP_OUT_L		0x42
-#define MPUREG_GYRO_XOUT_H		0x43
-#define MPUREG_GYRO_XOUT_L		0x44
-#define MPUREG_GYRO_YOUT_H		0x45
-#define MPUREG_GYRO_YOUT_L		0x46
-#define MPUREG_GYRO_ZOUT_H		0x47
-#define MPUREG_GYRO_ZOUT_L		0x48
-#define MPUREG_EXT_SENS_DATA_00		0x49
-#define MPUREG_I2C_SLV0_D0		0x63
-#define MPUREG_I2C_SLV1_D0		0x64
-#define MPUREG_I2C_SLV2_D0		0x65
-#define MPUREG_I2C_SLV3_D0		0x66
-#define MPUREG_I2C_MST_DELAY_CTRL	0x67
-#define MPUREG_SIGNAL_PATH_RESET	0x68
-#define MPUREG_MOT_DETECT_CTRL		0x69
-#define MPUREG_USER_CTRL		0x6A
-#define MPUREG_PWR_MGMT_1		0x6B
-#define MPUREG_PWR_MGMT_2		0x6C
-#define MPUREG_FIFO_COUNTH		0x72
-#define MPUREG_FIFO_COUNTL		0x73
-#define MPUREG_FIFO_R_W			0x74
-
-// Configuration bits MPU 9250
-#define BIT_SLEEP			0x40
-#define BIT_H_RESET			0x80
-#define MPU_CLK_SEL_AUTO		0x01
-
-#define BITS_GYRO_ST_X			0x80
-#define BITS_GYRO_ST_Y			0x40
-#define BITS_GYRO_ST_Z			0x20
-#define BITS_FS_250DPS			0x00
-#define BITS_FS_500DPS			0x08
-#define BITS_FS_1000DPS			0x10
-#define BITS_FS_2000DPS			0x18
-#define BITS_FS_MASK			0x18
-
-#define BITS_DLPF_CFG_250HZ		0x00
-#define BITS_DLPF_CFG_184HZ		0x01
-#define BITS_DLPF_CFG_92HZ		0x02
-#define BITS_DLPF_CFG_41HZ		0x03
-#define BITS_DLPF_CFG_20HZ		0x04
-#define BITS_DLPF_CFG_10HZ		0x05
-#define BITS_DLPF_CFG_5HZ		0x06
-#define BITS_DLPF_CFG_3600HZ		0x07
-#define BITS_DLPF_CFG_MASK		0x07
-
-#define BITS_ACCEL_CONFIG2_41HZ		0x03
-
-#define BIT_RAW_RDY_EN			0x01
-#define BIT_INT_ANYRD_2CLEAR		0x10
-
-#define MPU_WHOAMI_9250			0x71
-
-#define MPU9250_ACCEL_DEFAULT_RATE	1000
-#define MPU9250_ACCEL_MAX_OUTPUT_RATE			280
-#define MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30
-#define MPU9250_GYRO_DEFAULT_RATE	1000
-/* rates need to be the same between accel and gyro */
-#define MPU9250_GYRO_MAX_OUTPUT_RATE			MPU9250_ACCEL_MAX_OUTPUT_RATE
-#define MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30
-
-#define MPU9250_DEFAULT_ONCHIP_FILTER_FREQ	41
-
-#define MPU9250_ONE_G					9.80665f
-
 /*
   we set the timer interrupt to run a bit faster than the desired
   sample rate and then throw away duplicates by comparing
@@ -215,15 +109,19 @@ const uint8_t MPU9250::_checked_registers[MPU9250_NUM_CHECKED_REGISTERS] = { MPU
 									   };
 
 
-MPU9250::MPU9250(device::Device *interface, const char *path_accel, const char *path_gyro, const char *path_mag,
+MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const char *path_accel, const char *path_gyro, const char *path_mag,
 		 enum Rotation rotation) :
 	CDev("MPU9250", path_accel),
 	_interface(interface),
-	_interface_bus(interface_bus),
-	//SPI("MPU9250", path_accel, bus, device, SPIDEV_MODE3, MPU9250_LOW_BUS_SPEED),
 	_gyro(new MPU9250_gyro(this, path_gyro)),
-	_mag(new MPU9250_mag(this, path_mag)),
+	_mag(new MPU9250_mag(this, mag_interface, path_mag)),
 	_whoami(0),
+#if defined(USE_I2C)
+	_work {},
+	_use_hrt(false),
+#else
+	_use_hrt(true),
+#endif
 	_call{},
 	_call_interval(0),
 	_accel_reports(nullptr),
@@ -277,6 +175,9 @@ MPU9250::MPU9250(device::Device *interface, const char *path_accel, const char *
 	_mag->_device_id.devid = _device_id.devid;
 	_mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_MPU9250;
 
+	/* For an independent mag, ensure that it is connected to the i2c bus */
+
+
 	// default accel scale factors
 	_accel_scale.x_offset = 0;
 	_accel_scale.x_scale  = 1.0f;
@@ -294,6 +195,7 @@ MPU9250::MPU9250(device::Device *interface, const char *path_accel, const char *
 	_gyro_scale.z_scale  = 1.0f;
 
 	memset(&_call, 0, sizeof(_call));
+	memset(&_work, 0, sizeof(_work));
 }
 
 MPU9250::~MPU9250()
@@ -334,8 +236,19 @@ MPU9250::~MPU9250()
 int
 MPU9250::init()
 {
-	int ret;
 
+#if defined(USE_I2C)
+	unsigned dummy;
+	use_i2c(_interface->ioctl(MPUIOCGIS_I2C, dummy));
+#endif
+
+
+	int ret = probe();
+
+	if (ret != OK) {
+		DEVICE_DEBUG("MPU9250 probe failed");
+		return ret;
+	}
 
 
 	/* do init */
@@ -349,21 +262,7 @@ MPU9250::init()
 	}
 
 
-//	/* do SPI init (and probe) first */
-//	ret = SPI::init();
 
-//	/* if probe/setup failed, bail now */
-//	if (ret != OK) {
-//		DEVICE_DEBUG("SPI setup failed");
-//		return ret;
-//	}
-
-	ret = probe();
-
-	if (ret != OK) {
-		DEVICE_DEBUG("MPU9250 probe failed");
-		return ret;
-	}
 
 	/* allocate basic report buffers */
 	_accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report));
@@ -406,6 +305,11 @@ MPU9250::init()
 		return ret;
 	}
 
+
+	if (!_mag->is_passthrough() && _mag->_interface->init() != OK) {
+		warnx("failed to setup ak8963 interface");
+	}
+
 	/* do CDev init for the mag device node, keep it optional */
 	ret = _mag->init();
 
@@ -486,7 +390,7 @@ int MPU9250::reset()
 	// INT CFG => Interrupt on Data Ready
 	write_checked_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN);        // INT: Raw data ready
 	usleep(1000);
-	write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read
+	write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR | (_mag->is_passthrough()? 0 : BIT_INT_BYPASS_EN)); // INT: Clear on any read, also use i2c bypass is master mode isn't needed
 	usleep(1000);
 
 	write_checked_reg(MPUREG_ACCEL_CONFIG2, BITS_ACCEL_CONFIG2_41HZ);
@@ -758,8 +662,7 @@ MPU9250::test_error()
 	// development as a handy way to test the reset logic
 	uint8_t data[16];
 	memset(data, 0, sizeof(data));
-	_interface->read(MPU6000_SET_SPEED(MPUREG_INT_STATUS, MPU6000_LOW_BUS_SPEED), data, sizeof(data));
+	_interface->read(MPU9250_SET_SPEED(MPUREG_INT_STATUS, MPU9250_LOW_BUS_SPEED), data, sizeof(data));
-	//transfer(data, data, sizeof(data));
 	::printf("error triggered\n");
 	print_registers();
 }
@@ -973,7 +876,7 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	default:
 		/* give it to the superclass */
-		return SPI::ioctl(filp, cmd, arg);
+		return CDev::ioctl(filp, cmd, arg);
 	}
 }
 
@@ -1071,23 +974,9 @@ MPU9250::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 uint8_t
 MPU9250::read_reg(unsigned reg, uint32_t speed)
 {
-
-	// From MPU6000 implementation
 	uint8_t buf;
-	_interface->read(MPU6000_SET_SPEED(reg, speed), &buf, 1);
+	_interface->read(MPU9250_SET_SPEED(reg, speed), &buf, 1);
 	return buf;
-
-
-
-
-	uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0};
-
-	// general register transfer at low clock speed
-	set_frequency(speed);
-
-	transfer(cmd, cmd, sizeof(cmd));
-
-	return cmd[1];
 }
 
 uint16_t
@@ -1099,18 +988,6 @@ MPU9250::read_reg16(unsigned reg)
 
 	_interface->read(MPU9250_LOW_SPEED_OP(reg), &buf, arraySize(buf));
 	return (uint16_t)(buf[0] << 8) | buf[1];
-
-
-
-
-	uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 };
-
-	// general register transfer at low clock speed
-	set_frequency(MPU9250_LOW_BUS_SPEED);
-
-	transfer(cmd, cmd, sizeof(cmd));
-
-	return (uint16_t)(cmd[1] << 8) | cmd[2];
 }
 
 void
@@ -1118,19 +995,7 @@ MPU9250::write_reg(unsigned reg, uint8_t value)
 {
 	// general register transfer at low clock speed
 
-	return _interface->write(MPU9250_LOW_SPEED_OP(reg), &value, 1);
+	_interface->write(MPU9250_LOW_SPEED_OP(reg), &value, 1);
-
-
-
-	uint8_t	cmd[2];
-
-	cmd[0] = reg | DIR_WRITE;
-	cmd[1] = value;
-
-	// general register transfer at low clock speed
-	set_frequency(MPU9250_LOW_BUS_SPEED);
-
-	transfer(cmd, nullptr, sizeof(cmd));
 }
 
 void
@@ -1153,6 +1018,7 @@ MPU9250::write_checked_reg(unsigned reg, uint8_t value)
 		if (reg == _checked_registers[i]) {
 			_checked_values[i] = value;
 			_checked_bad[i] = value;
+			break; // TODO: Assumes no duplicates
 		}
 	}
 }
@@ -1203,18 +1069,69 @@ MPU9250::start()
 	_gyro_reports->flush();
 	_mag->_mag_reports->flush();
 
+	if (_use_hrt) {
 		/* start polling at the specified rate */
 		hrt_call_every(&_call,
 			       1000,
 			       _call_interval - MPU9250_TIMER_REDUCTION,
-		       (hrt_callout)&MPU9250::measure_trampoline, this);
+			       (hrt_callout)&MPU9250::measure_trampoline, this);;
+	} else {
+#ifdef USE_I2C
+		/* schedule a cycle to start things */
+		work_queue(HPWORK, &_work, (worker_t)&MPU9250::cycle_trampoline, this, 1);
+#endif
+	}
+
 }
 
 void
 MPU9250::stop()
 {
+	if (_use_hrt) {
 		hrt_cancel(&_call);
+
+	} else {
+#ifdef USE_I2C
+		work_cancel(HPWORK, &_work);
+#endif
 	}
+}
+
+
+#if defined(USE_I2C)
+void
+MPU9250::cycle_trampoline(void *arg)
+{
+	MPU9250 *dev = (MPU9250 *)arg;
+
+	dev->cycle();
+}
+
+void
+MPU9250::cycle()
+{
+
+//	int ret = measure();
+
+	measure();
+
+//	if (ret != OK) {
+//		/* issue a reset command to the sensor */
+//		reset();
+//		start();
+//		return;
+//	}
+
+	if (_call_interval != 0) {
+		work_queue(HPWORK,
+			   &_work,
+			   (worker_t)&MPU9250::cycle_trampoline,
+			   this,
+			   USEC2TICK(_call_interval - MPU9250_TIMER_REDUCTION));
+	}
+}
+#endif
+
 
 void
 MPU9250::measure_trampoline(void *arg)
@@ -1350,12 +1267,13 @@ MPU9250::measure()
 	/* start measuring */
 	perf_begin(_sample_perf);
 
+	// TODO: Don't fetch magnetometer data if it isn't being managed
+	// In that case, trigger a magnetometer read
+
 	/*
 	 * Fetch the full set of measurements from the MPU9250 in one pass.
 	 */
-	mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS;
+	if (OK != _interface->read(MPU9250_SET_SPEED(MPUREG_INT_STATUS, MPU9250_HIGH_BUS_SPEED),
-
-	if (sizeof(mpu_report) != _interface->read(MPU9250_SET_SPEED(MPUREG_INT_STATUS, MPU9250_HIGH_BUS_SPEED),
 			(uint8_t *)&mpu_report,
 			sizeof(mpu_report))) {
 		return;
@@ -1367,7 +1285,12 @@ MPU9250::measure()
 		return;
 	}
 
-	_mag->measure(mpu_report.mag);
+	if (_mag->is_passthrough()) {
+		_mag->_measure(mpu_report.mag);
+	}
+	else {
+		_mag->measure();
+	}
 
 	/*
 	 * Convert from big to little endian

src/drivers/mpu9250/mpu9250.h

@@ -1,13 +1,250 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 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.
+ *
+ ****************************************************************************/
+
+
+#pragma once
+
+
+
+// TODO: All these includes are from main.cpp
+#include <px4_config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <stdio.h>
+#include <getopt.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+#include <systemlib/conversions.h>
+
+// TODO: These three are new
+#include <nuttx/arch.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+
+#include <board_config.h>
+#include <drivers/drv_hrt.h>
+
+#include <drivers/device/spi.h>
+#include <drivers/device/i2c.h>
+#include <drivers/device/ringbuffer.h>
+#include <drivers/device/integrator.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_gyro.h>
+#include <drivers/drv_mag.h>
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
+#include <lib/conversion/rotation.h>
+
+#include "mag.h"
+#include "gyro.h"
+
+
+
+#if defined(PX4_I2C_OBDEV_MPU9250)
+#  define USE_I2C
+#endif
+
+#define DIR_READ			0x80
+#define DIR_WRITE			0x00
+
+// MPU 9250 registers
+#define MPUREG_WHOAMI			0x75
+#define MPUREG_SMPLRT_DIV		0x19
+#define MPUREG_CONFIG			0x1A
+#define MPUREG_GYRO_CONFIG		0x1B
+#define MPUREG_ACCEL_CONFIG		0x1C
+#define MPUREG_ACCEL_CONFIG2		0x1D
+#define MPUREG_LPACCEL_ODR		0x1E
+#define MPUREG_WOM_THRESH		0x1F
+#define MPUREG_FIFO_EN			0x23
+#define MPUREG_I2C_MST_CTRL		0x24
+#define MPUREG_I2C_SLV0_ADDR		0x25
+#define MPUREG_I2C_SLV0_REG		0x26
+#define MPUREG_I2C_SLV0_CTRL		0x27
+#define MPUREG_I2C_SLV1_ADDR		0x28
+#define MPUREG_I2C_SLV1_REG		0x29
+#define MPUREG_I2C_SLV1_CTRL		0x2A
+#define MPUREG_I2C_SLV2_ADDR		0x2B
+#define MPUREG_I2C_SLV2_REG		0x2C
+#define MPUREG_I2C_SLV2_CTRL		0x2D
+#define MPUREG_I2C_SLV3_ADDR		0x2E
+#define MPUREG_I2C_SLV3_REG		0x2F
+#define MPUREG_I2C_SLV3_CTRL		0x30
+#define MPUREG_I2C_SLV4_ADDR		0x31
+#define MPUREG_I2C_SLV4_REG		0x32
+#define MPUREG_I2C_SLV4_DO		0x33
+#define MPUREG_I2C_SLV4_CTRL		0x34
+#define MPUREG_I2C_SLV4_DI		0x35
+#define MPUREG_I2C_MST_STATUS		0x36
+#define MPUREG_INT_PIN_CFG		0x37
+#define MPUREG_INT_ENABLE		0x38
+#define MPUREG_INT_STATUS		0x3A
+#define MPUREG_ACCEL_XOUT_H		0x3B
+#define MPUREG_ACCEL_XOUT_L		0x3C
+#define MPUREG_ACCEL_YOUT_H		0x3D
+#define MPUREG_ACCEL_YOUT_L		0x3E
+#define MPUREG_ACCEL_ZOUT_H		0x3F
+#define MPUREG_ACCEL_ZOUT_L		0x40
+#define MPUREG_TEMP_OUT_H		0x41
+#define MPUREG_TEMP_OUT_L		0x42
+#define MPUREG_GYRO_XOUT_H		0x43
+#define MPUREG_GYRO_XOUT_L		0x44
+#define MPUREG_GYRO_YOUT_H		0x45
+#define MPUREG_GYRO_YOUT_L		0x46
+#define MPUREG_GYRO_ZOUT_H		0x47
+#define MPUREG_GYRO_ZOUT_L		0x48
+#define MPUREG_EXT_SENS_DATA_00		0x49
+#define MPUREG_I2C_SLV0_D0		0x63
+#define MPUREG_I2C_SLV1_D0		0x64
+#define MPUREG_I2C_SLV2_D0		0x65
+#define MPUREG_I2C_SLV3_D0		0x66
+#define MPUREG_I2C_MST_DELAY_CTRL	0x67
+#define MPUREG_SIGNAL_PATH_RESET	0x68
+#define MPUREG_MOT_DETECT_CTRL		0x69
+#define MPUREG_USER_CTRL		0x6A
+#define MPUREG_PWR_MGMT_1		0x6B
+#define MPUREG_PWR_MGMT_2		0x6C
+#define MPUREG_FIFO_COUNTH		0x72
+#define MPUREG_FIFO_COUNTL		0x73
+#define MPUREG_FIFO_R_W			0x74
+
+ // Configuration bits MPU 9250
+#define BIT_SLEEP			0x40
+#define BIT_H_RESET			0x80
+#define MPU_CLK_SEL_AUTO		0x01
+
+#define BITS_GYRO_ST_X			0x80
+#define BITS_GYRO_ST_Y			0x40
+#define BITS_GYRO_ST_Z			0x20
+#define BITS_FS_250DPS			0x00
+#define BITS_FS_500DPS			0x08
+#define BITS_FS_1000DPS			0x10
+#define BITS_FS_2000DPS			0x18
+#define BITS_FS_MASK			0x18
+
+#define BITS_DLPF_CFG_250HZ		0x00
+#define BITS_DLPF_CFG_184HZ		0x01
+#define BITS_DLPF_CFG_92HZ		0x02
+#define BITS_DLPF_CFG_41HZ		0x03
+#define BITS_DLPF_CFG_20HZ		0x04
+#define BITS_DLPF_CFG_10HZ		0x05
+#define BITS_DLPF_CFG_5HZ		0x06
+#define BITS_DLPF_CFG_3600HZ		0x07
+#define BITS_DLPF_CFG_MASK		0x07
+
+#define BITS_ACCEL_CONFIG2_41HZ		0x03
+
+#define BIT_RAW_RDY_EN			0x01
+#define BIT_INT_ANYRD_2CLEAR		0x10
+#define BIT_INT_BYPASS_EN		0x02
+
+#define MPU_WHOAMI_9250			0x71
+
+#define MPU9250_ACCEL_DEFAULT_RATE	1000
+#define MPU9250_ACCEL_MAX_OUTPUT_RATE			280
+#define MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30
+#define MPU9250_GYRO_DEFAULT_RATE	1000
+/* rates need to be the same between accel and gyro */
+#define MPU9250_GYRO_MAX_OUTPUT_RATE			MPU9250_ACCEL_MAX_OUTPUT_RATE
+#define MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30
+
+#define MPU9250_DEFAULT_ONCHIP_FILTER_FREQ	41
 
 #define MPU9250_ONE_G					9.80665f
 
+#define MPUIOCGIS_I2C	(unsigned)(DEVIOCGDEVICEID+100)
+
+
+#pragma pack(push, 1)
+/**
+ * Report conversation within the mpu, including command byte and
+ * interrupt status.
+ */
+struct MPUReport {
+	uint8_t		cmd;
+	uint8_t		status;
+	uint8_t		accel_x[2];
+	uint8_t		accel_y[2];
+	uint8_t		accel_z[2];
+	uint8_t		temp[2];
+	uint8_t		gyro_x[2];
+	uint8_t		gyro_y[2];
+	uint8_t		gyro_z[2];
+	struct ak8963_regs mag;
+};
+#pragma pack(pop)
+
+// TODO: Big enough?
+#define MPU_MAX_WRITE_BUFFER_SIZE (2)
+
+
+/*
+  The MPU9250 can only handle high bus speeds on the sensor and
+  interrupt status registers. All other registers have a maximum 1MHz
+  Communication with all registers of the device is performed using either
+  I2C at 400kHz or SPI at 1MHz. For applications requiring faster communications,
+  the sensor and interrupt registers may be read using SPI at 20MHz
+ */
+#define MPU9250_LOW_BUS_SPEED				0
+#define MPU9250_HIGH_BUS_SPEED				0x8000
+#  define MPU9250_IS_HIGH_SPEED(r) 			((r) & MPU9250_HIGH_BUS_SPEED)
+#  define MPU9250_REG(r) 					((r) &~MPU9250_HIGH_BUS_SPEED)
+#  define MPU9250_SET_SPEED(r, s) 			((r)|(s))
+#  define MPU9250_HIGH_SPEED_OP(r) 			MPU9250_SET_SPEED((r), MPU9250_HIGH_BUS_SPEED)
+#  define MPU9250_LOW_SPEED_OP(r)			MPU9250_REG((r))
+
+/* interface factories */
+extern device::Device *MPU9250_SPI_interface(int bus, bool external_bus);
+extern device::Device *MPU9250_I2C_interface(int bus, bool external_bus);
+extern int MPU9250_probe(device::Device *dev, int device_type);
+
+typedef device::Device *(*MPU9250_constructor)(int, bool);
+
+
+
+
 class MPU9250_mag;
 class MPU9250_gyro;
 
 class MPU9250 : public device::CDev
 {
 public:
-	MPU9250(device::Device *interface, const char *path_accel, const char *path_gyro, const char *path_mag,
+	MPU9250(device::Device *interface, device::Device *mag_interface, const char *path_accel, const char *path_gyro, const char *path_mag,
 		enum Rotation rotation);
 	virtual ~MPU9250();
 
@@ -42,6 +279,15 @@ private:
 	MPU9250_mag     *_mag;
 	uint8_t			_whoami;	/** whoami result */
 
+#if defined(USE_I2C)
+	/*
+	 * SPI bus based device use hrt
+	 * I2C bus needs to use work queue
+	 */
+	work_s			_work;
+#endif
+	bool 			_use_hrt;
+
 	struct hrt_call		_call;
 	unsigned		_call_interval;
 
@@ -123,6 +369,42 @@ private:
 	 */
 	int			reset();
 
+
+
+#if defined(USE_I2C)
+	/**
+	 * When the I2C interfase is on
+	 * Perform a poll cycle; collect from the previous measurement
+ 	 * and start a new one.
+	 *
+	 * This is the heart of the measurement state machine.  This function
+	 * alternately starts a measurement, or collects the data from the
+		 * previous measurement.
+		 *
+		 * When the interval between measurements is greater than the minimum
+		 * measurement interval, a gap is inserted between collection
+		 * and measurement to provide the most recent measurement possible
+		 * at the next interval.
+		 */
+	void			cycle();
+
+	/**
+	 * Static trampoline from the workq context; because we don't have a
+	 * generic workq wrapper yet.
+	 *
+	 * @param arg		Instance pointer for the driver that is polling.
+	 */
+	static void		cycle_trampoline(void *arg);
+
+	void use_i2c(bool on_true) { _use_hrt = !on_true; }
+
+#endif
+
+	bool is_i2c(void) { return !_use_hrt; }
+
+
+
+
 	/**
 	 * Static trampoline from the hrt_call context; because we don't have a
 	 * generic hrt wrapper yet.
@@ -193,7 +475,11 @@ private:
 	 *
 	 * @return true if the sensor is not on the main MCU board
 	 */
-	bool			is_external() { return (_bus == EXTERNAL_BUS); }
+	bool			is_external()
+	{
+		unsigned dummy;
+		return !_interface->ioctl(ACCELIOCGEXTERNAL, dummy);
+	}
 
 	/**
 	 * Measurement self test
@@ -234,48 +520,4 @@ private:
 	/* do not allow to copy this class due to pointer data members */
 	MPU9250(const MPU9250 &);
 	MPU9250 operator=(const MPU9250 &);
-
-#pragma pack(push, 1)
-	/**
-	 * Report conversation within the mpu, including command byte and
-	 * interrupt status.
-	 */
-	struct MPUReport {
-		uint8_t		cmd;
-		uint8_t		status;
-		uint8_t		accel_x[2];
-		uint8_t		accel_y[2];
-		uint8_t		accel_z[2];
-		uint8_t		temp[2];
-		uint8_t		gyro_x[2];
-		uint8_t		gyro_y[2];
-		uint8_t		gyro_z[2];
-		struct ak8963_regs mag;
 };
-#pragma pack(pop)
-};
-
-
-
-/*
-  The MPU9250 can only handle high bus speeds on the sensor and
-  interrupt status registers. All other registers have a maximum 1MHz
-  Communication with all registers of the device is performed using either
-  I2C at 400kHz or SPI at 1MHz. For applications requiring faster communications,
-  the sensor and interrupt registers may be read using SPI at 20MHz
- */
-#define MPU9250_LOW_BUS_SPEED				0
-#define MPU9250_HIGH_BUS_SPEED				0x8000
-#  define MPU9250_IS_HIGH_SPEED(r) 			((r) & MPU9250_HIGH_BUS_SPEED)
-#  define MPU9250_REG(r) 					((r) &~MPU9250_HIGH_BUS_SPEED)
-#  define MPU9250_SET_SPEED(r, s) 			((r)|(s))
-#  define MPU9250_HIGH_SPEED_OP(r) 			MPU9250_SET_SPEED((r), MPU9250_HIGH_BUS_SPEED)
-#  define MPU9250_LOW_SPEED_OP(r)			MPU9250_REG((r))
-
-
-/* interface factories */
-extern device::Device *MPU9250_SPI_interface(int bus, int device_type, bool external_bus);
-extern device::Device *MPU9250_I2C_interface(int bus, int device_type, bool external_bus);
-extern int MPU9250_probe(device::Device *dev, int device_type);
-
-typedef device::Device *(*MPU9250_constructor)(int, int, bool);

src/drivers/mpu9250/mpu9250_i2c.cpp

@@ -55,18 +55,18 @@
 #include <drivers/drv_accel.h>
 #include <drivers/drv_device.h>
 
-#include "MPU9250.h"
+#include "mpu9250.h"
 
 #include "board_config.h"
 
 #ifdef USE_I2C
 
-device::Device *MPU9250_I2C_interface(int bus, int device_type, bool external_bus);
+device::Device *MPU9250_I2C_interface(int bus, bool external_bus);
 
 class MPU9250_I2C : public device::I2C
 {
 public:
-	MPU9250_I2C(int bus, int device_type);
+	MPU9250_I2C(int bus);
 	virtual ~MPU9250_I2C();
 
 	virtual int	init();
@@ -78,21 +78,17 @@ public:
 protected:
 	virtual int	probe();
 
-private:
-	int _device_type;
-
 };
 
 
 device::Device *
-MPU9250_I2C_interface(int bus, int device_type, bool external_bus)
+MPU9250_I2C_interface(int bus, bool external_bus)
 {
-	return new MPU9250_I2C(bus, device_type);
+	return new MPU9250_I2C(bus);
 }
 
-MPU9250_I2C::MPU9250_I2C(int bus, int device_type) :
+MPU9250_I2C::MPU9250_I2C(int bus) :
-	I2C("MPU9250_I2C", nullptr, bus, PX4_I2C_OBDEV_MPU9250, 400000),
+	I2C("MPU9250_I2C", nullptr, bus, PX4_I2C_OBDEV_MPU9250, 400000)
-	_device_type(device_type)
 {
 	_device_id.devid_s.devtype =  DRV_ACC_DEVTYPE_MPU9250;
 }
@@ -156,8 +152,7 @@ MPU9250_I2C::read(unsigned reg_speed, void *data, unsigned count)
 	 */
 	uint32_t offset = count < sizeof(MPUReport) ? 0 : offsetof(MPUReport, status);
 	uint8_t cmd = MPU9250_REG(reg_speed);
-	int ret = transfer(&cmd, 1, &((uint8_t *)data)[offset], count);
+	return transfer(&cmd, 1, &((uint8_t *)data)[offset], count);
-	return ret == OK ? count : ret;
 }
 
 
@@ -165,8 +160,8 @@ int
 MPU9250_I2C::probe()
 {
 	uint8_t whoami = 0;
-	uint8_t expected = _device_type == 6000 ? MPU_WHOAMI_6000 : ICM_WHOAMI_20608;
+	uint8_t expected = MPU_WHOAMI_9250;
-	return (read(MPUREG_WHOAMI, &whoami, 1) > 0 && (whoami == expected)) ? 0 : -EIO;
+	return (read(MPUREG_WHOAMI, &whoami, 1) == OK && (whoami == expected)) ? 0 : -EIO;
-
 }
-#endif /* PX4_I2C_OBDEV_HMC5883 */
+
+#endif /* USE_I2C */

src/drivers/mpu9250/mpu9250_spi.cpp

@@ -64,6 +64,7 @@
 #define DIR_READ			0x80
 #define DIR_WRITE			0x00
 
+
 #if PX4_SPIDEV_MPU
 #ifdef PX4_SPI_BUS_EXT
 #define EXTERNAL_BUS PX4_SPI_BUS_EXT
@@ -78,16 +79,16 @@
   SPI speed
  */
 #define MPU9250_LOW_SPI_BUS_SPEED	1000*1000
-#define MPU9250_HIGH_SPI_BUS_SPEED	11*1000*1000 /* will be rounded to 10.4 MHz, within margins for MPU6K */
+#define MPU9250_HIGH_SPI_BUS_SPEED	11*1000*1000 /* will be rounded to 10.4 MHz, within margins for MPU9250 */
 
 
-device::Device *MPU9250_SPI_interface(int bus, int device_type, bool external_bus);
+device::Device *MPU9250_SPI_interface(int bus, bool external_bus);
 
 
 class MPU9250_SPI : public device::SPI
 {
 public:
-	MPU9250_SPI(int bus, spi_dev_e device, int device_type);
+	MPU9250_SPI(int bus, spi_dev_e device);
 	virtual ~MPU9250_SPI();
 
 	virtual int	init();
@@ -100,47 +101,37 @@ protected:
 
 private:
 
-	int _device_type;
 	/* Helper to set the desired speed and isolate the register on return */
 
 	void set_bus_frequency(unsigned &reg_speed_reg_out);
 };
 
 device::Device *
-MPU9250_SPI_interface(int bus, int device_type, bool external_bus)
+MPU9250_SPI_interface(int bus, bool external_bus)
 {
 	spi_dev_e cs = SPIDEV_NONE;
 	device::Device *interface = nullptr;
 
 	if (external_bus) {
 #ifdef PX4_SPI_BUS_EXT
-#  if defined(PX4_SPIDEV_EXT_ICM)
-		cs = (spi_dev_e)(device_type == 6000 ? PX4_SPIDEV_EXT_MPU : PX4_SPIDEV_EXT_ICM);
-#   else
 		cs = (spi_dev_e) PX4_SPIDEV_EXT_MPU;
-#  endif
-#endif
-
-	} else {
-
-#if defined(PX4_SPIDEV_ICM)
-		cs = (spi_dev_e)(device_type == 6000 ? PX4_SPIDEV_MPU : PX4_SPIDEV_ICM);
 #else
-		cs = (spi_dev_e) PX4_SPIDEV_MPU;
+		errx(0, "External SPI not available");
 #endif
+	} else {
+		cs = (spi_dev_e) PX4_SPIDEV_MPU;
 	}
 
 	if (cs != SPIDEV_NONE) {
 
-		interface = new MPU9250_SPI(bus, cs, device_type);
+		interface = new MPU9250_SPI(bus, cs);
 	}
 
 	return interface;
 }
 
-MPU9250_SPI::MPU9250_SPI(int bus, spi_dev_e device, int device_type) :
+MPU9250_SPI::MPU9250_SPI(int bus, spi_dev_e device) :
-	SPI("MPU9250", nullptr, bus, device, SPIDEV_MODE3, MPU9250_LOW_SPI_BUS_SPEED),
+	SPI("MPU9250", nullptr, bus, device, SPIDEV_MODE3, MPU9250_LOW_SPI_BUS_SPEED)
-	_device_type(device_type)
 {
 	_device_id.devid_s.devtype =  DRV_ACC_DEVTYPE_MPU9250;
 }
@@ -266,16 +257,15 @@ MPU9250_SPI::read(unsigned reg_speed, void *data, unsigned count)
 
 	}
 
-	return ret == OK ? count : ret;
+	return ret;
 }
 
 int
 MPU9250_SPI::probe()
 {
 	uint8_t whoami = 0;
-	uint8_t expected = _device_type == 6000 ? MPU_WHOAMI_6000 : ICM_WHOAMI_20608;
+	uint8_t expected = MPU_WHOAMI_9250;
-	return (read(MPUREG_WHOAMI, &whoami, 1) > 0 && (whoami == expected)) ? 0 : -EIO;
+	return (read(MPUREG_WHOAMI, &whoami, 1) == OK && (whoami == expected)) ? 0 : -EIO;
-
 }
 
 #endif // PX4_SPIDEV_MPU