From b280e28623f1c4b95b0edad114d1ea0eef99dc74 Mon Sep 17 00:00:00 2001
From: Henry Zhang <zhanghui629@gmail.com>
Date: Wed, 17 Aug 2016 15:37:19 +0800
Subject: [PATCH] MindPX: Remove hardcode for sensors rotation.

---
 ROMFS/px4fmu_common/init.d/rc.sensors | 27 ++++++++++++------------
 src/drivers/l3gd20/l3gd20.cpp         | 12 -----------
 src/drivers/lsm303d/lsm303d.cpp       | 22 --------------------
 src/drivers/mpu6500/mpu6500.cpp       | 30 ++-------------------------
 4 files changed, 16 insertions(+), 75 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index 7a17a61026..21276b4684 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -184,24 +184,25 @@ fi
 
 if ver hwcmp MINDPX_V2
 then
-	if mpu6500 start
-	then
-	fi
-
-	if lsm303d start
-	then
-	fi
-
-	if l3gd20 start
-	then
-	fi
-
 	# External I2C bus
 	if hmc5883 -C -T -X start
 	then
 	fi
 
-	if lis3mdl -R 2 start
+	# Internal I2C bus
+	if hmc5883 -C -T -I -R 8 start
+	then
+	fi
+
+	if mpu6500 -R 8 start
+	then
+	fi
+
+	if lsm303d -R 10 start
+	then
+	fi
+
+	if l3gd20 -R 14 start
 	then
 	fi
 fi
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index f6e3275a0a..6f14d64fd8 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -1045,18 +1045,6 @@ L3GD20::measure()
 
 	report.z_raw = raw_report.z;
 
-#if defined(CONFIG_ARCH_BOARD_MINDPX_V2)
-	int16_t tx = -report.y_raw;
-	int16_t ty = -report.x_raw;
-	int16_t tz = -report.z_raw;
-	report.x_raw = tx;
-	report.y_raw = ty;
-	report.z_raw = tz;
-#endif
-
-
-
-
 	report.temperature_raw = raw_report.temp;
 
 	float xraw_f = report.x_raw;
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 35975bb2f0..87b19f12ee 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -1569,18 +1569,8 @@ LSM303D::measure()
 	 *		  74 from all measurements centers them around zero.
 	 */
 
-
 	accel_report.timestamp = hrt_absolute_time();
 
-#if defined(CONFIG_ARCH_BOARD_MINDPX_V2)
-	int16_t tx = raw_accel_report.y;
-	int16_t ty = raw_accel_report.x;
-	int16_t tz = -raw_accel_report.z;
-	raw_accel_report.x = tx;
-	raw_accel_report.y = ty;
-	raw_accel_report.z = tz;
-#endif
-
 	// use the temperature from the last mag reading
 	accel_report.temperature = _last_temperature;
 
@@ -1711,20 +1701,8 @@ LSM303D::mag_measure()
 	 *		  74 from all measurements centers them around zero.
 	 */
 
-
 	mag_report.timestamp = hrt_absolute_time();
 
-#if defined(CONFIG_ARCH_BOARD_MINDPX_V2)
-	int16_t tx = raw_mag_report.y;
-	int16_t ty = raw_mag_report.x;
-	int16_t tz = -raw_mag_report.z;
-	raw_mag_report.x = tx;
-	raw_mag_report.y = ty;
-	raw_mag_report.z = tz;
-#endif
-
-
-
 	mag_report.x_raw = raw_mag_report.x;
 	mag_report.y_raw = raw_mag_report.y;
 	mag_report.z_raw = raw_mag_report.z;
diff --git a/src/drivers/mpu6500/mpu6500.cpp b/src/drivers/mpu6500/mpu6500.cpp
index 96256c2909..a55168af65 100644
--- a/src/drivers/mpu6500/mpu6500.cpp
+++ b/src/drivers/mpu6500/mpu6500.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015, 2016 Airmind Development Team. All rights reserved.
+ *   Copyright (c) 2015, 2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -1719,9 +1719,6 @@ MPU6500::measure()
 	report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z);
 
 
-
-
-
 	if (report.accel_x == 0 &&
 	    report.accel_y == 0 &&
 	    report.accel_z == 0 &&
@@ -1749,29 +1746,6 @@ MPU6500::measure()
 		return;
 	}
 
-#if defined(CONFIG_ARCH_BOARD_MINDPX_V2)
-	/*
-	 * Swap axes and negate z
-	 */
-	int16_t accel_xt = report.accel_y;
-	int16_t accel_yt = report.accel_x;
-	int16_t accel_zt = ((report.accel_z == -32768) ? 32767 : -report.accel_z);
-
-	int16_t gyro_xt = report.gyro_y;
-	int16_t gyro_yt = report.gyro_x;
-	int16_t gyro_zt = ((report.gyro_z == -32768) ? 32767 : -report.gyro_z);
-
-	/*
-	 * Apply the swap
-	 */
-	report.accel_x = accel_xt;
-	report.accel_y = accel_yt;
-	report.accel_z = accel_zt;
-	report.gyro_x = gyro_xt;
-	report.gyro_y = gyro_yt;
-	report.gyro_z = gyro_zt;
-
-#else
 	/*
 	 * Swap axes and negate y
 	 */
@@ -1788,7 +1762,7 @@ MPU6500::measure()
 	report.accel_y = accel_yt;
 	report.gyro_x = gyro_xt;
 	report.gyro_y = gyro_yt;
-#endif
+
 	/*
 	 * Report buffers.
 	 */