ST LSM9DS1 9DOF IMU driver

- this is a replacement for df_lsm9ds1_wrapper on the Emlid Navio2 once DriverFramework is removed

src/drivers/imu/st/lsm9ds1/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__st__lsm9ds1
+	MAIN lsm9ds1
+	SRCS
+		lsm9ds1_main.cpp
+		LSM9DS1.cpp
+		LSM9DS1.hpp
+		ST_LSM9DS1_Registers.hpp
+	DEPENDS
+		drivers_accelerometer
+		drivers_gyroscope
+		px4_work_queue
+	)

src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp

@@ -0,0 +1,328 @@
+/****************************************************************************
+ *
+ *   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 "LSM9DS1.hpp"
+
+using namespace time_literals;
+using namespace ST_LSM9DS1;
+
+static constexpr int16_t combine(uint8_t lsb, uint8_t msb) { return (msb << 8u) | lsb; }
+
+LSM9DS1::LSM9DS1(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_DEFAULT, rotation),
+	_px4_gyro(get_device_id(), ORB_PRIO_DEFAULT, rotation)
+{
+	set_device_type(DRV_IMU_DEVTYPE_ST_LSM9DS1_AG);
+	_px4_accel.set_device_type(DRV_IMU_DEVTYPE_ST_LSM9DS1_AG);
+	_px4_gyro.set_device_type(DRV_IMU_DEVTYPE_ST_LSM9DS1_AG);
+
+	_px4_accel.set_sample_rate(ST_LSM9DS1::LA_ODR);
+	_px4_gyro.set_sample_rate(ST_LSM9DS1::G_ODR);
+
+	_px4_accel.set_update_rate(1000000 / _fifo_interval);
+	_px4_gyro.set_update_rate(1000000 / _fifo_interval);
+}
+
+LSM9DS1::~LSM9DS1()
+{
+	Stop();
+
+	perf_free(_interval_perf);
+	perf_free(_transfer_perf);
+	perf_free(_fifo_empty_perf);
+	perf_free(_fifo_overflow_perf);
+	perf_free(_fifo_reset_perf);
+}
+
+int LSM9DS1::probe()
+{
+	uint8_t whoami = RegisterRead(Register::WHO_AM_I);
+
+	if (whoami == LSM9DS1_WHO_AM_I) {
+		return PX4_OK;
+	}
+
+	return PX4_ERROR;
+}
+
+bool LSM9DS1::Init()
+{
+	if (SPI::init() != PX4_OK) {
+		PX4_ERR("SPI::init failed");
+		return false;
+	}
+
+	if (!Reset()) {
+		PX4_ERR("reset failed");
+		return false;
+	}
+
+	Start();
+
+	return true;
+}
+
+bool LSM9DS1::Reset()
+{
+	// Reset
+	// CTRL_REG8: SW_RESET
+	RegisterSetBits(Register::CTRL_REG8, CTRL_REG8_BIT::SW_RESET);
+	usleep(50); // Wait 50 μs (or wait until the SW_RESET bit of the CTRL_REG8 register returns to 0).
+
+	RegisterSetBits(Register::CTRL_REG9, CTRL_REG9_BIT::I2C_DISABLE);
+
+	// Gyroscope configuration
+	// CTRL_REG1_G: Gyroscope 2000 degrees/second and ODR 952 Hz
+	RegisterWrite(Register::CTRL_REG1_G,
+		      CTRL_REG1_G_BIT::ODR_G_952HZ | CTRL_REG1_G_BIT::FS_G_2000DPS | CTRL_REG1_G_BIT::BW_G_100Hz);
+	_px4_gyro.set_scale(math::radians(70.0f / 1000.0f)); // 70 mdps/LSB
+	_px4_gyro.set_range(math::radians(2000.0f));
+
+	// Accelerometer configuration
+	// CTRL_REG6_XL: Accelerometer 16 G range and ODR 952 Hz
+	RegisterWrite(Register::CTRL_REG6_XL, CTRL_REG6_XL_BIT::ODR_XL_952HZ | CTRL_REG6_XL_BIT::FS_XL_16);
+	_px4_accel.set_scale(0.732f * (CONSTANTS_ONE_G / 1000.0f));	// 0.732 mg/LSB
+	_px4_accel.set_range(16.0f * CONSTANTS_ONE_G);
+
+	return true;
+}
+
+void LSM9DS1::ResetFIFO()
+{
+	perf_count(_fifo_reset_perf);
+
+	// CTRL_REG9: disable FIFO
+	RegisterClearBits(Register::CTRL_REG9, CTRL_REG9_BIT::FIFO_EN);
+
+	// FIFO_CTRL: to reset FIFO content, Bypass mode (0) should be selected
+	RegisterWrite(Register::FIFO_CTRL, 0);
+
+	// CTRL_REG9: enable FIFO
+	RegisterSetBits(Register::CTRL_REG9, CTRL_REG9_BIT::FIFO_EN);
+
+	// CTRL_REG8: Note: When the FIFO is used, the IF_INC and BDU bits must be equal to 1.
+	RegisterWrite(Register::CTRL_REG8, CTRL_REG8_BIT::BDU | CTRL_REG8_BIT::IF_ADD_INC);
+	usleep(1);
+
+	// FIFO_CTRL: FIFO continuous mode enabled
+	RegisterWrite(Register::FIFO_CTRL, FIFO_CTRL_BIT::FIFO_MODE_CONTINUOUS);
+	usleep(1);
+}
+
+uint8_t LSM9DS1::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 LSM9DS1::RegisterWrite(Register reg, uint8_t value)
+{
+	uint8_t cmd[2] { (uint8_t)reg, value };
+	transfer(cmd, cmd, sizeof(cmd));
+}
+
+void LSM9DS1::RegisterSetBits(Register reg, uint8_t setbits)
+{
+	uint8_t val = RegisterRead(reg);
+
+	if (!(val & setbits)) {
+		val |= setbits;
+		RegisterWrite(reg, val);
+	}
+}
+
+void LSM9DS1::RegisterClearBits(Register reg, uint8_t clearbits)
+{
+	uint8_t val = RegisterRead(reg);
+
+	if (val & clearbits) {
+		val &= !clearbits;
+		RegisterWrite(reg, val);
+	}
+}
+
+void LSM9DS1::Start()
+{
+	Stop();
+
+	ResetFIFO();
+
+	ScheduleOnInterval(_fifo_interval / 2, _fifo_interval);
+}
+
+void LSM9DS1::Stop()
+{
+	ScheduleClear();
+}
+
+void LSM9DS1::Run()
+{
+	perf_count(_interval_perf);
+
+	// Number of unread words (16-bit axes) stored in FIFO.
+	const hrt_abstime timestamp_fifo_level = hrt_absolute_time();
+	const uint8_t FIFO_SRC = RegisterRead(Register::FIFO_SRC);
+
+	if (FIFO_SRC & FIFO_SRC_BIT::OVRN) {
+		// overflow
+		perf_count(_fifo_overflow_perf);
+		ResetFIFO();
+		return;
+	}
+
+	const uint8_t samples = FIFO_SRC & static_cast<uint8_t>(FIFO_SRC_BIT::FSS);
+
+	if (samples < 1) {
+		perf_count(_fifo_empty_perf);
+		return;
+
+	} else if (samples > 32) {
+		// not technically an overflow, but more samples than we expected
+		perf_count(_fifo_overflow_perf);
+		ResetFIFO();
+		return;
+	}
+
+	// estimate timestamp of first sample in the FIFO from number of samples and fill rate
+	const hrt_abstime timestamp_sample = timestamp_fifo_level;
+
+	int16_t accel_data[3] {};
+	int16_t gyro_data[3] {};
+	int accel_samples = 0;
+	int gyro_samples = 0;
+
+	perf_begin(_transfer_perf);
+
+	for (int i = 0; i < samples; i++) {
+		// Gyro
+		{
+			struct GyroReport {
+				uint8_t cmd;
+				uint8_t STATUS_REG;
+				uint8_t OUT_X_L_G;
+				uint8_t OUT_X_H_G;
+				uint8_t OUT_Y_L_G;
+				uint8_t OUT_Y_H_G;
+				uint8_t OUT_Z_L_G;
+				uint8_t OUT_Z_H_G;
+			} greport{};
+			greport.cmd = static_cast<uint8_t>(Register::STATUS_REG_G) | DIR_READ;
+
+			if (transfer((uint8_t *)&greport, (uint8_t *)&greport, sizeof(GyroReport)) != PX4_OK) {
+				perf_end(_transfer_perf);
+				return;
+			}
+
+			if (greport.STATUS_REG & STATUS_REG_BIT::GDA) {
+				// Gyroscope new data available
+
+				// sensor Z is up (RHC), flip z for publication
+				gyro_data[0] = combine(greport.OUT_X_L_G, greport.OUT_X_H_G);
+				gyro_data[1] = combine(greport.OUT_Y_L_G, greport.OUT_Y_H_G);
+				gyro_data[2] = -combine(greport.OUT_Z_L_G, greport.OUT_Z_H_G);
+				gyro_samples++;
+			}
+		}
+
+		// Accel
+		{
+			struct AccelReport {
+				uint8_t cmd;
+				uint8_t STATUS_REG;
+				uint8_t OUT_X_L_XL;
+				uint8_t OUT_X_H_XL;
+				uint8_t OUT_Y_L_XL;
+				uint8_t OUT_Y_H_XL;
+				uint8_t OUT_Z_L_XL;
+				uint8_t OUT_Z_H_XL;
+			} areport{};
+			areport.cmd = static_cast<uint8_t>(Register::STATUS_REG_A) | DIR_READ;
+
+			if (transfer((uint8_t *)&areport, (uint8_t *)&areport, sizeof(AccelReport)) != PX4_OK) {
+				perf_end(_transfer_perf);
+				return;
+			}
+
+			if (areport.STATUS_REG & STATUS_REG_BIT::XLDA) {
+				// Accelerometer new data available
+
+				// sensor Z is up (RHC), flip z for publication
+				accel_data[0] = combine(areport.OUT_X_L_XL, areport.OUT_X_H_XL);
+				accel_data[1] = combine(areport.OUT_Y_L_XL, areport.OUT_Y_H_XL);
+				accel_data[2] = -combine(areport.OUT_Z_L_XL, areport.OUT_Z_H_XL);
+				accel_samples++;
+			}
+		}
+	}
+
+	perf_end(_transfer_perf);
+
+	// get current temperature at 1 Hz
+	if (hrt_elapsed_time(&_time_last_temperature_update) > 1_s) {
+		uint8_t temperature_buf[3] {};
+		temperature_buf[0] = static_cast<uint8_t>(Register::OUT_TEMP_L) | DIR_READ;
+
+		if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) {
+			return;
+		}
+
+		// 16 bits in two’s complement format with a sensitivity of 256 LSB/°C. The output zero level corresponds to 25 °C.
+		const int16_t OUT_TEMP = combine(temperature_buf[1], temperature_buf[2] & 0x0F);
+		const float temperature = (OUT_TEMP / 256.0f) + 25.0f;
+
+		_px4_accel.set_temperature(temperature);
+		_px4_gyro.set_temperature(temperature);
+	}
+
+	if ((accel_samples > 0) && (gyro_samples > 0)) {
+		// published synchronized IMU
+		_px4_accel.update(timestamp_sample, accel_data[0], accel_data[1], accel_data[2]);
+		_px4_gyro.update(timestamp_sample, gyro_data[0], gyro_data[1], gyro_data[2]);
+	}
+}
+
+void LSM9DS1::PrintInfo()
+{
+	perf_print_counter(_interval_perf);
+	perf_print_counter(_transfer_perf);
+	perf_print_counter(_fifo_empty_perf);
+	perf_print_counter(_fifo_overflow_perf);
+	perf_print_counter(_fifo_reset_perf);
+
+	_px4_accel.print_status();
+	_px4_gyro.print_status();
+}

src/drivers/imu/st/lsm9ds1/LSM9DS1.hpp

@@ -0,0 +1,91 @@
+/****************************************************************************
+ *
+ *   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 LSM9DS1.hpp
+ *
+ * Driver for the ST LSM9DS1 connected via SPI.
+ *
+ */
+
+#pragma once
+
+#include "ST_LSM9DS1_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_config.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+
+using ST_LSM9DS1::Register;
+
+class LSM9DS1 : public device::SPI, public px4::ScheduledWorkItem
+{
+public:
+	LSM9DS1(int bus, uint32_t device, enum Rotation rotation = ROTATION_NONE);
+	~LSM9DS1() override;
+
+	bool     Init();
+	void     Start();
+	void     Stop();
+	bool     Reset();
+	void     PrintInfo();
+
+private:
+	int      probe() override;
+	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();
+
+	PX4Accelerometer _px4_accel;
+	PX4Gyroscope     _px4_gyro;
+
+	static constexpr uint32_t _fifo_interval{1000000 / ST_LSM9DS1::LA_ODR};
+
+	perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": run interval")};
+	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")};
+
+	hrt_abstime _time_last_temperature_update{0};
+};

