diff --git a/CREDITS.txt b/CREDITS.txt
index bc2d5e175..c5fa1be78 100644
--- a/CREDITS.txt
+++ b/CREDITS.txt
@@ -46,6 +46,7 @@ Patrick Huebner
 Ryan Hunt
 Mark James
 Michael Johnston
+Stefan Karlsson
 Ricky King
 Thorsten Klose
 Karl Knutsson
diff --git a/flight/libraries/auxmagsupport.c b/flight/libraries/auxmagsupport.c
new file mode 100644
index 000000000..3d27e336a
--- /dev/null
+++ b/flight/libraries/auxmagsupport.c
@@ -0,0 +1,72 @@
+/**
+ ******************************************************************************
+ *
+ * @file       auxmagsupport.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @brief      Functions to handle aux mag data and calibration.
+ *             --
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include <stdint.h>
+#include "inc/auxmagsupport.h"
+#include "CoordinateConversions.h"
+
+static float mag_bias[3] = { 0, 0, 0 };
+static float mag_transform[3][3] = {
+    { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }
+};
+
+AuxMagSettingsTypeOptions option;
+
+void auxmagsupport_reload_settings()
+{
+    AuxMagSettingsTypeGet(&option);
+    float a[3][3];
+    float b[3][3];
+    float rotz;
+    AuxMagSettingsmag_transformArrayGet((float *)a);
+    AuxMagSettingsOrientationGet(&rotz);
+    rotz = DEG2RAD(rotz);
+    rot_about_axis_z(rotz, b);
+    matrix_mult_3x3f(a, b, mag_transform);
+    AuxMagSettingsmag_biasArrayGet(mag_bias);
+}
+
+void auxmagsupport_publish_samples(float mags[3], uint8_t status)
+{
+    float mag_out[3];
+
+    mags[0] -= mag_bias[0];
+    mags[1] -= mag_bias[1];
+    mags[2] -= mag_bias[2];
+    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 = status;
+    AuxMagSensorSet(&data);
+}
+
+AuxMagSettingsTypeOptions auxmagsupport_get_type()
+{
+    return option;
+}
diff --git a/flight/libraries/inc/CoordinateConversions.h b/flight/libraries/inc/CoordinateConversions.h
index c8a391f43..e9369bd26 100644
--- a/flight/libraries/inc/CoordinateConversions.h
+++ b/flight/libraries/inc/CoordinateConversions.h
@@ -112,5 +112,73 @@ static inline void matrix_mult_3x3f(float a[3][3], float b[3][3], float result[3
     result[2][2] = a[0][2] * b[2][0] + a[1][2] * b[2][1] + a[2][2] * b[2][2];
 }
 
+inline void matrix_inline_scale_3f(float a[3][3], float scale)
+{
+    a[0][0] *= scale;
+    a[0][1] *= scale;
+    a[0][2] *= scale;
+
+    a[1][0] *= scale;
+    a[1][1] *= scale;
+    a[1][2] *= scale;
+
+    a[2][0] *= scale;
+    a[2][1] *= scale;
+    a[2][2] *= scale;
+}
+
+inline void rot_about_axis_x(const float rotation, float R[3][3])
+{
+    float s = sinf(rotation);
+    float c = cosf(rotation);
+
+    R[0][0] = 1;
+    R[0][1] = 0;
+    R[0][2] = 0;
+
+    R[1][0] = 0;
+    R[1][1] = c;
+    R[1][2] = -s;
+
+    R[2][0] = 0;
+    R[2][1] = s;
+    R[2][2] = c;
+}
+
+inline void rot_about_axis_y(const float rotation, float R[3][3])
+{
+    float s = sinf(rotation);
+    float c = cosf(rotation);
+
+    R[0][0] = c;
+    R[0][1] = 0;
+    R[0][2] = s;
+
+    R[1][0] = 0;
+    R[1][1] = 1;
+    R[1][2] = 0;
+
+    R[2][0] = -s;
+    R[2][1] = 0;
+    R[2][2] = c;
+}
+
+inline void rot_about_axis_z(const float rotation, float R[3][3])
+{
+    float s = sinf(rotation);
+    float c = cosf(rotation);
+
+    R[0][0] = c;
+    R[0][1] = -s;
+    R[0][2] = 0;
+
+    R[1][0] = s;
+    R[1][1] = c;
+    R[1][2] = 0;
+
+    R[2][0] = 0;
+    R[2][1] = 0;
+    R[2][2] = 1;
+}
 
 #endif // COORDINATECONVERSIONS_H_
diff --git a/flight/libraries/inc/auxmagsupport.h b/flight/libraries/inc/auxmagsupport.h
new file mode 100644
index 000000000..90b063e9e
--- /dev/null
+++ b/flight/libraries/inc/auxmagsupport.h
@@ -0,0 +1,51 @@
+/**
+ ******************************************************************************
+ *
+ * @file       auxmagsupport.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @brief      Functions to handle aux mag data and calibration.
+ *             --
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#ifndef AUXMAGSUPPORT_H_
+#define AUXMAGSUPPORT_H_
+#include <openpilot.h>
+#include <auxmagsettings.h>
+#include <auxmagsensor.h>
+/**
+ * @brief reload Aux Mag settings
+ */
+void auxmagsupport_reload_settings();
+
+/**
+ * @brief Publish a new Aux Magnetometer sample
+ * @param[in] mags  Mag sample in milliGauss
+ * @param[in] status one of AuxMagSensorStatusOptions option
+ */
+void auxmagsupport_publish_samples(float mags[3], uint8_t status);
+
+/**
+ * @brief Get the Aux Mag settings Type option
+ * @param[in] mags  Mag sample in milliGauss
+ * @return one of AuxMagSettingsTypeOptions option
+ */
+AuxMagSettingsTypeOptions auxmagsupport_get_type();
+
+
+#endif /* AUXMAGSUPPORT_H_ */
diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c
index 382f1ba00..9729c5189 100644
--- a/flight/modules/GPS/GPS.c
+++ b/flight/modules/GPS/GPS.c
@@ -40,26 +40,36 @@
 #include "gpssettings.h"
 #include "taskinfo.h"
 #include "hwsettings.h"
-
+#include "auxmagsensor.h"
 #include "WorldMagModel.h"
 #include "CoordinateConversions.h"
+#include <pios_com.h>
 
 #include "GPS.h"
 #include "NMEA.h"
 #include "UBX.h"
-
+#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
+#include "inc/ubx_autoconfig.h"
+#endif
 
 // ****************
 // Private functions
 
 static void gpsTask(void *parameters);
-static void updateSettings();
+static void updateHwSettings();
 
 #ifdef PIOS_GPS_SETS_HOMELOCATION
 static void setHomeLocation(GPSPositionSensorData *gpsData);
 static float GravityAccel(float latitude, float longitude, float altitude);
 #endif
 
+#ifdef PIOS_INCLUDE_GPS_UBX_PARSER
+void AuxMagSettingsUpdatedCb(UAVObjEvent *ev);
+#ifndef PIOS_GPS_MINIMAL
+void updateGpsSettings(UAVObjEvent *ev);
+#endif
+#endif
+
 // ****************
 // Private constants
 
@@ -153,8 +163,16 @@ int32_t GPSInitialize(void)
     GPSTimeInitialize();
     GPSSatellitesInitialize();
     HomeLocationInitialize();
-    updateSettings();
+#ifdef PIOS_INCLUDE_GPS_UBX_PARSER
+    AuxMagSensorInitialize();
+    AuxMagSettingsInitialize();
+    GPSExtendedStatusInitialize();
 
+    // Initialize mag parameters
+    AuxMagSettingsUpdatedCb(NULL);
+    AuxMagSettingsConnectCallback(AuxMagSettingsUpdatedCb);
+#endif
+    updateHwSettings();
 #else
     if (gpsPort && gpsEnabled) {
         GPSPositionSensorInitialize();
@@ -166,7 +184,7 @@ int32_t GPSInitialize(void)
 #if defined(PIOS_GPS_SETS_HOMELOCATION)
         HomeLocationInitialize();
 #endif
-        updateSettings();
+        updateHwSettings();
     }
 #endif /* if defined(REVOLUTION) */
 
@@ -184,7 +202,9 @@ int32_t GPSInitialize(void)
             gps_rx_buffer = NULL;
         }
         PIOS_Assert(gps_rx_buffer);
-
+#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
+        GPSSettingsConnectCallback(updateGpsSettings);
+#endif
         return 0;
     }
 
@@ -215,10 +235,23 @@ static void gpsTask(__attribute__((unused)) void *parameters)
     timeOfLastCommandMs = timeNowMs;
 
     GPSPositionSensorGet(&gpspositionsensor);
