Merged in LP-580_Add_QMC5883L_driver (pull request #496)

LP-580 Add QMC5883L driver Approved-by: Philippe Renon Approved-by: TheOtherCliff Approved-by: Jan NIJS Approved-by: Eric Price
2024-11-28 06:24:10 +01:00 · 2022-02-15 23:25:26 +00:00 · 2022-02-15 23:25:26 +00:00 · b4ccccdc13
commit b4ccccdc13
parent 7c9f04d87c f60b6bb630
18 changed files with 418 additions and 39 deletions
--- a/flight/make/apps-defs.mk
+++ b/flight/make/apps-defs.mk
@ -78,6 +78,7 @@ SRC += $(PIOSCOMMON)/pios_gcsrcvr.c
 SRC += $(PIOSCOMMON)/pios_hcsr04.c
 SRC += $(PIOSCOMMON)/pios_hmc5843.c
 SRC += $(PIOSCOMMON)/pios_hmc5x83.c
+SRC += $(PIOSCOMMON)/pios_qmc5883l.c
 SRC += $(PIOSCOMMON)/pios_i2c_esc.c
 SRC += $(PIOSCOMMON)/pios_l3gd20.c
 SRC += $(PIOSCOMMON)/pios_mpu6000.c
--- a/flight/modules/GPS/GPS.c
+++ b/flight/modules/GPS/GPS.c
@ -51,6 +51,9 @@
 #include "NMEA.h"
 #include "UBX.h"
 #include "DJI.h"
+#ifdef PIOS_INCLUDE_SENSORS_AUXMAG
+#include "sensors.h"
+#endif
 #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
 #include "inc/ubx_autoconfig.h"
 #define FULL_UBX_PARSER
@ -72,7 +75,7 @@ PERF_DEFINE_COUNTER(counterParse);
 #if defined(ANY_GPS_PARSER) && !defined(PIOS_GPS_MINIMAL)
 #define ANY_FULL_GPS_PARSER
 #endif
-#if (defined(PIOS_INCLUDE_HMC5X83) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)) && !defined(PIOS_GPS_MINIMAL)
+#if (defined(PIOS_INCLUDE_SENSORS_AUXMAG) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)) && !defined(PIOS_GPS_MINIMAL)
 #define ANY_FULL_MAG_PARSER
 #endif

@ -653,8 +656,8 @@ void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 #if defined(PIOS_INCLUDE_GPS_DJI_PARSER)
    dji_load_mag_settings();
 #endif
-#if defined(PIOS_INCLUDE_HMC5X83)
-    aux_hmc5x83_load_mag_settings();
+#if defined(PIOS_INCLUDE_SENSORS_AUXMAG)
+    sensors_auxmag_load_mag_settings();
 #endif
 }
 #endif /* defined(ANY_FULL_MAG_PARSER) */
--- a/flight/modules/GPS/inc/UBX.h
+++ b/flight/modules/GPS/inc/UBX.h
@ -677,6 +677,5 @@ uint32_t parse_ubx_message(struct UBXPacket *, GPSPositionSensorData *);

 int parse_ubx_stream(uint8_t *rx, uint16_t len, char *, GPSPositionSensorData *, struct GPS_RX_STATS *);
 void op_gpsv9_load_mag_settings();
-void aux_hmc5x83_load_mag_settings();

 #endif /* UBX_H */
--- a/flight/modules/Sensors/inc/sensors.h
+++ b/flight/modules/Sensors/inc/sensors.h
@ -34,5 +34,6 @@
 #include "openpilot.h"

 int32_t SensorsInitialize(void);
+void sensors_auxmag_load_mag_settings(void);

 #endif // SENSORS_H
--- a/flight/modules/Sensors/sensors.c
+++ b/flight/modules/Sensors/sensors.c
@ -71,6 +71,7 @@
 #include <CoordinateConversions.h>
 #include <pios_board_info.h>
 #include <string.h>
+#include "sensors.h"

 // Private constants
 #define STACK_SIZE_BYTES         1000
@ -131,10 +132,6 @@ PERF_DEFINE_COUNTER(counterBaroPeriod);
 PERF_DEFINE_COUNTER(counterSensorPeriod);
 PERF_DEFINE_COUNTER(counterSensorResets);

