diff --git a/Makefile b/Makefile
index b47766f74..545107ab1 100644
--- a/Makefile
+++ b/Makefile
@@ -265,6 +265,7 @@ EUSTM32F103RC_SRC	 = startup_stm32f10x_hd_gcc.S \
 		   drivers/barometer_ms5611.c \
 		   drivers/bus_i2c_stm32f10x.c \
 		   drivers/bus_spi.c \
+		   drivers/compass_ak8975.c \
 		   drivers/compass_hmc5883l.c \
 		   drivers/display_ug2864hsweg01.c \
 		   drivers/gpio_stm32f10x.c \
@@ -427,6 +428,7 @@ SPARKY_SRC	 = $(STM32F30x_COMMON_SRC) \
 		   drivers/display_ug2864hsweg01.c \
 		   drivers/accgyro_mpu9150.c \
 		   drivers/barometer_ms5611.c \
+		   drivers/compass_ak8975.c \
 		   $(HIGHEND_SRC) \
 		   $(COMMON_SRC)
 
diff --git a/src/main/config/config.c b/src/main/config/config.c
index 3b3ef472e..736aad70d 100644
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -27,7 +27,10 @@
 #include "common/axis.h"
 #include "flight/flight.h"
 
+#include "drivers/sensor.h"
 #include "drivers/accgyro.h"
+#include "drivers/compass.h"
+
 #include "drivers/system.h"
 #include "drivers/gpio.h"
 #include "drivers/timer.h"
@@ -35,6 +38,7 @@
 
 #include "sensors/sensors.h"
 #include "sensors/gyro.h"
+#include "sensors/compass.h"
 
 #include "io/statusindicator.h"
 #include "sensors/acceleration.h"
@@ -104,7 +108,7 @@ profile_t *currentProfile;
 static uint8_t currentControlRateProfileIndex = 0;
 controlRateConfig_t *currentControlRateProfile;
 