+#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
+    updateGpsSettings(0);
+#endif
     // Loop forever
     while (1) {
         uint8_t c;
-
+#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
+        if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX) {
+            char *buffer   = 0;
+            uint16_t count = 0;
+            ubx_autoconfig_run(&buffer, &count);
+            // Something to send?
+            if (count) {
+                PIOS_COM_SendBuffer(gpsPort, (uint8_t *)buffer, count);
+            }
+        }
+#endif
         // This blocks the task until there is something on the buffer
         while (PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, xDelay) > 0) {
             int res;
@@ -230,8 +263,18 @@ static void gpsTask(__attribute__((unused)) void *parameters)
 #endif
 #if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
             case GPSSETTINGS_DATAPROTOCOL_UBX:
+            {
+#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
+                int32_t ac_status = ubx_autoconfig_get_status();
+                gpspositionsensor.AutoConfigStatus =
+                    ac_status == UBX_AUTOCONFIG_STATUS_DISABLED ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED :
+                    ac_status == UBX_AUTOCONFIG_STATUS_DONE ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DONE :
+                    ac_status == UBX_AUTOCONFIG_STATUS_ERROR ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR :
+                    GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING;
+#endif
                 res = parse_ubx_stream(c, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
-                break;
+            }
+            break;
 #endif
             default:
                 res = NO_PARSER; // this should not happen
@@ -257,7 +300,7 @@ static void gpsTask(__attribute__((unused)) void *parameters)
             // we appear to be receiving GPS sentences OK, we've had an update
             // criteria for GPS-OK taken from this post...
             // http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220
-            if ((gpspositionsensor.PDOP < gpsSettings.MaxPDOP) && (gpspositionsensor.Satellites >= gpsSettings.MinSattelites) &&
+            if ((gpspositionsensor.PDOP < gpsSettings.MaxPDOP) && (gpspositionsensor.Satellites >= gpsSettings.MinSatellites) &&
                 (gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) &&
                 (gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) {
                 AlarmsClear(SYSTEMALARMS_ALARM_GPS);
@@ -346,7 +389,7 @@ static void setHomeLocation(GPSPositionSensorData *gpsData)
  * like protocol, etc. Thus the HwSettings object which contains the
  * GPS port speed is used for now.
  */
-static void updateSettings()
+static void updateHwSettings()
 {
     if (gpsPort) {
         // Retrieve settings
@@ -376,10 +419,92 @@ static void updateSettings()
         case HWSETTINGS_GPSSPEED_115200:
             PIOS_COM_ChangeBaud(gpsPort, 115200);
             break;
+        case HWSETTINGS_GPSSPEED_230400:
+            PIOS_COM_ChangeBaud(gpsPort, 230400);
+            break;
         }
     }
 }
 
+#ifdef PIOS_INCLUDE_GPS_UBX_PARSER
+void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    load_mag_settings();
+}
+#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
+void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev)
+{
+    uint8_t ubxAutoConfig;
+    uint8_t ubxDynamicModel;
+    uint8_t ubxSbasMode;
+    ubx_autoconfig_settings_t newconfig;
+    uint8_t ubxSbasSats;
+
+    GPSSettingsUbxRateGet(&newconfig.navRate);
+
+    GPSSettingsUbxAutoConfigGet(&ubxAutoConfig);
+    newconfig.autoconfigEnabled = ubxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_DISABLED ? false : true;
+    newconfig.storeSettings     = ubxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGUREANDSTORE;
+
+    GPSSettingsUbxDynamicModelGet(&ubxDynamicModel);
+    newconfig.dynamicModel = ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PORTABLE ? UBX_DYNMODEL_PORTABLE :
+                             ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_STATIONARY ? UBX_DYNMODEL_STATIONARY :
+                             ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PEDESTRIAN ? UBX_DYNMODEL_PEDESTRIAN :
+                             ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AUTOMOTIVE ? UBX_DYNMODEL_AUTOMOTIVE :
+                             ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_SEA ? UBX_DYNMODEL_SEA :
+                             ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE1G ? UBX_DYNMODEL_AIRBORNE1G :
+                             ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE2G ? UBX_DYNMODEL_AIRBORNE2G :
+                             ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE4G ? UBX_DYNMODEL_AIRBORNE4G :
+                             UBX_DYNMODEL_AIRBORNE1G;
+
+    GPSSettingsUbxSBASModeGet(&ubxSbasMode);
+    switch ((GPSSettingsUbxSBASModeOptions)ubxSbasMode) {
+    case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTION:
+    case GPSSETTINGS_UBXSBASMODE_CORRECTION:
+    case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTIONINTEGRITY:
+    case GPSSETTINGS_UBXSBASMODE_CORRECTIONINTEGRITY:
+        newconfig.SBASCorrection = true;
+        break;
+    default:
+        newconfig.SBASCorrection = false;
+    }
+
+    switch ((GPSSettingsUbxSBASModeOptions)ubxSbasMode) {
+    case GPSSETTINGS_UBXSBASMODE_RANGING:
+    case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTION:
+    case GPSSETTINGS_UBXSBASMODE_RANGINGINTEGRITY:
+    case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTIONINTEGRITY:
+        newconfig.SBASRanging = true;
+        break;
+    default:
+        newconfig.SBASRanging = false;
+    }
+
+    switch ((GPSSettingsUbxSBASModeOptions)ubxSbasMode) {
+    case GPSSETTINGS_UBXSBASMODE_INTEGRITY:
+    case GPSSETTINGS_UBXSBASMODE_RANGINGINTEGRITY:
+    case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTIONINTEGRITY:
+    case GPSSETTINGS_UBXSBASMODE_CORRECTIONINTEGRITY:
+        newconfig.SBASIntegrity = true;
+        break;
+    default:
+        newconfig.SBASIntegrity = false;
+    }
+
+    GPSSettingsUbxSBASChannelsUsedGet(&newconfig.SBASChannelsUsed);
+
+    GPSSettingsUbxSBASSatsGet(&ubxSbasSats);
+
+    newconfig.SBASSats = ubxSbasSats == GPSSETTINGS_UBXSBASSATS_WAAS ? UBX_SBAS_SATS_WAAS :
+                         ubxSbasSats == GPSSETTINGS_UBXSBASSATS_EGNOS ? UBX_SBAS_SATS_EGNOS :
+                         ubxSbasSats == GPSSETTINGS_UBXSBASSATS_MSAS ? UBX_SBAS_SATS_MSAS :
+                         ubxSbasSats == GPSSETTINGS_UBXSBASSATS_GAGAN ? UBX_SBAS_SATS_GAGAN :
+                         ubxSbasSats == GPSSETTINGS_UBXSBASSATS_SDCM ? UBX_SBAS_SATS_SDCM : UBX_SBAS_SATS_AUTOSCAN;
+
+    ubx_autoconfig_set(newconfig);
+}
+#endif /* if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) */
+#endif /* ifdef PIOS_INCLUDE_GPS_UBX_PARSER */
 /**
  * @}
  * @}
diff --git a/flight/modules/GPS/NMEA.c b/flight/modules/GPS/NMEA.c
index 7aac3b5c2..e45702990 100644
--- a/flight/modules/GPS/NMEA.c
+++ b/flight/modules/GPS/NMEA.c
@@ -489,7 +489,7 @@ static bool nmeaProcessGPGGA(GPSPositionSensorData *GpsData, bool *gpsDataUpdate
 
     // geoid separation
     GpsData->GeoidSeparation = NMEA_real_to_float(param[11]);
-
+    GpsData->SensorType = GPSPOSITIONSENSOR_SENSORTYPE_NMEA;
     return true;
 }
 
diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c
index 86910ef0d..ba417b5ac 100644
--- a/flight/modules/GPS/UBX.c
+++ b/flight/modules/GPS/UBX.c
@@ -30,10 +30,76 @@
 
 #include "openpilot.h"
 #include "pios.h"
-
+#include "pios_math.h"
+#include <pios_helpers.h>
+#include <pios_delay.h>
 #if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
 #include "inc/UBX.h"
 #include "inc/GPS.h"
+#include <string.h>
+#include <auxmagsupport.h>
+
+static bool useMag = false;
+static GPSPositionSensorSensorTypeOptions sensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN;
+
+static bool usePvt = false;
+static uint32_t lastPvtTime = 0;
+
+
+// parse table item
+typedef struct {
+    uint8_t msgClass;
+    uint8_t msgID;
+    void (*handler)(struct UBXPacket *, GPSPositionSensorData *GpsPosition);
+} ubx_message_handler;
+
+// parsing functions, roughly ordered by reception rate (higher rate messages on top)
+static void parse_ubx_op_mag(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
+
+static void parse_ubx_nav_pvt(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
+static void parse_ubx_nav_posllh(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
+static void parse_ubx_nav_velned(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
+static void parse_ubx_nav_sol(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
+static void parse_ubx_nav_dop(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
+#ifndef PIOS_GPS_MINIMAL
+static void parse_ubx_nav_timeutc(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
+static void parse_ubx_nav_svinfo(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
+
+static void parse_ubx_op_sys(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
+#endif
+static void parse_ubx_ack_ack(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
+static void parse_ubx_ack_nak(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
+
+static void parse_ubx_mon_ver(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
+
+
+const ubx_message_handler ubx_handler_table[] = {
+    { .msgClass = UBX_CLASS_OP_CUST, .msgID = UBX_ID_OP_MAG,      .handler = &parse_ubx_op_mag      },
+
+    { .msgClass = UBX_CLASS_NAV,     .msgID = UBX_ID_NAV_PVT,     .handler = &parse_ubx_nav_pvt     },
+    { .msgClass = UBX_CLASS_NAV,     .msgID = UBX_ID_NAV_POSLLH,  .handler = &parse_ubx_nav_posllh  },
+    { .msgClass = UBX_CLASS_NAV,     .msgID = UBX_ID_NAV_VELNED,  .handler = &parse_ubx_nav_velned  },
+    { .msgClass = UBX_CLASS_NAV,     .msgID = UBX_ID_NAV_SOL,     .handler = &parse_ubx_nav_sol     },
+    { .msgClass = UBX_CLASS_NAV,     .msgID = UBX_ID_NAV_DOP,     .handler = &parse_ubx_nav_dop     },
+#ifndef PIOS_GPS_MINIMAL
+    { .msgClass = UBX_CLASS_NAV,     .msgID = UBX_ID_NAV_SVINFO,  .handler = &parse_ubx_nav_svinfo  },
+    { .msgClass = UBX_CLASS_NAV,     .msgID = UBX_ID_NAV_TIMEUTC, .handler = &parse_ubx_nav_timeutc },
+
+    { .msgClass = UBX_CLASS_OP_CUST, .msgID = UBX_ID_OP_SYS,      .handler = &parse_ubx_op_sys      },
+#endif
+    { .msgClass = UBX_CLASS_ACK,     .msgID = UBX_ID_ACK_ACK,     .handler = &parse_ubx_ack_ack     },
+    { .msgClass = UBX_CLASS_ACK,     .msgID = UBX_ID_ACK_NAK,     .handler = &parse_ubx_ack_nak     },
+
+    { .msgClass = UBX_CLASS_MON,     .msgID = UBX_ID_MON_VER,     .handler = &parse_ubx_mon_ver     },
+};
+#define UBX_HANDLER_TABLE_SIZE NELEMENTS(ubx_handler_table)
+
+// detected hw version
+int32_t ubxHwVersion = -1;
+
+// Last received Ack/Nak
+struct UBX_ACK_ACK ubxLastAck;
+struct UBX_ACK_NAK ubxLastNak;
 
 // 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)
@@ -194,8 +260,13 @@ bool checksum_ubx_message(struct UBXPacket *ubx)
     }
 }
 
-void parse_ubx_nav_posllh(struct UBX_NAV_POSLLH *posllh, GPSPositionSensorData *GpsPosition)
+static void parse_ubx_nav_posllh(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition)
 {
+    if (usePvt) {
+        return;
+    }
+    struct UBX_NAV_POSLLH *posllh = &ubx->payload.nav_posllh;
+
     if (check_msgtracker(posllh->iTOW, POSLLH_RECEIVED)) {
         if (GpsPosition->Status != GPSPOSITIONSENSOR_STATUS_NOFIX) {
             GpsPosition->Altitude  = (float)posllh->hMSL * 0.001f;
@@ -206,8 +277,12 @@ void parse_ubx_nav_posllh(struct UBX_NAV_POSLLH *posllh, GPSPositionSensorData *
     }
 }
 
-void parse_ubx_nav_sol(struct UBX_NAV_SOL *sol, GPSPositionSensorData *GpsPosition)
+static void parse_ubx_nav_sol(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition)
 {
+    if (usePvt) {
+        return;
+    }
+    struct UBX_NAV_SOL *sol = &ubx->payload.nav_sol;
     if (check_msgtracker(sol->iTOW, SOL_RECEIVED)) {
         GpsPosition->Satellites = sol->numSV;
 
@@ -227,8 +302,10 @@ void parse_ubx_nav_sol(struct UBX_NAV_SOL *sol, GPSPositionSensorData *GpsPositi
     }
 }
 
-void parse_ubx_nav_dop(struct UBX_NAV_DOP *dop, GPSPositionSensorData *GpsPosition)
+static void parse_ubx_nav_dop(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition)
 {
+    struct UBX_NAV_DOP *dop = &ubx->payload.nav_dop;
+
     if (check_msgtracker(dop->iTOW, DOP_RECEIVED)) {
         GpsPosition->HDOP = (float)dop->hDOP * 0.01f;
         GpsPosition->VDOP = (float)dop->vDOP * 0.01f;
@@ -236,10 +313,13 @@ void parse_ubx_nav_dop(struct UBX_NAV_DOP *dop, GPSPositionSensorData *GpsPositi
     }
 }
 
-void parse_ubx_nav_velned(struct UBX_NAV_VELNED *velned, GPSPositionSensorData *GpsPosition)
+static void parse_ubx_nav_velned(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition)
 {
+    if (usePvt) {
+        return;
+    }
     GPSVelocitySensorData GpsVelocity;
-
+    struct UBX_NAV_VELNED *velned = &ubx->payload.nav_velned;
     if (check_msgtracker(velned->iTOW, VELNED_RECEIVED)) {
         if (GpsPosition->Status != GPSPOSITIONSENSOR_STATUS_NOFIX) {
             GpsVelocity.North        = (float)velned->velN / 100.0f;
@@ -252,10 +332,12 @@ void parse_ubx_nav_velned(struct UBX_NAV_VELNED *velned, GPSPositionSensorData *
     }
 }
 
-void parse_ubx_nav_pvt(struct UBX_NAV_PVT *pvt, GPSPositionSensorData *GpsPosition)
+static void parse_ubx_nav_pvt(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition)
 {
-    GPSVelocitySensorData GpsVelocity;
+    lastPvtTime = PIOS_DELAY_GetuS();
 
+    GPSVelocitySensorData GpsVelocity;
+    struct UBX_NAV_PVT *pvt = &ubx->payload.nav_pvt;
     check_msgtracker(pvt->iTOW, (ALL_RECEIVED));
 
     GpsVelocity.North = (float)pvt->velN * 0.001f;
@@ -293,9 +375,15 @@ void parse_ubx_nav_pvt(struct UBX_NAV_PVT *pvt, GPSPositionSensorData *GpsPositi
     }
 #endif
 }
+
 #if !defined(PIOS_GPS_MINIMAL)
-void parse_ubx_nav_timeutc(struct UBX_NAV_TIMEUTC *timeutc)
+static void parse_ubx_nav_timeutc(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition)
 {
+    if (usePvt) {
+        return;
+    }
+
+    struct UBX_NAV_TIMEUTC *timeutc = &ubx->payload.nav_timeutc;
     // Test if time is valid
     if ((timeutc->valid & TIMEUTC_VALIDTOW) && (timeutc->valid & TIMEUTC_VALIDWKN)) {
         // Time is valid, set GpsTime
@@ -317,10 +405,11 @@ void parse_ubx_nav_timeutc(struct UBX_NAV_TIMEUTC *timeutc)
 #endif /* if !defined(PIOS_GPS_MINIMAL) */
 
 #if !defined(PIOS_GPS_MINIMAL)
-void parse_ubx_nav_svinfo(struct UBX_NAV_SVINFO *svinfo)
+static void parse_ubx_nav_svinfo(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition)
 {
     uint8_t chan;
     GPSSatellitesData svdata;
+    struct UBX_NAV_SVINFO *svinfo = &ubx->payload.nav_svinfo;
 
     svdata.SatsInView = 0;
     for (chan = 0; chan < svinfo->numCh; chan++) {
@@ -344,14 +433,64 @@ void parse_ubx_nav_svinfo(struct UBX_NAV_SVINFO *svinfo)
 }
 #endif /* if !defined(PIOS_GPS_MINIMAL) */
 
+static void parse_ubx_ack_ack(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition)
+{
+    struct UBX_ACK_ACK *ack_ack = &ubx->payload.ack_ack;
+
+    ubxLastAck = *ack_ack;
+}
+
+static void parse_ubx_ack_nak(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition)
+{
+    struct UBX_ACK_NAK *ack_nak = &ubx->payload.ack_nak;
+
+    ubxLastNak = *ack_nak;
+}
+
+static void parse_ubx_mon_ver(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition)
+{
+    struct UBX_MON_VER *mon_ver = &ubx->payload.mon_ver;
+
+    ubxHwVersion = atoi(mon_ver->hwVersion);
+
+    sensorType   = (ubxHwVersion >= 80000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX8 :
+                   ((ubxHwVersion >= 70000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX7 : GPSPOSITIONSENSOR_SENSORTYPE_UBX);
+}
+
+
+#if !defined(PIOS_GPS_MINIMAL)
+static void parse_ubx_op_sys(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition)
+{
+    struct UBX_OP_SYSINFO *sysinfo = &ubx->payload.op_sysinfo;
+    GPSExtendedStatusData data;
+
+    data.FlightTime           = sysinfo->flightTime;
+    data.HeapRemaining        = sysinfo->HeapRemaining;
+    data.IRQStackRemaining    = sysinfo->IRQStackRemaining;
+    data.SysModStackRemaining = sysinfo->SystemModStackRemaining;
+    data.Options = sysinfo->options;
+    data.Status  = GPSEXTENDEDSTATUS_STATUS_GPSV9;
+    GPSExtendedStatusSet(&data);
+}
+#endif
+static void parse_ubx_op_mag(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition)
+{
+    if (!useMag) {
+        return;
+    }
+    struct UBX_OP_MAG *mag = &ubx->payload.op_mag;
+    float mags[3] = { mag->x, mag->y, mag->z };
+    auxmagsupport_publish_samples(mags, AUXMAGSENSOR_STATUS_OK);
+}
+
+
 // UBX message parser
 // returns UAVObjectID if a UAVObject structure is ready for further processing
 
 uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition)
 {
     uint32_t id = 0;
-    static uint32_t lastPvtTime = 0;
-    static bool ubxInitialized  = false;
+    static bool ubxInitialized = false;
 
     if (!ubxInitialized) {
         // initialize dop values. If no DOP sentence is received it is safer to initialize them to a high value rather than 0.
@@ -361,48 +500,17 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi
         ubxInitialized    = true;
     }
     // is it using PVT?
-    bool usePvt = (lastPvtTime) && (PIOS_DELAY_GetuSSince(lastPvtTime) < UBX_PVT_TIMEOUT * 1000);
-
-    switch (ubx->header.class) {
-    case UBX_CLASS_NAV:
-        if (!usePvt) {
-            // Set of messages to be decoded when not using pvt
-            switch (ubx->header.id) {
-            case UBX_ID_POSLLH:
-                parse_ubx_nav_posllh(&ubx->payload.nav_posllh, GpsPosition);
-                break;
-            case UBX_ID_SOL:
-                parse_ubx_nav_sol(&ubx->payload.nav_sol, GpsPosition);
-                break;
-            case UBX_ID_VELNED:
-                parse_ubx_nav_velned(&ubx->payload.nav_velned, GpsPosition);
-                break;
-#if !defined(PIOS_GPS_MINIMAL)
-            case UBX_ID_TIMEUTC:
-                parse_ubx_nav_timeutc(&ubx->payload.nav_timeutc);
-                break;
-#endif
-            }
+    usePvt = (lastPvtTime) && (PIOS_DELAY_GetuSSince(lastPvtTime) < UBX_PVT_TIMEOUT * 1000);
+    for (uint8_t i = 0; i < UBX_HANDLER_TABLE_SIZE; i++) {
+        const ubx_message_handler *handler = &ubx_handler_table[i];
+        if (handler->msgClass == ubx->header.class && handler->msgID == ubx->header.id) {
+            handler->handler(ubx, GpsPosition);
+            break;
         }
-        // messages used always
-        switch (ubx->header.id) {
-        case UBX_ID_DOP:
-            parse_ubx_nav_dop(&ubx->payload.nav_dop, GpsPosition);
-            break;
-
-        case UBX_ID_PVT:
-            parse_ubx_nav_pvt(&ubx->payload.nav_pvt, GpsPosition);
-            lastPvtTime = PIOS_DELAY_GetuS();
-            break;
-#if !defined(PIOS_GPS_MINIMAL)
-        case UBX_ID_SVINFO:
-            parse_ubx_nav_svinfo(&ubx->payload.nav_svinfo);
-            break;
-#endif
-        }
-        break;
     }
 
+    GpsPosition->SensorType = sensorType;
+
     if (msgtracker.msg_received == ALL_RECEIVED) {
         GPSPositionSensorSet(GpsPosition);
         msgtracker.msg_received = NONE_RECEIVED;
@@ -411,4 +519,17 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi
     return id;
 }
 
+void load_mag_settings()
+{
+    auxmagsupport_reload_settings();
+
+    if (auxmagsupport_get_type() != AUXMAGSETTINGS_TYPE_GPSV9) {
+        useMag = false;
+        const uint8_t status = AUXMAGSENSOR_STATUS_NONE;
+        // next sample from other external mags will provide the right status if present
+        AuxMagSensorStatusSet((uint8_t *)&status);
+    } else {
+        useMag = true;
+    }
+}
 #endif // PIOS_INCLUDE_GPS_UBX_PARSER
diff --git a/flight/modules/GPS/inc/GPS.h b/flight/modules/GPS/inc/GPS.h
index 6cecaa845..c952db4cb 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 "auxmagsettings.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 f32a267bc..1b1256e1f 100644
--- a/flight/modules/GPS/inc/UBX.h
+++ b/flight/modules/GPS/inc/UBX.h
@@ -32,27 +32,99 @@
 #define UBX_H
 #include "openpilot.h"
 #include "gpspositionsensor.h"
+#include "gpsextendedstatus.h"
+#include "auxmagsensor.h"
 #include "GPS.h"
 
+#define UBX_HW_VERSION_8 80000
+#define UBX_HW_VERSION_7 70000
 
-#define UBX_SYNC1      0xb5 // UBX protocol synchronization characters
-#define UBX_SYNC2      0x62
+#define UBX_SYNC1        0xb5 // UBX protocol synchronization characters
+#define UBX_SYNC2        0x62
 
 // From u-blox6 receiver protocol specification
 
 // Messages classes
-#define UBX_CLASS_NAV  0x01
+typedef enum {
+    UBX_CLASS_NAV     = 0x01,
+    UBX_CLASS_ACK     = 0x05,
+    UBX_CLASS_CFG     = 0x06,
+    UBX_CLASS_MON     = 0x0A,
+    UBX_CLASS_OP_CUST = 0x99,
+    UBX_CLASS_AID     = 0x0B,
+    // unused class IDs, used for disabling them
+    UBX_CLASS_RXM     = 0x02,
+} ubx_class;
 
 // Message IDs
-#define UBX_ID_POSLLH  0x02
-#define UBX_ID_STATUS  0x03
-#define UBX_ID_DOP     0x04
-#define UBX_ID_SOL     0x06
-#define UBX_ID_VELNED  0x12
-#define UBX_ID_TIMEUTC 0x21
-#define UBX_ID_SVINFO  0x30
-#define UBX_ID_PVT     0x07
+typedef enum {
+    UBX_ID_NAV_POSLLH    = 0x02,
+    UBX_ID_NAV_STATUS    = 0x03,
+    UBX_ID_NAV_DOP       = 0x04,
+    UBX_ID_NAV_SOL       = 0x06,
+    UBX_ID_NAV_VELNED    = 0x12,
+    UBX_ID_NAV_TIMEUTC   = 0x21,
+    UBX_ID_NAV_SVINFO    = 0x30,
+    UBX_ID_NAV_PVT       = 0x07,
 
+    UBX_ID_NAV_AOPSTATUS = 0x60,
+    UBX_ID_NAV_CLOCK     = 0x22,
+    UBX_ID_NAV_DGPS      = 0x31,
+    UBX_ID_NAV_POSECEF   = 0x01,
+    UBX_ID_NAV_SBAS      = 0x32,
+    UBX_ID_NAV_TIMEGPS   = 0x20,
+    UBX_ID_NAV_VELECEF   = 0x11
+} ubx_class_nav_id;
+
+typedef enum {
+    UBX_ID_OP_SYS = 0x01,
+    UBX_ID_OP_MAG = 0x02,
+} ubx_class_op_id;
+
+typedef enum {
+    UBX_ID_MON_VER    = 0x04,
+    // unused messages IDs, used for disabling them
+    UBX_ID_MON_HW2    = 0x0B,
+    UBX_ID_MON_HW     = 0x09,
+    UBX_ID_MON_IO     = 0x02,
+    UBX_ID_MON_MSGPP  = 0x06,
+    UBX_ID_MON_RXBUFF = 0x07,
+    UBX_ID_MON_RXR    = 0x21,
+    UBX_ID_MON_TXBUF  = 0x08,
+} ubx_class_mon_id;
+
+typedef enum {
+    UBX_ID_CFG_NAV5 = 0x24,
+    UBX_ID_CFG_RATE = 0x08,
+    UBX_ID_CFG_MSG  = 0x01,
+    UBX_ID_CFG_CFG  = 0x09,
+    UBX_ID_CFG_SBAS = 0x16,
+} ubx_class_cfg_id;
+
+typedef enum {
+    UBX_ID_ACK_ACK = 0x01,
+    UBX_ID_ACK_NAK = 0x00,
+} ubx_class_ack_id;
+
+typedef enum {
+    UBX_ID_AID_ALM    = 0x0B,
+    UBX_ID_AID_ALPSRV = 0x32,
+    UBX_ID_AID_ALP    = 0x50,
+    UBX_ID_AID_AOP    = 0x33,
+    UBX_ID_AID_DATA   = 0x10,
+    UBX_ID_AID_EPH    = 0x31,
+    UBX_ID_AID_HUI    = 0x02,
+    UBX_ID_AID_INI    = 0x01,
+    UBX_ID_AID_REQ    = 0x00,
+} ubx_class_aid_id;
+
+typedef enum {
+    UBX_ID_RXM_ALM  = 0x30,
+    UBX_ID_RXM_EPH  = 0x31,
+    UBX_ID_RXM_RAW  = 0x10,
+    UBX_ID_RXM_SFRB = 0x11,
+    UBX_ID_RXM_SVSI = 0x20,
+} ubx_class_rxm_id;
 // private structures
 
 // Geodetic Position Solution
@@ -245,8 +317,49 @@ struct UBX_NAV_SVINFO {
     struct UBX_NAV_SVINFO_SV sv[MAX_SVS]; // Repeated 'numCh' times
 };
 
+// ACK message class
+
+struct UBX_ACK_ACK {
+    uint8_t clsID; // ClassID
+    uint8_t msgID; // MessageID
+};
+
+struct UBX_ACK_NAK {
+    uint8_t clsID; // ClassID
+    uint8_t msgID; // MessageID
+};
+
+// MON message Class
+#define UBX_MON_MAX_EXT 5
+struct UBX_MON_VER {
+    char swVersion[30];
+    char hwVersion[10];
+#if UBX_MON_MAX_EXT > 0
+    char extension[UBX_MON_MAX_EXT][30];
+#endif
+};
+
+
+// OP custom messages
+struct UBX_OP_SYSINFO {
+    uint32_t flightTime;
+    uint16_t HeapRemaining;
+    uint16_t IRQStackRemaining;
+    uint16_t SystemModStackRemaining;
+    uint16_t options;
+};
+
+// OP custom messages
+struct UBX_OP_MAG {
+    int16_t  x;
+    int16_t  y;
+    int16_t  z;
+    uint16_t Status;
+};
+
 typedef union {
     uint8_t payload[0];
+    // Nav Class
     struct UBX_NAV_POSLLH  nav_posllh;
     struct UBX_NAV_STATUS  nav_status;
     struct UBX_NAV_DOP     nav_dop;
@@ -257,6 +370,13 @@ typedef union {
     struct UBX_NAV_TIMEUTC nav_timeutc;
     struct UBX_NAV_SVINFO  nav_svinfo;
 #endif
+    // Ack Class
+    struct UBX_ACK_ACK     ack_ack;
+    struct UBX_ACK_NAK     ack_nak;
+    // Mon Class
+    struct UBX_MON_VER     mon_ver;
+    struct UBX_OP_SYSINFO  op_sysinfo;
+    struct UBX_OP_MAG op_mag;
 } UBXPayload;
 
 struct UBXHeader {
@@ -272,8 +392,15 @@ struct UBXPacket {
     UBXPayload payload;
 };
 
+// Used by AutoConfig code
+extern int32_t ubxHwVersion;
+extern struct UBX_ACK_ACK ubxLastAck;
+extern struct UBX_ACK_NAK ubxLastNak;
+
 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 */
diff --git a/flight/modules/GPS/inc/ubx_autoconfig.h b/flight/modules/GPS/inc/ubx_autoconfig.h
new file mode 100644
index 000000000..e01391a7d
--- /dev/null
+++ b/flight/modules/GPS/inc/ubx_autoconfig.h
@@ -0,0 +1,185 @@
+/**
+ ******************************************************************************
+ *
+ * @file       %FILENAME%
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @addtogroup [Group]
+ * @{
+ * @addtogroup %CLASS%
+ * @{
+ * @brief [Brief]
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef UBX_AUTOCONFIG_H_
+#define UBX_AUTOCONFIG_H_
+#include "UBX.h"
+#include <stdint.h>
+#include <stdbool.h>
+
+// defines
+// TODO: NEO8 max rate is for Rom version, flash is limited to 10Hz, need to handle that.
+#define UBX_MAX_RATE_VER8 18
+#define UBX_MAX_RATE_VER7 10
+#define UBX_MAX_RATE      5
+
+#define UBX_REPLY_TIMEOUT (500 * 1000)
+#define UBX_MAX_RETRIES   5
+
+// types
+typedef enum {
+    UBX_AUTOCONFIG_STATUS_DISABLED = 0,
+    UBX_AUTOCONFIG_STATUS_RUNNING,
+    UBX_AUTOCONFIG_STATUS_DONE,
+    UBX_AUTOCONFIG_STATUS_ERROR
+} ubx_autoconfig_status_t;
+// Enumeration options for field UBXDynamicModel
+typedef enum {
+    UBX_DYNMODEL_PORTABLE   = 0,
+    UBX_DYNMODEL_STATIONARY = 2,
+    UBX_DYNMODEL_PEDESTRIAN = 3,
+    UBX_DYNMODEL_AUTOMOTIVE = 4,
+    UBX_DYNMODEL_SEA = 5,
+    UBX_DYNMODEL_AIRBORNE1G = 6,
+    UBX_DYNMODEL_AIRBORNE2G = 7,
+    UBX_DYNMODEL_AIRBORNE4G = 8
+} ubx_config_dynamicmodel_t;
+
+typedef enum {
+    UBX_SBAS_SATS_AUTOSCAN = 0,
+    UBX_SBAS_SATS_WAAS     = 1,
+    UBX_SBAS_SATS_EGNOS    = 2,
+    UBX_SBAS_SATS_MSAS     = 3,
+    UBX_SBAS_SATS_GAGAN    = 4,
+    UBX_SBAS_SATS_SDCM     = 5
+} ubx_config_sats_t;
+
+#define UBX_
+typedef struct {
+    bool    autoconfigEnabled;
+    bool    storeSettings;
+
+    bool    SBASRanging;
+    bool    SBASCorrection;
+    bool    SBASIntegrity;
+    ubx_config_sats_t SBASSats;
+    uint8_t SBASChannelsUsed;
+
+    int8_t  navRate;
+    ubx_config_dynamicmodel_t dynamicModel;
+} ubx_autoconfig_settings_t;
+
+// Mask for "all supported devices": battery backed RAM, Flash, EEPROM, SPI Flash
+#define UBX_CFG_CFG_ALL_DEVICES_MASK (0x01 | 0x02 | 0x04 | 0x10)
+
+// Sent messages for configuration support
+typedef struct {
+    uint32_t clearMask;
+    uint32_t saveMask;
+    uint32_t loadMask;
+    uint8_t  deviceMask;
+} __attribute__((packed)) ubx_cfg_cfg_t;
+
+typedef struct {
+    uint16_t mask;
+    uint8_t  dynModel;
+    uint8_t  fixMode;
+    int32_t  fixedAlt;
+    uint32_t fixedAltVar;
+    int8_t   minElev;
+    uint8_t  drLimit;
+    uint16_t pDop;
+    uint16_t tDop;
+    uint16_t pAcc;
+    uint16_t tAcc;
+    uint8_t  staticHoldThresh;
+    uint8_t  dgpsTimeOut;
+    uint8_t  cnoThreshNumSVs;
+    uint8_t  cnoThresh;
+    uint16_t reserved2;
+    uint32_t reserved3;
+    uint32_t reserved4;
+} __attribute__((packed)) ubx_cfg_nav5_t;
+
+typedef struct {
+    uint16_t measRate;
+    uint16_t navRate;
+    uint16_t timeRef;
+} __attribute__((packed)) ubx_cfg_rate_t;
+
+typedef struct {
+    uint8_t msgClass;
+    uint8_t msgID;
+    uint8_t rate;
+} __attribute__((packed)) ubx_cfg_msg_t;
+
+#define UBX_CFG_SBAS_MODE_ENABLED    0x01
+#define UBX_CFG_SBAS_MODE_TEST       0x02
+#define UBX_CFG_SBAS_USAGE_RANGE     0x01
+#define UBX_CFG_SBAS_USAGE_DIFFCORR  0x02
+#define UBX_CFG_SBAS_USAGE_INTEGRITY 0x04
+
+// SBAS used satellite PNR bitmask (120-151)
+// -------------------------------------1---------1---------1---------1
+// -------------------------------------5---------4---------3---------2
+// ------------------------------------10987654321098765432109876543210
+// WAAS 122, 133, 134, 135, 138---------|---------|---------|---------|
+#define UBX_CFG_SBAS_SCANMODE1_WAAS  0b00000000000001001110000000000100
+// EGNOS 120, 124, 126, 131-------------|---------|---------|---------|
+#define UBX_CFG_SBAS_SCANMODE1_EGNOS 0b00000000000000000000100001010001
+// MSAS 129, 137------------------------|---------|---------|---------|
+#define UBX_CFG_SBAS_SCANMODE1_MSAS  0b00000000000000100000001000000000
+// GAGAN 127, 128-----------------------|---------|---------|---------|
+#define UBX_CFG_SBAS_SCANMODE1_GAGAN 0b00000000000000000000000110000000
+// SDCM 125, 140, 141-------------------|---------|---------|---------|
+#define UBX_CFG_SBAS_SCANMODE1_SDCM  0b00000000001100000000000000100000
+
+#define UBX_CFG_SBAS_SCANMODE2       0x00
+typedef struct {
+    uint8_t  mode;
+    uint8_t  usage;
+    uint8_t  maxSBAS;
+    uint8_t  scanmode2;
+    uint32_t scanmode1;
+} __attribute__((packed)) ubx_cfg_sbas_t;
+
+typedef struct {
+    uint8_t  prolog[2];
+    uint8_t  class;
+    uint8_t  id;
+    uint16_t len;
+} __attribute__((packed)) UBXSentHeader_t;
+
+typedef union {
+    uint8_t buffer[0];
+    struct {
+        UBXSentHeader_t header;
+        union {
+            ubx_cfg_cfg_t  cfg_cfg;
+            ubx_cfg_msg_t  cfg_msg;
+            ubx_cfg_nav5_t cfg_nav5;
+            ubx_cfg_rate_t cfg_rate;
+            ubx_cfg_sbas_t cfg_sbas;
+        } payload;
+        uint8_t resvd[2]; // added space for checksum bytes
+    } message;
+} __attribute__((packed)) UBXSentPacket_t;
+
+void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send);
+void ubx_autoconfig_set(ubx_autoconfig_settings_t config);
+int32_t ubx_autoconfig_get_status();
+#endif /* UBX_AUTOCONFIG_H_ */
diff --git a/flight/modules/GPS/ubx_autoconfig.c b/flight/modules/GPS/ubx_autoconfig.c
new file mode 100644
index 000000000..74aa89179
--- /dev/null
+++ b/flight/modules/GPS/ubx_autoconfig.c
@@ -0,0 +1,399 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup GSPModule GPS Module
+ * @brief Support code for UBX AutoConfig
+ * @{
+ *
+ * @file       ubx_autoconfig.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @brief      Support code for UBX AutoConfig
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#include "inc/ubx_autoconfig.h"
+#include <pios_mem.h>
+// private type definitions
+typedef enum {
+    INIT_STEP_DISABLED = 0,
+    INIT_STEP_START,
+    INIT_STEP_ASK_VER,
+    INIT_STEP_WAIT_VER,
+    INIT_STEP_ENABLE_SENTENCES,
+    INIT_STEP_ENABLE_SENTENCES_WAIT_ACK,
+    INIT_STEP_CONFIGURE,
+    INIT_STEP_CONFIGURE_WAIT_ACK,
+    INIT_STEP_DONE,
+    INIT_STEP_ERROR,
+} initSteps_t;
+
+typedef struct {
+    initSteps_t        currentStep; // Current configuration "fsm" status
+    uint32_t           lastStepTimestampRaw; // timestamp of last operation
+    UBXSentPacket_t    working_packet; // outbound "buffer"
+    ubx_autoconfig_settings_t currentSettings;
+    int8_t lastConfigSent; // index of last configuration string sent
+    struct UBX_ACK_ACK requiredAck; // Class and id of the message we are waiting for an ACK from GPS
+    uint8_t retryCount;
+} status_t;
+
+ubx_cfg_msg_t msg_config_ubx6[] = {
+    // messages to disable
+    { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_CLOCK,   .rate = 0  },
+    { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_POSECEF, .rate = 0  },
+    { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SBAS,    .rate = 0  },
+    { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_TIMEGPS, .rate = 0  },
+    { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_VELECEF, .rate = 0  },
+
+    { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_HW,      .rate = 0  },
+    { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_HW2,     .rate = 0  },
+    { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_IO,      .rate = 0  },
+    { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_MSGPP,   .rate = 0  },
+    { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_RXBUFF,  .rate = 0  },
+    { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_RXR,     .rate = 0  },
+    { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_TXBUF,   .rate = 0  },
+
+    { .msgClass = UBX_CLASS_RXM, .msgID = UBX_ID_RXM_SVSI,    .rate = 0  },
+
+    // message to enable
+    { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_POSLLH,  .rate = 1  },
+    { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_DOP,     .rate = 1  },
+    { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SOL,     .rate = 1  },
+    { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_STATUS,  .rate = 1  },
+    { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_VELNED,  .rate = 1  },
+    { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_TIMEUTC, .rate = 1  },
+    { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SVINFO,  .rate = 10 },
+};
+
+ubx_cfg_msg_t msg_config_ubx7[] = {
+    // messages to disable
+    { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_AOPSTATUS, .rate = 0  },
+    { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_CLOCK,     .rate = 0  },
+    { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_DGPS,      .rate = 0  },
+    { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_POSECEF,   .rate = 0  },
+    { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SBAS,      .rate = 0  },
+    { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_TIMEGPS,   .rate = 0  },
+    { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_VELECEF,   .rate = 0  },
+    { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SOL,       .rate = 0  },
+    { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_STATUS,    .rate = 0  },
+    { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_VELNED,    .rate = 0  },
+    { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_TIMEUTC,   .rate = 0  },
+    { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_POSLLH,    .rate = 0  },
+
+    { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_HW,        .rate = 0  },
+    { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_HW2,       .rate = 0  },
+    { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_IO,        .rate = 0  },
+    { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_MSGPP,     .rate = 0  },
+    { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_RXBUFF,    .rate = 0  },
+    { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_RXR,       .rate = 0  },
+    { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_TXBUF,     .rate = 0  },
+
+    { .msgClass = UBX_CLASS_RXM, .msgID = UBX_ID_RXM_SVSI,      .rate = 0  },
+
+    // message to enable
+    { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_PVT,       .rate = 1  },
+    { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_DOP,       .rate = 1  },
+    { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SVINFO,    .rate = 10 },
+};
+
+// private defines
+#define LAST_CONFIG_SENT_START     (-1)
+#define LAST_CONFIG_SENT_COMPLETED (-2)
+
+// private variables
+
+// enable the autoconfiguration system
+static bool enabled;
+
+static status_t *status = 0;
+
+static void append_checksum(UBXSentPacket_t *packet)
+{
+    uint8_t i;
+    uint8_t ck_a = 0;
+    uint8_t ck_b = 0;
+    uint16_t len = packet->message.header.len + sizeof(UBXSentHeader_t);
+
+    for (i = 2; i < len; i++) {
+        ck_a += packet->buffer[i];
+        ck_b += ck_a;
+    }
+
+    packet->buffer[len]     = ck_a;
+    packet->buffer[len + 1] = ck_b;
+}
+/**
+ * prepare a packet to be sent, fill the header and appends the checksum.
+ * return the total packet lenght comprising header and checksum
+ */
+static uint16_t prepare_packet(UBXSentPacket_t *packet, uint8_t classID, uint8_t messageID, uint16_t len)
+{
+    packet->message.header.prolog[0] = UBX_SYNC1;
+    packet->message.header.prolog[1] = UBX_SYNC2;
+    packet->message.header.class     = classID;
+    packet->message.header.id  = messageID;
+    packet->message.header.len = len;
+    append_checksum(packet);
+    return packet->message.header.len + sizeof(UBXSentHeader_t) + 2; // header + payload + checksum
+}
+
+static void build_request(UBXSentPacket_t *packet, uint8_t classID, uint8_t messageID, uint16_t *bytes_to_send)
+{
+    *bytes_to_send = prepare_packet(packet, classID, messageID, 0);
+}
+
+void config_rate(uint16_t *bytes_to_send)
+{
+    memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_rate_t));
+    // if rate is less than 1 uses the highest rate for current hardware
+    uint16_t rate = status->currentSettings.navRate > 0 ? status->currentSettings.navRate : 99;
+    if (ubxHwVersion < UBX_HW_VERSION_7 && rate > UBX_MAX_RATE) {
+        rate = UBX_MAX_RATE;
+    } else if (ubxHwVersion < UBX_HW_VERSION_8 && rate > UBX_MAX_RATE_VER7) {
+        rate = UBX_MAX_RATE_VER7;
+    } else if (ubxHwVersion >= UBX_HW_VERSION_8 && rate > UBX_MAX_RATE_VER8) {
+        rate = UBX_MAX_RATE_VER8;
+    }
+    uint16_t period = 1000 / rate;
+
+    status->working_packet.message.payload.cfg_rate.measRate = period;
+    status->working_packet.message.payload.cfg_rate.navRate  = 1; // must be set to 1
+    status->working_packet.message.payload.cfg_rate.timeRef  = 1; // 0 = UTC Time, 1 = GPS Time
+    *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_RATE, sizeof(ubx_cfg_rate_t));
+    status->requiredAck.clsID = UBX_CLASS_CFG;
+    status->requiredAck.msgID = UBX_ID_CFG_RATE;
+}
+
+void config_nav(uint16_t *bytes_to_send)
+{
+    memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_nav5_t));
+
+    status->working_packet.message.payload.cfg_nav5.dynModel = status->currentSettings.dynamicModel;
+    status->working_packet.message.payload.cfg_nav5.fixMode  = 2; // 1=2D only, 2=3D only, 3=Auto 2D/3D
+    // mask LSB=dyn|minEl|posFixMode|drLim|posMask|statisticHoldMask|dgpsMask|......|reservedBit0 = MSB
+
+    status->working_packet.message.payload.cfg_nav5.mask     = 0x01 + 0x04; // Dyn Model | posFixMode configuration
+    *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_NAV5, sizeof(ubx_cfg_nav5_t));
+    status->requiredAck.clsID = UBX_CLASS_CFG;
+    status->requiredAck.msgID = UBX_ID_CFG_NAV5;
+}
+
+void config_sbas(uint16_t *bytes_to_send)
+{
+    memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_sbas_t));
+
+    status->working_packet.message.payload.cfg_sbas.maxSBAS = status->currentSettings.SBASChannelsUsed < 4 ?
+                                                              status->currentSettings.SBASChannelsUsed : 3;
+
+    status->working_packet.message.payload.cfg_sbas.usage   =
+        (status->currentSettings.SBASCorrection ? UBX_CFG_SBAS_USAGE_DIFFCORR : 0) |
+        (status->currentSettings.SBASIntegrity ? UBX_CFG_SBAS_USAGE_INTEGRITY : 0) |
+        (status->currentSettings.SBASRanging ? UBX_CFG_SBAS_USAGE_RANGE : 0);
+    // If sbas is used for anything then set mode as enabled
+    status->working_packet.message.payload.cfg_sbas.mode =
+        status->working_packet.message.payload.cfg_sbas.usage != 0 ? UBX_CFG_SBAS_MODE_ENABLED : 0;
+
+    status->working_packet.message.payload.cfg_sbas.scanmode1 =
+        status->currentSettings.SBASSats == UBX_SBAS_SATS_WAAS ? UBX_CFG_SBAS_SCANMODE1_WAAS :
+        status->currentSettings.SBASSats == UBX_SBAS_SATS_EGNOS ? UBX_CFG_SBAS_SCANMODE1_EGNOS :
+        status->currentSettings.SBASSats == UBX_SBAS_SATS_MSAS ? UBX_CFG_SBAS_SCANMODE1_MSAS :
+        status->currentSettings.SBASSats == UBX_SBAS_SATS_GAGAN ? UBX_CFG_SBAS_SCANMODE1_GAGAN :
+        status->currentSettings.SBASSats == UBX_SBAS_SATS_SDCM ? UBX_CFG_SBAS_SCANMODE1_SDCM : UBX_SBAS_SATS_AUTOSCAN;
+
+    status->working_packet.message.payload.cfg_sbas.scanmode2 = UBX_CFG_SBAS_SCANMODE2;
+
+    *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_SBAS, sizeof(ubx_cfg_sbas_t));
+    status->requiredAck.clsID = UBX_CLASS_CFG;
+    status->requiredAck.msgID = UBX_ID_CFG_SBAS;
+}
+
+void config_save(uint16_t *bytes_to_send)
+{
+    memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_cfg_t));
+    // mask LSB=ioPort|msgConf|infMsg|navConf|rxmConf|||||rinvConf|antConf|....|= MSB
+    status->working_packet.message.payload.cfg_cfg.saveMask   = 0x01 | 0x08; // msgConf + navConf
+    status->working_packet.message.payload.cfg_cfg.deviceMask = UBX_CFG_CFG_ALL_DEVICES_MASK;
+    *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_CFG, sizeof(ubx_cfg_cfg_t));
+    status->requiredAck.clsID = UBX_CLASS_CFG;
+    status->requiredAck.msgID = UBX_ID_CFG_CFG;
+}
+static void configure(uint16_t *bytes_to_send)
+{
+    switch (status->lastConfigSent) {
+    case LAST_CONFIG_SENT_START:
+        config_rate(bytes_to_send);
+        break;
+    case LAST_CONFIG_SENT_START + 1:
+        config_nav(bytes_to_send);
+        break;
+    case LAST_CONFIG_SENT_START + 2:
+        config_sbas(bytes_to_send);
+        if (!status->currentSettings.storeSettings) {
+            // skips saving
+            status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED;
+        }
+        break;
+    case LAST_CONFIG_SENT_START + 3:
+        config_save(bytes_to_send);
+        status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED;
+        return;
+
+    default:
+        status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED;
+        return;
+    }
+}
+
+static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send)
+{
+    int8_t msg = status->lastConfigSent + 1;
+    uint8_t msg_count = (ubxHwVersion >= UBX_HW_VERSION_7) ?
+                        NELEMENTS(msg_config_ubx7) : NELEMENTS(msg_config_ubx6);
+    ubx_cfg_msg_t *msg_config = (ubxHwVersion >= UBX_HW_VERSION_7) ?
+                                &msg_config_ubx7[0] : &msg_config_ubx6[0];
+
+    if (msg >= 0 && msg < msg_count) {
+        status->working_packet.message.payload.cfg_msg = msg_config[msg];
+
+        *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_MSG, sizeof(ubx_cfg_msg_t));
+        status->requiredAck.clsID = UBX_CLASS_CFG;
+        status->requiredAck.msgID = UBX_ID_CFG_MSG;
+    } else {
+        status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED;
+    }
+}
+
+void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
+{
+    *bytes_to_send = 0;
+    *buffer = (char *)status->working_packet.buffer;
+    if (!status || !enabled) {
+        return; // autoconfig not enabled
+    }
+    switch (status->currentStep) {
+    case INIT_STEP_ERROR: // TODO: what to do? retries after a while? maybe gps was disconnected (this can be detected)?
+    case INIT_STEP_DISABLED:
+    case INIT_STEP_DONE:
+        return;
+
+    case INIT_STEP_START:
+    case INIT_STEP_ASK_VER:
+        status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
+        build_request(&status->working_packet, UBX_CLASS_MON, UBX_ID_MON_VER, bytes_to_send);
+        status->currentStep = INIT_STEP_WAIT_VER;
+        break;
+
+    case INIT_STEP_WAIT_VER:
+        if (ubxHwVersion > 0) {
+            status->lastConfigSent = LAST_CONFIG_SENT_START;
+            status->currentStep    = INIT_STEP_ENABLE_SENTENCES;
+            status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
+            return;
+        }
+
+        if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) > UBX_REPLY_TIMEOUT) {
+            status->currentStep = INIT_STEP_ASK_VER;
+        }
+        return;
+
+    case INIT_STEP_ENABLE_SENTENCES:
+    case INIT_STEP_CONFIGURE:
+    {
+        bool step_configure = (status->currentStep == INIT_STEP_CONFIGURE);
+        if (step_configure) {
+            configure(bytes_to_send);
+        } else {
+            enable_sentences(bytes_to_send);
+        }
+
+        status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
+        if (status->lastConfigSent == LAST_CONFIG_SENT_COMPLETED) {
+            status->lastConfigSent = LAST_CONFIG_SENT_START;
+            status->currentStep    = step_configure ? INIT_STEP_DONE : INIT_STEP_CONFIGURE;
+        } else {
+            status->currentStep = step_configure ? INIT_STEP_CONFIGURE_WAIT_ACK : INIT_STEP_ENABLE_SENTENCES_WAIT_ACK;
+        }
+        return;
+    }
+    case INIT_STEP_ENABLE_SENTENCES_WAIT_ACK:
+    case INIT_STEP_CONFIGURE_WAIT_ACK: // Wait for an ack from GPS
+    {
+        bool step_configure = (status->currentStep == INIT_STEP_CONFIGURE_WAIT_ACK);
+        if (ubxLastAck.clsID == status->requiredAck.clsID && ubxLastAck.msgID == status->requiredAck.msgID) {
+            // clear ack
+            ubxLastAck.clsID   = 0x00;
+            ubxLastAck.msgID   = 0x00;
+
+            // Continue with next configuration option
+            status->retryCount = 0;
+            status->lastConfigSent++;
+        } else if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) > UBX_REPLY_TIMEOUT) {
+            // timeout, resend the message or abort
+            status->retryCount++;
+            if (status->retryCount > UBX_MAX_RETRIES) {
+                status->currentStep = INIT_STEP_ERROR;
+                return;
+            }
+        }
+        // no abort condition, continue or retries.,
+        if (step_configure) {
+            status->currentStep = INIT_STEP_CONFIGURE;
+        } else {
+            status->currentStep = INIT_STEP_ENABLE_SENTENCES;
+        }
+        return;
+    }
+    }
+}
+
+void ubx_autoconfig_set(ubx_autoconfig_settings_t config)
+{
+    enabled = false;
+    if (config.autoconfigEnabled) {
+        if (!status) {
+            status = (status_t *)pios_malloc(sizeof(status_t));
+            memset(status, 0, sizeof(status_t));
+            status->currentStep = INIT_STEP_DISABLED;
+        }
+        status->currentSettings = config;
+        status->currentStep     = INIT_STEP_START;
+        enabled = true;
+    }
+}
+
+int32_t ubx_autoconfig_get_status()
+{
+    if (!status) {
+        return UBX_AUTOCONFIG_STATUS_DISABLED;
+    }
+    switch (status->currentStep) {
+    case INIT_STEP_ERROR:
+        return UBX_AUTOCONFIG_STATUS_ERROR;
+
+    case INIT_STEP_DISABLED:
+        return UBX_AUTOCONFIG_STATUS_DISABLED;
+
+    case INIT_STEP_DONE:
+        return UBX_AUTOCONFIG_STATUS_DONE;
+
+    default:
+        break;
+    }
+    return UBX_AUTOCONFIG_STATUS_RUNNING;
+}
diff --git a/flight/modules/StateEstimation/filterlla.c b/flight/modules/StateEstimation/filterlla.c
index 327fa9a00..352e0000c 100644
--- a/flight/modules/StateEstimation/filterlla.c
+++ b/flight/modules/StateEstimation/filterlla.c
@@ -103,7 +103,7 @@ static filterResult filter(__attribute__((unused)) stateFilter *self, stateEstim
         GPSPositionSensorGet(&gpsdata);
 
         // check if we have a valid GPS signal (not checked by StateEstimation istelf)
-        if ((gpsdata.PDOP < this->settings.MaxPDOP) && (gpsdata.Satellites >= this->settings.MinSattelites) &&
+        if ((gpsdata.PDOP < this->settings.MaxPDOP) && (gpsdata.Satellites >= this->settings.MinSatellites) &&
             (gpsdata.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) &&
             (gpsdata.Latitude != 0 || gpsdata.Longitude != 0)) {
             int32_t LLAi[3] = {
diff --git a/flight/modules/StateEstimation/filtermag.c b/flight/modules/StateEstimation/filtermag.c
index c48c4463e..4fa5afa5c 100644
--- a/flight/modules/StateEstimation/filtermag.c
+++ b/flight/modules/StateEstimation/filtermag.c
@@ -36,8 +36,9 @@
 #include <revosettings.h>
 #include <systemalarms.h>
 #include <homelocation.h>
-
+#include <auxmagsettings.h>
 #include <CoordinateConversions.h>
+#include <mathmisc.h>
 
 // Private constants
 //
@@ -47,10 +48,12 @@
 struct data {
     RevoCalibrationData revoCalibration;
     RevoSettingsData    revoSettings;
+    AuxMagSettingsUsageOptions auxMagUsage;
     uint8_t warningcount;
     uint8_t errorcount;
     float   homeLocationBe[3];
-    float   magBe2;
+    float   magBe;
+    float   invMagBe;
     float   magBias[3];
 };
 
@@ -60,9 +63,9 @@ struct data {
 
 static int32_t init(stateFilter *self);
 static filterResult filter(stateFilter *self, stateEstimation *state);
-static void checkMagValidity(struct data *this, float mag[3]);
+static bool checkMagValidity(struct data *this, float error, bool setAlarms);
 static void magOffsetEstimation(struct data *this, float mag[3]);
-
+static float getMagError(struct data *this, float mag[3]);
 
 int32_t filterMagInitialize(stateFilter *handle)
 {
@@ -80,80 +83,132 @@ static int32_t init(stateFilter *self)
     this->magBias[0]   = this->magBias[1] = this->magBias[2] = 0.0f;
     this->warningcount = this->errorcount = 0;
     HomeLocationBeGet(this->homeLocationBe);
-    // magBe2 holds the squared magnetic vector length (extpected)
-    this->magBe2 = this->homeLocationBe[0] * this->homeLocationBe[0] + this->homeLocationBe[1] * this->homeLocationBe[1] + this->homeLocationBe[2] * this->homeLocationBe[2];
+    // magBe holds the magnetic vector length (expected)
+    this->magBe    = vector_lengthf(this->homeLocationBe, 3);
+    this->invMagBe = 1.0f / this->magBe;
     RevoCalibrationGet(&this->revoCalibration);
     RevoSettingsGet(&this->revoSettings);
+    AuxMagSettingsUsageGet(&this->auxMagUsage);
     return 0;
 }
 
 static filterResult filter(stateFilter *self, stateEstimation *state)
 {
-    struct data *this = (struct data *)self->localdata;
+    struct data *this   = (struct data *)self->localdata;
+    float auxMagError;
+    float boardMagError;
+    float temp_mag[3]   = { 0 };
+    uint8_t temp_status = MAGSTATUS_INVALID;
+    uint8_t magSamples  = 0;
 
-    if (IS_SET(state->updated, SENSORUPDATES_mag)) {
-        checkMagValidity(this, state->mag);
-        if (this->revoCalibration.MagBiasNullingRate > 0) {
-            magOffsetEstimation(this, state->mag);
+    // Uses the external mag when available
+    if ((this->auxMagUsage != AUXMAGSETTINGS_USAGE_ONBOARDONLY) &&
+        IS_SET(state->updated, SENSORUPDATES_auxMag)) {
+        auxMagError = getMagError(this, state->auxMag);
+        // Handles alarms only if it will rely on aux mag only
+        bool auxMagValid = checkMagValidity(this, auxMagError, (this->auxMagUsage == AUXMAGSETTINGS_USAGE_AUXONLY));
+        // if we are going to use Aux only, force the update even if mag is invalid
+        if (auxMagValid || (this->auxMagUsage == AUXMAGSETTINGS_USAGE_AUXONLY)) {
+            temp_mag[0] = state->auxMag[0];
+            temp_mag[1] = state->auxMag[1];
+            temp_mag[2] = state->auxMag[2];
+            temp_status = MAGSTATUS_AUX;
+            magSamples++;
         }
     }
 
+    if ((this->auxMagUsage != AUXMAGSETTINGS_USAGE_AUXONLY) &&
+        IS_SET(state->updated, SENSORUPDATES_boardMag)) {
+        // TODO:mag Offset estimation works with onboard mag only right now.
+        if (this->revoCalibration.MagBiasNullingRate > 0) {
+            magOffsetEstimation(this, state->boardMag);
+        }
+        boardMagError = getMagError(this, state->boardMag);
+        // sets warning only if no mag data are available (aux is invalid or missing)
+        bool boardMagValid = checkMagValidity(this, boardMagError, (temp_status == MAGSTATUS_INVALID));
+        // force it to be set to board mag value if no data has been feed to temp_mag yet.
+        // this works also as a failsafe in case aux mag stops feeding data.
+        if (boardMagValid || (temp_status == MAGSTATUS_INVALID)) {
+            temp_mag[0] += state->boardMag[0];
+            temp_mag[1] += state->boardMag[1];
+            temp_mag[2] += state->boardMag[2];
+            temp_status  = MAGSTATUS_OK;
+            magSamples++;
+        }
+    }
+
+    if (magSamples > 1) {
+        temp_mag[0] *= 0.5f;
+        temp_mag[1] *= 0.5f;
+        temp_mag[2] *= 0.5f;
+    }
+
+    if (temp_status != MAGSTATUS_INVALID) {
+        state->mag[0]   = temp_mag[0];
+        state->mag[1]   = temp_mag[1];
+        state->mag[2]   = temp_mag[2];
+        state->updated |= SENSORUPDATES_mag;
+    }
+    state->magStatus = temp_status;
     return FILTERRESULT_OK;
 }
 
 /**
  * check validity of magnetometers
  */
-static void checkMagValidity(struct data *this, float mag[3])
+static bool checkMagValidity(struct data *this, float error, bool setAlarms)
 {
-        #define ALARM_THRESHOLD 5
-
-    // mag2 holds the actual magnetic vector length (squared)
-    float mag2 = mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2];
-
-    // warning and error thresholds
-    // avoud sqrt() : minlimit<actual<maxlimit === minlimit²<actual²<maxlimit²
-    //
-    // actual = |mag|
-    // minlimit = |Be| - maxDeviation*|Be| = |Be| * (1 - maxDeviation)
-    // maxlimit = |Be| + maxDeviation*|Be| = |Be| * (1 + maxDeviation)
-    // minlimit² = |Be|² * ( 1 - 2*maxDeviation + maxDeviation²)
-    // maxlimit² = |Be|² * ( 1 + 2*maxDeviation + maxDeviation²)
-    //
-
-    float minWarning2 = this->magBe2 * (1.0f - 2.0f * this->revoSettings.MagnetometerMaxDeviation.Warning + this->revoSettings.MagnetometerMaxDeviation.Warning * this->revoSettings.MagnetometerMaxDeviation.Warning);
-    float maxWarning2 = this->magBe2 * (1.0f + 2.0f * this->revoSettings.MagnetometerMaxDeviation.Warning + this->revoSettings.MagnetometerMaxDeviation.Warning * this->revoSettings.MagnetometerMaxDeviation.Warning);
-    float minError2   = this->magBe2 * (1.0f - 2.0f * this->revoSettings.MagnetometerMaxDeviation.Error + this->revoSettings.MagnetometerMaxDeviation.Error * this->revoSettings.MagnetometerMaxDeviation.Error);
-    float maxError2   = this->magBe2 * (1.0f + 2.0f * this->revoSettings.MagnetometerMaxDeviation.Error + this->revoSettings.MagnetometerMaxDeviation.Error * this->revoSettings.MagnetometerMaxDeviation.Error);
+    #define ALARM_THRESHOLD 5
 
     // set errors
-    if (minWarning2 < mag2 && mag2 < maxWarning2) {
+    if (error < this->revoSettings.MagnetometerMaxDeviation.Warning) {
         this->warningcount = 0;
         this->errorcount   = 0;
         AlarmsClear(SYSTEMALARMS_ALARM_MAGNETOMETER);
-    } else if (minError2 < mag2 && mag2 < maxError2) {
+        return true;
+    }
+
+    if (error < this->revoSettings.MagnetometerMaxDeviation.Error) {
         this->errorcount = 0;
         if (this->warningcount > ALARM_THRESHOLD) {
-            AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_WARNING);
+            if (setAlarms) {
+                AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_WARNING);
+            }
+            return false;
         } else {
             this->warningcount++;
-        }
-    } else {
-        if (this->errorcount > ALARM_THRESHOLD) {
-            AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_CRITICAL);
-        } else {
-            this->errorcount++;
+            return true;
         }
     }
+
+    if (this->errorcount > ALARM_THRESHOLD) {
+        if (setAlarms) {
+            AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_CRITICAL);
+        }
+        return false;
+    } else {
+        this->errorcount++;
+    }
+    // still in "grace period"
+    return true;
 }
 
+static float getMagError(struct data *this, float mag[3])
+{
+    // vector norm
+    float magnitude = vector_lengthf(mag, 3);
+    // absolute value of relative error against Be
+    float error     = fabsf(magnitude - this->magBe) * this->invMagBe;
+
+    return error;
+}
 
 /**
  * Perform an update of the @ref MagBias based on
  * Magmeter Offset Cancellation: Theory and Implementation,
  * revisited William Premerlani, October 14, 2011
  */
-static void magOffsetEstimation(struct data *this, float mag[3])
+void magOffsetEstimation(struct data *this, float mag[3])
 {
 #if 0
     // Constants, to possibly go into a UAVO
@@ -244,7 +299,6 @@ static void magOffsetEstimation(struct data *this, float mag[3])
 #endif // if 0
 }
 
-
 /**
  * @}
  * @}
diff --git a/flight/modules/StateEstimation/inc/stateestimation.h b/flight/modules/StateEstimation/inc/stateestimation.h
index d93235f4e..fd399c818 100644
--- a/flight/modules/StateEstimation/inc/stateestimation.h
+++ b/flight/modules/StateEstimation/inc/stateestimation.h
@@ -47,6 +47,8 @@ typedef enum {
     SENSORUPDATES_gyro         = 1 << 0,
         SENSORUPDATES_accel    = 1 << 1,
         SENSORUPDATES_mag      = 1 << 2,
+        SENSORUPDATES_boardMag = 1 << 10,
+        SENSORUPDATES_auxMag   = 1 << 9,
         SENSORUPDATES_attitude = 1 << 3,
         SENSORUPDATES_pos      = 1 << 4,
         SENSORUPDATES_vel      = 1 << 5,
@@ -55,15 +57,21 @@ typedef enum {
         SENSORUPDATES_lla      = 1 << 8,
 } sensorUpdates;
 
+#define MAGSTATUS_OK      1
+#define MAGSTATUS_AUX     2
+#define MAGSTATUS_INVALID 0
 typedef struct {
-    float gyro[3];
-    float accel[3];
-    float mag[3];
-    float attitude[4];
-    float pos[3];
-    float vel[3];
-    float airspeed[2];
-    float baro[1];
+    float   gyro[3];
+    float   accel[3];
+    float   mag[3];
+    float   attitude[4];
+    float   pos[3];
+    float   vel[3];
+    float   airspeed[2];
+    float   baro[1];
+    float   auxMag[3];
+    uint8_t magStatus;
+    float   boardMag[3];
     sensorUpdates updated;
 } stateEstimation;
 
diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c
index 079db8ca1..cf33bbbc4 100644
--- a/flight/modules/StateEstimation/stateestimation.c
+++ b/flight/modules/StateEstimation/stateestimation.c
@@ -41,6 +41,7 @@
 #include <gpspositionsensor.h>
 #include <gpsvelocitysensor.h>
 #include <homelocation.h>
+#include <auxmagsensor.h>
 
 #include <gyrostate.h>
 #include <accelstate.h>
@@ -161,14 +162,17 @@ static float gyroDelta[3];
 
 // preconfigured filter chains selectable via revoSettings.FusionAlgorithm
 static const filterPipeline *cfQueue = &(filterPipeline) {
-    .filter = &airFilter,
+    .filter = &magFilter,
     .next   = &(filterPipeline) {
-        .filter = &baroiFilter,
+        .filter = &airFilter,
         .next   = &(filterPipeline) {
-            .filter = &altitudeFilter,
+            .filter = &baroiFilter,
             .next   = &(filterPipeline) {
-                .filter = &cfFilter,
-                .next   = NULL,
+                .filter = &altitudeFilter,
+                .next   = &(filterPipeline) {
+                    .filter = &cfFilter,
+                    .next   = NULL,
+                }
             }
         }
     }
@@ -267,6 +271,7 @@ int32_t StateEstimationInitialize(void)
 
     GyroSensorInitialize();
     MagSensorInitialize();
+    AuxMagSensorInitialize();
     BaroSensorInitialize();
     AirspeedSensorInitialize();
     GPSVelocitySensorInitialize();
@@ -290,6 +295,7 @@ int32_t StateEstimationInitialize(void)
     MagSensorConnectCallback(&sensorUpdatedCb);
     BaroSensorConnectCallback(&sensorUpdatedCb);
     AirspeedSensorConnectCallback(&sensorUpdatedCb);
+    AuxMagSensorConnectCallback(&sensorUpdatedCb);
     GPSVelocitySensorConnectCallback(&sensorUpdatedCb);
     GPSPositionSensorConnectCallback(&sensorUpdatedCb);
 
@@ -423,7 +429,8 @@ static void StateEstimationCb(void)
             gyroRaw[2] = states.gyro[2];
         }
         FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AccelSensor, accel, x, y, z);
-        FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(MagSensor, mag, x, y, z);
+        FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(MagSensor, boardMag, x, y, z);
+        FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AuxMagSensor, auxMag, x, y, z);
         FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GPSVelocitySensor, vel, North, East, Down);
         FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(BaroSensor, baro, Altitude, true);
         FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_2_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(AirspeedSensor, airspeed, CalibratedAirspeed, TrueAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE);
@@ -467,7 +474,26 @@ static void StateEstimationCb(void)
             gyroDelta[2] = states.gyro[2] - gyroRaw[2];
         }
         EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(AccelState, accel, x, y, z);
-        EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(MagState, mag, x, y, z);
+        if (IS_SET(states.updated, SENSORUPDATES_mag)) {
+            MagStateData s;
+
+            MagStateGet(&s);
+            s.x = states.mag[0];
+            s.y = states.mag[1];
+            s.z = states.mag[2];
+            switch (states.magStatus) {
+            case MAGSTATUS_OK:
+                s.Source = MAGSTATE_SOURCE_ONBOARD;
+                break;
+            case MAGSTATUS_AUX:
+                s.Source = MAGSTATE_SOURCE_AUX;
+                break;
+            default:
+                s.Source = MAGSTATE_SOURCE_INVALID;
+            }
+            MagStateSet(&s);
+        }
+
         EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(PositionState, pos, North, East, Down);
         EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(VelocityState, vel, North, East, Down);
         EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_2_DIMENSIONS(AirspeedState, airspeed, CalibratedAirspeed, TrueAirspeed);
@@ -567,7 +593,11 @@ static void sensorUpdatedCb(UAVObjEvent *ev)
     }
 
     if (ev->obj == MagSensorHandle()) {
-        updatedSensors |= SENSORUPDATES_mag;
+        updatedSensors |= SENSORUPDATES_boardMag;
+    }
+
+    if (ev->obj == AuxMagSensorHandle()) {
+        updatedSensors |= SENSORUPDATES_auxMag;
     }
 
     if (ev->obj == GPSPositionSensorHandle()) {
diff --git a/flight/targets/boards/coptercontrol/firmware/Makefile b/flight/targets/boards/coptercontrol/firmware/Makefile
index 7a5a7e90f..e476dbb85 100644
--- a/flight/targets/boards/coptercontrol/firmware/Makefile
+++ b/flight/targets/boards/coptercontrol/firmware/Makefile
@@ -76,6 +76,7 @@ ifndef TESTAPP
     SRC += $(OPUAVTALK)/uavtalk.c
     SRC += $(OPUAVOBJ)/uavobjectmanager.c
     SRC += $(OPUAVOBJ)/eventdispatcher.c
+    SRC += $(FLIGHTLIB)/auxmagsupport.c
 
     ## UAVObjects
     SRC += $(OPUAVSYNTHDIR)/accessorydesired.c
@@ -128,6 +129,7 @@ ifndef TESTAPP
     SRC += $(OPUAVSYNTHDIR)/airspeedstate.c
     SRC += $(OPUAVSYNTHDIR)/mpu6000settings.c
     SRC += $(OPUAVSYNTHDIR)/perfcounter.c
+    SRC += $(OPUAVSYNTHDIR)/auxmagsensor.c
 else
     ## Test Code
     SRC += $(OPTESTS)/test_common.c
diff --git a/flight/targets/boards/discoveryf4bare/firmware/Makefile b/flight/targets/boards/discoveryf4bare/firmware/Makefile
index bc8e72959..bf7d21d28 100644
--- a/flight/targets/boards/discoveryf4bare/firmware/Makefile
+++ b/flight/targets/boards/discoveryf4bare/firmware/Makefile
@@ -86,6 +86,7 @@ ifndef TESTAPP
 	SRC += $(FLIGHTLIB)/plans.c
     SRC += $(FLIGHTLIB)/WorldMagModel.c
     SRC += $(FLIGHTLIB)/insgps13state.c
+    SRC += $(FLIGHTLIB)/auxmagsupport.c
 
     ## UAVObjects
     include ./UAVObjects.inc
diff --git a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc
index 49867c39e..60ac23651 100644
--- a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc
+++ b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc
@@ -31,6 +31,8 @@ UAVOBJSRCFILENAMES += gyrosensor
 UAVOBJSRCFILENAMES += accelstate
 UAVOBJSRCFILENAMES += accelsensor
 UAVOBJSRCFILENAMES += magsensor
+UAVOBJSRCFILENAMES += auxmagsensor
+UAVOBJSRCFILENAMES += auxmagsettings
 UAVOBJSRCFILENAMES += magstate
 UAVOBJSRCFILENAMES += barosensor
 UAVOBJSRCFILENAMES += airspeedsensor
@@ -54,6 +56,7 @@ UAVOBJSRCFILENAMES += gpssatellites
 UAVOBJSRCFILENAMES += gpstime
 UAVOBJSRCFILENAMES += gpsvelocitysensor
 UAVOBJSRCFILENAMES += gpssettings
+UAVOBJSRCFILENAMES += gpsextendedstatus
 UAVOBJSRCFILENAMES += fixedwingpathfollowersettings
 UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus
 UAVOBJSRCFILENAMES += vtolpathfollowersettings
diff --git a/flight/targets/boards/osd/firmware/Makefile b/flight/targets/boards/osd/firmware/Makefile
index 928f1310b..35c8f6e25 100644
--- a/flight/targets/boards/osd/firmware/Makefile
+++ b/flight/targets/boards/osd/firmware/Makefile
@@ -70,6 +70,7 @@ ifndef TESTAPP
 
     ## Misc library functions
     SRC += $(FLIGHTLIB)/WorldMagModel.c
+    SRC += $(FLIGHTLIB)/auxmagsupport.c
 
     ## UAVObjects
     SRC += $(OPUAVSYNTHDIR)/objectpersistence.c
@@ -98,6 +99,8 @@ ifndef TESTAPP
     SRC += $(OPUAVSYNTHDIR)/osdsettings.c
     SRC += $(OPUAVSYNTHDIR)/barosensor.c
     SRC += $(OPUAVSYNTHDIR)/magsensor.c
+    SRC += $(OPUAVSYNTHDIR)/auxmagsensor.c
+    SRC += $(OPUAVSYNTHDIR)/gpsextendedstatus.c
 else
     ## Test Code
     SRC += $(OPTESTS)/test_common.c
diff --git a/flight/targets/boards/revolution/firmware/Makefile b/flight/targets/boards/revolution/firmware/Makefile
index 52b5f8a64..21b23ed68 100644
--- a/flight/targets/boards/revolution/firmware/Makefile
+++ b/flight/targets/boards/revolution/firmware/Makefile
@@ -87,6 +87,7 @@ ifndef TESTAPP
     SRC += $(FLIGHTLIB)/plans.c
     SRC += $(FLIGHTLIB)/WorldMagModel.c
     SRC += $(FLIGHTLIB)/insgps13state.c
+    SRC += $(FLIGHTLIB)/auxmagsupport.c
 
     ## UAVObjects
     include ./UAVObjects.inc
diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc
index 49867c39e..60ac23651 100644
--- a/flight/targets/boards/revolution/firmware/UAVObjects.inc
+++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc
@@ -31,6 +31,8 @@ UAVOBJSRCFILENAMES += gyrosensor
 UAVOBJSRCFILENAMES += accelstate
 UAVOBJSRCFILENAMES += accelsensor
 UAVOBJSRCFILENAMES += magsensor
+UAVOBJSRCFILENAMES += auxmagsensor
+UAVOBJSRCFILENAMES += auxmagsettings
 UAVOBJSRCFILENAMES += magstate
 UAVOBJSRCFILENAMES += barosensor
 UAVOBJSRCFILENAMES += airspeedsensor
@@ -54,6 +56,7 @@ UAVOBJSRCFILENAMES += gpssatellites
 UAVOBJSRCFILENAMES += gpstime
 UAVOBJSRCFILENAMES += gpsvelocitysensor
 UAVOBJSRCFILENAMES += gpssettings
+UAVOBJSRCFILENAMES += gpsextendedstatus
 UAVOBJSRCFILENAMES += fixedwingpathfollowersettings
 UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus
 UAVOBJSRCFILENAMES += vtolpathfollowersettings
diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c
index b26f75a8c..e029b90dc 100644
--- a/flight/targets/boards/revolution/firmware/pios_board.c
+++ b/flight/targets/boards/revolution/firmware/pios_board.c
@@ -217,6 +217,7 @@ uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE];
 #define PIOS_COM_TELEM_RF_TX_BUF_LEN     512
 
 #define PIOS_COM_GPS_RX_BUF_LEN          32
+#define PIOS_COM_GPS_TX_BUF_LEN          32
 
 #define PIOS_COM_TELEM_USB_RX_BUF_LEN    65
 #define PIOS_COM_TELEM_USB_TX_BUF_LEN    65
@@ -609,7 +610,7 @@ void PIOS_Board_Init(void)
         PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id);
         break;
     case HWSETTINGS_RM_MAINPORT_GPS:
-        PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_usart_com_driver, &pios_com_gps_id);
+        PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id);
         break;
     case HWSETTINGS_RM_MAINPORT_SBUS:
 #if defined(PIOS_INCLUDE_SBUS)
@@ -699,7 +700,7 @@ void PIOS_Board_Init(void)
 #endif /* PIOS_INCLUDE_I2C */
         break;
     case HWSETTINGS_RM_FLEXIPORT_GPS:
-        PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_usart_com_driver, &pios_com_gps_id);
+        PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id);
         break;
     case HWSETTINGS_RM_FLEXIPORT_DSM2:
     case HWSETTINGS_RM_FLEXIPORT_DSMX10BIT:
diff --git a/flight/targets/boards/revoproto/firmware/Makefile b/flight/targets/boards/revoproto/firmware/Makefile
index 9933124a3..f8f0f5336 100644
--- a/flight/targets/boards/revoproto/firmware/Makefile
+++ b/flight/targets/boards/revoproto/firmware/Makefile
@@ -85,6 +85,7 @@ ifndef TESTAPP
     SRC += $(FLIGHTLIB)/plans.c
     SRC += $(FLIGHTLIB)/WorldMagModel.c
     SRC += $(FLIGHTLIB)/insgps13state.c
+    SRC += $(FLIGHTLIB)/auxmagsupport.c
 
     ## UAVObjects
     include ./UAVObjects.inc
diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc
index 3905af42f..f30d855a2 100644
--- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc
+++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc
@@ -31,6 +31,8 @@ UAVOBJSRCFILENAMES += gyrosensor
 UAVOBJSRCFILENAMES += accelstate
 UAVOBJSRCFILENAMES += accelsensor
 UAVOBJSRCFILENAMES += magsensor
+UAVOBJSRCFILENAMES += auxmagsensor
+UAVOBJSRCFILENAMES += auxmagsettings
 UAVOBJSRCFILENAMES += magstate
 UAVOBJSRCFILENAMES += barosensor
 UAVOBJSRCFILENAMES += airspeedsensor
@@ -54,6 +56,7 @@ UAVOBJSRCFILENAMES += gpssatellites
 UAVOBJSRCFILENAMES += gpstime
 UAVOBJSRCFILENAMES += gpsvelocitysensor
 UAVOBJSRCFILENAMES += gpssettings
+UAVOBJSRCFILENAMES += gpsextendedstatus
 UAVOBJSRCFILENAMES += fixedwingpathfollowersettings
 UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus
 UAVOBJSRCFILENAMES += vtolpathfollowersettings
diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc
index 907ff6833..1498c2e72 100644
--- a/flight/targets/boards/simposix/firmware/UAVObjects.inc
+++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc
@@ -36,6 +36,8 @@ UAVOBJSRCFILENAMES += gyrosensor
 UAVOBJSRCFILENAMES += accelstate
 UAVOBJSRCFILENAMES += accelsensor
 UAVOBJSRCFILENAMES += magsensor
+UAVOBJSRCFILENAMES += auxmagsensor
+UAVOBJSRCFILENAMES += auxmagsettings
 UAVOBJSRCFILENAMES += magstate
 UAVOBJSRCFILENAMES += barosensor
 UAVOBJSRCFILENAMES += airspeedsensor
diff --git a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp
index 6a9200d8a..f877a44b5 100644
--- a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp
+++ b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp
@@ -29,7 +29,7 @@
 #include "extensionsystem/pluginmanager.h"
 #include "calibration/calibrationuiutils.h"
 
-#include "math.h"
+#include <math.h>
 #include <QThread>
 #include "QDebug"
 
@@ -45,6 +45,7 @@ namespace OpenPilot {
 SixPointCalibrationModel::SixPointCalibrationModel(QObject *parent) :
     QObject(parent),
     calibratingMag(false),
+    calibratingAuxMag(false),
     calibratingAccel(false),
     calibrationStepsMag(),
     calibrationStepsAccelOnly(),
@@ -68,6 +69,21 @@ SixPointCalibrationModel::SixPointCalibrationModel(QObject *parent) :
         << CalibrationStep(CALIBRATION_HELPER_IMAGE_SUW,
                        tr("Place with left side down, nose south and press Save Position..."));
 
+    // All mag calibration matrices have the same structure, this is also used when calculating bias/transforms
+    // this is enforced using assert.
+    Q_STATIC_ASSERT((int)RevoCalibration::MAG_TRANSFORM_R0C0 == (int)AuxMagSettings::MAG_TRANSFORM_R0C0 &&
+                    (int)RevoCalibration::MAG_TRANSFORM_R1C0 == (int)AuxMagSettings::MAG_TRANSFORM_R1C0 &&
+                    (int)RevoCalibration::MAG_TRANSFORM_R2C0 == (int)AuxMagSettings::MAG_TRANSFORM_R2C0 &&
+                    (int)RevoCalibration::MAG_TRANSFORM_R0C1 == (int)AuxMagSettings::MAG_TRANSFORM_R0C1 &&
+                    (int)RevoCalibration::MAG_TRANSFORM_R1C1 == (int)AuxMagSettings::MAG_TRANSFORM_R1C1 &&
+                    (int)RevoCalibration::MAG_TRANSFORM_R2C1 == (int)AuxMagSettings::MAG_TRANSFORM_R2C1 &&
+                    (int)RevoCalibration::MAG_TRANSFORM_R0C2 == (int)AuxMagSettings::MAG_TRANSFORM_R0C2 &&
+                    (int)RevoCalibration::MAG_TRANSFORM_R1C2 == (int)AuxMagSettings::MAG_TRANSFORM_R1C2 &&
+                    (int)RevoCalibration::MAG_TRANSFORM_R2C2 == (int)AuxMagSettings::MAG_TRANSFORM_R2C2 &&
+                    (int)RevoCalibration::MAG_BIAS_X == (int)AuxMagSettings::MAG_BIAS_X &&
+                    (int)RevoCalibration::MAG_BIAS_Y == (int)AuxMagSettings::MAG_BIAS_Y &&
+                    (int)RevoCalibration::MAG_BIAS_Z == (int)AuxMagSettings::MAG_BIAS_Z);
+
     calibrationStepsAccelOnly.clear();
     calibrationStepsAccelOnly << CalibrationStep(CALIBRATION_HELPER_IMAGE_NED,
                                                  tr("Place horizontally and press Save Position..."))
@@ -85,14 +101,20 @@ SixPointCalibrationModel::SixPointCalibrationModel(QObject *parent) :
     revoCalibration   = RevoCalibration::GetInstance(getObjectManager());
     Q_ASSERT(revoCalibration);
 
+    auxMagSettings = AuxMagSettings::GetInstance(getObjectManager());
+    Q_ASSERT(auxMagSettings);
+
     accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager());
     Q_ASSERT(accelGyroSettings);
 
     accelState   = AccelState::GetInstance(getObjectManager());
     Q_ASSERT(accelState);
 
-    magState     = MagState::GetInstance(getObjectManager());
-    Q_ASSERT(magState);
+    magSensor    = MagSensor::GetInstance(getObjectManager());
+    Q_ASSERT(magSensor);
+
+    auxMagSensor = AuxMagSensor::GetInstance(getObjectManager());
+    Q_ASSERT(auxMagSensor);
 
     homeLocation = HomeLocation::GetInstance(getObjectManager());
     Q_ASSERT(homeLocation);
@@ -166,6 +188,26 @@ void SixPointCalibrationModel::start(bool calibrateAccel, bool calibrateMag)
 
     revoCalibration->setData(revoCalibrationData);
 
+    // Calibration AuxMag
+    AuxMagSettings::DataFields auxMagSettingsData = auxMagSettings->getData();
+    memento.auxMagSettings = auxMagSettingsData;
+
+    // Reset the transformation matrix to identity
+    for (int i = 0; i < AuxMagSettings::MAG_TRANSFORM_R2C2; i++) {
+        auxMagSettingsData.mag_transform[i] = 0;
+    }
+    auxMagSettingsData.mag_transform[AuxMagSettings::MAG_TRANSFORM_R0C0] = 1;
+    auxMagSettingsData.mag_transform[AuxMagSettings::MAG_TRANSFORM_R1C1] = 1;
+    auxMagSettingsData.mag_transform[AuxMagSettings::MAG_TRANSFORM_R2C2] = 1;
+    auxMagSettingsData.mag_bias[AuxMagSettings::MAG_BIAS_X] = 0;
+    auxMagSettingsData.mag_bias[AuxMagSettings::MAG_BIAS_Y] = 0;
+    auxMagSettingsData.mag_bias[AuxMagSettings::MAG_BIAS_Z] = 0;
+
+    // Disable adaptive mag nulling
+    auxMagSettingsData.MagBiasNullingRate = 0;
+
+    auxMagSettings->setData(auxMagSettingsData);
+
     QThread::usleep(100000);
 
     mag_accum_x.clear();
@@ -186,12 +228,19 @@ void SixPointCalibrationModel::start(bool calibrateAccel, bool calibrateMag)
     }
 
     // Need to get as many mag updates as possible
