From 55be098e3b7082de12f5e4e2045bd13b6dce0663 Mon Sep 17 00:00:00 2001
From: Amir Melzer <amir.melzer@mavt.ethz.ch>
Date: Fri, 22 Dec 2017 19:17:54 +0100
Subject: [PATCH] adis16448 bmlz fixes (#8519)

* small bit mask fix

* Restore factory calibration values in drivers start-up sequence

* Restore factory calibration values in drivers start-up sequence (reverted from commit 09ba45501f87f77e53d670fcf880b3cfc419fe38)

* Restore factory calibration values in drivers start-up sequence

* Initialization of the Adis16448 report struct

* Add stall time after write and read cycle

* Increasing the stall time for being compatible with the B sensor version

* small clean up

* Add settling time after initialization
---
 src/drivers/adis16448/adis16448.cpp | 31 ++++++++++++++++++++++-------
 1 file changed, 24 insertions(+), 7 deletions(-)

diff --git a/src/drivers/adis16448/adis16448.cpp b/src/drivers/adis16448/adis16448.cpp
index 2b1b3d7cc8..3e576d8f08 100644
--- a/src/drivers/adis16448/adis16448.cpp
+++ b/src/drivers/adis16448/adis16448.cpp
@@ -176,7 +176,7 @@
 #define FW_FILTER									false
 
 #define SPI_BUS_SPEED								1000000
-#define T_STALL										5
+#define T_STALL										9
 
 #define GYROINITIALSENSITIVITY						250
 #define ACCELINITIALSENSITIVITY						(1.0f / 1200.0f)
@@ -293,6 +293,21 @@ private:
 		uint16_t		mag_z;
 		uint16_t		baro;
 		uint16_t		temp;
+
+		ADISReport():
+			cmd(0),
+			status(0),
+			gyro_x(0),
+			gyro_y(0),
+			gyro_z(0),
+			accel_x(0),
+			accel_y(0),
+			accel_z(0),
+			mag_x(0),
+			mag_y(0),
+			mag_z(0),
+			baro(0),
+			temp(0) {}
 	};
 #pragma pack(pop)
 
@@ -711,21 +726,21 @@ int ADIS16448::reset()
 
 	/* Set gyroscope scale to default value */
 	_set_gyro_dyn_range(GYROINITIALSENSITIVITY);
-	usleep(1000);
 
 	/* Set digital FIR filter tap */
 	_set_dlpf_filter(BITS_FIR_16_TAP_CFG);
-	usleep(1000);
 
 	/* Set IMU sample rate */
 	_set_sample_rate(_sample_rate);
-	usleep(1000);
 
 	_accel_range_scale = ADIS16448_ONE_G * ACCELINITIALSENSITIVITY;
 	_accel_range_m_s2  = ADIS16448_ONE_G * ACCELDYNAMICRANGE;
 	_mag_range_scale   = MAGINITIALSENSITIVITY;
 	_mag_range_mgauss  = MAGDYNAMICRANGE;
 
+	/* settling time */
+	up_udelay(50000);
+
 	return OK;
 
 }
@@ -753,6 +768,7 @@ ADIS16448::probe()
 	case ADIS16448_Product:
 		DEVICE_DEBUG("ADIS16448 is detected ID: 0x%02x, Serial: 0x%02x", _product, serial_number);
 		modify_reg16(ADIS16448_GPIO_CTRL, 0x0200, 0x0002);			/* Turn on ADIS16448 adaptor board led */
+		_set_factory_default();										/* Restore factory calibration 		   */
 		return OK;
 	}
 
@@ -782,9 +798,9 @@ ADIS16448::_set_sample_rate(uint16_t desired_sample_rate_hz)
 		smpl_prd = BITS_SMPL_PRD_NO_TAP_CFG;
 	}
 
-	modify_reg16(ADIS16448_SMPL_PRD, 0x0700, smpl_prd);
+	modify_reg16(ADIS16448_SMPL_PRD, 0x1f00, smpl_prd);
 
-	if ((read_reg16(ADIS16448_SMPL_PRD) & 0x0700) != smpl_prd) {
+	if ((read_reg16(ADIS16448_SMPL_PRD) & 0x1f00) != smpl_prd) {
 		DEVICE_DEBUG("failed to set IMU sample rate");
 	}
 
@@ -809,7 +825,6 @@ void
 ADIS16448::_set_factory_default()
 {
 	write_reg16(ADIS16448_GLOB_CMD, 0x02);
-	warnx("Set IMU to factory default!");
 }
 
 /* set the gyroscope dynamic range */
@@ -1316,6 +1331,7 @@ ADIS16448::read_reg16(unsigned reg)
 	transferhword(cmd, nullptr, 1);
 	up_udelay(T_STALL);
 	transferhword(nullptr, cmd, 1);
+	up_udelay(T_STALL);
 
 	return cmd[0];
 }
@@ -1331,6 +1347,7 @@ ADIS16448::write_reg16(unsigned reg, uint16_t value)
 	transferhword(cmd, nullptr, 1);
 	up_udelay(T_STALL);
 	transferhword(cmd + 1, nullptr, 1);
+	up_udelay(T_STALL);
 }
 
 void