-static const uint8_t EEPROM_CONF_VERSION = 87;
+static const uint8_t EEPROM_CONF_VERSION = 88;
 
 static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
 {
@@ -330,6 +334,8 @@ static void resetConf(void)
     masterConfig.yaw_control_direction = 1;
     masterConfig.gyroConfig.gyroMovementCalibrationThreshold = 32;
 
+    masterConfig.mag_hardware = MAG_DEFAULT;     // default/autodetect
+
     resetBatteryConfig(&masterConfig.batteryConfig);
 
     resetTelemetryConfig(&masterConfig.telemetryConfig);
diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h
index c354d9e58..e3f1eeb03 100644
--- a/src/main/config/config_master.h
+++ b/src/main/config/config_master.h
@@ -50,6 +50,8 @@ typedef struct master_t {
 
     gyroConfig_t gyroConfig;
 
+    uint8_t mag_hardware;                   // Which mag hardware to use on boards with more than one device
+
     uint16_t max_angle_inclination;         // max inclination allowed in angle (level) mode. default 500 (50 degrees).
     flightDynamicsTrims_t accZero;
     flightDynamicsTrims_t magZero;
diff --git a/src/main/drivers/accgyro.h b/src/main/drivers/accgyro.h
index 6919f86d9..90ddf510f 100644
--- a/src/main/drivers/accgyro.h
+++ b/src/main/drivers/accgyro.h
@@ -19,9 +19,6 @@
 
 extern uint16_t acc_1G;
 
-typedef void (*sensorInitFuncPtr)(void);                    // sensor init prototype
-typedef void (*sensorReadFuncPtr)(int16_t *data);           // sensor read and align prototype
-
 typedef struct gyro_s {
     sensorInitFuncPtr init;                                 // initialize function
     sensorReadFuncPtr read;                                 // read 3 axis data function
diff --git a/src/main/drivers/accgyro_adxl345.c b/src/main/drivers/accgyro_adxl345.c
index ce73e5f57..f1466accd 100644
--- a/src/main/drivers/accgyro_adxl345.c
+++ b/src/main/drivers/accgyro_adxl345.c
@@ -23,6 +23,7 @@
 #include "system.h"
 #include "bus_i2c.h"
 
+#include "sensor.h"
 #include "accgyro.h"
 #include "accgyro_adxl345.h"
 
diff --git a/src/main/drivers/accgyro_bma280.c b/src/main/drivers/accgyro_bma280.c
index 5a0b87f7a..536401ea8 100644
--- a/src/main/drivers/accgyro_bma280.c
+++ b/src/main/drivers/accgyro_bma280.c
@@ -22,6 +22,7 @@
 
 #include "bus_i2c.h"
 
+#include "sensor.h"
 #include "accgyro.h"
 #include "accgyro_bma280.h"
 
diff --git a/src/main/drivers/accgyro_l3g4200d.c b/src/main/drivers/accgyro_l3g4200d.c
index 2c2821ead..bece47171 100644
--- a/src/main/drivers/accgyro_l3g4200d.c
+++ b/src/main/drivers/accgyro_l3g4200d.c
@@ -26,6 +26,7 @@
 #include "common/maths.h"
 #include "common/axis.h"
 
+#include "sensor.h"
 #include "accgyro.h"
 #include "accgyro_l3g4200d.h"
 
diff --git a/src/main/drivers/accgyro_l3gd20.c b/src/main/drivers/accgyro_l3gd20.c
index addc47394..ebc223f68 100644
--- a/src/main/drivers/accgyro_l3gd20.c
+++ b/src/main/drivers/accgyro_l3gd20.c
@@ -26,6 +26,7 @@
 #include "gpio.h"
 #include "bus_spi.h"
 
+#include "sensor.h"
 #include "accgyro.h"
 #include "accgyro_l3gd20.h"
 
diff --git a/src/main/drivers/accgyro_lsm303dlhc.c b/src/main/drivers/accgyro_lsm303dlhc.c
index a01b1723c..324ea0368 100644
--- a/src/main/drivers/accgyro_lsm303dlhc.c
+++ b/src/main/drivers/accgyro_lsm303dlhc.c
@@ -27,6 +27,7 @@
 #include "gpio.h"
 #include "bus_i2c.h"
 
+#include "sensor.h"
 #include "accgyro.h"
 #include "accgyro_lsm303dlhc.h"
 
diff --git a/src/main/drivers/accgyro_mma845x.c b/src/main/drivers/accgyro_mma845x.c
index d5d06119d..ab3a01aca 100644
--- a/src/main/drivers/accgyro_mma845x.c
+++ b/src/main/drivers/accgyro_mma845x.c
@@ -24,6 +24,7 @@
 #include "gpio.h"
 #include "bus_i2c.h"
 
+#include "sensor.h"
 #include "accgyro.h"
 #include "accgyro_mma845x.h"
 
diff --git a/src/main/drivers/accgyro_mpu3050.c b/src/main/drivers/accgyro_mpu3050.c
index aecd78795..fbf8be1a9 100644
--- a/src/main/drivers/accgyro_mpu3050.c
+++ b/src/main/drivers/accgyro_mpu3050.c
@@ -25,6 +25,7 @@
 #include "system.h"
 #include "bus_i2c.h"
 
+#include "sensor.h"
 #include "accgyro.h"
 #include "accgyro_mpu3050.h"
 
diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c
index 6ffef1769..785f8e0be 100644
--- a/src/main/drivers/accgyro_mpu6050.c
+++ b/src/main/drivers/accgyro_mpu6050.c
@@ -27,6 +27,7 @@
 #include "gpio.h"
 #include "bus_i2c.h"
 
+#include "sensor.h"
 #include "accgyro.h"
 #include "accgyro_mpu6050.h"
 
diff --git a/src/main/drivers/accgyro_mpu9150.c b/src/main/drivers/accgyro_mpu9150.c
index df349d3ec..3cbb2d086 100644
--- a/src/main/drivers/accgyro_mpu9150.c
+++ b/src/main/drivers/accgyro_mpu9150.c
@@ -30,10 +30,11 @@
 #include "gpio.h"
 #include "bus_i2c.h"
 
+#include "sensor.h"
 #include "accgyro.h"
 #include "accgyro_mpu9150.h"
 
-#define MPU9150_ADDRESS         0xD0
+#define MPU9150_ADDRESS         0xD0 // (204) 1101000 // See http://www.invensense.com/mems/gyro/documents/PS-MPU-9150A-00v4_3.pdf, section 6.5.
 
 #define DMP_MEM_START_ADDR 0x6E
 #define DMP_MEM_R_W 0x6F
diff --git a/src/main/drivers/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro_spi_mpu6000.c
index ba1a23de7..590e6e922 100644
--- a/src/main/drivers/accgyro_spi_mpu6000.c
+++ b/src/main/drivers/accgyro_spi_mpu6000.c
@@ -35,6 +35,7 @@
 #include "gpio.h"
 #include "bus_spi.h"
 
+#include "sensor.h"
 #include "accgyro.h"
 #include "accgyro_spi_mpu6000.h"
 
diff --git a/src/main/drivers/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro_spi_mpu6500.c
index b187f88b7..123ac08c3 100644
--- a/src/main/drivers/accgyro_spi_mpu6500.c
+++ b/src/main/drivers/accgyro_spi_mpu6500.c
@@ -28,6 +28,7 @@
 #include "gpio.h"
 #include "bus_spi.h"
 
+#include "sensor.h"
 #include "accgyro.h"
 #include "accgyro_spi_mpu6500.h"
 
diff --git a/src/main/drivers/adc.c b/src/main/drivers/adc.c
index 57f8a0348..4215df21d 100644
--- a/src/main/drivers/adc.c
+++ b/src/main/drivers/adc.c
@@ -21,8 +21,6 @@
 #include "platform.h"
 #include "system.h"
 
-#include "accgyro.h"
-
 #include "adc.h"
 
 adc_config_t adcConfig[ADC_CHANNEL_COUNT];
diff --git a/src/main/drivers/adc_stm32f10x.c b/src/main/drivers/adc_stm32f10x.c
index 64ad57171..ae6680c4e 100644
--- a/src/main/drivers/adc_stm32f10x.c
+++ b/src/main/drivers/adc_stm32f10x.c
@@ -27,6 +27,7 @@
 
 #include "sensors/sensors.h" // FIXME dependency into the main code
 
+#include "sensor.h"
 #include "accgyro.h"
 
 #include "adc.h"
diff --git a/src/main/drivers/adc_stm32f30x.c b/src/main/drivers/adc_stm32f30x.c
index 1908afd5c..6c129d546 100644
--- a/src/main/drivers/adc_stm32f30x.c
+++ b/src/main/drivers/adc_stm32f30x.c
@@ -24,8 +24,7 @@
 
 #include "gpio.h"
 
-#include "sensors/sensors.h" // FIXME dependency into the main code
-
+#include "sensor.h"
 #include "accgyro.h"
 
 #include "adc.h"
diff --git a/src/main/drivers/compass.h b/src/main/drivers/compass.h
new file mode 100644
index 000000000..c5babb893
--- /dev/null
+++ b/src/main/drivers/compass.h
@@ -0,0 +1,23 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight 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.
+ *
+ * Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+typedef struct mag_s {
+    sensorInitFuncPtr init;                                 // initialize function
+    sensorReadFuncPtr read;                                 // read 3 axis data function
+} mag_t;
diff --git a/src/main/drivers/compass_ak8975.c b/src/main/drivers/compass_ak8975.c
new file mode 100644
index 000000000..2b63f5b23
--- /dev/null
+++ b/src/main/drivers/compass_ak8975.c
@@ -0,0 +1,90 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight 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.
+ *
+ * Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include <math.h>
+
+#include "platform.h"
+
+#include "common/axis.h"
+#include "common/maths.h"
+
+#include "system.h"
+#include "gpio.h"
+#include "bus_i2c.h"
+
+#include "sensors/boardalignment.h"
+#include "sensors/sensors.h"
+
+#include "sensor.h"
+#include "compass.h"
+
+#include "compass_ak8975.h"
+
+// This sensor is available in MPU-9150.
+
+// AK8975, mag sensor address
+#define AK8975_MAG_I2C_ADDRESS     0x0C
+#define AK8975_MAG_ID_ADDRESS      0x00
+#define AK8975_MAG_DATA_ADDRESS    0x03
+#define AK8975_MAG_CONTROL_ADDRESS 0x0A
+
+bool ak8975detect(mag_t *mag)
+{
+    bool ack = false;
+    uint8_t sig = 0;
+    uint8_t ackCount = 0;
+
+    for (uint8_t address = 0; address < 0xff; address++) {
+        ack = i2cRead(address, AK8975_MAG_ID_ADDRESS, 1, &sig);
+        if (ack) {
+            ackCount++;
+        }
+    }
+    // device ID is in register 0 and is equal to 'H'
+    ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_ID_ADDRESS, 1, &sig);
+    if (!ack || sig != 'H')
+        return false;
+
+    mag->init = ak8975Init;
+    mag->read = ak8975Read;
+
+    return true;
+}
+
+void ak8975Init()
+{
+    i2cWrite(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_CONTROL_ADDRESS, 0x01); // start reading
+}
+
+void ak8975Read(int16_t *magData)
+{
+    uint8_t buf[6];
+
+    i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_DATA_ADDRESS, 6, buf);
+    // align sensors to match MPU6050:
+    // x -> y
+    // y -> x
+    // z-> -z
+    magData[X] = -(int16_t)(buf[3] << 8 | buf[2]) * 4;
+    magData[Y] = -(int16_t)(buf[1] << 8 | buf[0]) * 4;
+    magData[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * 4;
+
+    i2cWrite(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_CONTROL_ADDRESS, 0x01); // start reading again
+}
diff --git a/src/main/drivers/compass_ak8975.h b/src/main/drivers/compass_ak8975.h
new file mode 100644
index 000000000..d0d78ecdf
--- /dev/null
+++ b/src/main/drivers/compass_ak8975.h
@@ -0,0 +1,22 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight 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.
+ *
+ * Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+bool ak8975detect(mag_t *mag);
+void ak8975Init(void);
+void ak8975Read(int16_t *magData);
diff --git a/src/main/drivers/compass_hmc5883l.c b/src/main/drivers/compass_hmc5883l.c
index 5273c964f..98788f18b 100644
--- a/src/main/drivers/compass_hmc5883l.c
+++ b/src/main/drivers/compass_hmc5883l.c
@@ -17,6 +17,7 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <stdlib.h>
 
 #include <math.h>
 
@@ -30,7 +31,9 @@
 #include "bus_i2c.h"
 #include "light_led.h"
 
-#include "sensors/boardalignment.h"
+#include "sensor.h"
+#include "compass.h"
+
 #include "sensors/sensors.h"
 
 #include "compass_hmc5883l.h"
@@ -110,19 +113,26 @@
 
 static float magGain[3] = { 1.0f, 1.0f, 1.0f };
 
-bool hmc5883lDetect(void)
+static hmc5883Config_t *hmc5883Config = NULL;
+
+bool hmc5883lDetect(mag_t* mag, hmc5883Config_t *hmc5883ConfigToUse)
 {
     bool ack = false;
     uint8_t sig = 0;
 
+    hmc5883Config = hmc5883ConfigToUse;
+
     ack = i2cRead(MAG_ADDRESS, 0x0A, 1, &sig);
     if (!ack || sig != 'H')
         return false;
 
+    mag->init = hmc5883lInit;
+    mag->read = hmc5883lRead;
+
     return true;
 }
 
-void hmc5883lInit(hmc5883Config_t *hmc5883Config)
+void hmc5883lInit(void)
 {
     int16_t magADC[3];
     int i;
diff --git a/src/main/drivers/compass_hmc5883l.h b/src/main/drivers/compass_hmc5883l.h
index 0f2f78a50..df07d21ad 100644
--- a/src/main/drivers/compass_hmc5883l.h
+++ b/src/main/drivers/compass_hmc5883l.h
@@ -23,6 +23,6 @@ typedef struct hmc5883Config_s {
     GPIO_TypeDef *gpioPort;
 } hmc5883Config_t;
 
-bool hmc5883lDetect(void);
-void hmc5883lInit(hmc5883Config_t *hmc5883Config);
+bool hmc5883lDetect(mag_t* mag, hmc5883Config_t *hmc5883ConfigToUse);
+void hmc5883lInit(void);
 void hmc5883lRead(int16_t *magData);
diff --git a/src/main/drivers/sensor.h b/src/main/drivers/sensor.h
new file mode 100644
index 000000000..f494c5713
--- /dev/null
+++ b/src/main/drivers/sensor.h
@@ -0,0 +1,21 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight 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.
+ *
+ * Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+typedef void (*sensorInitFuncPtr)(void);                    // sensor init prototype
+typedef void (*sensorReadFuncPtr)(int16_t *data);           // sensor read and align prototype
diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c
index 20ffbd45b..4d2257a01 100644
--- a/src/main/flight/altitudehold.c
+++ b/src/main/flight/altitudehold.c
@@ -27,6 +27,7 @@
 #include "common/maths.h"
 #include "common/axis.h"
 
+#include "drivers/sensor.h"
 #include "drivers/accgyro.h"
 
 #include "flight/flight.h"
diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c
index 6d7891d7c..f6dea18ce 100644
--- a/src/main/flight/flight.c
+++ b/src/main/flight/flight.c
@@ -29,9 +29,12 @@
 #include "config/runtime_config.h"
 
 #include "rx/rx.h"
+
+#include "drivers/sensor.h"
 #include "drivers/accgyro.h"
 #include "sensors/sensors.h"
 #include "sensors/gyro.h"
+
 #include "io/rc_controls.h"
 #include "flight/flight.h"
 #include "flight/navigation.h"
diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index b79562fa0..ffdbeb234 100644
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -30,8 +30,12 @@
 
 #include "drivers/system.h"
 
-#include "sensors/sensors.h"
+
+#include "drivers/sensor.h"
 #include "drivers/accgyro.h"
+#include "drivers/compass.h"
+
+#include "sensors/sensors.h"
 #include "sensors/gyro.h"
 #include "sensors/compass.h"
 #include "sensors/acceleration.h"
diff --git a/src/main/io/display.c b/src/main/io/display.c
index bfca1934d..691dc546f 100644
--- a/src/main/io/display.c
+++ b/src/main/io/display.c
@@ -34,6 +34,9 @@
 #include "drivers/system.h"
 #include "drivers/display_ug2864hsweg01.h"
 
+#include "drivers/sensor.h"
+#include "drivers/compass.h"
+
 #include "sensors/battery.h"
 
 #include "common/axis.h"
diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c
index 056c5f2b9..7247a8a22 100644
--- a/src/main/io/rc_controls.c
+++ b/src/main/io/rc_controls.c
@@ -31,6 +31,7 @@
 
 #include "flight/flight.h"
 
+#include "drivers/sensor.h"
 #include "drivers/accgyro.h"
 
 #include "sensors/battery.h"
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index 55fd5e8e9..d7f52ec1c 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -34,7 +34,11 @@
 #include "common/typeconversion.h"
 
 #include "drivers/system.h"
+
+#include "drivers/sensor.h"
 #include "drivers/accgyro.h"
+#include "drivers/compass.h"
+
 #include "drivers/serial.h"
 #include "drivers/bus_i2c.h"
 #include "drivers/gpio.h"
@@ -57,6 +61,7 @@
 #include "sensors/sensors.h"
 #include "sensors/acceleration.h"
 #include "sensors/gyro.h"
+#include "sensors/compass.h"
 #include "sensors/barometer.h"
 #include "telemetry/telemetry.h"
 
@@ -355,6 +360,7 @@ const clivalue_t valueTable[] = {
     { "baro_cf_vel",                VAR_FLOAT  | PROFILE_VALUE, &masterConfig.profile[0].barometerConfig.baro_cf_vel, 0, 1 },
     { "baro_cf_alt",                VAR_FLOAT  | PROFILE_VALUE, &masterConfig.profile[0].barometerConfig.baro_cf_alt, 0, 1 },
 
+    { "mag_hardware",               VAR_UINT8  | MASTER_VALUE,  &masterConfig.mag_hardware, 0, MAG_NONE },
     { "mag_declination",            VAR_INT16  | PROFILE_VALUE, &masterConfig.profile[0].mag_declination, -18000, 18000 },
 
     { "pid_controller",             VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidController, 0, 2 },
diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c
index 79d24e058..309785b85 100644
--- a/src/main/io/serial_msp.c
+++ b/src/main/io/serial_msp.c
@@ -30,7 +30,11 @@
 #include "common/maths.h"
 
 #include "drivers/system.h"
+
+#include "drivers/sensor.h"
 #include "drivers/accgyro.h"
+#include "drivers/compass.h"
+
 #include "drivers/serial.h"
 #include "drivers/bus_i2c.h"
 #include "drivers/gpio.h"
diff --git a/src/main/main.c b/src/main/main.c
index a68b97ddf..b3f605685 100644
--- a/src/main/main.c
+++ b/src/main/main.c
@@ -27,6 +27,7 @@
 #include "common/atomic.h"
 #include "drivers/nvic.h"
 
+#include "drivers/sensor.h"
 #include "drivers/system.h"
 #include "drivers/gpio.h"
 #include "drivers/light_led.h"
@@ -36,6 +37,7 @@
 #include "drivers/serial_softserial.h"
 #include "drivers/serial_uart.h"
 #include "drivers/accgyro.h"
+#include "drivers/compass.h"
 #include "drivers/pwm_mapping.h"
 #include "drivers/pwm_rx.h"
 #include "drivers/adc.h"
@@ -100,7 +102,7 @@ void rxInit(rxConfig_t *rxConfig, failsafe_t *failsafe);
 void beepcodeInit(failsafe_t *initialFailsafe);
 void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig);
 void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile);
-bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int16_t magDeclinationFromConfig);
+bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int8_t magHardwareToUse, int16_t magDeclinationFromConfig);
 void imuInit(void);
 void displayInit(rxConfig_t *intialRxConfig);
 void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, failsafe_t* failsafeToUse);