-    memento.magStateMetadata = magState->getMetadata();
+    memento.magSensorMetadata    = magSensor->getMetadata();
+    memento.auxMagSensorMetadata = auxMagSensor->getMetadata();
+
     if (calibrateMag) {
-        UAVObject::Metadata mdata = magState->getMetadata();
+        UAVObject::Metadata mdata = magSensor->getMetadata();
         UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
         mdata.flightTelemetryUpdatePeriod = 100;
-        magState->setMetadata(mdata);
+        magSensor->setMetadata(mdata);
+
+        mdata = auxMagSensor->getMetadata();
+        UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
+        mdata.flightTelemetryUpdatePeriod = 100;
+        auxMagSensor->setMetadata(mdata);
     }
 
     // reset dirty state to forget previous unsaved runs
@@ -229,6 +278,9 @@ void SixPointCalibrationModel::savePositionData()
     mag_accum_x.clear();
     mag_accum_y.clear();
     mag_accum_z.clear();
+    aux_mag_accum_x.clear();
+    aux_mag_accum_y.clear();
+    aux_mag_accum_z.clear();
 
     collectingData = true;
 
@@ -236,10 +288,12 @@ void SixPointCalibrationModel::savePositionData()
 #ifdef FITTING_USING_CONTINOUS_ACQUISITION
         // Mag samples are acquired during the whole calibration session, to be used for ellipsoid fit.
         if (!position) {
-            connect(magState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(continouslyGetMagSamples(UAVObject *)));
+            connect(magSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(continouslyGetMagSamples(UAVObject *)));
+            connect(auxMagSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(continouslyGetMagSamples(UAVObject *)));
         }
 #endif // FITTING_USING_CONTINOUS_ACQUISITION
