From baeddfe4f33f535de2ba4ffc4edf0d8df17ef3fe Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 1 Aug 2015 16:33:54 +1000
Subject: [PATCH] PX4: enable probe of MPU9250 if available

---
 mk/PX4/ROMFS/init.d/rc.APM     | 28 ++++++++++++++++++++++------
 mk/PX4/config_px4fmu-v2_APM.mk |  1 +
 2 files changed, 23 insertions(+), 6 deletions(-)

diff --git a/mk/PX4/ROMFS/init.d/rc.APM b/mk/PX4/ROMFS/init.d/rc.APM
index 8b07c7911a..a8c8befc30 100644
--- a/mk/PX4/ROMFS/init.d/rc.APM
+++ b/mk/PX4/ROMFS/init.d/rc.APM
@@ -264,8 +264,14 @@ else
        echo "Found MPU6000 external"
        set HAVE_FMUV3 true
     else
-       echo "No MPU6000 external"
-       set HAVE_FMUV3 false
+       if mpu9250 -X -R 4 start
+       then
+           echo "Found MPU9250 external"
+           set HAVE_FMUV3 true
+       else
+           echo "No MPU6000 or MPU9250 external"
+           set HAVE_FMUV3 false
+       fi
     fi
     if [ $HAVE_FMUV3 == true ]
     then
@@ -290,9 +296,14 @@ else
         then
                echo "Found MPU6000 internal"
         else
-               echo "No MPU6000"
-               echo "No MPU6000" >> $logfile
+           if mpu9250 -R 14 start
+           then
+               echo "Found MPU9250 internal"
+           else
+               echo "No MPU6000 or MPU9250"
+               echo "No MPU6000 or MPU9250" >> $logfile
                sh /etc/init.d/rc.error
+           fi
         fi
         if hmc5883 -C -T -S -R 8 start
         then
@@ -303,8 +314,13 @@ else
         then
                echo "Found MPU6000"
         else
-               echo "No MPU6000"
-               echo "No MPU6000" >> $logfile
+               if mpu9250 start
+               then
+                   echo "Found MPU9250"
+               else
+                   echo "No MPU6000 or MPU9250"
+                   echo "No MPU9250" >> $logfile
+               fi
         fi
         if l3gd20 start
         then
diff --git a/mk/PX4/config_px4fmu-v2_APM.mk b/mk/PX4/config_px4fmu-v2_APM.mk
index 7a19cf45b3..fc62e72b9a 100644
--- a/mk/PX4/config_px4fmu-v2_APM.mk
+++ b/mk/PX4/config_px4fmu-v2_APM.mk
@@ -5,6 +5,7 @@ include $(SKETCHBOOK)/mk/PX4/px4_common.mk
 
 MODULES		+= drivers/lsm303d
 MODULES		+= drivers/l3gd20
+MODULES		+= drivers/mpu9250
 MODULES		+= drivers/boards/px4fmu-v2
 MODULES		+= drivers/pwm_input
 MODULES         += modules/uavcan