Added I2C to MPU6000 driver

src/drivers/drv_accel.h

@@ -109,4 +109,8 @@ struct accel_calibration_s {
 /** get the hardware low-pass filter cut-off in Hz*/
 #define ACCELIOCGHWLOWPASS	_ACCELIOC(11)
 
+/** determine if hardware is external or onboard */
+#define ACCELIOCGEXTERNAL	_ACCELIOC(12)
+
+
 #endif /* _DRV_ACCEL_H */

src/drivers/drv_gyro.h

@@ -106,4 +106,8 @@ struct gyro_calibration_s {
 /** get the hardware low-pass filter cut-off in Hz*/
 #define GYROIOCGHWLOWPASS	_GYROIOC(10)
 
+/** determine if hardware is external or onboard */
+#define GYROIOCGEXTERNAL	_GYROIOC(12)
+
+
 #endif /* _DRV_GYRO_H */

src/drivers/mpu6000/CMakeLists.txt

@@ -39,6 +39,8 @@ px4_add_module(
 		-Os
 	SRCS
 		mpu6000.cpp
+		mpu6000_i2c.cpp
+		mpu6000_spi.cpp
 	DEPENDS
 		platforms__common
 	)

src/drivers/mpu6000/mpu6000.h

@@ -0,0 +1,217 @@
+/****************************************************************************
+ *
+ *   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 .h
+ *
+ * Shared defines for the mpu6000 driver.
+ */
+
+#pragma once
+
+#if defined(PX4_I2C_MPU6050_ADDR) || \
+	defined(PX4_I2C_MPU6000_ADDR) || \
+	defined(PX4_I2C_ICM_20608_G_ADDR)
+#  define USE_I2C
+#endif
+
+
+#define DIR_READ			0x80
+#define DIR_WRITE			0x00
+
+#define MPU_DEVICE_PATH_ACCEL		"/dev/mpu6000_accel"
+#define MPU_DEVICE_PATH_GYRO		"/dev/mpu6000_gyro"
+#define MPU_DEVICE_PATH_ACCEL_EXT	"/dev/mpu6000_accel_ext"
+#define MPU_DEVICE_PATH_GYRO_EXT	"/dev/mpu6000_gyro_ext"
+
+// MPU 6000 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_FIFO_EN			0x23
+#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_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
+#define MPUREG_PRODUCT_ID		0x0C
+#define MPUREG_TRIM1			0x0D
+#define MPUREG_TRIM2			0x0E
+#define MPUREG_TRIM3			0x0F
+#define MPUREG_TRIM4			0x10
+
+// Configuration bits MPU 3000 and MPU 6000 (not revised)?
+#define BIT_SLEEP				0x40
+#define BIT_H_RESET				0x80
+#define BITS_CLKSEL				0x07
+#define MPU_CLK_SEL_PLLGYROX	0x01
+#define MPU_CLK_SEL_PLLGYROZ	0x03
+#define MPU_EXT_SYNC_GYROX		0x02
+#define BITS_GYRO_ST_X			0x80
+#define BITS_GYRO_ST_Y			0x40
+#define BITS_GYRO_ST_Z			0x20
+#define BITS_FS_250DPS			0x00
+#define BITS_FS_500DPS			0x08
+#define BITS_FS_1000DPS			0x10
+#define BITS_FS_2000DPS			0x18
+#define BITS_FS_MASK			0x18
+#define BITS_DLPF_CFG_256HZ_NOLPF2	0x00
+#define BITS_DLPF_CFG_188HZ		0x01
+#define BITS_DLPF_CFG_98HZ		0x02
+#define BITS_DLPF_CFG_42HZ		0x03
+#define BITS_DLPF_CFG_20HZ		0x04
+#define BITS_DLPF_CFG_10HZ		0x05
+#define BITS_DLPF_CFG_5HZ		0x06
+#define BITS_DLPF_CFG_2100HZ_NOLPF	0x07
+#define BITS_DLPF_CFG_MASK		0x07
+#define BIT_INT_ANYRD_2CLEAR	0x10
+#define BIT_RAW_RDY_EN			0x01
+#define BIT_I2C_IF_DIS			0x10
+#define BIT_INT_STATUS_DATA		0x01
+
+#define MPU_WHOAMI_6000			0x68
+#define ICM_WHOAMI_20608		0xaf
+
+// ICM2608 specific registers
+
+/* this is an undocumented register which
+   if set incorrectly results in getting a 2.7m/s/s offset
+   on the Y axis of the accelerometer
+*/
+#define MPUREG_ICM_UNDOC1		0x11
+#define MPUREG_ICM_UNDOC1_VALUE	0xc9
+
+// Product ID Description for ICM2608
+// There is none
+
+#define ICM20608_REV_00		0
+
+// Product ID Description for MPU6000
+// high 4 bits 	low 4 bits
+// Product Name	Product Revision
+#define MPU6000ES_REV_C4		0x14
+#define MPU6000ES_REV_C5		0x15
+#define MPU6000ES_REV_D6		0x16
+#define MPU6000ES_REV_D7		0x17
+#define MPU6000ES_REV_D8		0x18
+#define MPU6000_REV_C4			0x54
+#define MPU6000_REV_C5			0x55
+#define MPU6000_REV_D6			0x56
+#define MPU6000_REV_D7			0x57
+#define MPU6000_REV_D8			0x58
+#define MPU6000_REV_D9			0x59
+#define MPU6000_REV_D10			0x5A
+#define MPU6050_REV_D8			0x28	// TODO:Need verification
+
+#define MPU6000_ACCEL_DEFAULT_RANGE_G				8
+#define MPU6000_ACCEL_DEFAULT_RATE					1000
+#define MPU6000_ACCEL_MAX_OUTPUT_RATE				280
+#define MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ	30
+
+#define MPU6000_GYRO_DEFAULT_RANGE_G				8
+#define MPU6000_GYRO_DEFAULT_RATE					1000
+/* rates need to be the same between accel and gyro */
+#define MPU6000_GYRO_MAX_OUTPUT_RATE				MPU6000_ACCEL_MAX_OUTPUT_RATE
+#define MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ		30
+
+#define MPU6000_DEFAULT_ONCHIP_FILTER_FREQ			42
+
+#define MPU6000_ONE_G					9.80665f
+
+#ifdef PX4_SPI_BUS_EXT
+#define EXTERNAL_BUS PX4_SPI_BUS_EXT
+#else
+#define EXTERNAL_BUS 0
+#endif
+
+#pragma pack(push, 1)
+/**
+ * Report conversation within the MPU6000, including command byte and
+ * interrupt status.
+ */
+struct MPUReport {
+	uint8_t		cmd;
+	uint8_t		status;
+	uint8_t		accel_x[2];
+	uint8_t		accel_y[2];
+	uint8_t		accel_z[2];
+	uint8_t		temp[2];
+	uint8_t		gyro_x[2];
+	uint8_t		gyro_y[2];
+	uint8_t		gyro_z[2];
+};
+#pragma pack(pop)
+
+#define MPU_MAX_READ_BUFFER_SIZE (sizeof(MPUReport) + 1)
+#define MPU_MAX_WRITE_BUFFER_SIZE (2)
+/*
+  The MPU6000 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 MPU6000_LOW_BUS_SPEED				0
+#define MPU6000_HIGH_BUS_SPEED				0x8000
+#  define MPU6000_IS_HIGH_SPEED(r) 			((r) & MPU6000_HIGH_BUS_SPEED)
+#  define MPU6000_REG(r) 					((r) &~MPU6000_HIGH_BUS_SPEED)
+#  define MPU6000_SET_SPEED(r, s) 			((r)|(s))
+#  define MPU6000_HIGH_SPEED_OP(r) 			MPU6000_SET_SPEED((r), MPU6000_HIGH_BUS_SPEED)
+#  define MPU6000_LOW_SPEED_OP(r)			MPU6000_REG((r))
+
+/* interface factories */
+extern device::Device *MPU6000_SPI_interface(int bus, int device_type, bool external_bus);
+extern device::Device *MPU6000_I2C_interface(int bus, int device_type, bool external_bus);
+extern int MPU6000_probe(device::Device *dev, int device_type);
+
+typedef device::Device *(*MPU6000_constructor)(int, int, bool);

src/drivers/mpu6000/mpu6000_i2c.cpp

@@ -0,0 +1,169 @@
+/****************************************************************************
+ *
+ *   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 mpu6000_i2c.cpp
+ *
+ * I2C interface for MPU6000 /MPU6050
+ */
+
+/* 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 "mpu6000.h"
+
+#include "board_config.h"
+
+#ifdef USE_I2C
+
+device::Device *MPU6000_I2C_interface(int bus, int device_type, bool external_bus);
+
+class MPU6000_I2C : public device::I2C
+{
+public:
+	MPU6000_I2C(int bus, int device_type);
+	virtual ~MPU6000_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();
+
+private:
+	int _device_type;
+
+};
+
+
+device::Device *
+MPU6000_I2C_interface(int bus, int device_type, bool external_bus)
+{
+	return new MPU6000_I2C(bus, device_type);
+}
+
+MPU6000_I2C::MPU6000_I2C(int bus, int device_type) :
+	I2C("MPU6000_I2C", nullptr, bus, PX4_I2C_MPU6050_ADDR, 400000),
+	_device_type(device_type)
+{
+	_device_id.devid_s.devtype =  DRV_ACC_DEVTYPE_MPU6000;
+}
+
+MPU6000_I2C::~MPU6000_I2C()
+{
+}
+
+int
+MPU6000_I2C::init()
+{
+	/* this will call probe() */
+	return I2C::init();
+}
+
+int
+MPU6000_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);
+
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+int
+MPU6000_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] = MPU6000_REG(reg_speed);
+	cmd[1] = *(uint8_t *)data;
+	return transfer(&cmd[0], count + 1, nullptr, 0);
+}
+
+int
+MPU6000_I2C::read(unsigned reg_speed, void *data, unsigned count)
+{
+	/* We want to avoid copying the data of MPUReport: So if the caller
+	 * supplies a buffer not MPUReport in size, it is assume to be a reg or
+	 * reg 16 read
+	 * Since MPUReport has a cmd at front, we must return the data
+	 * after that. Foe anthing else we must return it
+	 */
+	uint32_t offset = count < sizeof(MPUReport) ? 0 : offsetof(MPUReport, status);
+	uint8_t cmd = MPU6000_REG(reg_speed);
+	int ret = transfer(&cmd, 1, &((uint8_t *)data)[offset], count);
+	return ret == OK ? count : ret;
+}
+
+
+int
+MPU6000_I2C::probe()
+{
+	uint8_t whoami = 0;
+	uint8_t expected = _device_type == 6000 ? MPU_WHOAMI_6000 : ICM_WHOAMI_20608;
+	return (read(MPUREG_WHOAMI, &whoami, 1) > 0 && (whoami == expected)) ? 0 : -EIO;
+
+}
+#endif /* PX4_I2C_OBDEV_HMC5883 */

