From 75b96732b5a0de6b17f0646948077576052a0a11 Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Wed, 18 Nov 2015 09:20:40 -1000
Subject: [PATCH] Extended to support PX4FMU_V4 hw

---
 ROMFS/px4fmu_common/init.d/rc.sensors | 53 +++++++++++++++++++--------
 1 file changed, 38 insertions(+), 15 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index 08a044329f..7b8f20839d 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -67,32 +67,55 @@ then
 		fi
 	fi
 else
-	# FMUv1
-	if mpu6000 start
+	if ver hwcmp PX4FMU_V4
 	then
-	fi
+		# External I2C bus
+		if hmc5883 -C -T -X start
+		then
+		fi
 
-	if l3gd20 start
-	then
-	fi
+		# Internal SPI bus
+		if hmc5883 -C -T -S -R 8 start
+		then
+		fi
 
-	# MAG selection
-	if param compare SENS_EXT_MAG 2
-	then
-		if hmc5883 -C -I start
+		# Internal SPI bus mpu9250 is rotated ??
+		if mpu9250 -R 0 start
+		then
+		fi
+
+		# Internal SPI bus try MPU6K on ICM-20608-G is rotated 180 deg roll, 270 deg yaw
+		if mpu6000 -R 14 start
 		then
 		fi
 	else
-		# Use only external as primary
-		if param compare SENS_EXT_MAG 1
+		# FMUv1
+		if mpu6000 start
+		then
+		fi
+
+		if l3gd20 start
+		then
+		fi
+
+		# MAG selection
+		if param compare SENS_EXT_MAG 2
 		then
-			if hmc5883 -C -X start
+			if hmc5883 -C -I start
 			then
 			fi
 		else
-		# auto-detect the primary, prefer external
-			if hmc5883 start
+			# Use only external as primary
+			if param compare SENS_EXT_MAG 1
 			then
+				if hmc5883 -C -X start
+				then
+				fi
+			else
+			# auto-detect the primary, prefer external
+				if hmc5883 start
+				then
+				fi
 			fi
 		fi
 	fi