Bosch BMI088 initial driver

boards/px4/fmu-v5x/default.cmake

@@ -30,7 +30,7 @@ px4_add_board(
 		imu/adis16448
 		imu/adis16497
 		#imu # all available imu drivers
-# TBD		imu/bmi088 - needs bus selection
+		imu/bmi088
 # TBD		imu/ism330dlc - needs bus selection
 		imu/mpu6000
 		irlock

src/drivers/drv_sensor.h

@@ -113,6 +113,7 @@
 #define DRV_ACC_DEVTYPE_ADIS16497	0x63
 #define DRV_GYR_DEVTYPE_ADIS16497	0x64
 #define DRV_BARO_DEVTYPE_BAROSIM	0x65
+#define DRV_DEVTYPE_BMI088		0x66
 
 /*
  * ioctl() definitions

src/drivers/imu/bmi088/BMI088.hpp

@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 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
+
+#include <drivers/device/spi.h>
+#include <ecl/geo/geo.h>
+#include <lib/conversion/rotation.h>
+#include <lib/perf/perf_counter.h>
+#include <px4_getopt.h>
+#include <px4_work_queue/ScheduledWorkItem.hpp>
+
+#define DIR_READ                0x80
+#define DIR_WRITE               0x00
+
+//Soft-reset command Value
+#define BMI088_SOFT_RESET       0xB6
+
+#define BMI088_BUS_SPEED				10*1000*1000
+
+#define BMI088_TIMER_REDUCTION				200
+
+class BMI088 : public device::SPI
+{
+
+protected:
+
+	uint8_t         _whoami;    /** whoami result */
+
+	uint8_t         _register_wait;
+	uint64_t        _reset_wait;
+
+	enum Rotation       _rotation;
+
+	uint8_t         _checked_next;
+
+	/**
+	* Read a register from the BMI088
+	*
+	* @param       The register to read.
+	* @return      The value that was read.
+	*/
+	virtual uint8_t         read_reg(unsigned reg); // This needs to be declared as virtual, because the
+	virtual uint16_t        read_reg16(unsigned reg);
+
+	/**
+	* Write a register in the BMI088
+	*
+	* @param reg       The register to write.
+	* @param value     The new value to write.
+	*/
+	void            write_reg(unsigned reg, uint8_t value);
+
+	/* do not allow to copy this class due to pointer data members */
+	BMI088(const BMI088 &);
+	BMI088 operator=(const BMI088 &);
+
+public:
+
+	BMI088(const char *name, const char *devname, int bus, uint32_t device, enum spi_mode_e mode, uint32_t frequency,
+	       enum Rotation rotation);
+
+	virtual ~BMI088() = default;
+
+
+};

src/drivers/imu/bmi088/BMI088_accel.cpp

