new mpu6000 driver using FIFOs and raw sampling (8 kHz gyro, 1 kHz accel)

boards/px4/fmu-v2/default.cmake

@@ -36,6 +36,7 @@ px4_add_board(
 		#imu/adis16448
 		#imu/adis16477
 		#imu/adis16497
+		#imu/invensense/mpu6000 # WIP
 		imu/l3gd20
 		imu/lsm303d
 		imu/mpu6000

src/drivers/imu/invensense/mpu6000/CMakeLists.txt

@@ -0,0 +1,46 @@
+############################################################################
+#
+#   Copyright (c) 2020 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.
+#
+############################################################################
+
+px4_add_module(
+	MODULE drivers__imu__invensense__mpu6000
+	MAIN mpu6000
+	COMPILE_FLAGS
+	SRCS
+		MPU6000.cpp
+		MPU6000.hpp
+		mpu6000_main.cpp
+	DEPENDS
+		drivers_accelerometer
+		drivers_gyroscope
+		px4_work_queue
+	)

src/drivers/imu/invensense/mpu6000/InvenSense_MPU6000_registers.hpp

@@ -0,0 +1,164 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 InvenSense_MPU6000_registers.hpp
+ *
+ * Invensense MPU6000 registers.
+ *
+ */
+
+#pragma once
+
+// TODO: move to a central header
+static constexpr uint8_t Bit0 = (1 << 0);
+static constexpr uint8_t Bit1 = (1 << 1);
+static constexpr uint8_t Bit2 = (1 << 2);
+static constexpr uint8_t Bit3 = (1 << 3);
+static constexpr uint8_t Bit4 = (1 << 4);
+static constexpr uint8_t Bit5 = (1 << 5);
+static constexpr uint8_t Bit6 = (1 << 6);
+static constexpr uint8_t Bit7 = (1 << 7);
+
+namespace InvenSense_MPU6000
+{
+static constexpr uint32_t SPI_SPEED = 20 * 1000 * 1000;
+static constexpr uint8_t DIR_READ = 0x80;
+
+static constexpr uint8_t WHOAMI = 0x68;
+
+enum class Register : uint8_t {
+	CONFIG        = 0x1A,
+	GYRO_CONFIG   = 0x1B,
+	ACCEL_CONFIG  = 0x1C,
+
+	FIFO_EN       = 0x23,
+
+	INT_STATUS    = 0x3A,
+
+	INT_ENABLE    = 0x38,
+
+	TEMP_OUT_H    = 0x41,
+	TEMP_OUT_L    = 0x42,
+
+	USER_CTRL     = 0x6A,
+	PWR_MGMT_1    = 0x6B,
+
+	FIFO_COUNTH   = 0x72,
+	FIFO_COUNTL   = 0x73,
+	FIFO_R_W      = 0x74,
+	WHO_AM_I      = 0x75,
+};
+
+// CONFIG
+enum CONFIG_BIT : uint8_t {
+	DLPF_CFG_BYPASS_DLPF = 7, // Reserved
+};
+
+// GYRO_CONFIG
+enum GYRO_CONFIG_BIT : uint8_t {
+	// FS_SEL [4:3]
+	FS_SEL_250_DPS	= 0,           // 0b00000
+	FS_SEL_500_DPS	= Bit3,        // 0b01000
+	FS_SEL_1000_DPS	= Bit4,        // 0b10000
+	FS_SEL_2000_DPS	= Bit4 | Bit3, // 0b11000
+};
+
+// ACCEL_CONFIG
+enum ACCEL_CONFIG_BIT : uint8_t {
+	// AFS_SEL [4:3]
+	AFS_SEL_2G  = 0,           // 0b00000
+	AFS_SEL_4G  = Bit3,        // 0b01000
+	AFS_SEL_8G  = Bit4,        // 0b10000
+	AFS_SEL_16G = Bit4 | Bit3, // 0b11000
+};
+
+// FIFO_EN
+enum FIFO_EN_BIT : uint8_t {
+	TEMP_FIFO_EN  = Bit7,
+	XG_FIFO_EN    = Bit6,
+	YG_FIFO_EN    = Bit5,
+	ZG_FIFO_EN    = Bit4,
+	ACCEL_FIFO_EN = Bit3,
+};
+
+// INT_ENABLE
+enum INT_ENABLE_BIT : uint8_t {
+	FIFO_OFLOW_EN   = Bit4,
+	DATA_RDY_INT_EN = Bit0
+};
+
+// INT_STATUS
+enum INT_STATUS_BIT : uint8_t {
+	FIFO_OFLOW_INT = Bit4,
+	DATA_RDY_INT   = Bit0,
+};
+
+// USER_CTRL
+enum USER_CTRL_BIT : uint8_t {
+	FIFO_EN    = Bit6,
+	FIFO_RESET = Bit2,
+};
+
+// PWR_MGMT_1
+enum PWR_MGMT_1_BIT : uint8_t {
+	DEVICE_RESET = Bit7,
+
+	CLKSEL_2   = Bit2,
+	CLKSEL_1   = Bit1,
+	CLKSEL_0   = Bit0,
+};
+
+
+namespace FIFO
+{
+static constexpr size_t SIZE = 1024;
+
+// FIFO_DATA layout when FIFO_EN has {X, Y, Z}G_FIFO_EN and ACCEL set
+struct DATA {
+	uint8_t ACCEL_XOUT_H;
+	uint8_t ACCEL_XOUT_L;
+	uint8_t ACCEL_YOUT_H;
+	uint8_t ACCEL_YOUT_L;
+	uint8_t ACCEL_ZOUT_H;
+	uint8_t ACCEL_ZOUT_L;
+	uint8_t GYRO_XOUT_H;
+	uint8_t GYRO_XOUT_L;
+	uint8_t GYRO_YOUT_H;
+	uint8_t GYRO_YOUT_L;
+	uint8_t GYRO_ZOUT_H;
+	uint8_t GYRO_ZOUT_L;
+};
+}
+
+} // namespace InvenSense_MPU6000

src/drivers/imu/invensense/mpu6000/MPU6000.cpp

@@ -0,0 +1,373 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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.
+ *
+ ****************************************************************************/
+
+#include "MPU6000.hpp"
+
+#include <px4_platform/board_dma_alloc.h>
+
+using namespace time_literals;
+using namespace InvenSense_MPU6000;
+
+static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) | lsb; }
+
+static constexpr uint32_t GYRO_RATE{8000};  // 8 kHz gyro
+static constexpr uint32_t ACCEL_RATE{1000}; // 1 kHz accel
+
+static constexpr uint32_t FIFO_INTERVAL{1000}; // 1000 us / 1000 Hz interval
+
+static constexpr uint32_t FIFO_GYRO_SAMPLES{FIFO_INTERVAL / (1000000 / GYRO_RATE)};
+static constexpr uint32_t FIFO_ACCEL_SAMPLES{FIFO_INTERVAL / (1000000 / ACCEL_RATE)};
+
+MPU6000::MPU6000(int bus, uint32_t device, enum Rotation rotation) :
+	SPI(MODULE_NAME, nullptr, bus, device, SPIDEV_MODE3, SPI_SPEED),
+	ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
+	_px4_accel(get_device_id(), ORB_PRIO_VERY_HIGH, rotation),
+	_px4_gyro(get_device_id(), ORB_PRIO_VERY_HIGH, rotation)
+{
+	set_device_type(DRV_ACC_DEVTYPE_MPU6000);
+	_px4_accel.set_device_type(DRV_ACC_DEVTYPE_MPU6000);
+	_px4_gyro.set_device_type(DRV_GYR_DEVTYPE_MPU6000);
+
+	_px4_accel.set_update_rate(1000000 / FIFO_INTERVAL);
+	_px4_gyro.set_update_rate(1000000 / FIFO_INTERVAL);
+}
+
+MPU6000::~MPU6000()
+{
+	Stop();
+
+	if (_dma_data_buffer != nullptr) {
+		board_dma_free(_dma_data_buffer, FIFO::SIZE);
+	}
+
+	perf_free(_transfer_perf);
+	perf_free(_fifo_empty_perf);
+	perf_free(_fifo_overflow_perf);
+	perf_free(_fifo_reset_perf);
+	perf_free(_drdy_interval_perf);
+}
+
+int MPU6000::probe()
+{
+	const uint8_t whoami = RegisterRead(Register::WHO_AM_I);
+
+	if (whoami != WHOAMI) {
+		PX4_WARN("unexpected WHO_AM_I 0x%02x", whoami);
+		return PX4_ERROR;
+	}
+
+	return PX4_OK;
+}
+
+bool MPU6000::Init()
+{
+	if (SPI::init() != PX4_OK) {
+		PX4_ERR("SPI::init failed");
+		return false;
+	}
+
+	if (!Reset()) {
+		PX4_ERR("reset failed");
+		return false;
+	}
+
+	// allocate DMA capable buffer
+	_dma_data_buffer = (uint8_t *)board_dma_alloc(FIFO::SIZE);
+
+	if (_dma_data_buffer == nullptr) {
+		PX4_ERR("DMA alloc failed");
+		return false;
+	}
+
+	Start();
+
+	return true;
+}
+
+bool MPU6000::Reset()
+{
+	// PWR_MGMT_1: Device Reset
+	// CLKSEL[2:0] must be set to 001 to achieve full gyroscope performance.
+	RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::DEVICE_RESET);
+	usleep(2000);
+
+	// PWR_MGMT_1: CLKSEL[2:0] must be set to 001 to achieve full gyroscope performance.
+	RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0);
+	usleep(100);
+
+	// ACCEL_CONFIG: Accel 16 G range
+	RegisterSetBits(Register::ACCEL_CONFIG, ACCEL_CONFIG_BIT::AFS_SEL_16G);
+	_px4_accel.set_scale(CONSTANTS_ONE_G / 2048);
+	_px4_accel.set_range(16.0f * CONSTANTS_ONE_G);
+
+	// GYRO_CONFIG: Gyro 2000 degrees/second
+	RegisterSetBits(Register::GYRO_CONFIG, GYRO_CONFIG_BIT::FS_SEL_2000_DPS);
+	_px4_gyro.set_scale(math::radians(1.0f / 16.4f));
+	_px4_gyro.set_range(math::radians(2000.0f));
+
+	// reset done once data is ready
+	const bool reset_done = !(RegisterRead(Register::PWR_MGMT_1) & PWR_MGMT_1_BIT::DEVICE_RESET);
+	const bool clksel_done = (RegisterRead(Register::PWR_MGMT_1) & PWR_MGMT_1_BIT::CLKSEL_0);
+	const bool data_ready = (RegisterRead(Register::INT_STATUS) & INT_STATUS_BIT::DATA_RDY_INT);
+
+	return reset_done && clksel_done && data_ready;
+}
+
+void MPU6000::ResetFIFO()
+{
+	perf_count(_fifo_reset_perf);
+
+	// FIFO_EN: disable FIFO
+	RegisterWrite(Register::FIFO_EN, 0);
+	RegisterClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN | USER_CTRL_BIT::FIFO_RESET);
+
+	// USER_CTRL: reset FIFO then re-enable
+	RegisterSetBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RESET);
+	up_udelay(1); // bit auto clears after one clock cycle of the internal 20 MHz clock
+	RegisterSetBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN);
+
+	// CONFIG:
+	RegisterSetBits(Register::CONFIG, CONFIG_BIT::DLPF_CFG_BYPASS_DLPF);
+
+	// FIFO_EN: enable both gyro and accel
+	_data_ready_count = 0;
+	RegisterWrite(Register::FIFO_EN, FIFO_EN_BIT::XG_FIFO_EN | FIFO_EN_BIT::YG_FIFO_EN | FIFO_EN_BIT::ZG_FIFO_EN |
+		      FIFO_EN_BIT::ACCEL_FIFO_EN);
+	up_udelay(10);
+}
+
+uint8_t MPU6000::RegisterRead(Register reg)
+{
+	uint8_t cmd[2] {};
+	cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
+	transfer(cmd, cmd, sizeof(cmd));
+	return cmd[1];
+}
+
+void MPU6000::RegisterWrite(Register reg, uint8_t value)
+{
+	uint8_t cmd[2] { (uint8_t)reg, value };
+	transfer(cmd, cmd, sizeof(cmd));
+}
+
+void MPU6000::RegisterSetBits(Register reg, uint8_t setbits)
+{
+	uint8_t val = RegisterRead(reg);
+
+	if (!(val & setbits)) {
+		val |= setbits;
+		RegisterWrite(reg, val);
+	}
+}
+
+void MPU6000::RegisterClearBits(Register reg, uint8_t clearbits)
+{
+	uint8_t val = RegisterRead(reg);
+
+	if (val & clearbits) {
+		val &= !clearbits;
+		RegisterWrite(reg, val);
+	}
+}
+
+int MPU6000::DataReadyInterruptCallback(int irq, void *context, void *arg)
+{
+	MPU6000 *dev = reinterpret_cast<MPU6000 *>(arg);
+	dev->DataReady();
+	return 0;
+}
+
+void MPU6000::DataReady()
+{
+	perf_count(_drdy_interval_perf);
+
+	_data_ready_count++;
+
+	if (_data_ready_count >= 8) {
+		_time_data_ready = hrt_absolute_time();
+
+		_data_ready_count = 0;
+
+		// make another measurement
+		ScheduleNow();
+	}
+}
+
+void MPU6000::Start()
+{
+	Stop();
+
+	ResetFIFO();
+
+	// TODO: cleanup horrible DRDY define mess
+#if defined(GPIO_SPI1_EXTI_MPU_DRDY)
+	// Setup data ready on rising edge
+	px4_arch_gpiosetevent(GPIO_SPI1_EXTI_MPU_DRDY, true, false, true, &MPU6000::DataReadyInterruptCallback, this);
+	RegisterSetBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN);
+#else
+	ScheduleOnInterval(FIFO_INTERVAL, FIFO_INTERVAL);
+#endif
+}
+
+void MPU6000::Stop()
+{
+	// TODO: cleanup horrible DRDY define mess
+#if defined(GPIO_SPI1_EXTI_MPU_DRDY)
+	// Disable data ready callback
+	px4_arch_gpiosetevent(GPIO_SPI1_EXTI_MPU_DRDY, false, false, false, nullptr, nullptr);
+	RegisterClearBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN);
+#else
+	ScheduleClear();
+#endif
+}
+
+void MPU6000::Run()
+{
+	// use timestamp from the data ready interrupt if available,
+	//  otherwise use the time now roughly corresponding with the last sample we'll pull from the FIFO
+	const hrt_abstime timestamp_sample = (hrt_elapsed_time(&_time_data_ready) < FIFO_INTERVAL) ? _time_data_ready :
+					     hrt_absolute_time();
+
+	// read FIFO count
+	uint8_t fifo_count_buf[3] {};
+	fifo_count_buf[0] = static_cast<uint8_t>(Register::FIFO_COUNTH) | DIR_READ;
+
+	if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
+		return;
+	}
+
+	const size_t fifo_count = combine(fifo_count_buf[1], fifo_count_buf[2]);
+	const int samples = (fifo_count / sizeof(FIFO::DATA) / 2) * 2; // round down to nearest 2
+
+	if (samples < 1) {
+		perf_count(_fifo_empty_perf);
+		return;
+
+	} else if (samples < 8) {
+		// don't transfer fewer than 8 samples (1 accel + 8 gyro)
+		return;
+
+	} else if (samples > 16) {
+		// not technically an overflow, but more samples than we expected
+		perf_count(_fifo_overflow_perf);
+		ResetFIFO();
+		return;
+	}
+
+	// Transfer data
+	struct TransferBuffer {
+		uint8_t cmd;
+		FIFO::DATA f[16]; // max 16 samples
+	};
+	static_assert(sizeof(TransferBuffer) == (sizeof(uint8_t) + 16 * sizeof(FIFO::DATA))); // ensure no struct padding
+
+	TransferBuffer *report = (TransferBuffer *)_dma_data_buffer;
+	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
+	memset(report, 0, transfer_size);
+	report->cmd = static_cast<uint8_t>(Register::FIFO_R_W) | DIR_READ;
+
+	perf_begin(_transfer_perf);
+
+	if (transfer(_dma_data_buffer, _dma_data_buffer, transfer_size) != PX4_OK) {
+		perf_end(_transfer_perf);
+		return;
+	}
+
+	perf_end(_transfer_perf);
+
+	PX4Accelerometer::FIFOSample accel;
+	accel.timestamp_sample = timestamp_sample;
+	accel.samples = samples / 8;
+	accel.dt = FIFO_INTERVAL / FIFO_ACCEL_SAMPLES;
+
+	PX4Gyroscope::FIFOSample gyro;
+	gyro.timestamp_sample = timestamp_sample;
+	gyro.samples = samples;
+	gyro.dt = FIFO_INTERVAL / FIFO_GYRO_SAMPLES;
+
+	// accel data is duplicated 8 times
+	for (int i = 0; i < accel.samples; i++) {
+		const FIFO::DATA &fifo_sample = report->f[i];
+
+		// coordinate convention (x forward, y right, z down)
+		accel.x[i] = combine(fifo_sample.ACCEL_XOUT_H, fifo_sample.ACCEL_XOUT_L);
+		accel.y[i] = -combine(fifo_sample.ACCEL_YOUT_H, fifo_sample.ACCEL_YOUT_L);
+		accel.z[i] = -combine(fifo_sample.ACCEL_ZOUT_H, fifo_sample.ACCEL_ZOUT_L);
+	}
+
+	for (int i = 0; i < samples; i++) {
+		const FIFO::DATA &fifo_sample = report->f[i];
+
+		// coordinate convention (x forward, y right, z down)
+		gyro.x[i] = combine(fifo_sample.GYRO_XOUT_H, fifo_sample.GYRO_XOUT_L);
+		gyro.y[i] = -combine(fifo_sample.GYRO_YOUT_H, fifo_sample.GYRO_YOUT_L);
+		gyro.z[i] = -combine(fifo_sample.GYRO_ZOUT_H, fifo_sample.GYRO_ZOUT_L);
+	}
+
+
+	// Temperature
+	if (hrt_elapsed_time(&_time_last_temperature_update) > 1_s) {
+		// read current temperature
+		uint8_t temperature_buf[3] {};
+		temperature_buf[0] = static_cast<uint8_t>(Register::TEMP_OUT_H) | DIR_READ;
+
+		if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) {
+			return;
+		}
+
+		const int16_t TEMP_OUT = combine(temperature_buf[1], temperature_buf[2]);
+
+		static constexpr float RoomTemp_Offset = 25.0f; // Room Temperature Offset 25°C
+		static constexpr float Temp_Sensitivity = 326.8f; // Sensitivity 326.8 LSB/°C
+
+		const float TEMP_degC = ((TEMP_OUT - RoomTemp_Offset) / Temp_Sensitivity) + 25.0f;
+
+		_px4_accel.set_temperature(TEMP_degC);
+		_px4_gyro.set_temperature(TEMP_degC);
+	}
+
+	_px4_gyro.updateFIFO(gyro);
+	_px4_accel.updateFIFO(accel);
+}
+
+void MPU6000::PrintInfo()
+{
+	perf_print_counter(_transfer_perf);
+	perf_print_counter(_fifo_empty_perf);
+	perf_print_counter(_fifo_overflow_perf);
+	perf_print_counter(_fifo_reset_perf);
+	perf_print_counter(_drdy_interval_perf);
+
+	_px4_accel.print_status();
+	_px4_gyro.print_status();
+}

