From 58d362d33e00b4040dd9aee47747224d3db915e0 Mon Sep 17 00:00:00 2001
From: "timecop@gmail.com"
 <timecop@gmail.com@7c89a4a9-59b9-e629-4cfe-3a2d53b20e61>
Date: Wed, 12 Jun 2013 02:45:56 +0000
Subject: [PATCH] corrected magnetometer alignment bug found by CrashPilot1000
 - during mag calibration, axes must be swapped per sensor alignment, which
 didn't happen.

git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@336 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
---
 src/drv_hmc5883l.c | 22 ++++++++++++++++++----
 src/drv_hmc5883l.h |  2 +-
 src/sensors.c      | 12 +++---------
 3 files changed, 22 insertions(+), 14 deletions(-)

diff --git a/src/drv_hmc5883l.c b/src/drv_hmc5883l.c
index 91b3bae72..65f62442c 100755
--- a/src/drv_hmc5883l.c
+++ b/src/drv_hmc5883l.c
@@ -17,7 +17,9 @@
 #define HMC_POS_BIAS 1
 #define HMC_NEG_BIAS 2
 
-bool hmc5883lDetect(void)
+static int8_t sensor_align[3];
+
+bool hmc5883lDetect(int8_t *align)
 {
     bool ack = false;
     uint8_t sig = 0;
@@ -26,6 +28,8 @@ bool hmc5883lDetect(void)
     if (!ack || sig != 'H')
         return false;
 
+    memcpy(sensor_align, align, 3);
+
     return true;
 }
 
@@ -117,10 +121,20 @@ void hmc5883lInit(float *calibrationGain)
 void hmc5883lRead(int16_t *magData)
 {
     uint8_t buf[6];
+    int16_t mag[3];
+    int i;
 
     i2cRead(MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf);
 
-    magData[0] = buf[0] << 8 | buf[1];
-    magData[1] = buf[2] << 8 | buf[3];
-    magData[2] = buf[4] << 8 | buf[5];
+    mag[0] = ((int16_t)((uint16_t) buf[0] << 8) + buf[1]);
+    mag[1] = ((int16_t)((uint16_t) buf[2] << 8) + buf[3]);
+    mag[2] = ((int16_t)((uint16_t) buf[4] << 8) + buf[5]);
+
+    for (i = 0; i < 3; i++) {
+        int8_t axis = sensor_align[i];
+        if (axis > 0)
+            magData[axis - 1] = mag[i];
+        else
+            magData[-axis - 1] = -mag[i];
+    }
 }
diff --git a/src/drv_hmc5883l.h b/src/drv_hmc5883l.h
index 92096fa1a..397817479 100755
--- a/src/drv_hmc5883l.h
+++ b/src/drv_hmc5883l.h
@@ -1,5 +1,5 @@
 #pragma once
 
-bool hmc5883lDetect(void);
+bool hmc5883lDetect(int8_t *align);
 void hmc5883lInit(float *calibrationGain);
 void hmc5883lRead(int16_t *magData);
diff --git a/src/sensors.c b/src/sensors.c
index 57e9f8362..be3439e68 100755
--- a/src/sensors.c
+++ b/src/sensors.c
@@ -107,7 +107,7 @@ retry:
     gyro.init();
 
 #ifdef MAG
-    if (!hmc5883lDetect())
+    if (!hmc5883lDetect(mcfg.align[ALIGN_MAG]))
         sensorsClear(SENSOR_MAG);
 #endif
 
@@ -402,19 +402,13 @@ void Gyro_getADC(void)
 }
 
 #ifdef MAG
-static float magCal[3] = { 1.0, 1.0, 1.0 };     // gain for each axis, populated at sensor init
+static float magCal[3] = { 1.0f, 1.0f, 1.0f };     // gain for each axis, populated at sensor init
 static uint8_t magInit = 0;
 
 static void Mag_getRawADC(void)
 {
+    // MAG driver will align itself, so no need to alignSensors()
     hmc5883lRead(magADC);
-
-    // Default mag orientation is -2, -3, 1 or
-    // no way? is THIS finally the proper orientation?? (by GrootWitBaas)
-    // magADC[ROLL] = rawADC[2]; // X
-    // magADC[PITCH] = -rawADC[0]; // Y
-    // magADC[YAW] = -rawADC[1]; // Z
-    alignSensors(ALIGN_MAG, magADC);
 }
 
 void Mag_init(void)