@@ -0,0 +1,614 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 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 "BMI088_accel.hpp"
+
+/*
+ * Global variable of the accelerometer temperature reading, to read it in the bmi055_gyro driver. The variable is changed in bmi055_accel.cpp.
+ * This is a HACK! The driver should be rewritten with the gyro as subdriver.
+ */
+extern float _accel_last_temperature_copy;
+
+/*
+  list of registers that will be checked in check_registers(). Note
+  that ADDR_WHO_AM_I must be first in the list.
+ */
+const uint8_t BMI088_accel::_checked_registers[BMI088_ACCEL_NUM_CHECKED_REGISTERS] = {BMI088_ACC_CHIP_ID,
+										      BMI088_ACC_CONF,
+										      BMI088_ACC_RANGE,
+										      BMI088_ACC_INT1_IO_CONF,
+										      BMI088_ACC_INT1_INT2_MAP_DATA,
+										      BMI088_ACC_PWR_CONF,
+										      BMI088_ACC_PWR_CTRL,
+										     };
+
+BMI088_accel::BMI088_accel(int bus, const char *path_accel, uint32_t device, enum Rotation rotation) :
+	BMI088("BMI088_ACCEL", path_accel, bus, device, SPIDEV_MODE3, BMI088_BUS_SPEED, rotation),
+	ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())),
+	_px4_accel(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation),
+	_sample_perf(perf_alloc(PC_ELAPSED, "bmi088_accel_read")),
+	_measure_interval(perf_alloc(PC_INTERVAL, "bmi088_accel_measure_interval")),
+	_bad_transfers(perf_alloc(PC_COUNT, "bmi088_accel_bad_transfers")),
+	_bad_registers(perf_alloc(PC_COUNT, "bmi088_accel_bad_registers")),
+	_duplicates(perf_alloc(PC_COUNT, "bmi088_accel_duplicates")),
+	_got_duplicate(false)
+{
+	_px4_accel.set_device_type(DRV_DEVTYPE_BMI088);
+}
+
+BMI088_accel::~BMI088_accel()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* delete the perf counter */
+	perf_free(_sample_perf);
+	perf_free(_measure_interval);
+	perf_free(_bad_transfers);
+	perf_free(_bad_registers);
+	perf_free(_duplicates);
+}
+
+int
+BMI088_accel::init()
+{
+	/* do SPI init (and probe) first */
+	int ret = SPI::init();
+
+	/* if probe/setup failed, bail now */
+	if (ret != OK) {
+		DEVICE_DEBUG("SPI setup failed");
+		return ret;
+	}
+
+	return reset();
+}
+
+uint8_t
+BMI088_accel::read_reg(unsigned reg)
+{
+	// For the BMI088, you need to read out a dummy byte before you can read out the normal data (see section "SPI interface of accelerometer part" of the BMI088 datasheet)
+	uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0};
+
+	transfer(cmd, cmd, sizeof(cmd));
+
+	return cmd[2]; // Skip dummy byte in cmd[1] and read out actual data
+}
+
+uint16_t
+BMI088_accel::read_reg16(unsigned reg)
+{
+	// For the BMI088, you need to read out the dummy byte before you can read out the normal data (see section "SPI interface of accelerometer part" of the BMI088 datasheet)
+	uint8_t cmd[4] = { (uint8_t)(reg | DIR_READ), 0, 0, 0 };
+
+	transfer(cmd, cmd, sizeof(cmd));
+
+	return (uint16_t)(cmd[2] << 8) | cmd[3]; // Skip dummy byte in cmd[1]
+}
+
+int BMI088_accel::reset()
+{
+	write_reg(BMI088_ACC_SOFTRESET, BMI088_SOFT_RESET); // Soft-reset
+	/* After a POR or soft-reset, the sensor needs up to 1ms boot time
+	 * (see section "Power Modes: Accelerometer" in the BMI088 datasheet).
+	 * Based off of testing it seems this value needs to be increased from 1ms.
+	 * Setting it to 5ms.
+	 */
+
+	up_udelay(5000);
+
+	// Perform a dummy read here to put the accelerometer part of the BMI088 back into SPI mode after the reset
+	// The dummy read basically pulls the chip select line low and then high
+	// See section "Serial Peripheral Interface (SPI)" of the BMI088 datasheet for more details.
+	read_reg(BMI088_ACC_CHIP_ID);
+
+	// Enable Accelerometer
+	// The accelerometer needs to be enabled first, before writing to its registers
+	write_checked_reg(BMI088_ACC_PWR_CTRL, BMI088_ACC_PWR_CTRL_EN);
+	/* After changing power modes, the sensor requires up to 5ms to settle.
+	 * Any communication with the sensor during this time should be avoided
+	 * (see section "Power Modes: Acceleromter" in the BMI datasheet) */
+
+	up_udelay(5000);
+
+	// Set the PWR CONF to be active
+	write_checked_reg(BMI088_ACC_PWR_CONF, BMI088_ACC_PWR_CONF_ACTIVE); // Sets the accelerometer to active mode
+
+	// Write accel bandwidth and output data rate
+	// ToDo set the bandwidth
+	accel_set_sample_rate(BMI088_ACCEL_DEFAULT_RATE); //set accel ODR
+	set_accel_range(BMI088_ACCEL_DEFAULT_RANGE_G); //set accel range
+
+	// Configure the accel INT1
+	write_checked_reg(BMI088_ACC_INT1_IO_CONF,
+			  BMI088_ACC_INT1_IO_CONF_INT1_OUT | BMI088_ACC_INT1_IO_CONF_PP |
+			  BMI088_ACC_INT1_IO_CONF_ACTIVE_HIGH); // Configure INT1 pin as output, push-pull, active high
+	write_checked_reg(BMI088_ACC_INT1_INT2_MAP_DATA,
+			  BMI088_ACC_INT1_INT2_MAP_DATA_INT1_DRDY); // Map DRDY interrupt on pin INT1
+
+	uint8_t retries = 10;
+
+	while (retries--) {
+		bool all_ok = true;
+
+		for (uint8_t i = 0; i < BMI088_ACCEL_NUM_CHECKED_REGISTERS; i++) {
+			if (read_reg(_checked_registers[i]) != _checked_values[i]) {
+				write_reg(_checked_registers[i], _checked_values[i]);
+				all_ok = false;
+			}
+		}
+
+		if (all_ok) {
+			break;
+		}
+	}
+
+	return OK;
+}
+
+int
+BMI088_accel::probe()
+{
+	// Perform a dummy read here to put the accelerometer part of the BMI088 back into SPI mode after the reset
+	// The dummy read basically pulls the chip select line low and then high
+	// See section "Serial Peripheral Interface (SPI)" of the BMI088 datasheet for more details.
+	read_reg(BMI088_ACC_CHIP_ID);
+
+	/* look for device ID */
+	_whoami = read_reg(BMI088_ACC_CHIP_ID);
+
+	// verify product revision
+	switch (_whoami) {
+	case BMI088_ACC_WHO_AM_I:
+		memset(_checked_values, 0, sizeof(_checked_values));
+		memset(_checked_bad, 0, sizeof(_checked_bad));
+		_checked_values[0] = _whoami;
+		_checked_bad[0] = _whoami;
+		return OK;
+	}
+
+	DEVICE_DEBUG("unexpected whoami 0x%02x", _whoami);
+	return -EIO;
+}
+
+int
+BMI088_accel::accel_set_sample_rate(float frequency)
+{
+	uint8_t setbits = 0;
+	uint8_t clearbits = 0x0F;
+
+	if (frequency < 25) {
+		setbits |= BMI088_ACC_CONF_ODR_12_5;
+		//_accel_sample_rate = 12.5f;
+
+	} else if (frequency < 50) {
+		setbits |= BMI088_ACC_CONF_ODR_25;
+		//_accel_sample_rate = 25.f;
+
+	} else if (frequency < 100) {
+		setbits |= BMI088_ACC_CONF_ODR_50;
+		//_accel_sample_rate = 50.f;
+
+	} else if (frequency < 200) {
+		setbits |= BMI088_ACC_CONF_ODR_100;
+		//_accel_sample_rate = 100.f;
+
+	} else if (frequency < 400) {
+		setbits |= BMI088_ACC_CONF_ODR_200;
+		//_accel_sample_rate = 200.f;
+
+	} else if (frequency < 800) {
+		setbits |= BMI088_ACC_CONF_ODR_400;
+		//_accel_sample_rate = 400.f;
+
+	} else if (frequency < 1600) {
+		setbits |= BMI088_ACC_CONF_ODR_800;
+		//_accel_sample_rate = 800.f;
+
+	} else if (frequency >= 1600) {
+		setbits |= BMI088_ACC_CONF_ODR_1600;
+		//_accel_sample_rate = 1600.f;
+
+	} else {
+		printf("Set sample rate error \n");
+		return -EINVAL;
+	}
+
+	/* Write accel ODR */
+	modify_reg(BMI088_ACC_CONF, clearbits, setbits);
+
+	return OK;
+}
+
+/*
+  deliberately trigger an error in the sensor to trigger recovery
+ */
+void
+BMI088_accel::test_error()
+{
+	write_reg(BMI088_ACC_SOFTRESET, BMI088_SOFT_RESET);
+	::printf("error triggered\n");
+	print_registers();
+}
+
+void
+BMI088_accel::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
+{
+	uint8_t val = read_reg(reg);
+	val &= ~clearbits;
+	val |= setbits;
+	write_checked_reg(reg, val);
+}
+
+void
+BMI088_accel::write_checked_reg(unsigned reg, uint8_t value)
+{
+	write_reg(reg, value);
+
+	for (uint8_t i = 0; i < BMI088_ACCEL_NUM_CHECKED_REGISTERS; i++) {
+		if (reg == _checked_registers[i]) {
+			_checked_values[i] = value;
+			_checked_bad[i] = value;
+		}
+	}
+}
+
+int
+BMI088_accel::set_accel_range(unsigned max_g)
+{
+	uint8_t setbits = 0;
+	uint8_t clearbits = BMI088_ACCEL_RANGE_24_G;
+	float lsb_per_g;
+
+	if (max_g == 0) {
+		max_g = 24;
+	}
+
+	if (max_g <= 3) {
+		//max_accel_g = 3;
+		setbits |= BMI088_ACCEL_RANGE_3_G;
+		lsb_per_g = 10922.67;
+
+	} else if (max_g <= 6) {
+		//max_accel_g = 6;
+		setbits |= BMI088_ACCEL_RANGE_6_G;
+		lsb_per_g = 5461.33;
+
+	} else if (max_g <= 12) {
+		//max_accel_g = 12;
+		setbits |= BMI088_ACCEL_RANGE_12_G;
+		lsb_per_g = 2730.67;
+
+	} else if (max_g <= 24) {
+		//max_accel_g = 24;
+		setbits |= BMI088_ACCEL_RANGE_24_G;
+		lsb_per_g = 1365.33;
+
+	} else {
+		return -EINVAL;
+	}
+
+	_px4_accel.set_scale(CONSTANTS_ONE_G / lsb_per_g);
+
+	modify_reg(BMI088_ACC_RANGE, clearbits, setbits);
+
+	return OK;
+}
+
+void
+BMI088_accel::start()
+{
+	/* make sure we are stopped first */
+	stop();
+
+	// Reset the accelerometer
+	reset();
+
+	/* start polling at the specified rate */
+	ScheduleOnInterval(BMI088_ACCEL_DEFAULT_RATE - BMI088_TIMER_REDUCTION, 1000);
+
+}
+
+void
+BMI088_accel::stop()
+{
+	ScheduleClear();
+}
+
+void
+BMI088_accel::Run()
+{
+	/* make another measurement */
+	measure();
+}
+
+void
+BMI088_accel::check_registers(void)
+{
+	uint8_t v;
+
+	if ((v = read_reg(_checked_registers[_checked_next])) !=
+	    _checked_values[_checked_next]) {
+		_checked_bad[_checked_next] = v;
+
+		/*
+		          if we get the wrong value then we know the SPI bus
+		          or sensor is very sick. We set _register_wait to 20
+		          and wait until we have seen 20 good values in a row
+		          before we consider the sensor to be OK again.
+		*/
+		perf_count(_bad_registers);
+
+		/*
+		          try to fix the bad register value. We only try to
+		          fix one per loop to prevent a bad sensor hogging the
+		          bus.
+		*/
+		if (_register_wait == 0 || _checked_next == 0) {
+			// if the product_id is wrong then reset the
+			// sensor completely
+			write_reg(BMI088_ACC_SOFTRESET, BMI088_SOFT_RESET);
+			_reset_wait = hrt_absolute_time() + 10000;
+			_checked_next = 0;
+
+		} else {
+			write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]);
+			// waiting 3ms between register writes seems
+			// to raise the chance of the sensor
+			// recovering considerably
+			_reset_wait = hrt_absolute_time() + 3000;
+		}
+
+		_register_wait = 20;
+	}
+
+	_checked_next = (_checked_next + 1) % BMI088_ACCEL_NUM_CHECKED_REGISTERS;
+}
+
+void
+BMI088_accel::measure()
+{
+	perf_count(_measure_interval);
+
+	if (hrt_absolute_time() < _reset_wait) {
+		// we're waiting for a reset to complete
+		return;
+	}
+
+	struct Report {
+		int16_t     accel_x;
+		int16_t     accel_y;
+		int16_t     accel_z;
+		int16_t     temp;
+	} report;
+
+	/* start measuring */
+	perf_begin(_sample_perf);
+
+	// Checking the status of new data
+	uint8_t status;
+	status = read_reg(BMI088_ACC_STATUS);
+
+	if (!(status & BMI088_ACC_STATUS_DRDY)) {
+		perf_end(_sample_perf);
+		perf_count(_duplicates);
+		_got_duplicate = true;
+		return;
+	}
+
+	_got_duplicate = false;
+
+	/*
+	     * Fetch the full set of measurements from the BMI088 in one pass.
+	     */
+	uint8_t index = 0;
+	uint8_t accel_data[8]; // Need an extra byte for the command, and an an extra dummy byte for the read (see section "SPI interface of accelerometer part" of the BMI088 datasheet)
+	accel_data[index] = BMI088_ACC_X_L | DIR_READ;
+
+	const hrt_abstime timestamp_sample = hrt_absolute_time();
+
+	if (OK != transfer(accel_data, accel_data, sizeof(accel_data))) {
+		return;
+	}
+
+	check_registers();
+
+	/* Extracting accel data from the read data */
+	index = 2; // Skip the dummy byte at index=1
+	uint16_t lsb, msb, msblsb;
+
+	lsb = (uint16_t)accel_data[index++];
+	msb = (uint16_t)accel_data[index++];
+	msblsb = (msb << 8) | lsb;
+	report.accel_x = (int16_t)msblsb; /* Data in X axis */
+
+	lsb = (uint16_t)accel_data[index++];
+	msb = (uint16_t)accel_data[index++];
+	msblsb = (msb << 8) | lsb;
+	report.accel_y = (int16_t)msblsb; /* Data in Y axis */
+
+	lsb = (uint16_t)accel_data[index++];
+	msb = (uint16_t)accel_data[index++];
+	msblsb = (msb << 8) | lsb;
+	report.accel_z = (int16_t)msblsb; /* Data in Z axis */
+
+	// Extract the temperature data
+	// Note: the temp sensor data is only updated every 1.28s (see "Register 0x22-0x23 Temperature Sensor Data" section in BMI088 Datasheet)
+	index = 0;
+	accel_data[index] = BMI088_ACC_TEMP_H | DIR_READ;
+
+	// Need to perform a dummy read, hence the num bytes to read is 3 (plus 1 send byte)
+	if (OK != transfer(accel_data, accel_data, 4)) {
+		return;
+	}
+
+	index = 2;
+	msb = (uint16_t)accel_data[index++];
+	lsb = (uint16_t)accel_data[index++];
+	uint16_t temp = msb * 8 + lsb / 32;
+
+	if (temp > 1023) {
+		report.temp = temp - 2048;
+
+	} else {
+		report.temp = temp;
+	}
+
+
+	if (report.accel_x == 0 &&
+	    report.accel_y == 0 &&
+	    report.accel_z == 0) {
+		// all zero data - probably a SPI bus error
+		perf_count(_bad_transfers);
+		perf_end(_sample_perf);
+		// note that we don't call reset() here as a reset()
+		// costs 20ms with interrupts disabled. That means if
+		// the bmi088 accel does go bad it would cause a FMU failure,
+		// regardless of whether another sensor is available,
+		return;
+	}
+
+	if (_register_wait != 0) {
+		// we are waiting for some good transfers before using
+		// the sensor again, but don't return any data yet
+		_register_wait--;
+		return;
+	}
+
+	// report the error count as the sum of the number of bad
+	// transfers and bad register reads. This allows the higher
+	// level code to decide if it should use this sensor based on
+	// whether it has had failures
+	const uint64_t error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);
+	_px4_accel.set_error_count(error_count);
+
+	// Convert the bit-wise representation of temperature to degrees C
+	_accel_last_temperature_copy = (report.temp * 0.125f) + 23.0f;
+	_px4_accel.set_temperature(_accel_last_temperature_copy);
+
+	/*
+	     * 1) Scale raw value to SI units using scaling from datasheet.
+	     * 2) Subtract static offset (in SI units)
+	     * 3) Scale the statically calibrated values with a linear
+	     *    dynamically obtained factor
+	     *
+	     * Note: the static sensor offset is the number the sensor outputs
+	     *   at a nominally 'zero' input. Therefore the offset has to
+	     *   be subtracted.
+	     *
+	     */
+	_px4_accel.update(timestamp_sample, report.accel_x, report.accel_y, report.accel_z);
+
+	/* stop measuring */
+	perf_end(_sample_perf);
+}
+
+void
+BMI088_accel::print_info()
+{
+	PX4_INFO("Accel");
+
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_measure_interval);
+	perf_print_counter(_bad_transfers);
+	perf_print_counter(_bad_registers);
+	perf_print_counter(_duplicates);
+
+	::printf("checked_next: %u\n", _checked_next);
+
+	for (uint8_t i = 0; i < BMI088_ACCEL_NUM_CHECKED_REGISTERS; i++) {
+		uint8_t v = read_reg(_checked_registers[i]);
+
+		if (v != _checked_values[i]) {
+			::printf("reg %02x:%02x should be %02x\n",
+				 (unsigned)_checked_registers[i],
+				 (unsigned)v,
+				 (unsigned)_checked_values[i]);
+		}
+
+		if (v != _checked_bad[i]) {
+			::printf("reg %02x:%02x was bad %02x\n",
+				 (unsigned)_checked_registers[i],
+				 (unsigned)v,
+				 (unsigned)_checked_bad[i]);
+		}
+	}
+
+	_px4_accel.print_status();
+}
+
+void
+BMI088_accel::print_registers()
+{
+	uint8_t index = 0;
+	printf("BMI088 accel registers\n");
+
+	uint8_t reg = _checked_registers[index++];
+	uint8_t v = read_reg(reg);
+	printf("Accel Chip Id: %02x:%02x ", (unsigned)reg, (unsigned)v);
+	printf("\n");
+
+	reg = _checked_registers[index++];
+	v = read_reg(reg);
+	printf("Accel Conf: %02x:%02x ", (unsigned)reg, (unsigned)v);
+	printf("\n");
+
+	reg = _checked_registers[index++];
+	v = read_reg(reg);
+	printf("Accel Range: %02x:%02x ", (unsigned)reg, (unsigned)v);
+	printf("\n");
+
+	reg = _checked_registers[index++];
+	v = read_reg(reg);
+	printf("Accel Int1 Conf: %02x:%02x ", (unsigned)reg, (unsigned)v);
+	printf("\n");
+
+	reg = _checked_registers[index++];
+	v = read_reg(reg);
+	printf("Accel Int1-Int2_Map-Data: %02x:%02x ", (unsigned)reg, (unsigned)v);
+	printf("\n");
+
+	reg = _checked_registers[index++];
+	v = read_reg(reg);
+	printf("Accel Pwr Conf: %02x:%02x ", (unsigned)reg, (unsigned)v);
+	printf("\n");
+
+	reg = _checked_registers[index++];
+	v = read_reg(reg);
+	printf("Accel Pwr Ctrl: %02x:%02x ", (unsigned)reg, (unsigned)v);
+	printf("\n");
+
+
+
+	printf("\n");
+}