-#if defined(PIOS_INCLUDE_HMC5X83)
-void aux_hmc5x83_load_settings();
-#endif
-
 // Private functions
 static void SensorsTask(void *parameters);
 static void settingsUpdatedCb(UAVObjEvent *objEv);
@ -148,7 +145,7 @@ static void clearContext(sensor_fetch_context *sensor_context);
 static void handleAccel(float *samples, float temperature);
 static void handleGyro(float *samples, float temperature, uint32_t timestamp);
 static void handleMag(float *samples, float temperature);
-#if defined(PIOS_INCLUDE_HMC5X83)
+#if defined(PIOS_INCLUDE_SENSORS_AUXMAG)
 static void handleAuxMag(float *samples);
 #endif
 static void handleBaro(float sample, float temperature);
@ -193,7 +190,7 @@ static float baro_temp_bias   = 0;
 static float baro_temperature = NAN;
 static uint8_t baro_temp_calibration_count = 0;

-#if defined(PIOS_INCLUDE_HMC5X83)
+#if defined(PIOS_INCLUDE_SENSORS_AUXMAG)
 // Allow AuxMag to be disabled without reboot
 // because the other mags are that way
 static bool useAuxMag = false;
@ -211,7 +208,7 @@ int32_t SensorsInitialize(void)
    MagSensorInitialize();
    BaroSensorInitialize();

-#if defined(PIOS_INCLUDE_HMC5X83)
+#if defined(PIOS_INCLUDE_SENSORS_AUXMAG)
    // for auxmagsupport.c helpers
    AuxMagSensorInitialize();
 #endif