src/drivers/imu/st/lsm9ds1/ST_LSM9DS1_Registers.hpp

@@ -0,0 +1,156 @@
+/****************************************************************************
+ *
+ *   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 ST_LSM9DS1_registers.hpp
+ *
+ * ST LSM9DS1 registers.
+ *
+ */
+
+#pragma once
+
+#include <cstdint>
+
+// 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 ST_LSM9DS1
+{
+
+static constexpr uint8_t DIR_READ = 0x80;
+
+static constexpr uint8_t LSM9DS1_WHO_AM_I = 0b01101000; // Who I am ID
+
+static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10 MHz SPI clock frequency
+
+static constexpr uint32_t LA_ODR = 952; // Linear acceleration output data rate
+static constexpr uint32_t G_ODR = 952;  // Angular rate output data rate
+
+enum class
+Register : uint8_t {
+	WHO_AM_I        = 0x0F,
+
+	CTRL_REG1_G     = 0x10,	// Angular rate sensor Control Register 1.
+
+	OUT_TEMP_L      = 0x15,
+	OUT_TEMP_H      = 0x16,
+	STATUS_REG_G    = 0x17,
+	OUT_X_L_G       = 0x18,
+	OUT_X_H_G       = 0x19,
+	OUT_Y_L_G       = 0x1A,
+	OUT_Y_H_G       = 0x1B,
+	OUT_Z_L_G       = 0x1C,
+	OUT_Z_H_G       = 0x1D,
+
+	CTRL_REG6_XL    = 0x20, // Linear acceleration sensor Control Register 6.
+
+	CTRL_REG8       = 0x22, // Control register 8.
+	CTRL_REG9       = 0x23, // Control register 9.
+
+	STATUS_REG_A    = 0x27,
+	OUT_X_L_XL      = 0x28,
+	OUT_X_H_XL      = 0x29,
+	OUT_Y_L_XL      = 0x2A,
+	OUT_Y_H_XL      = 0x2B,
+	OUT_Z_L_XL      = 0x2C,
+	OUT_Z_H_XL      = 0x2D,
+	FIFO_CTRL       = 0x2E, // FIFO control register.
+	FIFO_SRC        = 0x2F, // FIFO status control register.
+};
+
+// CTRL_REG1_G
+enum
+CTRL_REG1_G_BIT : uint8_t {
+	ODR_G_952HZ  = Bit7 | Bit6,
+	FS_G_2000DPS = Bit4 | Bit3,
+	BW_G_100Hz   = Bit1 | Bit2, // BW_G 100 Hz
+};
+
+// STATUS_REG (both STATUS_REG_A 0x17 and STATUS_REG_G 0x27)
+enum
+STATUS_REG_BIT : uint8_t {
+	TDA  = Bit2, // Temperature sensor new data available.
+	GDA  = Bit1, // Gyroscope new data available.
+	XLDA = Bit0, // Accelerometer new data available.
+};
+
+// CTRL_REG6_XL
+enum
+CTRL_REG6_XL_BIT : uint8_t {
+	ODR_XL_952HZ = Bit7 | Bit6,	// 952 Hz ODR
+	FS_XL_16     = Bit3,		// FS_XL 01: ±16 g
+};
+
+// CTRL_REG8
+enum
+CTRL_REG8_BIT : uint8_t {
+	BDU        = Bit6, // Block data update
+	IF_ADD_INC = Bit2, // Register address automatically incremented
+	SW_RESET   = Bit0, // Software reset
+};
+
+// CTRL_REG9
+enum
+CTRL_REG9_BIT : uint8_t {
+	I2C_DISABLE = Bit2,
+	FIFO_EN     = Bit1,
+};
+
+// FIFO_CTRL
+enum
+FIFO_CTRL_BIT : uint8_t {
+	FIFO_MODE_CONTINUOUS = Bit7 | Bit6, // Continuous mode. If the FIFO is full, the new sample over-writes the older sample.
+};
+
+// FIFO_SRC
+enum
+FIFO_SRC_BIT : uint8_t {
+	OVRN = Bit6, // FIFO overrun status.
+	FSS  = Bit5 | Bit4 | Bit3 | Bit2 | Bit1 | Bit0,
+};
+
+
+namespace FIFO
+{
+static constexpr size_t SIZE = 32 * 6; // 32 samples max
+}
+
+} // namespace ST_LSM9DS1