src/drivers/imu/bmi088/BMI088_accel.hpp

@@ -0,0 +1,268 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 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
+
+#include "BMI088.hpp"
+
+#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
+#include <lib/conversion/rotation.h>
+
+#define BMI088_DEVICE_PATH_ACCEL	"/dev/bmi088_accel"
+#define BMI088_DEVICE_PATH_ACCEL_EXT	"/dev/bmi088_accel_ext"
+
+// BMI088 Accel registers
+#define BMI088_ACC_CHIP_ID            0x00
+
+#define BMI088_ACC_ERR_REG            0x02
+#define BMI088_ACC_STATUS             0x03
+#define BMI088_ACC_X_L                0x12
+#define BMI088_ACC_X_H                0x13
+#define BMI088_ACC_Y_L                0x14
+#define BMI088_ACC_Y_H                0x15
+#define BMI088_ACC_Z_L                0x16
+#define BMI088_ACC_Z_H                0x17
+#define BMI088_ACC_SENSORTIME_0       0x18
+#define BMI088_ACC_SENSORTIME_1       0x19
+#define BMI088_ACC_SENSORTIME_2       0x1A
+#define BMI088_ACC_INT_STAT_1         0x1D
+#define BMI088_ACC_TEMP_H             0x22
+#define BMI088_ACC_TEMP_L             0x23
+#define BMI088_ACC_CONF               0x40
+#define BMI088_ACC_RANGE              0x41
+#define BMI088_ACC_INT1_IO_CONF       0x53
+#define BMI088_ACC_INT2_IO_CONF       0x54
+#define BMI088_ACC_INT1_INT2_MAP_DATA 0x58
+#define BMI088_ACC_SELF_TEST          0x6D
+#define BMI088_ACC_PWR_CONF           0x7C
+#define BMI088_ACC_PWR_CTRL           0x7D
+#define BMI088_ACC_SOFTRESET          0x7E
+
+// BMI088 Accelerometer Chip-Id
+#define BMI088_ACC_WHO_AM_I         0x1E
+
+// BMI088_ACC_ERR_REG       0x02
+#define BMI088_ACC_ERR_REG_NO_ERROR (0x00<<2)
+#define BMI088_ACC_ERR_REG_ERROR    (0x01<<2)
+
+#define BMI088_ACC_ERR_REG_FATAL_ERROR    (0x01<<0)
+
+// BMI088_ACC_STATUS       0x03
+#define BMI088_ACC_STATUS_DRDY      (0x01<<7)
+
+// BMI088_ACC_INT_STAT_1   0x01D
+#define BMI088_ACC_INT_STAT_1_DRDY  (0x01<<7)
+
+// BMI088_ACC_CONF           0x40
+#define BMI088_ACC_CONF_BWP_4       (0x08<<4)
+#define BMI088_ACC_CONF_BWP_2       (0x09<<4)
+#define BMI088_ACC_CONF_BWP_NORMAL  (0x0A<<4)
+
+#define BMI088_ACC_CONF_ODR_12_5    (0x05<<0)
+#define BMI088_ACC_CONF_ODR_25      (0x06<<0)
+#define BMI088_ACC_CONF_ODR_50      (0x07<<0)
+#define BMI088_ACC_CONF_ODR_100     (0x08<<0)
+#define BMI088_ACC_CONF_ODR_200     (0x09<<0)
+#define BMI088_ACC_CONF_ODR_400     (0x0A<<0)
+#define BMI088_ACC_CONF_ODR_800     (0x0B<<0)
+#define BMI088_ACC_CONF_ODR_1600    (0x0C<<0)
+
+// BMI088_ACC_RANGE        0x41
+#define BMI088_ACCEL_RANGE_3_G      (0x00<<0)
+#define BMI088_ACCEL_RANGE_6_G      (0x01<<0)
+#define BMI088_ACCEL_RANGE_12_G     (0x02<<0)
+#define BMI088_ACCEL_RANGE_24_G     (0x03<<0)
+
+// BMI088_ACC_INT1_IO_CONF      0x53
+#define BMI088_ACC_INT1_IO_CONF_INT1_IN     (0x01<<4)
+
+#define BMI088_ACC_INT1_IO_CONF_INT1_OUT    (0x01<<3)
+
+#define BMI088_ACC_INT1_IO_CONF_PP          (0x00<<2)
+#define BMI088_ACC_INT1_IO_CONF_OD          (0x01<<2)
+
+#define BMI088_ACC_INT1_IO_CONF_ACTIVE_LOW  (0x00<<1)
+#define BMI088_ACC_INT1_IO_CONF_ACTIVE_HIGH (0x01<<1)
+
+// BMI088_ACC_INT2_IO_CONF      0x54
+#define BMI088_ACC_INT2_IO_CONF_INT1_IN     (0x01<<4)
+
+#define BMI088_ACC_INT2_IO_CONF_INT1_OUT    (0x01<<3)
+
+#define BMI088_ACC_INT2_IO_CONF_PP          (0x00<<2)
+#define BMI088_ACC_INT2_IO_CONF_OD          (0x01<<2)
+
+#define BMI088_ACC_INT2_IO_CONF_ACTIVE_LOW  (0x00<<1)
+#define BMI088_ACC_INT2_IO_CONF_ACTIVE_HIGH (0x01<<1)
+
+// BMI088_ACC_INT1_INT2_MAP_DATA  0x54
+#define BMI088_ACC_INT1_INT2_MAP_DATA_INT2_DRDY  (0x01<<6)
+#define BMI088_ACC_INT1_INT2_MAP_DATA_INT1_DRDY  (0x01<<2)
+
+// BMI088_ACC_SELF_TEST            0x6D
+#define BMI088_ACC_SELF_TEST_OFF        (0x00<<0)
+#define BMI088_ACC_SELF_TEST_POSITIVE   (0x0D<<0)
+#define BMI088_ACC_SELF_TEST_NEGATIVE   (0x09<<0)
+
+// BMI088_ACC_PWR_CONF            0x7C
+#define BMI088_ACC_PWR_CONF_SUSPEND     (0x03<<0)
+#define BMI088_ACC_PWR_CONF_ACTIVE      (0x00<<0)
+
+// BMI088_ACC_PWR_CTRL            0x7D
+#define BMI088_ACC_PWR_CTRL_EN          (0x04<<0)
+
+
+///////
+// To Do check these defaults and maks below
+///
+
+// Default and Max values
+#define BMI088_ACCEL_DEFAULT_RANGE_G		    24
+#define BMI088_ACCEL_DEFAULT_RATE           800
+#define BMI088_ACCEL_MAX_RATE               800
+#define BMI088_ACCEL_MAX_PUBLISH_RATE       800
+
+#define BMI088_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 50
+
+class BMI088_accel : public BMI088, public px4::ScheduledWorkItem
+{
+public:
+	BMI088_accel(int bus, const char *path_accel, uint32_t device, enum Rotation rotation);
+	virtual ~BMI088_accel();
+
+	virtual int     init();
+
+	// Start automatic measurement.
+	void            start();
+
+	// We need to override the read_reg function from the BMI088 base class, because the accelerometer requires a dummy byte read before each read operation
+	virtual uint8_t   read_reg(unsigned reg);
+
+	// We need to override the read_reg16 function from the BMI088 base class, because the accelerometer requires a dummy byte read before each read operation
+	virtual uint16_t read_reg16(unsigned reg);
+
+	/**
+	    * Diagnostics - print some basic information about the driver.
+	    */
+	void            print_info();
+
+	void            print_registers();
+
+	// deliberately cause a sensor error
+	void            test_error();
+
+protected:
+
+	virtual int     probe();
+
+private:
+
+	PX4Accelerometer	_px4_accel;
+
+	perf_counter_t      _sample_perf;
+	perf_counter_t      _measure_interval;
+	perf_counter_t      _bad_transfers;
+	perf_counter_t      _bad_registers;
+	perf_counter_t      _duplicates;
+
+	// this is used to support runtime checking of key
+	// configuration registers to detect SPI bus errors and sensor
+	// reset
+#define BMI088_ACCEL_NUM_CHECKED_REGISTERS 7
+	static const uint8_t    _checked_registers[BMI088_ACCEL_NUM_CHECKED_REGISTERS];
+	uint8_t         _checked_values[BMI088_ACCEL_NUM_CHECKED_REGISTERS];
+	uint8_t         _checked_bad[BMI088_ACCEL_NUM_CHECKED_REGISTERS];
+
+	bool            _got_duplicate;
+
+	/**
+	     * Stop automatic measurement.
+	     */
+	void            stop();
+
+	/**
+	     * Reset chip.
+	     *
+	     * Resets the chip and measurements ranges, but not scale and offset.
+	     */
+	int         reset();
+
+	void     Run() override;
+
+	/**
+	     * Fetch measurements from the sensor and update the report buffers.
+	     */
+	void            measure();
+
+	/**
+	     * Modify a register in the BMI088_accel
+	     *
+	     * Bits are cleared before bits are set.
+	     *
+	     * @param reg       The register to modify.
+	     * @param clearbits Bits in the register to clear.
+	     * @param setbits   Bits in the register to set.
+	     */
+	void            modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
+
+	/**
+	     * Write a register in the BMI088_accel, updating _checked_values
+	     *
+	     * @param reg       The register to write.
+	     * @param value     The new value to write.
+	     */
+	void            write_checked_reg(unsigned reg, uint8_t value);
+
+	/**
+	     * Set the BMI088_accel measurement range.
+	     *
+	     * @param max_g     The maximum G value the range must support.
+	     * @return      OK if the value can be supported, -EINVAL otherwise.
+	     */
+	int         set_accel_range(unsigned max_g);
+
+	/**
+	     * Set accel sample rate
+	     */
+	int accel_set_sample_rate(float desired_sample_rate_hz);
+
+	/*
+	     * check that key registers still have the right value
+	     */
+	void check_registers(void);
+
+	/* do not allow to copy this class due to pointer data members */
+	BMI088_accel(const BMI088_accel &);
+	BMI088_accel operator=(const BMI088_accel &);
+
+};

