From ce1e4e728eb25c885b6e529588e7e9b5c285336d Mon Sep 17 00:00:00 2001
From: Alessio Morale <alessiomorale@gmail.com>
Date: Thu, 17 Jul 2014 20:43:16 +0200
Subject: [PATCH] OP-1434 - Support for mag calibration

---
 flight/modules/GPS/GPS.c     | 19 ++++++++++++++++---
 flight/modules/GPS/UBX.c     | 33 ++++++++++++++++++++++++++++-----
 flight/modules/GPS/inc/GPS.h |  1 +
 flight/modules/GPS/inc/UBX.h |  7 ++++---
 4 files changed, 49 insertions(+), 11 deletions(-)

diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c
index eafcb2974..632c3aea6 100644
--- a/flight/modules/GPS/GPS.c
+++ b/flight/modules/GPS/GPS.c
@@ -98,7 +98,9 @@ static uint32_t timeOfLastUpdateMs;
 #if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER)
 static struct GPS_RX_STATS gpsRxStats;
 #endif
-
+#ifdef PIOS_INCLUDE_GPS_UBX_PARSER
+void AuxMagCalibrationUpdatedCb(UAVObjEvent *ev);
+#endif
 // ****************
 /**
  * Initialise the gps module
@@ -153,10 +155,16 @@ int32_t GPSInitialize(void)
     GPSTimeInitialize();
     GPSSatellitesInitialize();
     HomeLocationInitialize();
+#ifdef PIOS_INCLUDE_GPS_UBX_PARSER
     AuxMagSensorInitialize();
+    AuxMagCalibrationInitialize();
     GPSExtendedStatusInitialize();
-    updateSettings();
 
+    // Initialize mag parameters
+    AuxMagCalibrationUpdatedCb(NULL);
+    AuxMagCalibrationConnectCallback(AuxMagCalibrationUpdatedCb);
+#endif
+    updateSettings();
 #else
     if (gpsPort && gpsEnabled) {
         GPSPositionSensorInitialize();
@@ -381,7 +389,12 @@ static void updateSettings()
         }
     }
 }
-
+#ifdef PIOS_INCLUDE_GPS_UBX_PARSER
+void AuxMagCalibrationUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    load_mag_settings();
+}
+#endif
 /**
  * @}
  * @}
diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c
index 57ff18ffa..32d03b870 100644
--- a/flight/modules/GPS/UBX.c
+++ b/flight/modules/GPS/UBX.c
@@ -34,6 +34,13 @@
 #if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
 #include "inc/UBX.h"
 #include "inc/GPS.h"
+#include "CoordinateConversions.h"
+#include "auxmagcalibration.h"
+
+static float mag_bias[3] = { 0, 0, 0 };
+static float mag_transform[3][3] = {
+    { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }
+};
 
 // If a PVT sentence is received in the last UBX_PVT_TIMEOUT (ms) timeframe it disables VELNED/POSLLH/SOL/TIMEUTC
 #define UBX_PVT_TIMEOUT (1000)
@@ -293,7 +300,7 @@ void parse_ubx_nav_pvt(struct UBX_NAV_PVT *pvt, GPSPositionSensorData *GpsPositi
     }
 #endif
 }
-
+#if !defined(PIOS_GPS_MINIMAL)
 void parse_ubx_op_sys(struct UBX_OP_SYSINFO *sysinfo)
 {
     GPSExtendedStatusData data;
@@ -306,13 +313,21 @@ void parse_ubx_op_sys(struct UBX_OP_SYSINFO *sysinfo)
     data.Status  = GPSEXTENDEDSTATUS_STATUS_GPSV9;
     GPSExtendedStatusSet(&data);
 }
+#endif
 void parse_ubx_op_mag(struct UBX_OP_MAG *mag)
 {
-    AuxMagSensorData data;
+    float mags[3] = { mag->x - mag_bias[0],
+                      mag->y - mag_bias[1],
+                      mag->z - mag_bias[2] };
 
-    data.x = mag->x;
-    data.y = mag->y;
-    data.z = mag->z;
+    float mag_out[3];
+
+    rot_mult(mag_transform, mags, mag_out);
+
+    AuxMagSensorData data;
+    data.x = mag_out[0];
+    data.y = mag_out[1];
+    data.z = mag_out[2];
     data.Status = mag->Status;
     AuxMagSensorSet(&data);
 }
@@ -427,9 +442,11 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi
         break;
     case UBX_CLASS_OP_CUST:
         switch (ubx->header.id) {
+#if !defined(PIOS_GPS_MINIMAL)
         case UBX_ID_SYS:
             parse_ubx_op_sys(&ubx->payload.op_sysinfo);
             break;
+#endif
         case UBX_ID_MAG:
             parse_ubx_op_mag(&ubx->payload.op_mag);
             break;
@@ -445,4 +462,10 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi
     return id;
 }
 
+
+void load_mag_settings()
+{
+    AuxMagCalibrationmag_transformArrayGet((float *)mag_transform);
+    AuxMagCalibrationmag_biasArrayGet(mag_bias);
+}
 #endif // PIOS_INCLUDE_GPS_UBX_PARSER
diff --git a/flight/modules/GPS/inc/GPS.h b/flight/modules/GPS/inc/GPS.h
index 6cecaa845..d88bcff1a 100644
--- a/flight/modules/GPS/inc/GPS.h
+++ b/flight/modules/GPS/inc/GPS.h
@@ -38,6 +38,7 @@
 #include "gpssatellites.h"
 #include "gpspositionsensor.h"
 #include "gpstime.h"
+#include "auxmagcalibration.h"
 
 #define NO_PARSER         -3 // no parser available
 #define PARSER_OVERRUN    -2 // message buffer overrun before completing the message
diff --git a/flight/modules/GPS/inc/UBX.h b/flight/modules/GPS/inc/UBX.h
index a4116e443..1393897e1 100644
--- a/flight/modules/GPS/inc/UBX.h
+++ b/flight/modules/GPS/inc/UBX.h
@@ -261,9 +261,9 @@ struct UBX_OP_SYSINFO {
 
 // OP custom messages
 struct UBX_OP_MAG {
-    uint16_t x;
-    uint16_t y;
-    uint16_t z;
+    int16_t x;
+    int16_t y;
+    int16_t z;
     uint16_t Status;
 };
 
@@ -299,5 +299,6 @@ struct UBXPacket {
 bool checksum_ubx_message(struct UBXPacket *);
 uint32_t parse_ubx_message(struct UBXPacket *, GPSPositionSensorData *);
 int parse_ubx_stream(uint8_t, char *, GPSPositionSensorData *, struct GPS_RX_STATS *);
+void load_mag_settings();
 
 #endif /* UBX_H */