src/drivers/imu/st/lsm9ds1/lsm9ds1_main.cpp

@@ -0,0 +1,143 @@
+/****************************************************************************
+ *
+ *   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 "LSM9DS1.hpp"
+
+#include <px4_platform_common/getopt.h>
+
+namespace lsm9ds1
+{
+
+LSM9DS1 *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_LSM9DS1_AG)
+	g_dev = new LSM9DS1(PX4_SPI_BUS_SENSORS, PX4_SPIDEV_LSM9DS1_AG, rotation);
+#endif // PX4_SPI_BUS_SENSORS && PX4_SPIDEV_LSM9DS1_AG
+
+	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");
+	}
+
+	g_dev->Stop();
+	delete g_dev;
+
+	return 0;
+}
+
+static int status()
+{
+	if (g_dev == nullptr) {
+		PX4_WARN("driver not running");
+		return -1;
+	}
+
+	g_dev->PrintInfo();
+
+	return 0;
+}
+
+static void usage()
+{
+	PX4_INFO("missing command: try 'start', 'stop', 'status'");
+	PX4_INFO("options:");
+	PX4_INFO("    -R rotation");
+}
+
+} // namespace lsm9ds1
+
+extern "C" __EXPORT int lsm9ds1_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:
+			lsm9ds1::usage();
+			return 0;
+		}
+	}
+
+	if (myoptind >= argc) {
+		lsm9ds1::usage();
+		return -1;
+	}
+
+	const char *verb = argv[myoptind];
+
+	if (!strcmp(verb, "start")) {
+		return lsm9ds1::start(rotation);
+
+	} else if (!strcmp(verb, "stop")) {
+		return lsm9ds1::stop();
+
+	} else if (!strcmp(verb, "status")) {
+		return lsm9ds1::status();
+	}
+
+	lsm9ds1::usage();
+
+	return 0;
+}