From a4a040aa4515c81017105ee5d46643fa5f62b0f2 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sun, 29 Jul 2018 11:41:17 -0400
Subject: [PATCH] ADIS16477 run onboard self test during init

---
 src/drivers/imu/adis16477/ADIS16477.cpp | 37 ++++++++++++++++++-------
 src/drivers/imu/adis16477/ADIS16477.hpp |  8 ++----
 2 files changed, 29 insertions(+), 16 deletions(-)

diff --git a/src/drivers/imu/adis16477/ADIS16477.cpp b/src/drivers/imu/adis16477/ADIS16477.cpp
index 27ae0daead..dc8e8cf062 100644
--- a/src/drivers/imu/adis16477/ADIS16477.cpp
+++ b/src/drivers/imu/adis16477/ADIS16477.cpp
@@ -227,11 +227,19 @@ ADIS16477::probe()
 
 		if (_product == PROD_ID_ADIS16477) {
 			DEVICE_DEBUG("PRODUCT: %X", _product);
-			return PX4_OK;
-		}
 
-		PX4_ERR("read product id attempt %d failed, resetting", i);
-		reset();
+			if (self_test()) {
+				return PX4_OK;
+
+			} else {
+				PX4_ERR("probe attempt %d: self test failed, resetting", i);
+				reset();
+			}
+
+		} else {
+			PX4_ERR("probe attempt %d: read product id failed, resetting", i);
+			reset();
+		}
 	}
 
 	return -EIO;
@@ -270,15 +278,24 @@ ADIS16477::_set_gyro_dyn_range(uint16_t desired_gyro_dyn_range)
 {
 }
 
-int
+bool
 ADIS16477::self_test()
 {
-	if (perf_event_count(_sample_perf) == 0) {
-		measure();
+	DEVICE_DEBUG("self test");
+
+	// self test (global command bit 2)
+	uint8_t value[2] = {};
+	value[0] = (1 << 2);
+	write_reg16(GLOB_CMD, (uint16_t)value[0]);
+
+	// read DIAG_STAT to check result
+	uint16_t diag_stat = read_reg16(DIAG_STAT);
+
+	if (diag_stat != 0) {
+		return false;
 	}
 
-	/* return 0 on success, 1 else */
-	return (perf_event_count(_sample_perf) > 0) ? 0 : 1;
+	return true;
 }
 
 int
@@ -537,7 +554,7 @@ ADIS16477::measure()
 	}
 
 	if (adis_report.checksum != checksum) {
-		PX4_ERR("adis_report.checksum: %X vs calculated: %X", adis_report.checksum, checksum);
+		DEVICE_DEBUG("adis_report.checksum: %X vs calculated: %X", adis_report.checksum, checksum);
 		perf_event_count(_bad_transfers);
 		perf_end(_sample_perf);
 		return -EIO;
diff --git a/src/drivers/imu/adis16477/ADIS16477.hpp b/src/drivers/imu/adis16477/ADIS16477.hpp
index a6b9be25fd..931a2f0efc 100644
--- a/src/drivers/imu/adis16477/ADIS16477.hpp
+++ b/src/drivers/imu/adis16477/ADIS16477.hpp
@@ -186,12 +186,8 @@ private:
 	void			write_reg(uint8_t reg, uint8_t value);
 	void			write_reg16(uint8_t reg, uint16_t value);
 
-	/**
-	 * Measurement self test
-	 *
-	 * @return 0 on success, 1 on failure
-	 */
-	int 			self_test();
+	// ADIS16477 onboard self test
+	bool 			self_test();
 
 	/*
 	  set low pass filter frequency