src/drivers/imu/bmi088/BMI088_gyro.cpp

@@ -0,0 +1,506 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 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 "BMI088_gyro.hpp"
+#include "BMI088_accel.hpp"
+
+
+/*
+ * Global variable of the accelerometer temperature reading, to read it in the bmi055_gyro driver.
+ * This is a ugly HACK! The driver should potentially be rewritten with the gyro as subdriver.
+ */
+__EXPORT float _accel_last_temperature_copy = 0;
+
+/*
+  list of registers that will be checked in check_registers(). Note
+  that ADDR_WHO_AM_I must be first in the list.
+ */
+
+const uint8_t BMI088_gyro::_checked_registers[BMI088_GYRO_NUM_CHECKED_REGISTERS] = {    BMI088_GYR_CHIP_ID,
+											BMI088_GYR_LPM1,
+											BMI088_GYR_BW,
+											BMI088_GYR_RANGE,
+											BMI088_GYR_INT_EN_0,
+											BMI088_GYR_INT_EN_1,
+											BMI088_GYR_INT_MAP_1
+										   };
+
+BMI088_gyro::BMI088_gyro(int bus, const char *path_gyro, uint32_t device, enum Rotation rotation) :
+	BMI088("BMI088_GYRO", path_gyro, bus, device, SPIDEV_MODE3, BMI088_BUS_SPEED, rotation),
+	ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())),
+	_px4_gyro(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation),
+	_sample_perf(perf_alloc(PC_ELAPSED, "bmi088_gyro_read")),
+	_measure_interval(perf_alloc(PC_INTERVAL, "bmi088_gyro_measure_interval")),
+	_bad_transfers(perf_alloc(PC_COUNT, "bmi088_gyro_bad_transfers")),
+	_bad_registers(perf_alloc(PC_COUNT, "bmi088_gyro_bad_registers"))
+{
+	_px4_gyro.set_device_type(DRV_DEVTYPE_BMI088);
+}
+
+BMI088_gyro::~BMI088_gyro()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* delete the perf counter */
+	perf_free(_sample_perf);
+	perf_free(_measure_interval);
+	perf_free(_bad_transfers);
+	perf_free(_bad_registers);
+}
+
+int
+BMI088_gyro::init()
+{
+	/* do SPI init (and probe) first */
+	int ret = SPI::init();
+
+	/* if probe/setup failed, bail now */
+	if (ret != OK) {
+		DEVICE_DEBUG("SPI setup failed");
+		return ret;
+	}
+
+	return reset();
+}
+
+int BMI088_gyro::reset()
+{
+	write_reg(BMI088_GYR_SOFTRESET, BMI088_SOFT_RESET);//Soft-reset
+	usleep(5000);
+	write_checked_reg(BMI088_GYR_BW,     0); // Write Gyro Bandwidth (will be overwritten in gyro_set_sample_rate())
+	write_checked_reg(BMI088_GYR_RANGE,     0);// Write Gyro range
+	write_checked_reg(BMI088_GYR_INT_EN_0,  BMI088_GYR_DRDY_INT_EN); //Enable DRDY interrupt
+	write_checked_reg(BMI088_GYR_INT_MAP_1, BMI088_GYR_DRDY_INT1); //Map DRDY interrupt on pin INT1
+
+	set_gyro_range(BMI088_GYRO_DEFAULT_RANGE_DPS);// set Gyro range
+	gyro_set_sample_rate(BMI088_GYRO_DEFAULT_RATE);// set Gyro ODR & Filter Bandwidth
+
+	//Enable Gyroscope in normal mode
+	write_reg(BMI088_GYR_LPM1, BMI088_GYRO_NORMAL);
+	up_udelay(1000);
+
+	uint8_t retries = 10;
+
+	while (retries--) {
+		bool all_ok = true;
+
+		for (uint8_t i = 0; i < BMI088_GYRO_NUM_CHECKED_REGISTERS; i++) {
+			if (read_reg(_checked_registers[i]) != _checked_values[i]) {
+				write_reg(_checked_registers[i], _checked_values[i]);
+				all_ok = false;
+			}
+		}
+
+		if (all_ok) {
+			break;
+		}
+	}
+
+	return OK;
+}
+
+int
+BMI088_gyro::probe()
+{
+	/* look for device ID */
+	_whoami = read_reg(BMI088_GYR_CHIP_ID);
+
+	// verify product revision
+	switch (_whoami) {
+	case BMI088_GYR_WHO_AM_I:
+		memset(_checked_values, 0, sizeof(_checked_values));
+		memset(_checked_bad, 0, sizeof(_checked_bad));
+		_checked_values[0] = _whoami;
+		_checked_bad[0] = _whoami;
+		return OK;
+	}
+
+	printf("unexpected whoami 0x%02x\n", _whoami);
+	DEVICE_DEBUG("unexpected whoami 0x%02x", _whoami);
+	return -EIO;
+}
+
+int
+BMI088_gyro::gyro_set_sample_rate(float frequency)
+{
+	uint8_t setbits = 0;
+	uint8_t clearbits = BMI088_GYRO_BW_MASK;
+
+	if (frequency <= 100) {
+		setbits |= BMI088_GYRO_RATE_100; /* 32 Hz cutoff */
+		//_gyro_sample_rate = 100;
+
+	} else if (frequency <= 250) {
+		setbits |= BMI088_GYRO_RATE_400; /* 47 Hz cutoff */
+		//_gyro_sample_rate = 400;
+
+	} else if (frequency <= 1000) {
+		setbits |= BMI088_GYRO_RATE_1000; /* 116 Hz cutoff */
+		//_gyro_sample_rate = 1000;
+
+	} else if (frequency > 1000) {
+		setbits |= BMI088_GYRO_RATE_2000; /* 230 Hz cutoff */
+		//_gyro_sample_rate = 2000;
+
+	} else {
+		return -EINVAL;
+	}
+
+	modify_reg(BMI088_GYR_BW, clearbits, setbits);
+
+	return OK;
+}
+
+/*
+  deliberately trigger an error in the sensor to trigger recovery
+ */
+void
+BMI088_gyro::test_error()
+{
+	write_reg(BMI088_GYR_SOFTRESET, BMI088_SOFT_RESET);
+	::printf("error triggered\n");
+	print_registers();
+}
+
+void
+BMI088_gyro::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
+{
+	uint8_t val = read_reg(reg);
+	val &= ~clearbits;
+	val |= setbits;
+	write_checked_reg(reg, val);
+}
+
+void
+BMI088_gyro::write_checked_reg(unsigned reg, uint8_t value)
+{
+	write_reg(reg, value);
+
+	for (uint8_t i = 0; i < BMI088_GYRO_NUM_CHECKED_REGISTERS; i++) {
+		if (reg == _checked_registers[i]) {
+			_checked_values[i] = value;
+			_checked_bad[i] = value;
+		}
+	}
+}
+
+int
+BMI088_gyro::set_gyro_range(unsigned max_dps)
+{
+	uint8_t setbits = 0;
+	uint8_t clearbits = BMI088_GYRO_RANGE_125_DPS | BMI088_GYRO_RANGE_250_DPS;
+	float lsb_per_dps;
+
+	if (max_dps == 0) {
+		max_dps = 2000;
+	}
+
+	if (max_dps <= 125) {
+		//max_gyro_dps = 125;
+		lsb_per_dps = 262.4;
+		setbits |= BMI088_GYRO_RANGE_125_DPS;
+
+	} else if (max_dps <= 250) {
+		//max_gyro_dps = 250;
+		lsb_per_dps = 131.2;
+		setbits |= BMI088_GYRO_RANGE_250_DPS;
+
+	} else if (max_dps <= 500) {
+		//max_gyro_dps = 500;
+		lsb_per_dps = 65.6;
+		setbits |= BMI088_GYRO_RANGE_500_DPS;
+
+	} else if (max_dps <= 1000) {
+		//max_gyro_dps = 1000;
+		lsb_per_dps = 32.8;
+		setbits |= BMI088_GYRO_RANGE_1000_DPS;
+
+	} else if (max_dps <= 2000) {
+		//max_gyro_dps = 2000;
+		lsb_per_dps = 16.4;
+		setbits |= BMI088_GYRO_RANGE_2000_DPS;
+
+	} else {
+		return -EINVAL;
+	}
+
+	_px4_gyro.set_scale(M_PI_F / (180.0f * lsb_per_dps));
+
+	modify_reg(BMI088_GYR_RANGE, clearbits, setbits);
+
+	return OK;
+}
+
+void
+BMI088_gyro::start()
+{
+	/* make sure we are stopped first */
+	stop();
+
+	/* start polling at the specified rate */
+	ScheduleOnInterval(BMI088_GYRO_DEFAULT_RATE - BMI088_TIMER_REDUCTION, 1000);
+}
+
+void
+BMI088_gyro::stop()
+{
+	ScheduleClear();
+}
+
+void
+BMI088_gyro::Run()
+{
+	/* make another measurement */
+	measure();
+}
+
+void
+BMI088_gyro::measure_trampoline(void *arg)
+{
+	BMI088_gyro *dev = reinterpret_cast<BMI088_gyro *>(arg);
+
+	/* make another measurement */
+	dev->measure();
+}
+
+void
+BMI088_gyro::check_registers(void)
+{
+	uint8_t v;
+
+	if ((v = read_reg(_checked_registers[_checked_next])) !=
+	    _checked_values[_checked_next]) {
+		_checked_bad[_checked_next] = v;
+
+		/*
+		          if we get the wrong value then we know the SPI bus
+		          or sensor is very sick. We set _register_wait to 20
+		          and wait until we have seen 20 good values in a row
+		          before we consider the sensor to be OK again.
+		*/
+		perf_count(_bad_registers);
+
+		/*
+		          try to fix the bad register value. We only try to
+		          fix one per loop to prevent a bad sensor hogging the
+		          bus.
+		*/
+		if (_register_wait == 0 || _checked_next == 0) {
+			// if the product_id is wrong then reset the
+			// sensor completely
+			write_reg(BMI088_GYR_SOFTRESET, BMI088_SOFT_RESET);
+			_reset_wait = hrt_absolute_time() + 10000;
+			_checked_next = 0;
+
+		} else {
+			write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]);
+			// waiting 3ms between register writes seems
+			// to raise the chance of the sensor
+			// recovering considerably
+			_reset_wait = hrt_absolute_time() + 3000;
+		}
+
+		_register_wait = 20;
+	}
+
+	_checked_next = (_checked_next + 1) % BMI088_GYRO_NUM_CHECKED_REGISTERS;
+}
+
+void
+BMI088_gyro::measure()
+{
+	perf_count(_measure_interval);
+
+	if (hrt_absolute_time() < _reset_wait) {
+		// we're waiting for a reset to complete
+		return;
+	}
+
+	struct BMI_GyroReport bmi_gyroreport;
+
+	struct Report {
+		int16_t     gyro_x;
+		int16_t     gyro_y;
+		int16_t     gyro_z;
+	} report;
+
+	/* start measuring */
+	perf_begin(_sample_perf);
+
+	/*
+	* Fetch the full set of measurements from the BMI088 gyro in one pass.
+	*/
+	bmi_gyroreport.cmd = BMI088_GYR_X_L | DIR_READ;
+
+	const hrt_abstime timestamp_sample = hrt_absolute_time();
+
+	if (OK != transfer((uint8_t *)&bmi_gyroreport, ((uint8_t *)&bmi_gyroreport), sizeof(bmi_gyroreport))) {
+		return;
+	}
+
+	check_registers();
+
+
+	// Get the last temperature from the accelerometer (the Gyro does not have its own temperature measurement)
+	_last_temperature = _accel_last_temperature_copy;
+
+	report.gyro_x = bmi_gyroreport.gyro_x;
+	report.gyro_y = bmi_gyroreport.gyro_y;
+	report.gyro_z = bmi_gyroreport.gyro_z;
+
+	if (report.gyro_x == 0 &&
+	    report.gyro_y == 0 &&
+	    report.gyro_z == 0) {
+		// all zero data - probably an SPI bus error
+		perf_count(_bad_transfers);
+		perf_end(_sample_perf);
+		// note that we don't call reset() here as a reset()
+		// costs 20ms with interrupts disabled. That means if
+		// the bmi088 does go bad it would cause a FMU failure,
+		// regardless of whether another sensor is available,
+		return;
+	}
+
+	if (_register_wait != 0) {
+		// we are waiting for some good transfers before using
+		// the sensor again, but don't return any data yet
+		_register_wait--;
+		return;
+	}
+
+	// report the error count as the sum of the number of bad
+	// transfers and bad register reads. This allows the higher
+	// level code to decide if it should use this sensor based on
+	// whether it has had failures
+	const uint64_t error_count  = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);
+	_px4_gyro.set_error_count(error_count);
+
+	// Get the temperature from the accelerometer part of the BMI088, because the gyro part does not have a temperature register
+	_px4_gyro.set_temperature(_accel_last_temperature_copy);
+
+	/*
+	* 1) Scale raw value to SI units using scaling from datasheet.
+	* 2) Subtract static offset (in SI units)
+	* 3) Scale the statically calibrated values with a linear
+	*    dynamically obtained factor
+	*
+	* Note: the static sensor offset is the number the sensor outputs
+	*   at a nominally 'zero' input. Therefore the offset has to
+	*   be subtracted.
+	*
+	*   Example: A gyro outputs a value of 74 at zero angular rate
+	*        the offset is 74 from the origin and subtracting
+	*        74 from all measurements centers them around zero.
+	*/
+	_px4_gyro.update(timestamp_sample, report.gyro_x, report.gyro_y, report.gyro_z);
+
+	/* stop measuring */
+	perf_end(_sample_perf);
+}
+
+void
+BMI088_gyro::print_info()
+{
+	PX4_INFO("Gyro");
+
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_measure_interval);
+	perf_print_counter(_bad_transfers);
+	perf_print_counter(_bad_registers);
+
+	::printf("checked_next: %u\n", _checked_next);
+
+	for (uint8_t i = 0; i < BMI088_GYRO_NUM_CHECKED_REGISTERS; i++) {
+		uint8_t v = read_reg(_checked_registers[i]);
+
+		if (v != _checked_values[i]) {
+			::printf("reg %02x:%02x should be %02x\n",
+				 (unsigned)_checked_registers[i],
+				 (unsigned)v,
+				 (unsigned)_checked_values[i]);
+		}
+
+		if (v != _checked_bad[i]) {
+			::printf("reg %02x:%02x was bad %02x\n",
+				 (unsigned)_checked_registers[i],
+				 (unsigned)v,
+				 (unsigned)_checked_bad[i]);
+		}
+	}
+
+	_px4_gyro.print_status();
+}
+
+void
+BMI088_gyro::print_registers()
+{
+	uint8_t index = 0;
+	printf("BMI088 gyro registers\n");
+
+	uint8_t reg = _checked_registers[index++];
+	uint8_t v = read_reg(reg);
+	printf("Gyro Chip Id: %02x:%02x ", (unsigned)reg, (unsigned)v);
+	printf("\n");
+
+	reg = _checked_registers[index++];
+	v = read_reg(reg);
+	printf("Gyro Power: %02x:%02x ", (unsigned)reg, (unsigned)v);
+	printf("\n");
+
+	reg = _checked_registers[index++];
+	v = read_reg(reg);
+	printf("Gyro Bw: %02x:%02x ", (unsigned)reg, (unsigned)v);
+	printf("\n");
+
+	reg = _checked_registers[index++];
+	v = read_reg(reg);
+	printf("Gyro Range: %02x:%02x ", (unsigned)reg, (unsigned)v);
+	printf("\n");
+
+	reg = _checked_registers[index++];
+	v = read_reg(reg);
+	printf("Gyro Int-en-0: %02x:%02x ", (unsigned)reg, (unsigned)v);
+	printf("\n");
+
+	reg = _checked_registers[index++];
+	v = read_reg(reg);
+	printf("Gyro Int-en-1: %02x:%02x ", (unsigned)reg, (unsigned)v);
+	printf("\n");
+
+	reg = _checked_registers[index++];
+	v = read_reg(reg);
+	printf("Gyro Int-Map-1: %02x:%02x ", (unsigned)reg, (unsigned)v);
+
+	printf("\n");
+}

