From 7eb1e00c5464bd949944113832d98fdc698ef64e Mon Sep 17 00:00:00 2001
From: kevindehecker <kevindehecker@hotmail.com>
Date: Wed, 30 Mar 2016 16:43:48 +0200
Subject: [PATCH] Rotate external magneto fix

---
 conf/airframes/TUDELFT/tudelft_iris_indi.xml | 16 +++++++++-----
 sw/airborne/modules/sensors/mag_hmc58xx.c    | 23 +++++++++++++++-----
 sw/airborne/subsystems/imu/imu_px4fmu_v2.4.c |  4 ++--
 3 files changed, 30 insertions(+), 13 deletions(-)

diff --git a/conf/airframes/TUDELFT/tudelft_iris_indi.xml b/conf/airframes/TUDELFT/tudelft_iris_indi.xml
index 59e0e12333..04bcd11521 100644
--- a/conf/airframes/TUDELFT/tudelft_iris_indi.xml
+++ b/conf/airframes/TUDELFT/tudelft_iris_indi.xml
@@ -87,6 +87,12 @@
     <load name="mag_hmc58xx.xml">
       <configure name="MAG_HMC58XX_I2C_DEV" value="i2c1"/>
       <define name="MODULE_HMC58XX_UPDATE_AHRS" value="TRUE"/>
+      <define name="HMC58XX_CHAN_X" value="1"/>
+      <define name="HMC58XX_CHAN_Y" value="0"/>
+      <define name="HMC58XX_CHAN_Z" value="2"/>
+      <define name="HMC58XX_CHAN_X_SIGN" value="-"/>
+      <define name="HMC58XX_CHAN_Y_SIGN" value="+"/>
+      <define name="HMC58XX_CHAN_Z_SIGN" value="+"/>
     </load>
     <!--  <load name="spektrum_soft_bind.xml"/>-->
   </modules>
@@ -104,12 +110,12 @@
     <define name="ACCEL_Y_SENS" value="0.609921478775" integer="16"/>
     <define name="ACCEL_Z_SENS" value="0.619923942578" integer="16"/>
 
-    <define name="MAG_X_NEUTRAL" value="21"/>
-    <define name="MAG_Y_NEUTRAL" value="-93"/>
+    <define name="MAG_X_NEUTRAL" value="92"/>
+    <define name="MAG_Y_NEUTRAL" value="35"/>
     <define name="MAG_Z_NEUTRAL" value="-15"/>
-    <define name="MAG_X_SENS" value="5.1784060163" integer="16"/>
-    <define name="MAG_Y_SENS" value="5.55867516912" integer="16"/>
-    <define name="MAG_Z_SENS" value="5.06809390796" integer="16"/>
+    <define name="MAG_X_SENS" value="5.52543569751" integer="16"/>
+    <define name="MAG_Y_SENS" value="4.91233764159" integer="16"/>
+    <define name="MAG_Z_SENS" value="4.99453775296" integer="16"/>
   </section>
   <commands>
     <axis name="PITCH" failsafe_value="0" />
diff --git a/sw/airborne/modules/sensors/mag_hmc58xx.c b/sw/airborne/modules/sensors/mag_hmc58xx.c
index f38e046fbb..7388dee787 100644
--- a/sw/airborne/modules/sensors/mag_hmc58xx.c
+++ b/sw/airborne/modules/sensors/mag_hmc58xx.c
@@ -30,6 +30,8 @@
 #include "pprzlink/messages.h"
 #include "subsystems/datalink/downlink.h"
 
+#include BOARD_CONFIG
+
 #ifndef HMC58XX_CHAN_X
 #define HMC58XX_CHAN_X 0
 #endif
@@ -39,6 +41,15 @@
 #ifndef HMC58XX_CHAN_Z
 #define HMC58XX_CHAN_Z 2
 #endif
+#ifndef HMC58XX_CHAN_X_SIGN
+#define HMC58XX_CHAN_X_SIGN +
+#endif
+#ifndef HMC58XX_CHAN_Y_SIGN
+#define HMC58XX_CHAN_Y_SIGN +
+#endif
+#ifndef HMC58XX_CHAN_Z_SIGN
+#define HMC58XX_CHAN_Z_SIGN +
+#endif
 
 #if MODULE_HMC58XX_UPDATE_AHRS
 #include "subsystems/imu.h"
@@ -68,9 +79,9 @@ void mag_hmc58xx_module_event(void)
 
     // set channel order
     struct Int32Vect3 mag = {
-      (int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_X]),
-      (int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Y]),
-      (int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Z])
+      HMC58XX_CHAN_X_SIGN(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_X]),
+      HMC58XX_CHAN_Y_SIGN(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Y]),
+      HMC58XX_CHAN_Z_SIGN(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Z])
     };
     // unscaled vector
     VECT3_COPY(imu.mag_unscaled, mag);
@@ -91,9 +102,9 @@ void mag_hmc58xx_module_event(void)
 void mag_hmc58xx_report(void)
 {
   struct Int32Vect3 mag = {
-    (int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_X]),
-    (int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Y]),
-    (int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Z])
+    HMC58XX_CHAN_X_SIGN(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_X]),
+    HMC58XX_CHAN_Y_SIGN(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Y]),
+    HMC58XX_CHAN_Z_SIGN(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Z])
   };
   DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &mag.x, &mag.y, &mag.z);
 }
diff --git a/sw/airborne/subsystems/imu/imu_px4fmu_v2.4.c b/sw/airborne/subsystems/imu/imu_px4fmu_v2.4.c
index 63cdfe352b..6e1b73391d 100644
--- a/sw/airborne/subsystems/imu/imu_px4fmu_v2.4.c
+++ b/sw/airborne/subsystems/imu/imu_px4fmu_v2.4.c
@@ -50,7 +50,7 @@ void imu_impl_init(void)
   /* LSM303dlhc acc + magneto init */
   lsm303dlhc_spi_init(&imu_px4.lsm_acc, &IMU_SPI_DEV, IMU_LSM_SPI_SLAVE_IDX, LSM_TARGET_ACC);
 #if !MODULE_HMC58XX_UPDATE_AHRS
-  lsm303dlhc_spi_init(&imu_px4.lsm_mag, &IMU_SPI_DEV, IMU_LSM_SPI_SLAVE_IDX, LSM_TARGET_MAG);
+  lsm303dlhc_spi_init(&imu_px4.lsm_mag, &IMU_SPI_DEV, IMU_LSM_SPI_SLAVE_IDX, LSM_TARGET_MAG);  
 #endif
 
 }
@@ -63,7 +63,7 @@ void imu_periodic(void)
 #if !MODULE_HMC58XX_UPDATE_AHRS
   /* Read magneto's every 10 times of main freq
    * at ~50Hz (main loop for rotorcraft: 512Hz)
-   */
+   */  
   RunOnceEvery(10, lsm303dlhc_spi_periodic(&imu_px4.lsm_mag));
 #endif
 }