@@ -120,7 +122,6 @@ void init(void)
 {
     uint8_t i;
     drv_pwm_config_t pwm_params;
-    drv_adc_config_t adc_params;
     bool sensorsOK = false;
 
     initPrintfSupport();
@@ -215,6 +216,8 @@ void init(void)
 #endif
 
 #if !defined(SPARKY)
+    drv_adc_config_t adc_params;
+
     adc_params.enableRSSI = feature(FEATURE_RSSI_ADC);
     adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER);
     adc_params.enableExternal1 = false;
@@ -241,7 +244,7 @@ void init(void)
     // We have these sensors; SENSORS_SET defined in board.h depending on hardware platform
     sensorsSet(SENSORS_SET);
     // drop out any sensors that don't seem to work, init all the others. halt if gyro is dead.
-    sensorsOK = sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, currentProfile->mag_declination);
+    sensorsOK = sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, currentProfile->mag_declination);
 
     // if gyro was not detected due to whatever reason, we give up now.
     if (!sensorsOK)
diff --git a/src/main/mw.c b/src/main/mw.c
index 93ba14dbc..58c56f85c 100644
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -26,7 +26,9 @@
 #include "common/axis.h"
 #include "common/color.h"
 
+#include "drivers/sensor.h"
 #include "drivers/accgyro.h"