src/drivers/imu/invensense/mpu6000/MPU6000.hpp

@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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.hpp
+ *
+ * Driver for the Invensense MPU6000 connected via SPI.
+ *
+ */
+
+#pragma once
+
+#include "InvenSense_MPU6000_registers.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
+#include <lib/drivers/device/spi.h>
+#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
+#include <lib/ecl/geo/geo.h>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+
+using InvenSense_MPU6000::Register;
+
+class MPU6000 : public device::SPI, public px4::ScheduledWorkItem
+{
+public:
+	MPU6000(int bus, uint32_t device, enum Rotation rotation = ROTATION_NONE);
+	~MPU6000() override;
+
+	bool Init();
+	void Start();
+	void Stop();
+	bool Reset();
+	void PrintInfo();
+
+private:
+	int probe() override;
+
+	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
+	void DataReady();
+
+	void Run() override;
+
+	uint8_t RegisterRead(Register reg);
+	void RegisterWrite(Register reg, uint8_t value);
+	void RegisterSetBits(Register reg, uint8_t setbits);
+	void RegisterClearBits(Register reg, uint8_t clearbits);
+
+	void ResetFIFO();
+
+	uint8_t *_dma_data_buffer{nullptr};
+
+	PX4Accelerometer _px4_accel;
+	PX4Gyroscope _px4_gyro;
+
+	perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": transfer")};
+	perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo empty")};
+	perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo overflow")};
+	perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo reset")};
+	perf_counter_t _drdy_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": drdy interval")};
+
+	hrt_abstime _time_data_ready{0};
+	hrt_abstime _time_last_temperature_update{0};
+	int _data_ready_count{0};
+};