@ -388,7 +385,7 @@ static void processSamples3d(sensor_fetch_context *sensor_context, const PIOS_SE
    float inv_count = 1.0f / (float)sensor_context->count;
    if ((sensor->type & PIOS_SENSORS_TYPE_3AXIS_ACCEL)
        || (sensor->type == PIOS_SENSORS_TYPE_3AXIS_MAG)
-#if defined(PIOS_INCLUDE_HMC5X83)
+#if defined(PIOS_INCLUDE_SENSORS_AUXMAG)
        || (sensor->type == PIOS_SENSORS_TYPE_3AXIS_AUXMAG)
 #endif
        ) {
@ -403,7 +400,7 @@ static void processSamples3d(sensor_fetch_context *sensor_context, const PIOS_SE
            PERF_MEASURE_PERIOD(counterMagPeriod);
            return;

-#if defined(PIOS_INCLUDE_HMC5X83)
+#if defined(PIOS_INCLUDE_SENSORS_AUXMAG)
        case PIOS_SENSORS_TYPE_3AXIS_AUXMAG:
            handleAuxMag(samples);
            PERF_MEASURE_PERIOD(counterMagPeriod);
@ -502,7 +499,7 @@ static void handleMag(float *samples, float temperature)
    MagSensorSet(&mag);
 }

-#if defined(PIOS_INCLUDE_HMC5X83)
+#if defined(PIOS_INCLUDE_SENSORS_AUXMAG)
 static void handleAuxMag(float *samples)
 {
    if (useAuxMag) {
@ -641,8 +638,8 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
          fabsf(baroCorrection.d) > 1e-9f));
 }

-#if defined(PIOS_INCLUDE_HMC5X83)
-void aux_hmc5x83_load_mag_settings()
+#if defined(PIOS_INCLUDE_SENSORS_AUXMAG)
+void sensors_auxmag_load_mag_settings()
 {
    uint8_t magType = auxmagsupport_get_type();

--- a/flight/modules/StateEstimation/filtercf.c
+++ b/flight/modules/StateEstimation/filtercf.c
@ -259,16 +259,10 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float

    // During initialization and
    if (this->first_run) {
-#if defined(PIOS_INCLUDE_HMC5X83)
        // wait until mags have been updated
        if (!this->magUpdated && this->useMag) {
            return FILTERRESULT_ERROR;
        }
-#else
-        mag[0] = 100.0f;
-        mag[1] = 0.0f;
-        mag[2] = 0.0f;
-#endif

        pseudo_windowed_variance_init(&this->gyro_var[0], VARIANCE_WINDOW_SIZE);
        pseudo_windowed_variance_init(&this->gyro_var[1], VARIANCE_WINDOW_SIZE);
--- a/flight/pios/common/pios_board_sensors.c
+++ b/flight/pios/common/pios_board_sensors.c
@ -60,6 +60,10 @@ pios_hmc5x83_dev_t pios_hmc5x83_internal_id;
 # endif
 #endif

+#ifdef PIOS_INCLUDE_QMC5883L
+# include "pios_qmc5883l.h"
+#endif
+
 #ifdef PIOS_INCLUDE_L3GD20
 # include "pios_l3gd20.h"
 #endif
@ -146,35 +150,36 @@ void PIOS_BOARD_Sensors_Configure()

 # endif /* PIOS_INCLUDE_HMC5X83_INTERNAL */

-# ifdef PIOS_INCLUDE_HMC5X83
+# ifdef PIOS_INCLUDE_SENSORS_AUXMAG
+    AuxMagSettingsInitialize();

    AuxMagSettingsTypeOptions option;
    AuxMagSettingsTypeGet(&option);

-    const struct pios_hmc5x83_cfg *hmc5x83_external_cfg = PIOS_BOARD_HW_DEFS_GetExternalHMC5x83Cfg(pios_board_info_blob.board_rev);
+    uint32_t auxmag_i2c_id = 0;

-    if (hmc5x83_external_cfg) {
-        uint32_t i2c_id = 0;
-
-        if (option == AUXMAGSETTINGS_TYPE_FLEXI) {
-            // i2c_external
+    if (option == AUXMAGSETTINGS_TYPE_FLEXI) {
+        // i2c_external
 #ifdef PIOS_I2C_FLEXI_ADAPTER
-            i2c_id = PIOS_I2C_FLEXI_ADAPTER;
+        auxmag_i2c_id = PIOS_I2C_FLEXI_ADAPTER;
 #endif
-        } else if (option == AUXMAGSETTINGS_TYPE_I2C) {
-            // i2c_internal (or Sparky2/F3 dedicated I2C port)
+    } else if (option == AUXMAGSETTINGS_TYPE_I2C) {
+        // i2c_internal (or Sparky2/F3 dedicated I2C port)
 #ifdef PIOS_I2C_EXTERNAL_ADAPTER
-            i2c_id = PIOS_I2C_EXTERNAL_ADAPTER;
+        auxmag_i2c_id = PIOS_I2C_EXTERNAL_ADAPTER;
 #endif
-        }
+    }

-        if (i2c_id) {
-            uint32_t external_mag = PIOS_HMC5x83_Init(hmc5x83_external_cfg, i2c_id, 0);
-#  ifdef PIOS_INCLUDE_WDG
+    if (auxmag_i2c_id) {
+#  ifdef PIOS_INCLUDE_HMC5X83
+        const struct pios_hmc5x83_cfg *hmc5x83_external_cfg = PIOS_BOARD_HW_DEFS_GetExternalHMC5x83Cfg(pios_board_info_blob.board_rev);
+        if (hmc5x83_external_cfg) {
+            uint32_t external_mag = PIOS_HMC5x83_Init(hmc5x83_external_cfg, auxmag_i2c_id, 0);
+#   ifdef PIOS_INCLUDE_WDG
            // give HMC5x83 on I2C some extra time to allow for reset, etc. if needed
            // this is not in a loop, so it is safe
            PIOS_WDG_Clear();
-#  endif /* PIOS_INCLUDE_WDG */
+#   endif /* PIOS_INCLUDE_WDG */
            // add this sensor to the sensor task's list
            // be careful that you don't register a slow, unimportant sensor after registering the fastest sensor
            // and before registering some other fast and important sensor
@ -183,8 +188,16 @@ void PIOS_BOARD_Sensors_Configure()
            // mag alarm is cleared later, so use I2C
            AlarmsSet(SYSTEMALARMS_ALARM_I2C, (external_mag) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_WARNING);
        }
+#  endif /* PIOS_INCLUDE_HMC5X83 */
+#  ifdef PIOS_INCLUDE_QMC5883L
+        pios_qmc5883l_dev_t qmc5883l_dev = PIOS_QMC5883L_Init(auxmag_i2c_id);
+#   ifdef PIOS_INCLUDE_WDG
+        PIOS_WDG_Clear();
+#   endif /* PIOS_INCLUDE_WDG */
+        AlarmsSet(SYSTEMALARMS_ALARM_I2C, (qmc5883l_dev) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_WARNING);
+#  endif /* PIOS_INCLUDE_QMC5883L */
    }
-# endif /* PIOS_INCLUDE_HMC5X83 */
+# endif /* PIOS_INCLUDE_SENSORS_AUXMAG */

    // internal ms5611 baro
 #ifdef PIOS_INCLUDE_MS56XX
--- a/flight/pios/common/pios_qmc5883l.c
+++ b/flight/pios/common/pios_qmc5883l.c
@ -0,0 +1,319 @@
+/**
+ ******************************************************************************
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @{
+ * @addtogroup PIOS_QMC5883L QMC5883L Functions
+ * @brief Deals with the hardware interface to the QMC5883L magnetometer
+ * @{
+ *
+ * @file       pios_qmc5883l.c
+ * @author     The LibrePilot Project, http://www.librepilot.org, Copyright (C) 2018
+ * @brief      QMC5883L functions implementation.
+ * @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 "pios.h"
+#ifdef PIOS_INCLUDE_QMC5883L
+
+#include <pios_sensors.h>
+#include "pios_qmc5883l.h"
+
+#define PIOS_QMC5883L_I2C_ADDR                 0x0D
+#define PIOS_QMC5883L_CHIP_ID                  0xFF
+
+#define PIOS_QMC5883L_REG_DATA                 0x00
+#define PIOS_QMC5883L_REG_STATUS               0x06
+#define PIOS_QMC5883L_REG_TEMPL                0x07
+#define PIOS_QMC5883L_REG_TEMPH                0x08
+#define PIOS_QMC5883L_REG_CTRL1                0x09
+#define PIOS_QMC5883L_REG_CTRL2                0x0A
+#define PIOS_QMC5883L_REG_FBR                  0x0B
+#define PIOS_QMC5883L_REG_CHIP_ID              0x0D
+
+
+#define    PIOS_QMC5883L_CTRL1_ODR_10HZ        (0 << 2)
+#define    PIOS_QMC5883L_CTRL1_ODR_50HZ        (1 << 2)
+#define    PIOS_QMC5883L_CTRL1_ODR_100HZ       (2 << 2)
+#define    PIOS_QMC5883L_CTRL1_ODR_200HZ       (3 << 2)
+
+#define    PIOS_QMC5883L_CTRL1_OSR_512         (0 << 6)
+#define    PIOS_QMC5883L_CTRL1_OSR_256         (1 << 6)
+#define    PIOS_QMC5883L_CTRL1_OSR_128         (2 << 6)
+#define    PIOS_QMC5883L_CTRL1_OSR_64          (3 << 6)
+
+#define    PIOS_QMC5883L_CTRL1_RNG_2G          (0 << 4)
+#define    PIOS_QMC5883L_CTRL1_RNG_8G          (1 << 4)
+
+#define    PIOS_QMC5883L_CTRL1_MODE_STANDBY    (0 << 0)
+#define    PIOS_QMC5883L_CTRL1_MODE_CONTINUOUS (1 << 0)
+
+#define PIOS_QMC5883L_CTRL2_INT_ENB            (1 << 0)
+#define PIOS_QMC5883L_CTRL2_ROL_PNT            (1 << 6)
+#define PIOS_QMC5883L_CTRL2_SOFT_RST           (1 << 7)
+
+#define PIOS_QMC5883L_FBR_RECOMMENDED          0x01
+
+#define    PIOS_QMC5883L_STATUS_DOR            (1 << 2)
+#define    PIOS_QMC5883L_STATUS_OVL            (1 << 1)
+#define    PIOS_QMC5883L_STATUS_DRDY           (1 << 0)
+
+
+#define PIOS_QMC5883L_I2C_CONFIG_RETRY_DELAY   1000000
+#define PIOS_QMC5883L_I2C_CONFIG_DATA_DELAY    10000
+#define PIOS_QMC5883L_I2C_CONFIG_DATA_TIMEOUT  (PIOS_QMC5883L_I2C_CONFIG_DATA_DELAY * 10)
+#define PIOS_QMC5883L_I2C_RETRIES              2
+
+enum pios_qmc5883l_dev_magic {
+    PIOS_QMC5883L_MAGIC = 0xF8D3262A
+};
+
+struct pios_qmc5883l_dev {
+    enum pios_qmc5883l_dev_magic magic;
+    uint32_t i2c_id;
+
+    bool     sensorIsAlive;
+    uint32_t configTime;
+    uint32_t readTime;
+
+    int16_t  data_X, data_Y, data_Z;
+};
+
+
+static int32_t PIOS_QMC5883L_Read(uintptr_t i2c_id, uint8_t address, uint8_t *buffer, uint8_t len);
+static int32_t PIOS_QMC5883L_Write(uintptr_t i2c_id, uint8_t address, uint8_t buffer);
+static int32_t PIOS_QMC5883L_Configure(struct pios_qmc5883l_dev *dev);
+
+// sensor driver interface
+static bool PIOS_QMC5883L_driver_Test(uintptr_t context);
+static void PIOS_QMC5883L_driver_Reset(uintptr_t context);
+static void PIOS_QMC5883L_driver_get_scale(float *scales, uint8_t size, uintptr_t context);
+static void PIOS_QMC5883L_driver_fetch(void *, uint8_t size, uintptr_t context);
+static bool PIOS_QMC5883L_driver_poll(uintptr_t context);
+
+const PIOS_SENSORS_Driver PIOS_QMC5883L_Driver = {
+    .test      = PIOS_QMC5883L_driver_Test,
+    .poll      = PIOS_QMC5883L_driver_poll,
+    .fetch     = PIOS_QMC5883L_driver_fetch,
+    .reset     = PIOS_QMC5883L_driver_Reset,
+    .get_queue = NULL,
+    .get_scale = PIOS_QMC5883L_driver_get_scale,
+    .is_polled = true,
+};
+
+static bool PIOS_QMC5883L_Validate(struct pios_qmc5883l_dev *dev)
+{
+    return dev && (dev->magic == PIOS_QMC5883L_MAGIC);
+}
+
+pios_qmc5883l_dev_t PIOS_QMC5883L_Init(uint32_t i2c_device)
+{
+    struct pios_qmc5883l_dev *dev = (struct pios_qmc5883l_dev *)pios_malloc(sizeof(*dev));
+
+    memset(dev, 0, sizeof(*dev));
+
+    dev->i2c_id = i2c_device;
+    dev->magic  = PIOS_QMC5883L_MAGIC;
+
+    PIOS_SENSORS_Register(&PIOS_QMC5883L_Driver, PIOS_SENSORS_TYPE_3AXIS_AUXMAG, (uintptr_t)dev);
+
+    return dev;
+}
+
+static int32_t PIOS_QMC5883L_Configure(struct pios_qmc5883l_dev *dev)
+{
+    // read chip id?
+    uint8_t chip_id;
+
+    if (dev->sensorIsAlive) {
+        return 0;
+    }
+
+    if (PIOS_DELAY_DiffuS(dev->configTime) < PIOS_QMC5883L_I2C_CONFIG_RETRY_DELAY) { // Do not reinitialize too often
+        return -1;
+    }
+
+    dev->configTime    = PIOS_DELAY_GetRaw();
+    // Reset chip
+    dev->sensorIsAlive = (PIOS_QMC5883L_Write(dev->i2c_id, PIOS_QMC5883L_REG_CTRL2, PIOS_QMC5883L_CTRL2_SOFT_RST) == 0);
+    if (!dev->sensorIsAlive) {
+        return -1;
+    }
+
+    // Read ID
+    dev->sensorIsAlive = (PIOS_QMC5883L_Read(dev->i2c_id, PIOS_QMC5883L_REG_CHIP_ID, &chip_id, sizeof(chip_id)) == 0);
+    if (!dev->sensorIsAlive) {
+        return -1;
+    }
+
+    if (chip_id != PIOS_QMC5883L_CHIP_ID) {
+        return -2;
+    }
+
+    // Set FBR
+    dev->sensorIsAlive = (PIOS_QMC5883L_Write(dev->i2c_id, PIOS_QMC5883L_REG_FBR, PIOS_QMC5883L_FBR_RECOMMENDED) == 0);
+    if (!dev->sensorIsAlive) {
+        return -1;
+    }
+
+    // Set control registers
+    dev->sensorIsAlive = (PIOS_QMC5883L_Write(dev->i2c_id, PIOS_QMC5883L_REG_CTRL1, PIOS_QMC5883L_CTRL1_MODE_CONTINUOUS | PIOS_QMC5883L_CTRL1_ODR_200HZ | PIOS_QMC5883L_CTRL1_OSR_512 | PIOS_QMC5883L_CTRL1_RNG_8G) == 0);
+    if (!dev->sensorIsAlive) {
+        return -1;
+    }
+
+    return 0;
+}
+
+/**
+ * Reads one or more bytes into a buffer
+ * \param[in] the command indicating the address to read
+ * \param[out] buffer destination buffer
+ * \param[in] len number of bytes which should be read
+ * \return 0 if operation was successful
+ * \return -1 if error during I2C transfer
+ */
+static int32_t PIOS_QMC5883L_Read(uintptr_t i2c_id, uint8_t address, uint8_t *buffer, uint8_t len)
+{
+    const struct pios_i2c_txn txn_list[] = {
+        {
+            .info = __func__,
+            .addr = PIOS_QMC5883L_I2C_ADDR,
+            .rw   = PIOS_I2C_TXN_WRITE,
+            .len  = 1,
+            .buf  = &address,
+        }
+        ,
+        {
+            .info = __func__,
+            .addr = PIOS_QMC5883L_I2C_ADDR,
+            .rw   = PIOS_I2C_TXN_READ,
+            .len  = len,
+            .buf  = buffer,
+        }
+    };
+
+    for (uint8_t retry = PIOS_QMC5883L_I2C_RETRIES; retry > 0; --retry) {
+        if (PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list)) == 0) {
+            return 0;
+        }
+    }
+
+    return -1;
+}
+
+static int32_t PIOS_QMC5883L_Write(uintptr_t i2c_id, uint8_t address, uint8_t value)
+{
+    uint8_t data[] = { address, value };
+
+    const struct pios_i2c_txn txn_list[] = {
+        {
+            .info = __func__,
+            .addr = PIOS_QMC5883L_I2C_ADDR,
+            .rw   = PIOS_I2C_TXN_WRITE,
+            .len  = sizeof(data),
+            .buf  = data,
+        }
+    };
+
+    for (uint8_t retry = PIOS_QMC5883L_I2C_RETRIES; retry > 0; --retry) {
+        if (PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list)) == 0) {
+            return 0;
+        }
+    }
+
+    return -1;
+}
+
+bool PIOS_QMC5883L_driver_Test(__attribute__((unused)) uintptr_t context)
+{
+    return true;
+}
+
+static void PIOS_QMC5883L_driver_Reset(__attribute__((unused)) uintptr_t context)
+{}
+
+static void PIOS_QMC5883L_driver_get_scale(float *scales, uint8_t size, __attribute__((unused))  uintptr_t context)
+{
+    PIOS_Assert(size > 0);
+    scales[0] = 0.35f;
+}
+
+static void PIOS_QMC5883L_driver_fetch(void *data, __attribute__((unused)) uint8_t size, uintptr_t context)
+{
+    struct pios_qmc5883l_dev *dev = (struct pios_qmc5883l_dev *)context;
+
+    PIOS_Assert(PIOS_QMC5883L_Validate(dev));
+    PIOS_Assert(data);
+
+    PIOS_SENSORS_3Axis_SensorsWithTemp *tmp = data;
+
+    tmp->count = 1;
+    tmp->sample[0].x = dev->data_X;
+    tmp->sample[0].y = dev->data_Y;
+    tmp->sample[0].z = dev->data_Z;
+    tmp->temperature = 0;
+}
+
+static bool PIOS_QMC5883L_driver_poll(uintptr_t context)
+{
+    struct pios_qmc5883l_dev *dev = (struct pios_qmc5883l_dev *)context;
+
+    PIOS_Assert(PIOS_QMC5883L_Validate(dev));
+
+    if (!dev->sensorIsAlive) {
+        if (PIOS_QMC5883L_Configure(dev) < 0) {
+            return false;
+        }
+    }
+
+    if (PIOS_DELAY_DiffuS(dev->readTime) < PIOS_QMC5883L_I2C_CONFIG_DATA_DELAY) {
+        return false;
+    }
+
+    // Read Status
+    uint8_t status;
+    dev->sensorIsAlive = (PIOS_QMC5883L_Read(dev->i2c_id, PIOS_QMC5883L_REG_STATUS, &status, sizeof(status)) == 0);
+    if (!dev->sensorIsAlive) {
+        return false;
+    }
+
+    // Is it ready?
+    if (!(status & PIOS_QMC5883L_STATUS_DRDY)) {
+        if (PIOS_DELAY_DiffuS(dev->readTime) > PIOS_QMC5883L_I2C_CONFIG_DATA_TIMEOUT) { // the sensor has reset and it is not in continuous mode anymore
+            dev->sensorIsAlive = false;
+        }
+        return false;
+    }
+
+    uint8_t data[6];
+    dev->sensorIsAlive = (PIOS_QMC5883L_Read(dev->i2c_id, PIOS_QMC5883L_REG_DATA, data, sizeof(data)) == 0);
+    if (!dev->sensorIsAlive) {
+        return false;
+    }
+
+    dev->readTime = PIOS_DELAY_GetRaw();
+
+    dev->data_X   = (int16_t)(data[1] << 8 | data[0]);
+    dev->data_Y   = (int16_t)(data[3] << 8 | data[2]);
+    dev->data_Z   = (int16_t)(data[5] << 8 | data[4]);
+
+    return true;
+}
+
+
+#endif /* PIOS_INCLUDE_QMC5883L */
--- a/flight/pios/inc/pios_qmc5883l.h
+++ b/flight/pios/inc/pios_qmc5883l.h
@ -0,0 +1,40 @@
+/**
+ ******************************************************************************
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @{
+ * @addtogroup PIOS_QMC5883L QMC5883L Functions
+ * @brief Deals with the hardware interface to the QMC5883L magnetometer
+ * @{
+ *
+ * @file       pios_qmc5883l.h
+ * @author     The LibrePilot Project, http://www.librepilot.org, Copyright (C) 2018
+ * @brief      QMC5883L functions header.
+ * @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 PIOS_QMC5883L_H
+#define PIOS_QMC5883L_H
+
+struct pios_qmc5883l_dev;
+typedef struct pios_qmc5883l_dev *pios_qmc5883l_dev_t;
+
+extern pios_qmc5883l_dev_t PIOS_QMC5883L_Init(uint32_t i2c_device);
+
+
+#endif /* PIOS_QMC5883L_H */
--- a/flight/pios/pios.h
+++ b/flight/pios/pios.h
@ -339,6 +339,10 @@ extern "C" {
 /* Performance counters */
 /* #define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 995998 */

+#if defined(PIOS_INCLUDE_HMC5X83) || defined(PIOS_INCLUDE_QMC5883L)
+#define PIOS_INCLUDE_SENSORS_AUXMAG
+#endif
+
 #endif /* USE_SIM_POSIX */


--- a/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h
+++ b/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h
@ -87,6 +87,7 @@
 /* #define PIOS_INCLUDE_HMC5843 */
 #define PIOS_INCLUDE_HMC5X83
 #define PIOS_INCLUDE_HMC5X83_INTERNAL
+#define PIOS_INCLUDE_QMC5883L
 // #define PIOS_HMC5X83_HAS_GPIOS
 /* #define PIOS_INCLUDE_BMP085 */
 /* #define PIOS_INCLUDE_MS56XX */
--- a/flight/targets/boards/nucleof303re/firmware/inc/pios_config.h
+++ b/flight/targets/boards/nucleof303re/firmware/inc/pios_config.h
@ -86,6 +86,7 @@
 /* #define PIOS_MPU6000_ACCEL */
 /* #define PIOS_INCLUDE_HMC5843 */
 #define PIOS_INCLUDE_HMC5X83
+#define PIOS_INCLUDE_QMC5883L
 /* #define PIOS_HMC5883_HAS_GPIOS */
 /* #define PIOS_INCLUDE_MPU9250 */
 /* #define PIOS_MPU9250_ACCEL */
--- a/flight/targets/boards/revolution/firmware/inc/pios_config.h
+++ b/flight/targets/boards/revolution/firmware/inc/pios_config.h
@ -88,6 +88,7 @@
 #define PIOS_INCLUDE_HMC5X83
 #define PIOS_INCLUDE_HMC5X83_INTERNAL
 #define PIOS_HMC5X83_HAS_GPIOS
+#define PIOS_INCLUDE_QMC5883L
 /* #define PIOS_INCLUDE_BMP085 */
 #define PIOS_INCLUDE_MS56XX
 #define PIOS_INCLUDE_MPXV
--- a/flight/targets/boards/revonano/firmware/inc/pios_config.h
+++ b/flight/targets/boards/revonano/firmware/inc/pios_config.h
@ -86,6 +86,7 @@
 // #define PIOS_MPU6000_ACCEL
 /* #define PIOS_INCLUDE_HMC5843 */
 #define PIOS_INCLUDE_HMC5X83
+#define PIOS_INCLUDE_QMC5883L
 // #define PIOS_HMC5X83_HAS_GPIOS
 /* #define PIOS_INCLUDE_BMP085 */
 #define PIOS_INCLUDE_MS56XX
--- a/flight/targets/boards/revoproto/firmware/inc/pios_config.h
+++ b/flight/targets/boards/revoproto/firmware/inc/pios_config.h
@ -85,6 +85,7 @@
 #define PIOS_INCLUDE_HMC5X83
 #define PIOS_INCLUDE_HMC5X83_INTERNAL
 #define PIOS_HMC5X83_HAS_GPIOS
+#define PIOS_INCLUDE_QMC5883L
 /* #define PIOS_INCLUDE_BMP085 */
 #define PIOS_INCLUDE_MS56XX
 #define PIOS_INCLUDE_MPXV
--- a/flight/targets/boards/sparky2/firmware/inc/pios_config.h
+++ b/flight/targets/boards/sparky2/firmware/inc/pios_config.h
@ -86,6 +86,7 @@
 /* seems to be completely unused #define PIOS_MPU6000_ACCEL */
 /* #define PIOS_INCLUDE_HMC5843 */
 #define PIOS_INCLUDE_HMC5X83
+#define PIOS_INCLUDE_QMC5883L
 /* Sparky2 5X83s are all external and thus don't have GPIOs #define PIOS_HMC5X83_HAS_GPIOS */
 /* #define PIOS_INCLUDE_BMP085 */
 #define PIOS_INCLUDE_MS56XX
--- a/flight/targets/boards/spracingf3/firmware/inc/pios_config.h
+++ b/flight/targets/boards/spracingf3/firmware/inc/pios_config.h
@ -88,6 +88,7 @@
 #define PIOS_INCLUDE_HMC5X83
 #define PIOS_INCLUDE_HMC5X83_INTERNAL
 #define PIOS_HMC5883_HAS_GPIOS
+#define PIOS_INCLUDE_QMC5883L
 /* #define PIOS_INCLUDE_BMP085 */
 #define PIOS_INCLUDE_MS56XX
 /* #define PIOS_INCLUDE_MPXV */
--- a/flight/targets/boards/spracingf3evo/firmware/inc/pios_config.h
+++ b/flight/targets/boards/spracingf3evo/firmware/inc/pios_config.h
@ -86,6 +86,7 @@
 /* #define PIOS_MPU6000_ACCEL */
 /* #define PIOS_INCLUDE_HMC5843 */
 #define PIOS_INCLUDE_HMC5X83
+#define PIOS_INCLUDE_QMC5883L
 /* #define PIOS_HMC5883_HAS_GPIOS */
 #define PIOS_INCLUDE_MPU9250
 #define PIOS_MPU9250_ACCEL