+#include "drivers/compass.h"
 #include "drivers/light_led.h"
 
 #include "drivers/gpio.h"
diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c
index 06ca45e54..303a2148a 100644
--- a/src/main/sensors/acceleration.c
+++ b/src/main/sensors/acceleration.c
@@ -22,8 +22,11 @@
 
 #include "common/axis.h"
 
+#include "drivers/sensor.h"
 #include "drivers/accgyro.h"
+
 #include "flight/flight.h"
+
 #include "sensors/battery.h"
 #include "sensors/sensors.h"
 #include "io/beeper.h"
diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h
index f10817629..0b397cd13 100644
--- a/src/main/sensors/acceleration.h
+++ b/src/main/sensors/acceleration.h
@@ -30,7 +30,7 @@ typedef enum AccelSensors {
     ACC_MPU9150 = 8,
     ACC_FAKE = 9,
     ACC_NONE = 10
-} AccelSensors;
+} accelSensor_e;
 
 extern uint8_t accHardware;
 extern sensor_align_e accAlign;
diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c
index bac409617..343c9d535 100644
--- a/src/main/sensors/compass.c
+++ b/src/main/sensors/compass.c
@@ -22,6 +22,8 @@
 
 #include "common/axis.h"
 
+#include "drivers/sensor.h"
+#include "drivers/compass.h"
 #include "drivers/compass_hmc5883l.h"
 #include "drivers/gpio.h"
 #include "drivers/light_led.h"
