From 102003c66496f032113a3a8616e931db17e224d2 Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Fri, 21 Jul 2017 09:01:43 -1000
Subject: [PATCH] rc.sensors:Use HW type to refine startup / deprecate mpu9250
 on PixhawkMini

   Using the hwvercmp on FMUv2 HW derivatives built with px4fmu_v3_default
   to ues a more targeted startup approach:

  1) Detect V3 and V2M

  2) On V3 use the external mpu9250 to further discriminate between 2.0
     and 2.1. Then only start the devices that are on that version of
     the board.

 3) Due to HW errata on PixhawkMini deprecate mpu9250.
     The mpu9250 will not start reliably on the PixhawkMini
     Since we have an ICM20608 and an External Mag the
     mpu9250 is not to be used.
---
 ROMFS/px4fmu_common/init.d/rc.sensors | 57 +++++++++++++++++++--------
 1 file changed, 40 insertions(+), 17 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index 508f78739a..2832a2e029 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -100,21 +100,27 @@ then
 	then
 	fi
 
-	# external MPU6K is rotated 180 degrees yaw
-	if mpu6000 -S -R 4 start
+	# V2 build hwtypecmp is always false
+	set BOARD_FMUV3 0
+	# V3 build hwtypecmp supports V2|V2M|V30
+	if ver hwtypecmp V30
 	then
-		set BOARD_FMUV3 true
-	else
-		# Check for Pixhawk 2.1 board
-		if mpu9250 -S -R 4 start
+		# Check for Pixhawk 2.0 cube
+		# external MPU6K is rotated 180 degrees yaw
+		if mpu6000 -S -R 4 start
 		then
-			set BOARD_FMUV3 true
+			set BOARD_FMUV3 20
 		else
-			set BOARD_FMUV3 false
+			# Check for Pixhawk 2.1 cube
+			# external MPU9250 is rotated 180 degrees yaw
+			if mpu9250 -S -R 4 start
+			then
+				set BOARD_FMUV3 21
+			fi
 		fi
 	fi
 
-	if [ $BOARD_FMUV3 == true ]
+	if [ $BOARD_FMUV3 != 0 ]
 	then
 		# sensor heating is available, but we disable it for now
 		param set SENS_EN_THERMAL 0
@@ -129,26 +135,43 @@ then
 		then
 		fi
 
-		# internal MPU6000 is rotated 180 deg roll, 270 deg yaw
-		if mpu6000 -R 14 start
+		if [ $BOARD_FMUV3 == 20 ]
 		then
-		else
+			# v2.0 internal MPU6000 is rotated 180 deg roll, 270 deg yaw
+			if mpu6000 -R 14 start
+			then
+			fi
+			# v2.0 Has internal hmc5883 on SPI1
+			if hmc5883 -C -T -S -R 8 start
+			then
+			fi
+
+		fi
+		if [ $BOARD_FMUV3 == 21 ]
+		then
+			# v2.1 internal MPU9250 is rotated 180 deg roll, 270 deg yaw
 			if mpu9250 -R 14 start
 			then
 			fi
 		fi
 
-		if hmc5883 -C -T -S -R 8 start
-		then
-		fi
 	else
-		# FMUv2
+		# $BOARD_FMUV3 == 0 -> FMUv2
+
 		if mpu6000 start
 		then
 		fi
 
-		if mpu9250 start
+		# As we will use the external mag and the ICM-20608-G
+		# V2 build hwtypecmp is always false
+		# V3 build hwtypecmp supports V2|V2M|V30
+		if ver hwtypecmp V2M
 		then
+		# On the PixhawkMini the mpu9250 has been disabled due to HW errata
+		else
+			if mpu9250 start
+			then
+			fi
 		fi
 
 		if l3gd20 start