src/drivers/imu/invensense/mpu6000/mpu6000_main.cpp

@@ -0,0 +1,153 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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.
+ *
+ ****************************************************************************/
+
+#include "MPU6000.hpp"
+
+#include <px4_platform_common/getopt.h>
+
+namespace mpu6000
+{
+MPU6000 *g_dev{nullptr};
+
+static int start(enum Rotation rotation)
+{
+	if (g_dev != nullptr) {
+		PX4_WARN("already started");
+		return 0;
+	}
+
+	// create the driver
+#if defined(PX4_SPI_BUS_SENSORS) && defined(PX4_SPIDEV_MPU)
+	g_dev = new MPU6000(PX4_SPI_BUS_SENSORS, PX4_SPIDEV_MPU, rotation);
+#elif defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_MPU)
+	g_dev = new MPU6000(PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_MPU, rotation);
+#endif
+
+	if (g_dev == nullptr) {
+		PX4_ERR("driver start failed");
+		return -1;
+	}
+
+	if (!g_dev->Init()) {
+		PX4_ERR("driver init failed");
+		delete g_dev;
+		g_dev = nullptr;
+		return -1;
+	}
+
+	return 0;
+}
+
+static int stop()
+{
+	if (g_dev == nullptr) {
+		PX4_WARN("driver not running");
+		return -1;
+	}
+
+	g_dev->Stop();
+	delete g_dev;
+	g_dev = nullptr;
+
+	return 0;
+}
+
+static int reset()
+{
+	if (g_dev == nullptr) {
+		PX4_WARN("driver not running");
+		return 0;
+	}
+
+	return g_dev->Reset();
+}
+
+static int status()
+{
+	if (g_dev == nullptr) {
+		PX4_INFO("driver not running");
+		return 0;
+	}
+
+	g_dev->PrintInfo();
+
+	return 0;
+}
+
+static int usage()
+{
+	PX4_INFO("missing command: try 'start', 'stop', 'reset', 'status'");
+	PX4_INFO("options:");
+	PX4_INFO("    -R rotation");
+
+	return 0;
+}
+
+} // namespace mpu6000
+
+extern "C" int mpu6000_main(int argc, char *argv[])
+{
+	enum Rotation rotation = ROTATION_NONE;
+	int myoptind = 1;
+	int ch = 0;
+	const char *myoptarg = nullptr;
+
+	// start options
+	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'R':
+			rotation = (enum Rotation)atoi(myoptarg);
+			break;
+
+		default:
+			return mpu6000::usage();
+		}
+	}
+
+	const char *verb = argv[myoptind];
+
+	if (!strcmp(verb, "start")) {
+		return mpu6000::start(rotation);
+
+	} else if (!strcmp(verb, "stop")) {
+		return mpu6000::stop();
+
+	} else if (!strcmp(verb, "status")) {
+		return mpu6000::status();
+
+	} else if (!strcmp(verb, "reset")) {
+		return mpu6000::reset();
+	}
+
+	return mpu6000::usage();
+}