@@ -38,6 +40,9 @@
 #include "hardware_revision.h"
 #endif
 
+mag_t mag;                   // mag access functions
+uint8_t magHardware = MAG_DEFAULT;
+
 extern uint32_t currentTime; // FIXME dependency on global variable, pass it in instead.
 
 int16_t magADC[XYZ_AXIS_COUNT];
@@ -49,27 +54,7 @@ void compassInit(void)
 {
     // initialize and calibration. turn on led during mag calibration (calibration routine blinks it)
     LED1_ON;
-
-    hmc5883Config_t *hmc5883Config = 0;
-#ifdef NAZE
-    hmc5883Config_t nazeHmc5883Config;
-
-    if (hardwareRevision < NAZE32_REV5) {
-        nazeHmc5883Config.gpioAPB2Peripherals = RCC_APB2Periph_GPIOB;
-        nazeHmc5883Config.gpioPin = Pin_12;
-        nazeHmc5883Config.gpioPort = GPIOB;
-    } else {
-        nazeHmc5883Config.gpioAPB2Peripherals = RCC_APB2Periph_GPIOC;
-        nazeHmc5883Config.gpioPin = Pin_14;
-        nazeHmc5883Config.gpioPort = GPIOC;
-    }
-
-    hmc5883Config = &nazeHmc5883Config;
-#endif
-
-    hmc5883lInit(hmc5883Config);
-
-
+    mag.init();
     LED1_OFF;
     magInit = 1;
 }