src/drivers/imu/bmi088/BMI088_gyro.hpp

@@ -0,0 +1,261 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 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
+
+#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
+
+#include "BMI088.hpp"
+
+#define BMI088_DEVICE_PATH_GYRO		"/dev/bmi088_gyro"
+#define BMI088_DEVICE_PATH_GYRO_EXT	"/dev/bmi088_gyro_ext"
+
+// BMI088 Gyro registers
+#define BMI088_GYR_CHIP_ID          0x00
+#define BMI088_GYR_X_L              0x02
+#define BMI088_GYR_X_H              0x03
+#define BMI088_GYR_Y_L              0x04
+#define BMI088_GYR_Y_H              0x05
+#define BMI088_GYR_Z_L              0x06
+#define BMI088_GYR_Z_H              0x07
+#define BMI088_GYR_INT_STATUS_0     0x09
+#define BMI088_GYR_INT_STATUS_1     0x0A
+#define BMI088_GYR_INT_STATUS_2     0x0B
+#define BMI088_GYR_INT_STATUS_3     0x0C
+#define BMI088_GYR_FIFO_STATUS      0x0E
+#define BMI088_GYR_RANGE            0x0F
+#define BMI088_GYR_BW               0x10
+#define BMI088_GYR_LPM1             0x11
+#define BMI088_GYR_LPM2             0x12
+#define BMI088_GYR_RATE_HBW         0x13
+#define BMI088_GYR_SOFTRESET        0x14
+#define BMI088_GYR_INT_EN_0         0x15
+#define BMI088_GYR_INT_EN_1         0x16
+#define BMI088_GYR_INT_MAP_0        0x17
+#define BMI088_GYR_INT_MAP_1        0x18
+#define BMI088_GYR_INT_MAP_2        0x19
+#define BMI088_GYRO_0_REG           0x1A
+#define BMI088_GYRO_1_REG           0x1B
+#define BMI088_GYRO_2_REG           0x1C
+#define BMI088_GYRO_3_REG           0x1E
+#define BMI088_GYR_INT_LATCH        0x21
+#define BMI088_GYR_INT_LH_0         0x22
+#define BMI088_GYR_INT_LH_1         0x23
+#define BMI088_GYR_INT_LH_2         0x24
+#define BMI088_GYR_INT_LH_3         0x25
+#define BMI088_GYR_INT_LH_4         0x26
+#define BMI088_GYR_INT_LH_5         0x27
+#define BMI088_GYR_SOC              0x31
+#define BMI088_GYR_A_FOC            0x32
+#define BMI088_GYR_TRIM_NVM_CTRL    0x33
+#define BMI088_BGW_SPI3_WDT         0x34
+#define BMI088_GYR_OFFSET_COMP      0x36
+#define BMI088_GYR_OFFSET_COMP_X    0x37
+#define BMI088_GYR_OFFSET_COMP_Y    0x38
+#define BMI088_GYR_OFFSET_COMP_Z    0x39
+#define BMI088_GYR_TRIM_GPO         0x3A
+#define BMI088_GYR_TRIM_GP1         0x3B
+#define BMI088_GYR_SELF_TEST        0x3C
+#define BMI088_GYR_FIFO_CONFIG_0    0x3D
+#define BMI088_GYR_FIFO_CONFIG_1    0x3E
+#define BMI088_GYR_FIFO_DATA        0x3F
+
+// BMI088 Gyroscope Chip-Id
+#define BMI088_GYR_WHO_AM_I         0x0F
+
+//ODR & DLPF filter bandwidth settings (they are coupled)
+#define BMI088_GYRO_RATE_100        (0<<3) | (1<<2) | (1<<1) | (1<<0)
+#define BMI088_GYRO_RATE_200        (0<<3) | (1<<2) | (1<<1) | (0<<0)
+#define BMI088_GYRO_RATE_400        (0<<3) | (0<<2) | (1<<1) | (1<<0)
+#define BMI088_GYRO_RATE_1000       (0<<3) | (0<<2) | (1<<1) | (0<<0)
+#define BMI088_GYRO_RATE_2000       (0<<3) | (0<<2) | (0<<1) | (1<<0)
+
+//BMI088_GYR_LPM1      0x11
+#define BMI088_GYRO_NORMAL          (0<<7) | (0<<5)
+#define BMI088_GYRO_DEEP_SUSPEND    (0<<7) | (1<<5)
+#define BMI088_GYRO_SUSPEND         (1<<7) | (0<<5)
+
+//BMI088_GYR_RANGE        0x0F
+#define BMI088_GYRO_RANGE_2000_DPS  (0<<2) | (0<<1) | (0<<0)
+#define BMI088_GYRO_RANGE_1000_DPS  (0<<2) | (0<<1) | (1<<0)
+#define BMI088_GYRO_RANGE_500_DPS   (0<<2) | (1<<1) | (0<<0)
+#define BMI088_GYRO_RANGE_250_DPS   (0<<2) | (1<<1) | (1<<0)
+#define BMI088_GYRO_RANGE_125_DPS   (1<<2) | (0<<1) | (0<<0)
+
+//BMI088_GYR_INT_EN_0         0x15
+#define BMI088_GYR_DRDY_INT_EN      (1<<7)
+
+//BMI088_GYR_INT_MAP_1     0x18
+#define BMI088_GYR_DRDY_INT1        (1<<0)
+
+// Default and Max values
+#define BMI088_GYRO_DEFAULT_RANGE_DPS		2000
+#define BMI088_GYRO_DEFAULT_RATE            1000
+#define BMI088_GYRO_MAX_RATE                1000
+#define BMI088_GYRO_MAX_PUBLISH_RATE        280
+
+#define BMI088_GYRO_DEFAULT_DRIVER_FILTER_FREQ  50
+
+/* Mask definitions for Gyro bandwidth */
+#define BMI088_GYRO_BW_MASK                  0x0F
+
+class BMI088_gyro : public BMI088, public px4::ScheduledWorkItem
+{
+public:
+	BMI088_gyro(int bus, const char *path_gyro, uint32_t device, enum Rotation rotation);
+	virtual ~BMI088_gyro();
+
+	virtual int     init();
+
+	// Start automatic measurement.
+	void            start();
+
+	/**
+	    * Diagnostics - print some basic information about the driver.
+	    */
+	void            print_info();
+
+	void            print_registers();
+
+	// deliberately cause a sensor error
+	void            test_error();
+
+protected:
+
+	virtual int     probe();
+
+private:
+
+	PX4Gyroscope	_px4_gyro;
+
+	perf_counter_t      _sample_perf;
+	perf_counter_t      _measure_interval;
+	perf_counter_t      _bad_transfers;
+	perf_counter_t      _bad_registers;
+
+	// this is used to support runtime checking of key
+	// configuration registers to detect SPI bus errors and sensor
+	// reset
+#define BMI088_GYRO_NUM_CHECKED_REGISTERS 7
+	static const uint8_t    _checked_registers[BMI088_GYRO_NUM_CHECKED_REGISTERS];
+	uint8_t         _checked_values[BMI088_GYRO_NUM_CHECKED_REGISTERS];
+	uint8_t         _checked_bad[BMI088_GYRO_NUM_CHECKED_REGISTERS];
+
+	// last temperature reading for print_info()
+	float           _last_temperature;
+
+	/**
+	     * Stop automatic measurement.
+	     */
+	void            stop();
+
+	/**
+	     * Reset chip.
+	     *
+	     * Resets the chip and measurements ranges, but not scale and offset.
+	     */
+	int         reset();
+
+	void     Run() override;
+
+	/**
+	     * Static trampoline from the hrt_call context; because we don't have a
+	     * generic hrt wrapper yet.
+	     *
+	     * Called by the HRT in interrupt context at the specified rate if
+	     * automatic polling is enabled.
+	     *
+	     * @param arg       Instance pointer for the driver that is polling.
+	     */
+	static void     measure_trampoline(void *arg);
+
+	/**
+	     * Fetch measurements from the sensor and update the report buffers.
+	     */
+	void            measure();
+
+	/**
+	     * Modify a register in the BMI088_gyro
+	     *
+	     * Bits are cleared before bits are set.
+	     *
+	     * @param reg       The register to modify.
+	     * @param clearbits Bits in the register to clear.
+	     * @param setbits   Bits in the register to set.
+	     */
+	void            modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
+
+	/**
+	     * Write a register in the BMI088_gyro, updating _checked_values
+	     *
+	     * @param reg       The register to write.
+	     * @param value     The new value to write.
+	     */
+	void            write_checked_reg(unsigned reg, uint8_t value);
+
+	/**
+	     * Set the BMI088_gyro measurement range.
+	     *
+	     * @param max_dps   The maximum DPS value the range must support.
+	     * @return      OK if the value can be supported, -EINVAL otherwise.
+	     */
+	int         set_gyro_range(unsigned max_dps);
+
+	/*
+	     * set gyro sample rate
+	     */
+	int gyro_set_sample_rate(float desired_sample_rate_hz);
+
+	/*
+	     * check that key registers still have the right value
+	     */
+	void check_registers(void);
+
+	/* do not allow to copy this class due to pointer data members */
+	BMI088_gyro(const BMI088_gyro &);
+	BMI088_gyro operator=(const BMI088_gyro &);
+
+#pragma pack(push, 1)
+	/**
+	     * Report conversation within the BMI088_gyro, including command byte and
+	     * interrupt status.
+	     */
+	struct BMI_GyroReport {
+		uint8_t     cmd;
+		int16_t     gyro_x;
+		int16_t     gyro_y;
+		int16_t     gyro_z;
+	};
+#pragma pack(pop)
+
+};