-        connect(magState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
+        connect(magSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
+        connect(auxMagSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
     }
     if (calibratingAccel) {
         connect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
@@ -264,8 +318,8 @@ void SixPointCalibrationModel::getSample(UAVObject *obj)
             accel_accum_x.append(accelStateData.x);
             accel_accum_y.append(accelStateData.y);
             accel_accum_z.append(accelStateData.z);
-        } else if (obj->getObjID() == MagState::OBJID) {
-            MagState::DataFields magData = magState->getData();
+        } else if (obj->getObjID() == MagSensor::OBJID) {
+            MagSensor::DataFields magData = magSensor->getData();
             mag_accum_x.append(magData.x);
             mag_accum_y.append(magData.y);
             mag_accum_z.append(magData.z);
@@ -274,6 +328,19 @@ void SixPointCalibrationModel::getSample(UAVObject *obj)
             mag_fit_y.append(magData.y);
             mag_fit_z.append(magData.z);
 #endif // FITTING_USING_CONTINOUS_ACQUISITION
+        } else if (obj->getObjID() == AuxMagSensor::OBJID) {
+            AuxMagSensor::DataFields auxMagData = auxMagSensor->getData();
+            if (auxMagData.Status == AuxMagSensor::STATUS_OK) {
+                aux_mag_accum_x.append(auxMagData.x);
+                aux_mag_accum_y.append(auxMagData.y);
+                aux_mag_accum_z.append(auxMagData.z);
+                calibratingAuxMag = true;
+#ifndef FITTING_USING_CONTINOUS_ACQUISITION
+                aux_mag_fit_x.append(auxMagData.x);
+                aux_mag_fit_y.append(auxMagData.y);
+                aux_mag_fit_z.append(auxMagData.z);
+#endif // FITTING_USING_CONTINOUS_ACQUISITION
+            }
         } else {
             Q_ASSERT(0);
         }
@@ -307,10 +374,8 @@ void SixPointCalibrationModel::getSample(UAVObject *obj)
 
         // Store the mean for this position for the mag
         if (calibratingMag) {
-            disconnect(magState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
-            mag_data_x[position] = CalibrationUtils::listMean(mag_accum_x);
-            mag_data_y[position] = CalibrationUtils::listMean(mag_accum_y);
-            mag_data_z[position] = CalibrationUtils::listMean(mag_accum_z);
+            disconnect(magSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
+            disconnect(auxMagSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
         }
 
         position = (position + 1) % 6;
@@ -322,17 +387,19 @@ void SixPointCalibrationModel::getSample(UAVObject *obj)
             // done...
 #ifdef FITTING_USING_CONTINOUS_ACQUISITION
             if (calibratingMag) {
-                disconnect(magState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(continouslyGetMagSamples(UAVObject *)));
+                disconnect(magSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(continouslyGetMagSamples(UAVObject *)));
+                disconnect(auxMagSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(continouslyGetMagSamples(UAVObject *)));
             }
 #endif // FITTING_USING_CONTINOUS_ACQUISITION
             compute();
 
             // Restore original settings
             accelState->setMetadata(memento.accelStateMetadata);
-            magState->setMetadata(memento.magStateMetadata);
+            magSensor->setMetadata(memento.magSensorMetadata);
+            auxMagSensor->setMetadata(memento.auxMagSensorMetadata);
             revoCalibration->setData(memento.revoCalibrationData);
             accelGyroSettings->setData(memento.accelGyroSettingsData);
-
+            auxMagSettings->setData(memento.auxMagSettings);
             // Recall saved board rotation
             recallBoardRotation();
 
@@ -347,11 +414,19 @@ void SixPointCalibrationModel::continouslyGetMagSamples(UAVObject *obj)
 {
     QMutexLocker lock(&sensorsUpdateLock);
 
-    if (obj->getObjID() == MagState::OBJID) {
-        MagState::DataFields magStateData = magState->getData();
-        mag_fit_x.append(magStateData.x);
-        mag_fit_y.append(magStateData.y);
-        mag_fit_z.append(magStateData.z);
+    if (obj->getObjID() == MagSensor::OBJID) {
+        MagSensor::DataFields magSensorData = magSensor->getData();
+        mag_fit_x.append(magSensorData.x);
+        mag_fit_y.append(magSensorData.y);
+        mag_fit_z.append(magSensorData.z);
+    } else if (obj->getObjID() == AuxMagSensor::OBJID) {
+        AuxMagSensor::DataFields auxMagData = auxMagSensor->getData();
+        if (auxMagData.Status == AuxMagSensor::STATUS_OK) {
+            aux_mag_fit_x.append(auxMagData.x);
+            aux_mag_fit_y.append(auxMagData.y);
+            aux_mag_fit_z.append(auxMagData.z);
+            calibratingAuxMag = true;
+        }
     }
 }
 
@@ -365,6 +440,7 @@ void SixPointCalibrationModel::compute()
     double Be_length;
 
     RevoCalibration::DataFields revoCalibrationData     = revoCalibration->getData();
+    AuxMagSettings::DataFields auxCalibrationData    = auxMagSettings->getData();
     AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
 
     HomeLocation::DataFields homeLocationData = homeLocation->getData();
@@ -384,51 +460,21 @@ void SixPointCalibrationModel::compute()
     // Calibration mag
     if (calibratingMag) {
         Be_length = sqrt(pow(homeLocationData.Be[0], 2) + pow(homeLocationData.Be[1], 2) + pow(homeLocationData.Be[2], 2));
-        int vectSize = mag_fit_x.count();
-        Eigen::VectorXf samples_x(vectSize);
-        Eigen::VectorXf samples_y(vectSize);
-        Eigen::VectorXf samples_z(vectSize);
-        for (int i = 0; i < vectSize; i++) {
-            samples_x(i) = mag_fit_x[i];
-            samples_y(i) = mag_fit_y[i];
-            samples_z(i) = mag_fit_z[i];
+
+        qDebug() << "-----------------------------------";
+        qDebug() << "Onboard Mag";
+        calcCalibration(mag_fit_x, mag_fit_y, mag_fit_z, Be_length, revoCalibrationData.mag_transform, revoCalibrationData.mag_bias);
+        if (calibratingAuxMag) {
+            qDebug() << "Aux Mag";
+            calcCalibration(aux_mag_fit_x, aux_mag_fit_y, aux_mag_fit_z, Be_length, auxCalibrationData.mag_transform, auxCalibrationData.mag_bias);
         }
-        OpenPilot::CalibrationUtils::EllipsoidCalibrationResult result;
-        OpenPilot::CalibrationUtils::EllipsoidCalibration(&samples_x, &samples_y, &samples_z, Be_length, &result, true);
-        qDebug() << "-----------------------------------";
-        qDebug() << "Mag Calibration results: Fit";
-        qDebug() << "scale(" << result.Scale.coeff(0) << ", " << result.Scale.coeff(1) << ", " << result.Scale.coeff(2) << ")";
-        qDebug() << "bias(" << result.Bias.coeff(0) << ", " << result.Bias.coeff(1) << ", " << result.Bias.coeff(2) << ")";
-
-        OpenPilot::CalibrationUtils::SixPointInConstFieldCal(Be_length, mag_data_x, mag_data_y, mag_data_z, S, b);
-
-        qDebug() << "-----------------------------------";
-        qDebug() << "Mag Calibration results: Six Point";
-        qDebug() << "scale(" << S[0] << ", " << S[1] << ", " << S[2] << ")";
-        qDebug() << "bias(" << -sign(S[0]) * b[0] << ", " << -sign(S[1]) * b[1] << ", " << -sign(S[2]) * b[2] << ")";
-        qDebug() << "-----------------------------------";
-
-        revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0] = result.CalibrationMatrix.coeff(0, 0);
-        revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C1] = result.CalibrationMatrix.coeff(0, 1);
-        revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C2] = result.CalibrationMatrix.coeff(0, 2);
-
-        revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C0] = result.CalibrationMatrix.coeff(1, 0);
-        revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C1] = result.CalibrationMatrix.coeff(1, 1);
-        revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C2] = result.CalibrationMatrix.coeff(1, 2);
-
-        revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C0] = result.CalibrationMatrix.coeff(2, 0);
-        revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C1] = result.CalibrationMatrix.coeff(2, 1);
-        revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C2] = result.CalibrationMatrix.coeff(2, 2);
-
-        revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X] = result.Bias.coeff(0);
-        revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y] = result.Bias.coeff(1);
-        revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z] = result.Bias.coeff(2);
     }
     // Restore the previous setting
     revoCalibrationData.MagBiasNullingRate = memento.revoCalibrationData.MagBiasNullingRate;;
 
     // Check the mag calibration is good