@@ -88,7 +73,7 @@ void updateCompass(flightDynamicsTrims_t *magZero)
 
     nextUpdateAt = currentTime + COMPASS_UPDATE_FREQUENCY_10HZ;
 
-    hmc5883lRead(magADC);
+    mag.read(magADC);
     alignSensors(magADC, magADC, magAlign);
 
     if (STATE(CALIBRATE_MAG)) {
diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h
index 533476aea..374cac185 100644
--- a/src/main/sensors/compass.h
+++ b/src/main/sensors/compass.h
@@ -17,10 +17,21 @@
 
 #pragma once
 
+// Type of accelerometer used/detected
+typedef enum MagSensors {
+    MAG_DEFAULT = 0,
+    MAG_HMC5883 = 1,
+    MAG_AK8975 = 2,
+    MAG_NONE = 3
+} magSensor_e;
+
 #ifdef MAG
 void compassInit(void);
 void updateCompass(flightDynamicsTrims_t *magZero);
 #endif
 
 extern int16_t magADC[XYZ_AXIS_COUNT];
+
+extern uint8_t magHardware;
 extern sensor_align_e magAlign;
+extern mag_t mag;
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 63cb47c22..d490a91f5 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -23,6 +23,7 @@
 #include "common/axis.h"
 #include "common/maths.h"
 
+#include "drivers/sensor.h"
 #include "drivers/accgyro.h"
 #include "flight/flight.h"
 #include "sensors/sensors.h"
diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c
index 323e0538a..1ec481f10 100644
--- a/src/main/sensors/initialisation.c
+++ b/src/main/sensors/initialisation.c
@@ -24,8 +24,9 @@
 
 #include "common/axis.h"
 
-#include "drivers/accgyro.h"
+#include "drivers/sensor.h"
 
+#include "drivers/accgyro.h"
 #include "drivers/accgyro_adxl345.h"
 #include "drivers/accgyro_bma280.h"
 #include "drivers/accgyro_l3g4200d.h"
@@ -43,8 +44,13 @@
 #include "drivers/barometer.h"
 #include "drivers/barometer_bmp085.h"
 #include "drivers/barometer_ms5611.h"
+
+#include "drivers/compass.h"
 #include "drivers/compass_hmc5883l.h"
+#include "drivers/compass_ak8975.h"
+
 #include "drivers/sonar_hcsr04.h"
+
 #include "drivers/gpio.h"
 #include "drivers/system.h"
 
@@ -330,7 +336,7 @@ retry:
 
     // Found anything? Check if error or ACC is really missing.
     if (accHardware == ACC_DEFAULT) {
-        if (accHardwareToUse > ACC_DEFAULT) {
+        if (accHardwareToUse > ACC_DEFAULT && accHardwareToUse < ACC_NONE) {
             // Nothing was found and we have a forced sensor that isn't present.
             accHardwareToUse = ACC_DEFAULT;
             goto retry;
@@ -384,6 +390,77 @@ static void detectBaro()
     sensorsClear(SENSOR_BARO);
 }
 
+static void detectMag(uint8_t magHardwareToUse)
+{
+#ifdef USE_MAG_HMC5883
+    static hmc5883Config_t *hmc5883Config = 0;
+
+#ifdef NAZE
+    hmc5883Config_t nazeHmc5883Config;
+
+    if (hardwareRevision < NAZE32_REV5) {
+        nazeHmc5883Config.gpioAPB2Peripherals = RCC_APB2Periph_GPIOB;
+        nazeHmc5883Config.gpioPin = Pin_12;
+        nazeHmc5883Config.gpioPort = GPIOB;
+    } else {
+        nazeHmc5883Config.gpioAPB2Peripherals = RCC_APB2Periph_GPIOC;
+        nazeHmc5883Config.gpioPin = Pin_14;
+        nazeHmc5883Config.gpioPort = GPIOC;
+    }
+
+    hmc5883Config = &nazeHmc5883Config;
+#endif
+#endif
+
+retry:
+
+    magAlign = ALIGN_DEFAULT;
+
+    switch(magHardwareToUse) {
+        case MAG_NONE: // disable MAG
+            sensorsClear(SENSOR_MAG);
+            break;
+        case MAG_DEFAULT: // autodetect
+
+#ifdef USE_MAG_HMC5883
+        case MAG_HMC5883:
+            if (hmc5883lDetect(&mag, hmc5883Config)) {
+#ifdef NAZE
+                magAlign = CW180_DEG;
+#endif
+                magHardware = MAG_HMC5883;
+                if (magHardwareToUse == MAG_HMC5883)
+                    break;
+
+            }
+            ; // fallthrough
+#endif
+
+#ifdef USE_MAG_AK8975
+        case MAG_AK8975:
+            if (ak8975detect(&mag)) {
+                magHardware = MAG_AK8975;
+                if (magHardwareToUse == MAG_AK8975)
+                    break;
+            }
+            ; // fallthrough
+#endif
+            ; // prevent compiler error.
+    }
+
+    if (magHardware == MAG_DEFAULT) {
+        if (magHardwareToUse > MAG_DEFAULT && magHardwareToUse < MAG_NONE) {
+            // Nothing was found and we have a forced sensor that isn't present.
+            magHardwareToUse = MAG_DEFAULT;
+            goto retry;
+        } else {
+            // No MAG was detected
+            sensorsClear(SENSOR_MAG);
+        }
+    }
+
+}
+
 void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
 {
     if (sensorAlignmentConfig->gyro_align != ALIGN_DEFAULT) {
@@ -397,7 +474,7 @@ void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
     }
 }
 
-bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int16_t magDeclinationFromConfig)
+bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, uint8_t magHardwareToUse, int16_t magDeclinationFromConfig)
 {
     int16_t deg, min;
     memset(&acc, sizeof(acc), 0);
@@ -409,16 +486,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t
     sensorsSet(SENSOR_GYRO);
     detectAcc(accHardwareToUse);
     detectBaro();
-
-#ifdef MAG
-    if (hmc5883lDetect()) {
-#ifdef NAZE
-        magAlign = CW180_DEG;
-#endif
-    } else {
-        sensorsClear(SENSOR_MAG);
-    }
-#endif
+    detectMag(magHardwareToUse);
 
     reconfigureAlignment(sensorAlignmentConfig);
 
diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h
index 60e74e528..6fb8d5999 100644
--- a/src/main/target/CJMCU/target.h
+++ b/src/main/target/CJMCU/target.h
@@ -43,6 +43,7 @@
 #define USE_GYRO_MPU6050
 
 #define MAG
+#define USE_MAG_HMC5883
 
 #define BRUSHED_MOTORS
 
diff --git a/src/main/target/EUSTM32F103RC/target.h b/src/main/target/EUSTM32F103RC/target.h
index 7bdc8eed3..bb2245d37 100644
--- a/src/main/target/EUSTM32F103RC/target.h
+++ b/src/main/target/EUSTM32F103RC/target.h
@@ -63,6 +63,10 @@
 #define USE_BARO_BMP085
 
 #define MAG
+#define USE_MAG_HMC5883
+#define USE_MAG_AK8975
+
+
 #define SONAR
 #define LED0
 #define LED1
diff --git a/src/main/target/NAZE/hardware_revision.c b/src/main/target/NAZE/hardware_revision.c
index f9b5eaae6..3726c79e4 100644
--- a/src/main/target/NAZE/hardware_revision.c
+++ b/src/main/target/NAZE/hardware_revision.c
@@ -25,6 +25,7 @@
 
 #include "drivers/system.h"
 #include "drivers/bus_spi.h"
+#include "drivers/sensor.h"
 #include "drivers/accgyro.h"
 #include "drivers/accgyro_spi_mpu6500.h"
 
diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h
index 3226382e3..a176c9d87 100644
--- a/src/main/target/NAZE/target.h
+++ b/src/main/target/NAZE/target.h
@@ -74,6 +74,8 @@
 #define USE_BARO_BMP085
 
 #define MAG
+#define USE_MAG_HMC5883
+
 #define SONAR
 #define BEEPER
 #define LED0
diff --git a/src/main/target/OLIMEXINO/target.h b/src/main/target/OLIMEXINO/target.h
index 0e0b3bf7f..f74fe1c94 100644
--- a/src/main/target/OLIMEXINO/target.h
+++ b/src/main/target/OLIMEXINO/target.h
@@ -63,6 +63,8 @@
 #define USE_BARO_BMP085
 
 #define MAG
+#define USE_MAG_HMC5883
+
 #define SONAR
 
 #define USE_USART1
diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h
index a26c9eaa9..b45b145ff 100644
--- a/src/main/target/SPARKY/target.h
+++ b/src/main/target/SPARKY/target.h
@@ -36,6 +36,9 @@
 #define BARO
 #define USE_BARO_MS5611
 
+#define MAG
+#define USE_MAG_AK8975
+
 #define LED0
 #define LED1
 
@@ -76,7 +79,7 @@
 #define I2C2_SDA_CLK_SOURCE  RCC_AHBPeriph_GPIOA
 
 
-#define SENSORS_SET (SENSOR_ACC)
+#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
 
 #define SERIAL_RX
 #define GPS
diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c
index 223525d99..be9e0b830 100644
--- a/src/main/telemetry/frsky.c
+++ b/src/main/telemetry/frsky.c
@@ -31,6 +31,7 @@
 #include "common/axis.h"
 
 #include "drivers/system.h"
+#include "drivers/sensor.h"
 #include "drivers/accgyro.h"
 #include "drivers/gpio.h"
 #include "drivers/timer.h"
diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c
index bcf5b36e0..4451fd856 100644
--- a/src/main/telemetry/smartport.c
+++ b/src/main/telemetry/smartport.c
@@ -16,7 +16,9 @@
 #include "common/maths.h"
 
 #include "drivers/system.h"
+#include "drivers/sensor.h"
 #include "drivers/accgyro.h"
+#include "drivers/compass.h"
 #include "drivers/serial.h"
 #include "drivers/bus_i2c.h"
 #include "drivers/gpio.h"