src/drivers/mpu6000/mpu6000_spi.cpp

@@ -0,0 +1,278 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-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 mpu6000_spi.cpp
+ *
+ * Driver for the Invensense MPU6000 connected via SPI.
+ *
+ * @author Andrew Tridgell
+ * @author Pat Hickey
+ * @author David sidrane
+ */
+
+#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/spi.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_device.h>
+
+#include "mpu6000.h"
+#include <board_config.h>
+
+#define DIR_READ			0x80
+#define DIR_WRITE			0x00
+
+#if PX4_SPIDEV_MPU
+#ifdef PX4_SPI_BUS_EXT
+#define EXTERNAL_BUS PX4_SPI_BUS_EXT
+#else
+#define EXTERNAL_BUS 0
+#endif
+
+/*
+  The MPU6000 can only handle high SPI bus speeds on the sensor and
+  interrupt status registers. All other registers have a maximum 1MHz
+  SPI speed
+
+ */
+#define MPU6000_LOW_SPI_BUS_SPEED	1000*1000
+#define MPU6000_HIGH_SPI_BUS_SPEED	11*1000*1000 /* will be rounded to 10.4 MHz, within margins for MPU6K */
+
+
+device::Device *MPU6000_SPI_interface(int bus, int device_type, bool external_bus);
+
+
+class MPU6000_SPI : public device::SPI
+{
+public:
+	MPU6000_SPI(int bus, spi_dev_e device, int device_type);
+	virtual ~MPU6000_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);
+protected:
+	virtual int probe();
+
+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 *
+MPU6000_SPI_interface(int bus, int device_type, 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;
+#endif
+	}
+
+	if (cs != SPIDEV_NONE) {
+
+		interface = new MPU6000_SPI(bus, cs, device_type);
+	}
+
+	return interface;
+}
+
+MPU6000_SPI::MPU6000_SPI(int bus, spi_dev_e device, int device_type) :
+	SPI("MPU6000", nullptr, bus, device, SPIDEV_MODE3, MPU6000_LOW_SPI_BUS_SPEED),
+	_device_type(device_type)
+{
+	_device_id.devid_s.devtype =  DRV_ACC_DEVTYPE_MPU6000;
+}
+
+MPU6000_SPI::~MPU6000_SPI()
+{
+}
+
+int
+MPU6000_SPI::init()
+{
+	int ret;
+
+	ret = SPI::init();
+
+	if (ret != OK) {
+		DEVICE_DEBUG("SPI init failed");
+		return -EIO;
+	}
+
+	return OK;
+}
+
+int
+MPU6000_SPI::ioctl(unsigned operation, unsigned &arg)
+{
+	int ret;
+
+	switch (operation) {
+
+	case ACCELIOCGEXTERNAL:
+#if defined(PX4_SPI_BUS_EXT)
+		return _bus == PX4_SPI_BUS_EXT ? 1 : 0;
+#else
+		return 0;
+#endif
+
+	case DEVIOCGDEVICEID:
+		return CDev::ioctl(nullptr, operation, arg);
+
+	default: {
+			ret = -EINVAL;
+		}
+	}
+
+	return ret;
+}
+
+void
+MPU6000_SPI::set_bus_frequency(unsigned &reg_speed)
+{
+	/* Set the desired speed */
+
+	set_frequency(MPU6000_IS_HIGH_SPEED(reg_speed) ? MPU6000_HIGH_SPI_BUS_SPEED : MPU6000_LOW_SPI_BUS_SPEED);
+
+	/* Isoolate the register on return */
+
+	reg_speed = MPU6000_REG(reg_speed);
+}
+
+
+int
+MPU6000_SPI::write(unsigned reg_speed, void *data, unsigned count)
+{
+	uint8_t cmd[MPU_MAX_WRITE_BUFFER_SIZE];
+
+	if (sizeof(cmd) < (count + 1)) {
+		return -EIO;
+	}
+
+	/* Set the desired speed and isolate the register */
+
+	set_bus_frequency(reg_speed);
+
+	cmd[0] = reg_speed | DIR_WRITE;
+	cmd[1] = *(uint8_t *)data;
+
+	return transfer(&cmd[0], &cmd[0], count + 1);
+}
+
+int
+MPU6000_SPI::read(unsigned reg_speed, void *data, unsigned count)
+{
+	/* We want to avoid copying the data of MPUReport: So if the caller
+	 * supplies a buffer not MPUReport in size, it is assume to be a reg or reg 16 read
+	 * and we need to provied the buffer large enough for the callers data
+	 * and our command.
+	 */
+	uint8_t cmd[3] = {0, 0, 0};
+
+	uint8_t *pbuff  =  count < sizeof(MPUReport) ? cmd : (uint8_t *) data ;
+
+
+	if (count < sizeof(MPUReport))  {
+
+		/* add command */
+
+		count++;
+	}
+
+	set_bus_frequency(reg_speed);
+
+	/* Set command */
+
+	pbuff[0] = reg_speed | DIR_READ ;
+
+	/* Transfer the command and get the data */
+
+	int ret = transfer(pbuff, pbuff, count);
+
+	if (ret == OK && pbuff == &cmd[0]) {
+
+		/* Adjust the count back */
+
+		count--;
+
+		/* Return the data */
+
+		memcpy(data, &cmd[1], count);
+
+	}
+
+	return ret == OK ? count : ret;
+}
+
+int
+MPU6000_SPI::probe()
+{
+	uint8_t whoami = 0;
+	uint8_t expected = _device_type == 6000 ? MPU_WHOAMI_6000 : ICM_WHOAMI_20608;
+	return (read(MPUREG_WHOAMI, &whoami, 1) > 0 && (whoami == expected)) ? 0 : -EIO;
+
+}
+
+#endif // PX4_SPIDEV_MPU