src/drivers/imu/bmi088/CMakeLists.txt

@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__bmi88
+	MAIN bmi088
+	STACK_MAIN 1500
+	COMPILE_FLAGS
+		-Wno-cast-align # TODO: fix and enable
+	SRCS		
+		BMI088_accel.cpp
+		BMI088_gyro.cpp
+		bmi088_main.cpp
+	)

src/drivers/imu/bmi088/bmi088_main.cpp

@@ -0,0 +1,413 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 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 "BMI088_accel.hpp"
+#include "BMI088_gyro.hpp"
+
+/** driver 'main' command */
+extern "C" { __EXPORT int bmi088_main(int argc, char *argv[]); }
+
+enum sensor_type {
+	BMI088_NONE = 0,
+	BMI088_ACCEL = 1,
+	BMI088_GYRO
+};
+
+namespace bmi088
+{
+
+BMI088_accel    *g_acc_dev_int; // on internal bus (accel)
+BMI088_accel    *g_acc_dev_ext; // on external bus (accel)
+BMI088_gyro     *g_gyr_dev_int; // on internal bus (gyro)
+BMI088_gyro     *g_gyr_dev_ext; // on external bus (gyro)
+
+void    start(bool, enum Rotation, enum sensor_type);
+void    stop(bool, enum sensor_type);
+void    info(bool, enum sensor_type);
+void    regdump(bool, enum sensor_type);
+void    testerror(bool, enum sensor_type);
+void    usage();
+
+/**
+ * Start the driver.
+ *
+ * This function only returns if the driver is up and running
+ * or failed to detect the sensor.
+ */
+void
+start(bool external_bus, enum Rotation rotation, enum sensor_type sensor)
+{
+
+	BMI088_accel **g_dev_acc_ptr = external_bus ? &g_acc_dev_ext : &g_acc_dev_int;
+	const char *path_accel = external_bus ? BMI088_DEVICE_PATH_ACCEL_EXT : BMI088_DEVICE_PATH_ACCEL;
+
+	BMI088_gyro **g_dev_gyr_ptr = external_bus ? &g_gyr_dev_ext : &g_gyr_dev_int;
+	const char *path_gyro  = external_bus ? BMI088_DEVICE_PATH_GYRO_EXT : BMI088_DEVICE_PATH_GYRO;
+
+	if (sensor == BMI088_ACCEL) {
+		if (*g_dev_acc_ptr != nullptr)
+			/* if already started, the still command succeeded */
+		{
+			errx(0, "bmi088 accel sensor already started");
+		}
+
+		/* create the driver */
+		if (external_bus) {
+#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_BMI)
+			*g_dev_acc_ptr = new BMI088_accel(PX4_SPI_BUS_EXT, path_accel, PX4_SPIDEV_EXT_BMI, rotation);
+#else
+			errx(0, "External SPI not available");
+#endif
+
+		} else {
+			*g_dev_acc_ptr = new BMI088_accel(PX4_SPI_BUS_SENSORS3, path_accel, PX4_SPIDEV_BMI088_ACC, rotation);
+		}
+
+		if (*g_dev_acc_ptr == nullptr) {
+			goto fail_accel;
+		}
+
+		if (OK != (*g_dev_acc_ptr)->init()) {
+			goto fail_accel;
+		}
+
+		// start automatic data collection
+		(*g_dev_acc_ptr)->start();
+	}
+
+	if (sensor == BMI088_GYRO) {
+
+		if (*g_dev_gyr_ptr != nullptr) {
+			errx(0, "bmi088 gyro sensor already started");
+		}
+
+		/* create the driver */
+		if (external_bus) {
+#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_BMI)
+			*g_dev_ptr = new BMI088_gyro(PX4_SPI_BUS_EXT, path_gyro, PX4_SPIDEV_EXT_BMI, rotation);
+#else
+			errx(0, "External SPI not available");
+#endif
+
+		} else {
+			*g_dev_gyr_ptr = new BMI088_gyro(PX4_SPI_BUS_SENSORS3, path_gyro, PX4_SPIDEV_BMI088_GYR, rotation);
+		}
+
+		if (*g_dev_gyr_ptr == nullptr) {
+			goto fail_gyro;
+		}
+
+		if (OK != (*g_dev_gyr_ptr)->init()) {
+			goto fail_gyro;
+		}
+
+		// start automatic data collection
+		(*g_dev_gyr_ptr)->start();
+	}
+
+	exit(PX4_OK);
+
+fail_accel:
+
+	if (*g_dev_acc_ptr != nullptr) {
+		delete (*g_dev_acc_ptr);
+		*g_dev_acc_ptr = nullptr;
+	}
+
+	PX4_WARN("No BMI088 accel found");
+	exit(PX4_ERROR);
+
+fail_gyro:
+
+	if (*g_dev_gyr_ptr != nullptr) {
+		delete (*g_dev_gyr_ptr);
+		*g_dev_gyr_ptr = nullptr;
+	}
+
+	PX4_WARN("No BMI088 gyro found");
+	exit(PX4_ERROR);
+}
+
+void
+stop(bool external_bus, enum sensor_type sensor)
+{
+	BMI088_accel **g_dev_acc_ptr = external_bus ? &g_acc_dev_ext : &g_acc_dev_int;
+	BMI088_gyro **g_dev_gyr_ptr = external_bus ? &g_gyr_dev_ext : &g_gyr_dev_int;
+
+	if (sensor == BMI088_ACCEL) {
+		if (*g_dev_acc_ptr != nullptr) {
+			delete *g_dev_acc_ptr;
+			*g_dev_acc_ptr = nullptr;
+
+		} else {
+			/* warn, but not an error */
+			warnx("bmi088 accel sensor already stopped.");
+		}
+	}
+
+	if (sensor == BMI088_GYRO) {
+		if (*g_dev_gyr_ptr != nullptr) {
+			delete *g_dev_gyr_ptr;
+			*g_dev_gyr_ptr = nullptr;
+
+		} else {
+			/* warn, but not an error */
+			warnx("bmi088 gyro sensor already stopped.");
+		}
+	}
+
+	exit(0);
+
+}
+
+/**
+ * Print a little info about the driver.
+ */
+void
+info(bool external_bus, enum sensor_type sensor)
+{
+	BMI088_accel **g_dev_acc_ptr = external_bus ? &g_acc_dev_ext : &g_acc_dev_int;
+	BMI088_gyro **g_dev_gyr_ptr = external_bus ? &g_gyr_dev_ext : &g_gyr_dev_int;
+
+	if (sensor == BMI088_ACCEL) {
+		if (*g_dev_acc_ptr == nullptr) {
+			errx(1, "bmi088 accel driver not running");
+		}
+
+		printf("state @ %p\n", *g_dev_acc_ptr);
+		(*g_dev_acc_ptr)->print_info();
+	}
+
+	if (sensor == BMI088_GYRO) {
+		if (*g_dev_gyr_ptr == nullptr) {
+			errx(1, "bmi088 gyro driver not running");
+		}
+
+		printf("state @ %p\n", *g_dev_gyr_ptr);
+		(*g_dev_gyr_ptr)->print_info();
+	}
+
+	exit(0);
+}
+
+/**
+ * Dump the register information
+ */
+void
+regdump(bool external_bus, enum sensor_type sensor)
+{
+	BMI088_accel **g_dev_acc_ptr = external_bus ? &g_acc_dev_ext : &g_acc_dev_int;
+	BMI088_gyro **g_dev_gyr_ptr = external_bus ? &g_gyr_dev_ext : &g_gyr_dev_int;
+
+	if (sensor == BMI088_ACCEL) {
+		if (*g_dev_acc_ptr == nullptr) {
+			errx(1, "bmi088 accel driver not running");
+		}
+
+		printf("regdump @ %p\n", *g_dev_acc_ptr);
+		(*g_dev_acc_ptr)->print_registers();
+	}
+
+	if (sensor == BMI088_GYRO) {
+		if (*g_dev_gyr_ptr == nullptr) {
+			errx(1, "bmi088 gyro driver not running");
+		}
+
+		printf("regdump @ %p\n", *g_dev_gyr_ptr);
+		(*g_dev_gyr_ptr)->print_registers();
+	}
+
+	exit(0);
+}
+
+/**
+ * deliberately produce an error to test recovery
+ */
+void
+testerror(bool external_bus, enum sensor_type sensor)
+{
+	BMI088_accel **g_dev_acc_ptr = external_bus ? &g_acc_dev_ext : &g_acc_dev_int;
+	BMI088_gyro **g_dev_gyr_ptr = external_bus ? &g_gyr_dev_ext : &g_gyr_dev_int;
+
+	if (sensor == BMI088_ACCEL) {
+		if (*g_dev_acc_ptr == nullptr) {
+			errx(1, "bmi088 accel driver not running");
+		}
+
+		(*g_dev_acc_ptr)->test_error();
+	}
+
+	if (sensor == BMI088_GYRO) {
+		if (*g_dev_gyr_ptr == nullptr) {
+			errx(1, "bmi088 gyro driver not running");
+		}
+
+		(*g_dev_gyr_ptr)->test_error();
+	}
+
+	exit(0);
+}
+
+void
+usage()
+{
+	warnx("missing command: try 'start', 'info', 'stop', 'regdump', 'testerror'");
+	warnx("options:");
+	warnx("    -X    (external bus)");
+	warnx("    -R    rotation");
+	warnx("    -A    (Enable Accelerometer)");
+	warnx("    -G    (Enable Gyroscope)");
+}
+
+}//namespace ends
+
+
+BMI088::BMI088(const char *name, const char *devname, int bus, uint32_t device, enum spi_mode_e mode,
+	       uint32_t frequency, enum Rotation rotation):
+	SPI(name, devname, bus, device, mode, frequency),
+	_whoami(0),
+	_register_wait(0),
+	_reset_wait(0),
+	_rotation(rotation),
+	_checked_next(0)
+{
+}
+
+uint8_t
+BMI088::read_reg(unsigned reg)
+{
+	uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0};
+
+	transfer(cmd, cmd, sizeof(cmd));
+
+	return cmd[1];
+}
+
+uint16_t
+BMI088::read_reg16(unsigned reg)
+{
+	uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 };
+
+	transfer(cmd, cmd, sizeof(cmd));
+
+	return (uint16_t)(cmd[1] << 8) | cmd[2];
+}
+
+void
+BMI088::write_reg(unsigned reg, uint8_t value)
+{
+	uint8_t cmd[2];
+
+	cmd[0] = reg | DIR_WRITE;
+	cmd[1] = value;
+
+	transfer(cmd, nullptr, sizeof(cmd));
+}
+
+int
+bmi088_main(int argc, char *argv[])
+{
+	bool external_bus = false;
+	int ch;
+	enum Rotation rotation = ROTATION_NONE;
+	enum sensor_type sensor = BMI088_NONE;
+	int myoptind = 1;
+	const char *myoptarg = NULL;
+
+	/* jump over start/off/etc and look at options first */
+	while ((ch = px4_getopt(argc, argv, "XR:AG", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'X':
+			external_bus = true;
+			break;
+
+		case 'R':
+			rotation = (enum Rotation)atoi(myoptarg);
+			break;
+
+		case 'A':
+			sensor = BMI088_ACCEL;
+			break;
+
+		case 'G':
+			sensor = BMI088_GYRO;
+			break;
+
+		default:
+			bmi088::usage();
+			exit(0);
+		}
+	}
+
+	const char *verb = argv[myoptind];
+
+	if (sensor == BMI088_NONE) {
+		bmi088::usage();
+		exit(0);
+	}
+
+	/*
+	* Start/load the driver.
+	*/
+	if (!strcmp(verb, "start")) {
+		bmi088::start(external_bus, rotation, sensor);
+	}
+
+	/*
+	* Stop the driver.
+	*/
+	if (!strcmp(verb, "stop")) {
+		bmi088::stop(external_bus, sensor);
+	}
+
+	/*
+	* Print driver information.
+	*/
+	if (!strcmp(verb, "info")) {
+		bmi088::info(external_bus, sensor);
+	}
+
+	/*
+	* Print register information.
+	*/
+	if (!strcmp(verb, "regdump")) {
+		bmi088::regdump(external_bus, sensor);
+	}
+
+	if (!strcmp(verb, "testerror")) {
+		bmi088::testerror(external_bus, sensor);
+	}
+
+	bmi088::usage();
+	exit(1);
+}