-    bool good_calibration = true;
+    bool good_calibration     = true;
+    bool good_aux_calibration = true;
     if (calibratingMag) {
         good_calibration &= !IS_NAN(revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0]);
         good_calibration &= !IS_NAN(revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C1]);
@@ -445,6 +491,23 @@ void SixPointCalibrationModel::compute()
         good_calibration &= !IS_NAN(revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X]);
         good_calibration &= !IS_NAN(revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y]);
         good_calibration &= !IS_NAN(revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z]);
+        if (calibratingAuxMag) {
+            good_calibration &= !IS_NAN(auxCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0]);
+            good_calibration &= !IS_NAN(auxCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C1]);
+            good_calibration &= !IS_NAN(auxCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C2]);
+
+            good_calibration &= !IS_NAN(auxCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C0]);
+            good_calibration &= !IS_NAN(auxCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C1]);
+            good_calibration &= !IS_NAN(auxCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C2]);
+
+            good_calibration &= !IS_NAN(auxCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C0]);
+            good_calibration &= !IS_NAN(auxCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C1]);
+            good_calibration &= !IS_NAN(auxCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C2]);
+
+            good_calibration &= !IS_NAN(auxCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X]);
+            good_calibration &= !IS_NAN(auxCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y]);
+            good_calibration &= !IS_NAN(auxCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z]);
+        }
     }
     // Check the accel calibration is good
     if (calibratingAccel) {
@@ -458,7 +521,8 @@ void SixPointCalibrationModel::compute()
     if (good_calibration) {
         m_dirty = true;
         if (calibratingMag) {
-            result.revoCalibrationData = revoCalibrationData;
+            result.revoCalibrationData   = revoCalibrationData;
+            result.auxMagSettingsData = auxCalibrationData;
             displayInstructions(tr("Magnetometer calibration completed successfully."), WizardModel::Success);
         }
         if (calibratingAccel) {
@@ -473,6 +537,42 @@ void SixPointCalibrationModel::compute()
     position = -1;
 }
 
+void SixPointCalibrationModel::calcCalibration(QList<float> x, QList<float> y, QList<float> z, double Be_length, float calibrationMatrix[], float bias[])
+{
+    int vectSize = x.count();
+    Eigen::VectorXf samples_x(vectSize);
+    Eigen::VectorXf samples_y(vectSize);
+    Eigen::VectorXf samples_z(vectSize);
+
+    for (int i = 0; i < vectSize; i++) {
+        samples_x(i) = x[i];
+        samples_y(i) = y[i];
+        samples_z(i) = z[i];
+    }
+    OpenPilot::CalibrationUtils::EllipsoidCalibrationResult result;
+    OpenPilot::CalibrationUtils::EllipsoidCalibration(&samples_x, &samples_y, &samples_z, Be_length, &result, true);
+
+    qDebug() << "Mag fitting results: ";
+    qDebug() << "scale(" << result.Scale.coeff(0) << ", " << result.Scale.coeff(1) << ", " << result.Scale.coeff(2) << ")";
+    qDebug() << "bias(" << result.Bias.coeff(0) << ", " << result.Bias.coeff(1) << ", " << result.Bias.coeff(2) << ")";
+    qDebug() << "-----------------------------------";
+    calibrationMatrix[RevoCalibration::MAG_TRANSFORM_R0C0] = result.CalibrationMatrix.coeff(0, 0);
+    calibrationMatrix[RevoCalibration::MAG_TRANSFORM_R0C1] = result.CalibrationMatrix.coeff(0, 1);
+    calibrationMatrix[RevoCalibration::MAG_TRANSFORM_R0C2] = result.CalibrationMatrix.coeff(0, 2);
+
+    calibrationMatrix[RevoCalibration::MAG_TRANSFORM_R1C0] = result.CalibrationMatrix.coeff(1, 0);
+    calibrationMatrix[RevoCalibration::MAG_TRANSFORM_R1C1] = result.CalibrationMatrix.coeff(1, 1);
+    calibrationMatrix[RevoCalibration::MAG_TRANSFORM_R1C2] = result.CalibrationMatrix.coeff(1, 2);
+
+    calibrationMatrix[RevoCalibration::MAG_TRANSFORM_R2C0] = result.CalibrationMatrix.coeff(2, 0);
+    calibrationMatrix[RevoCalibration::MAG_TRANSFORM_R2C1] = result.CalibrationMatrix.coeff(2, 1);
+    calibrationMatrix[RevoCalibration::MAG_TRANSFORM_R2C2] = result.CalibrationMatrix.coeff(2, 2);
+
+    bias[RevoCalibration::MAG_BIAS_X] = result.Bias.coeff(0);
+    bias[RevoCalibration::MAG_BIAS_Y] = result.Bias.coeff(1);
+    bias[RevoCalibration::MAG_BIAS_Z] = result.Bias.coeff(2);
+}
+
 void SixPointCalibrationModel::save()
 {
     if (!m_dirty) {
@@ -489,6 +589,18 @@ void SixPointCalibrationModel::save()
         }
 
         revoCalibration->setData(revoCalibrationData);
+        if (calibratingAuxMag) {
+            AuxMagSettings::DataFields auxCalibrationData = auxMagSettings->getData();
+            // Note that Revo/AuxMag MAG_TRANSFORM_RxCx are interchangeable, an assertion at initialization enforces the structs are equal
+            for (int i = 0; i < RevoCalibration::MAG_TRANSFORM_NUMELEM; i++) {
+                auxCalibrationData.mag_transform[i] = result.auxMagSettingsData.mag_transform[i];
+            }
+            for (int i = 0; i < 3; i++) {
+                auxCalibrationData.mag_bias[i] = result.auxMagSettingsData.mag_bias[i];
+            }
+
+            auxMagSettings->setData(auxCalibrationData);
+        }
     }
     if (calibratingAccel) {
         AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
diff --git a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h
index 37c7b7d29..b398aeb0c 100644
--- a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h
+++ b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h
@@ -31,10 +31,13 @@
 #include "wizardmodel.h"
 #include "calibration/calibrationutils.h"
 #include <revocalibration.h>
+
+#include <auxmagsettings.h>
 #include <accelgyrosettings.h>
 #include <homelocation.h>
 #include <accelstate.h>
-#include <magstate.h>
+#include <magsensor.h>
+#include <auxmagsensor.h>
 
 #include <QMutex>
 #include <QObject>
@@ -87,17 +90,21 @@ public:
 
     typedef struct {
         UAVObject::Metadata accelStateMetadata;
-        UAVObject::Metadata magStateMetadata;
-        RevoCalibration::DataFields revoCalibrationData;
+        UAVObject::Metadata magSensorMetadata;
+        UAVObject::Metadata auxMagSensorMetadata;
+        AuxMagSettings::DataFields auxMagSettings;
+        RevoCalibration::DataFields   revoCalibrationData;
         AccelGyroSettings::DataFields accelGyroSettingsData;
     } Memento;
 
     typedef struct {
         RevoCalibration::DataFields   revoCalibrationData;
+        AuxMagSettings::DataFields auxMagSettingsData;
         AccelGyroSettings::DataFields accelGyroSettingsData;
     } Result;
 
     bool calibratingMag;
+    bool calibratingAuxMag;
     bool calibratingAccel;
 
     QList<CalibrationStep> calibrationStepsMag;
@@ -114,11 +121,11 @@ public:
     QMutex sensorsUpdateLock;
 
     double accel_data_x[6], accel_data_y[6], accel_data_z[6];
-    double mag_data_x[6], mag_data_y[6], mag_data_z[6];
 
     QList<double> accel_accum_x;
     QList<double> accel_accum_y;
     QList<double> accel_accum_z;
+
     QList<double> mag_accum_x;
     QList<double> mag_accum_y;
     QList<double> mag_accum_z;
@@ -126,11 +133,20 @@ public:
     QList<float> mag_fit_y;
     QList<float> mag_fit_z;
 
+    QList<double> aux_mag_accum_x;
+    QList<double> aux_mag_accum_y;
+    QList<double> aux_mag_accum_z;
+    QList<float> aux_mag_fit_x;
+    QList<float> aux_mag_fit_y;
+    QList<float> aux_mag_fit_z;
+
     // convenience pointers
     RevoCalibration *revoCalibration;
     AccelGyroSettings *accelGyroSettings;
+    AuxMagSettings *auxMagSettings;
     AccelState *accelState;
-    MagState *magState;
+    MagSensor *magSensor;
+    AuxMagSensor *auxMagSensor;
     HomeLocation *homeLocation;
 
     void start(bool calibrateAccel, bool calibrateMag);
@@ -138,6 +154,7 @@ public:
     void compute();
     void showHelp(QString image);
     UAVObjectManager *getObjectManager();
+    void calcCalibration(QList<float> x, QList<float> y, QList<float> z, double Be_length, float calibrationMatrix[], float bias[]);
 };
 }
 
diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
index 0f08a3536..ecf662715 100644
--- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
+++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
@@ -127,6 +127,9 @@ HEADERS += \
     $$UAVOBJECT_SYNTHETICS/waypointactive.h \
     $$UAVOBJECT_SYNTHETICS/mpu6000settings.h \
     $$UAVOBJECT_SYNTHETICS/takeofflocation.h \
+    $$UAVOBJECT_SYNTHETICS/auxmagsensor.h \
+    $$UAVOBJECT_SYNTHETICS/auxmagsettings.h \
+    $$UAVOBJECT_SYNTHETICS/gpsextendedstatus.h \
     $$UAVOBJECT_SYNTHETICS/perfcounter.h
 
 SOURCES += \
@@ -231,5 +234,8 @@ SOURCES += \
     $$UAVOBJECT_SYNTHETICS/waypointactive.cpp \
     $$UAVOBJECT_SYNTHETICS/mpu6000settings.cpp \
     $$UAVOBJECT_SYNTHETICS/takeofflocation.cpp \
+    $$UAVOBJECT_SYNTHETICS/auxmagsensor.cpp \
+    $$UAVOBJECT_SYNTHETICS/auxmagsettings.cpp \
+    $$UAVOBJECT_SYNTHETICS/gpsextendedstatus.cpp \
     $$UAVOBJECT_SYNTHETICS/perfcounter.cpp
-    
+
diff --git a/shared/uavobjectdefinition/auxmagsensor.xml b/shared/uavobjectdefinition/auxmagsensor.xml
new file mode 100644
index 000000000..1deb44da8
--- /dev/null
+++ b/shared/uavobjectdefinition/auxmagsensor.xml
@@ -0,0 +1,13 @@
+<xml>
+    <object name="AuxMagSensor" singleinstance="true" settings="false" category="Sensors">
+        <description>Calibrated sensor data from aux 3 axis magnetometer in MilliGauss.</description>
+		<field name="x" units="mGa" type="float" elements="1"/>
+		<field name="y" units="mGa" type="float" elements="1"/>
+        <field name="z" units="mGa" type="float" elements="1"/>
+        <field name="Status" units="" type="enum" elements="1" options="None,Ok" defaultvalue="None"/>
+        <access gcs="readwrite" flight="readwrite"/>
+        <telemetrygcs acked="false" updatemode="manual" period="0"/>
+        <telemetryflight acked="false" updatemode="periodic" period="10000"/>
+        <logging updatemode="manual" period="0"/>
+    </object>
+</xml>
diff --git a/shared/uavobjectdefinition/auxmagsettings.xml b/shared/uavobjectdefinition/auxmagsettings.xml
new file mode 100644
index 000000000..fda802031
--- /dev/null
+++ b/shared/uavobjectdefinition/auxmagsettings.xml
@@ -0,0 +1,16 @@
+<xml>
+    <object name="AuxMagSettings" singleinstance="true" settings="true" category="Sensors">
+        <description>Settings for auxiliary magnetometer calibration</description>
+        <field name="mag_bias" units="mGau" type="float" elementnames="X,Y,Z" defaultvalue="0,0,0"/>
+        <field name="mag_transform" units="gain" type="float" elementnames="r0c0,r0c1,r0c2,r1c0,r1c1,r1c2,r2c0,r2c1,r2c2"
+         defaultvalue="1,0,0,0,1,0,0,0,1"/>
+        <field name="MagBiasNullingRate" units="" type="float" elements="1" defaultvalue="0"/>
+        <field name="Orientation" units="degrees" type="float" elements="1" defaultvalue="0"/>
+        <field name="Type" units="" type="enum" elements="1" options="GPSV9,Ext" defaultvalue="GPSV9"/>
+        <field name="Usage" units="" type="enum" elements="1" options="Both,OnboardOnly,AuxOnly" defaultvalue="Both"/>
+        <access gcs="readwrite" flight="readwrite"/>
+        <telemetrygcs acked="true" updatemode="onchange" period="0"/>
+        <telemetryflight acked="true" updatemode="onchange" period="0"/>
+        <logging updatemode="manual" period="0"/>
+    </object>
+</xml>
diff --git a/shared/uavobjectdefinition/gpsextendedstatus.xml b/shared/uavobjectdefinition/gpsextendedstatus.xml
new file mode 100644
index 000000000..d5da25352
--- /dev/null
+++ b/shared/uavobjectdefinition/gpsextendedstatus.xml
@@ -0,0 +1,15 @@
+<xml>
+    <object name="GPSExtendedStatus" singleinstance="true" settings="false" category="Sensors">
+        <description>Extended GPS status.</description>
+        <field name="Status" units="" type="enum" elements="1" options="NONE,GPSV9" defaultvalue="NONE"/>
+        <field name="FlightTime" units="" type="uint32" elements="1"/>
+        <field name="HeapRemaining" units="bytes" type="uint32" elements="1"/>
+        <field name="IRQStackRemaining" units="bytes" type="uint16" elements="1"/>
+        <field name="SysModStackRemaining" units="bytes" type="uint16" elements="1"/>
+        <field name="Options" units="" type="uint16" elements="1"/>
+        <access gcs="readwrite" flight="readwrite"/>
+        <telemetrygcs acked="false" updatemode="manual" period="0"/>
+        <telemetryflight acked="false" updatemode="periodic" period="1000"/>
+        <logging updatemode="manual" period="0"/>
+    </object>
+</xml>
diff --git a/shared/uavobjectdefinition/gpspositionsensor.xml b/shared/uavobjectdefinition/gpspositionsensor.xml
index cfaba9db0..a6a5ea194 100644
--- a/shared/uavobjectdefinition/gpspositionsensor.xml
+++ b/shared/uavobjectdefinition/gpspositionsensor.xml
@@ -12,6 +12,8 @@
         <field name="PDOP" units="" type="float" elements="1"/>
         <field name="HDOP" units="" type="float" elements="1"/>
         <field name="VDOP" units="" type="float" elements="1"/>
+        <field name="SensorType" units="" type="enum" elements="1" options="Unknown,NMEA,UBX,UBX7,UBX8" defaultvalue="Unknown" />
+        <field name="AutoConfigStatus" units="" type="enum" elements="1" options="DISABLED,RUNNING,DONE,ERROR" defaultvalue="DISABLED" />
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="false" updatemode="manual" period="0"/>
         <telemetryflight acked="false" updatemode="periodic" period="1000"/>
diff --git a/shared/uavobjectdefinition/gpssettings.xml b/shared/uavobjectdefinition/gpssettings.xml
index dd4dd8cd5..e2919eec8 100644
--- a/shared/uavobjectdefinition/gpssettings.xml
+++ b/shared/uavobjectdefinition/gpssettings.xml
@@ -2,8 +2,20 @@
     <object name="GPSSettings" singleinstance="true" settings="true" category="Sensors">
         <description>GPS Module specific settings</description>
         <field name="DataProtocol" units="" type="enum" elements="1" options="NMEA,UBX" defaultvalue="UBX"/>
-        <field name="MinSattelites" units="" type="uint8" elements="1" defaultvalue="7"/>
+        <field name="MinSatellites" units="" type="uint8" elements="1" defaultvalue="7"/>
         <field name="MaxPDOP" units="" type="float" elements="1" defaultvalue="3.5"/>
+        <!-- Ubx self configuration. Enable to allow the flight board to auto configure ubx GPS options,
+            store does AutoConfig and save GPS settings (i.e. to prevent issues if gps is power cycled)  -->
+        <field name="UbxAutoConfig" units="" type="enum" elements="1" options="Disabled,Configure,ConfigureAndStore" defaultvalue="Disabled"/>
+        <!-- Ubx position update rate, -1 for auto -->
+        <field name="UbxRate" units="Hz" type="int8" elements="1" defaultvalue="5" />
+        <!-- Ubx dynamic model, see UBX datasheet for more details -->
+        <field name="UbxDynamicModel" units="" type="enum" elements="1" 
+        options="Portable,Stationary,Pedestrian,Automotive,Sea,Airborne1G,Airborne2G,Airborne4G" defaultvalue="Airborne1G" />
+        <!-- Ubx SBAS settings -->
+        <field name="UbxSBASMode" units="" type="enum" elements="1" options="Disabled,Ranging,Correction,Integrity,Ranging+Correction,Ranging+Integrity,Ranging+Correction+Integrity,Correction+Integrity" defaultvalue="Ranging" />
+        <field name="UbxSBASChannelsUsed" units="" type="uint8" elements="1" defaultvalue="3"/>
+        <field name="UbxSBASSats" units="" type="enum" elements="1" options="AutoScan,WAAS,EGNOS,MSAS,GAGAN,SDCM" defaultvalue="Auto-Scan" />
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="true" updatemode="onchange" period="0"/>
         <telemetryflight acked="true" updatemode="onchange" period="0"/>
diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml
index 7fa821da4..0f24744a4 100644
--- a/shared/uavobjectdefinition/hwsettings.xml
+++ b/shared/uavobjectdefinition/hwsettings.xml
@@ -17,7 +17,7 @@
 		<field name="RM_FlexiPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,I2C,DSM2,DSMX (10bit),DSMX (11bit),DebugConsole,ComBridge,OsdHk" defaultvalue="Disabled"/>
 
 		<field name="TelemetrySpeed" units="bps" type="enum" elements="1" options="2400,4800,9600,19200,38400,57600,115200" defaultvalue="57600"/>
-		<field name="GPSSpeed" units="bps" type="enum" elements="1" options="2400,4800,9600,19200,38400,57600,115200" defaultvalue="57600"/>
+		<field name="GPSSpeed" units="bps" type="enum" elements="1" options="2400,4800,9600,19200,38400,57600,115200,230400" defaultvalue="57600"/>
 		<field name="ComUsbBridgeSpeed" units="bps" type="enum" elements="1" options="2400,4800,9600,19200,38400,57600,115200" defaultvalue="57600"/>
 		<field name="USB_HIDPort" units="function" type="enum" elements="1" options="USBTelemetry,RCTransmitter,Disabled" defaultvalue="USBTelemetry"/>
 		<field name="USB_VCPPort" units="function" type="enum" elements="1" options="USBTelemetry,ComBridge,DebugConsole,Disabled" defaultvalue="Disabled"/>
diff --git a/shared/uavobjectdefinition/magstate.xml b/shared/uavobjectdefinition/magstate.xml
index 1cd0fefaa..0de27ce17 100644
--- a/shared/uavobjectdefinition/magstate.xml
+++ b/shared/uavobjectdefinition/magstate.xml
@@ -1,9 +1,10 @@
 <xml>
     <object name="MagState" singleinstance="true" settings="false" category="State">
         <description>The filtered magnet vector.</description>
-	<field name="x" units="mGa" type="float" elements="1"/>
-	<field name="y" units="mGa" type="float" elements="1"/>
-	<field name="z" units="mGa" type="float" elements="1"/>
+        <field name="x" units="mGa" type="float" elements="1"/>
+        <field name="y" units="mGa" type="float" elements="1"/>
+        <field name="z" units="mGa" type="float" elements="1"/>
+        <field name="Source" units="" type="enum" elements="1" options="Invalid,OnBoard,Aux" defaultvalue="Invalid"/>
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="false" updatemode="manual" period="0"/>
         <telemetryflight acked="false" updatemode="periodic" period="1000"/>