From 5d110101b55ace07be9d2307f002ed8b2d1a8c3f Mon Sep 17 00:00:00 2001
From: Kabir Mohammed <kabir@uasys.io>
Date: Tue, 7 Jan 2020 04:47:41 +0530
Subject: [PATCH] adis16497: add sensor model autodetection (#13863)

The sensor comes in three variants with different gyro measurement ranges. This patch allows auto-detection of the model at startup and sets the scale factor associated with the particular model.
---
 src/drivers/imu/adis16497/ADIS16497.cpp | 56 ++++++++++++++++++++++---
 src/drivers/imu/adis16497/ADIS16497.hpp |  5 +++
 2 files changed, 55 insertions(+), 6 deletions(-)

diff --git a/src/drivers/imu/adis16497/ADIS16497.cpp b/src/drivers/imu/adis16497/ADIS16497.cpp
index 824527976c..358b837dc4 100644
--- a/src/drivers/imu/adis16497/ADIS16497.cpp
+++ b/src/drivers/imu/adis16497/ADIS16497.cpp
@@ -53,11 +53,15 @@ static constexpr uint8_t CONFIG  	= 0x0A; // Control, clock and miscellaneous co
 static constexpr uint8_t DEC_RATE  	= 0x0C; // Control, output sample rate decimation
 static constexpr uint8_t NULL_CNFG  	= 0x0E; // Control, automatic bias correction configuration
 static constexpr uint8_t SYNC_SCALE  	= 0x10; // Control, automatic bias correction configuration
-static constexpr uint8_t RANG_MDL   	= 0x12; // Measurement range (model-specific) Identifier TODO use this
+static constexpr uint8_t RANG_MDL   	= 0x12; // Measurement range (model-specific) identifier
 static constexpr uint8_t FILTR_BNK_0   	= 0x16; // Filter selection
 static constexpr uint8_t FILTR_BNK_1   	= 0x18; // Filter selection
 
-static constexpr uint16_t PROD_ID_ADIS16497 = 0x4071;	// ADIS16497 Identification, device number
+static constexpr uint16_t PROD_ID_ADIS16497 = 0x4071; // ADIS16497 device number
+
+static constexpr uint16_t RANG_MDL_1BMLZ = 0b0011; // ADIS16497-1 (±125°/sec)
+static constexpr uint16_t RANG_MDL_2BMLZ = 0b0111; // ADIS16497-2 (±450°/sec)
+static constexpr uint16_t RANG_MDL_3BMLZ = 0b1111; // ADIS16497-3 (±2000°/sec)
 
 // Stall time between SPI transfers
 static constexpr uint8_t T_STALL = 2;
@@ -81,11 +85,9 @@ ADIS16497::ADIS16497(int bus, uint32_t device, enum Rotation rotation) :
 
 	_px4_accel.set_device_type(DRV_ACC_DEVTYPE_ADIS16497);
 	_px4_accel.set_sample_rate(ADIS16497_DEFAULT_RATE);
-	_px4_accel.set_scale(1.25f * CONSTANTS_ONE_G / 1000.0f); // accel 1.25 mg/LSB
 
 	_px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ADIS16497);
 	_px4_gyro.set_sample_rate(ADIS16497_DEFAULT_RATE);
-	_px4_gyro.set_scale(math::radians(0.025f)); // gyro 0.025 °/sec/LSB
 }
 
 ADIS16497::~ADIS16497()
@@ -247,7 +249,19 @@ ADIS16497::probe()
 			PX4_DEBUG("PRODUCT: %X", product_id);
 
 			if (self_test()) {
-				return PX4_OK;
+
+				// Switch to config page
+				write_reg16(PAGE_ID, 0x03);
+
+				uint16_t model_id = read_reg16(RANG_MDL);
+
+				if (set_measurement_range(model_id)) {
+					return PX4_OK;
+
+				} else {
+					PX4_ERR("probe attempt %d: reading model id failed, resetting", i);
+					reset();
+				}
 
 			} else {
 				PX4_ERR("probe attempt %d: self test failed, resetting", i);
@@ -269,7 +283,7 @@ ADIS16497::self_test()
 	// Switch to configuration page
 	write_reg16(PAGE_ID, 0x03);
 
-	// Self test (globa l command bit 1)
+	// Self test (global command bit 1)
 	uint8_t value[2] {};
 	value[0] = (1 << 1);
 	write_reg16(GLOB_CMD, (uint16_t)value[0]);
@@ -298,6 +312,36 @@ ADIS16497::self_test()
 	return true;
 }
 
+bool
+ADIS16497::set_measurement_range(uint16_t model)
+{
+	_px4_accel.set_scale(1.25f * CONSTANTS_ONE_G / 1000.0f); // 1.25 mg/LSB
+	_px4_accel.set_range(40.0f * CONSTANTS_ONE_G); // 40g
+
+	switch (model) {
+	case RANG_MDL_1BMLZ:
+		_px4_gyro.set_scale(math::radians(0.00625f)); // 0.00625 °/sec/LSB
+		_px4_gyro.set_range(math::radians(125.0f)); // 125 °/s
+		break;
+
+	case RANG_MDL_2BMLZ:
+		_px4_gyro.set_scale(math::radians(0.025f)); // 0.025 °/sec/LSB
+		_px4_gyro.set_range(math::radians(450.0f)); // 450 °/s
+		break;
+
+	case RANG_MDL_3BMLZ:
+		_px4_gyro.set_scale(math::radians(0.1f)); // 0.1 °/sec/LSB
+		_px4_gyro.set_range(math::radians(2000.0f)); // 2000 °/s
+		break;
+
+	default:
+		PX4_ERR("RANG_MDL: %#X", model);
+		return false;
+	}
+
+	return true;
+}
+
 uint16_t
 ADIS16497::read_reg16(uint8_t reg)
 {
diff --git a/src/drivers/imu/adis16497/ADIS16497.hpp b/src/drivers/imu/adis16497/ADIS16497.hpp
index a5a08484ac..ceb0ad5799 100644
--- a/src/drivers/imu/adis16497/ADIS16497.hpp
+++ b/src/drivers/imu/adis16497/ADIS16497.hpp
@@ -164,6 +164,11 @@ private:
 	// ADIS16497 onboard self test
 	bool 			self_test();
 
+	/**
+	 * Set measurement range based on the sensor model.
+	 */
+	bool 			set_measurement_range(uint16_t model);
+
 	uint32_t crc32(const uint16_t *data, size_t len) const
 	{
 		uint32_t crc = 0xffffffff;