AP_Compass: Add in QMC5883P Driver

libraries/AP_Compass/AP_Compass.cpp

@@ -27,6 +27,7 @@
 #if AP_COMPASS_DRONECAN_ENABLED
 #include "AP_Compass_DroneCAN.h"
 #endif
+#include "AP_Compass_QMC5883P.h"
 #include "AP_Compass_MMC3416.h"
 #include "AP_Compass_MMC5xx3.h"
 #include "AP_Compass_MAG3110.h"
@@ -1132,6 +1133,26 @@ void Compass::_probe_external_i2c_compasses(void)
 #endif
 #endif  // AP_COMPASS_QMC5883L_ENABLED
 
+#if AP_COMPASS_QMC5883P_ENABLED
+    //external i2c bus
+    FOREACH_I2C_EXTERNAL(i) {
+        ADD_BACKEND(DRIVER_QMC5883P, AP_Compass_QMC5883P::probe(GET_I2C_DEVICE(i, HAL_COMPASS_QMC5883P_I2C_ADDR),
+                    true, HAL_COMPASS_QMC5883P_ORIENTATION_EXTERNAL));
+    }
+
+    // internal i2c bus
+#if !defined(HAL_SKIP_AUTO_INTERNAL_I2C_PROBE)
+    if (all_external) {
+        // only probe QMC5883P on internal if we are treating internals as externals
+        FOREACH_I2C_INTERNAL(i) {
+            ADD_BACKEND(DRIVER_QMC5883P, AP_Compass_QMC5883P::probe(GET_I2C_DEVICE(i, HAL_COMPASS_QMC5883P_I2C_ADDR),
+                        all_external,
+                        all_external?HAL_COMPASS_QMC5883P_ORIENTATION_EXTERNAL:HAL_COMPASS_QMC5883P_ORIENTATION_INTERNAL));
+        }
+    }
+#endif
+#endif  // AP_COMPASS_QMC5883P_ENABLED
+
 #ifndef HAL_BUILD_AP_PERIPH
     // AK09916 on ICM20948
 #if AP_COMPASS_AK09916_ENABLED && AP_COMPASS_ICM20948_ENABLED

libraries/AP_Compass/AP_Compass.h

@@ -481,7 +481,10 @@ friend class AP_Compass_Backend;
 #if AP_COMPASS_MMC5XX3_ENABLED
         DRIVER_MMC5XX3  =19,
 #endif
-    };
+#if AP_COMPASS_QMC5883P_ENABLED
+        DRIVER_QMC5883P =20,
+#endif
+};
 
     bool _driver_enabled(enum DriverType driver_type);
 

libraries/AP_Compass/AP_Compass_Backend.h

@@ -73,6 +73,7 @@ class AP_Compass_Backend
         DEVTYPE_MMC5983 = 0x13,
         DEVTYPE_AK09918 = 0x14,
         DEVTYPE_AK09915 = 0x15,
+    	DEVTYPE_QMC5883P = 0x16,
     };
 
 #if AP_COMPASS_MSP_ENABLED

libraries/AP_Compass/AP_Compass_QMC5883P.cpp

@@ -0,0 +1,221 @@
+/*
+ * This file 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 file 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, see <http://www.gnu.org/licenses/>.
+ *
+ * Driver by Lokesh Ramina, Jan 2022
+ */
+#include "AP_Compass_QMC5883P.h"
+
+#include <stdio.h>
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Math/AP_Math.h>
+
+#if AP_COMPASS_QMC5883P_ENABLED
+
+//Register Address
+#define QMC5883P_REG_ID                 0x00
+#define QMC5883P_REG_DATA_OUTPUT_X      0x01
+#define QMC5883P_REG_DATA_OUTPUT_Z_MSB  0x06
+#define QMC5883P_REG_STATUS             0x09
+#define QMC5883P_REG_CONF1              0x0A
+#define QMC5883P_REG_CONF2              0x0B
+
+#define QMC5883P_ID_VAL 0x80
+
+//Register values
+// Sensor operation modes
+#define QMC5883P_MODE_SUSPEND    0x00
+#define QMC5883P_MODE_NORMAL     0x01
+#define QMC5883P_MODE_SINGLE     0x02
+#define QMC5883P_MODE_CONTINUOUS 0x03
+
+// ODR data output rates for 5883L
+#define QMC5883P_ODR_10HZ  (0x00 << 2)
+#define QMC5883P_ODR_50HZ  (0x01 << 2)
+#define QMC5883P_ODR_100HZ (0x02 << 2)
+#define QMC5883P_ODR_200HZ (0x03 << 2)
+
+// Over sampling Ratio OSR1
+#define QMC5883P_OSR1_8 (0x00 << 4)
+#define QMC5883P_OSR1_4 (0x01 << 4)
+#define QMC5883P_OSR1_2	(0x02 << 4)
+#define QMC5883P_OSR1_1	(0x03 << 4)
+
+// Down sampling Rate OSR2
+#define QMC5883P_OSR2_8	0x08
+
+//RNG
+#define QMC5883P_RNG_30G (0x00 << 2)
+#define QMC5883P_RNG_12G (0x01 << 2)
+#define QMC5883P_RNG_8G  (0x10 << 2)
+#define QMC5883P_RNG_2G  (0x11 << 2)
+
+#define QMC5883P_SET_XYZ_SIGN 0x29
+
+//Reset
+#define QMC5883P_RST 0x80
+
+//Status Val
+#define QMC5883P_STATUS_DATA_READY 0x01
+
+#ifndef DEBUG
+#define DEBUG 0
+#endif
+
+extern const AP_HAL::HAL &hal;
+
+AP_Compass_Backend *AP_Compass_QMC5883P::probe(AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
+        bool force_external,
+        enum Rotation rotation)
+{
+    if (!dev) {
+        return nullptr;
+    }
+    AP_Compass_QMC5883P *sensor = new AP_Compass_QMC5883P(std::move(dev),force_external,rotation);
+    if (!sensor || !sensor->init()) {
+        delete sensor;
+        return nullptr;
+    }
+    return sensor;
+}
+
+AP_Compass_QMC5883P::AP_Compass_QMC5883P(AP_HAL::OwnPtr<AP_HAL::Device> dev,
+        bool force_external,
+        enum Rotation rotation)
+    : _dev(std::move(dev))
+    , _rotation(rotation)
+    , _force_external(force_external)
+{
+}
+
+bool AP_Compass_QMC5883P::init()
+{
+    _dev->get_semaphore()->take_blocking();
+
+    _dev->set_retries(10);
+#if DEBUG
+    _dump_registers();
+#endif
+    if (!_check_whoami()) {
+        goto fail;
+    }
+    //As mentioned in the Datasheet 7.2 to do continues mode 0x29 will set sign for X,Y,Z
+    if (!_dev->write_register(QMC5883P_REG_DATA_OUTPUT_Z_MSB, QMC5883P_SET_XYZ_SIGN)||
+        !_dev->write_register(QMC5883P_REG_CONF1,
+                              QMC5883P_MODE_CONTINUOUS|
+                              QMC5883P_ODR_100HZ|
+                              QMC5883P_OSR1_8|
+                              QMC5883P_OSR2_8)||
+        !_dev->write_register(QMC5883P_REG_CONF2,QMC5883P_OSR2_8)) {
+        goto fail;
+    }
+
+    // lower retries for run
+    _dev->set_retries(3);
+
+    _dev->get_semaphore()->give();
+
+    //register compass instance
+    _dev->set_device_type(DEVTYPE_QMC5883P);
+    if (!register_compass(_dev->get_bus_id(), _instance)) {
+        return false;
+    }
+    set_dev_id(_instance, _dev->get_bus_id());
+
+    printf("%s found on bus %u id %u address 0x%02x\n", name,
+           _dev->bus_num(), _dev->get_bus_id(), _dev->get_bus_address());
+
+    set_rotation(_instance, _rotation);
+
+    if (_force_external) {
+        set_external(_instance, true);
+    }
+
+    //Enable 100HZ
+    _dev->register_periodic_callback(10000,
+                                     FUNCTOR_BIND_MEMBER(&AP_Compass_QMC5883P::timer, void));
+
+    return true;
+
+fail:
+    _dev->get_semaphore()->give();
+    return false;
+}
+bool AP_Compass_QMC5883P::_check_whoami()
+{
+    uint8_t whoami;
+    if (!_dev->read_registers(QMC5883P_REG_ID, &whoami,1)||
+        whoami != QMC5883P_ID_VAL) {
+        return false;
+    }
+    return true;
+}
+
+void AP_Compass_QMC5883P::timer()
+{
+    struct PACKED {
+        int16_t rx;
+        int16_t ry;
+        int16_t rz;
+    } buffer;
+
+    const float range_scale = 1000.0f / 3000.0f;
+
+    uint8_t status;
+    if (!_dev->read_registers(QMC5883P_REG_STATUS,&status,1)) {
+        return;
+    }
+    //new data is ready
+    if (!(status & QMC5883P_STATUS_DATA_READY)) {
+        return;
+    }
+
+    if (!_dev->read_registers(QMC5883P_REG_DATA_OUTPUT_X, (uint8_t *) &buffer, sizeof(buffer))) {
+        return ;
+    }
+
+    auto x = buffer.rx;
+    auto y = buffer.ry;
+    auto z = buffer.rz;
+
+#if DEBUG
+    printf("mag.x:%d\n",x);
+    printf("mag.y:%d\n",y);
+    printf("mag.z:%d\n",z);
+#endif
+
+    Vector3f field = Vector3f{x * range_scale, y * range_scale, z * range_scale };
+
+    accumulate_sample(field, _instance, 20);
+}
+
+void AP_Compass_QMC5883P::read()
+{
+    drain_accumulated_samples(_instance);
+}
+
+void AP_Compass_QMC5883P::_dump_registers()
+{
+    printf("QMC5883P registers dump\n");
+    for (uint8_t reg = QMC5883P_REG_DATA_OUTPUT_X; reg <= 0x30; reg++) {
+        uint8_t v;
+        _dev->read_registers(reg,&v,1);
+        printf("%02x:%02x ", (unsigned)reg, (unsigned)v);
+        if ((reg - ( QMC5883P_REG_DATA_OUTPUT_X-1)) % 16 == 0) {
+            printf("\n");
+        }
+    }
+}
+
+#endif //AP_COMPASS_QMC5883P_ENABLED

libraries/AP_Compass/AP_Compass_QMC5883P.h

@@ -0,0 +1,73 @@
+/*
+ * This file 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 file 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, see <http://www.gnu.org/licenses/>.
+ *
+ * Driver by Lokesh Ramina, Jan 2022
+ */
+#pragma once
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_HAL/I2CDevice.h>
+#include <AP_Math/AP_Math.h>
+
+#include "AP_Compass_config.h"
+
+#ifdef AP_COMPASS_QMC5883P_ENABLED
+
+#include "AP_Compass.h"
+#include "AP_Compass_Backend.h"
+
+#ifndef HAL_COMPASS_QMC5883P_I2C_ADDR
+#define HAL_COMPASS_QMC5883P_I2C_ADDR 0x2C
+#endif
+
+/*
+  setup default orientations
+ */
+#ifndef HAL_COMPASS_QMC5883P_ORIENTATION_EXTERNAL
+#define HAL_COMPASS_QMC5883P_ORIENTATION_EXTERNAL ROTATION_ROLL_180
+#endif
+
+#ifndef HAL_COMPASS_QMC5883P_ORIENTATION_INTERNAL
+#define HAL_COMPASS_QMC5883P_ORIENTATION_INTERNAL ROTATION_ROLL_180_YAW_270
+#endif
+
+class AP_Compass_QMC5883P : public AP_Compass_Backend
+{
+public:
+    static AP_Compass_Backend *probe(AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
+                                     bool force_external,
+                                     enum Rotation rotation);
+
+    void read() override;
+
+    static constexpr const char *name = "QMC5883P";
+
+private:
+    AP_Compass_QMC5883P(AP_HAL::OwnPtr<AP_HAL::Device> dev,
+                        bool force_external,
+                        enum Rotation rotation);
+
+    void _dump_registers();
+    bool _check_whoami();
+    void timer();
+    bool init();
+
+    AP_HAL::OwnPtr<AP_HAL::Device> _dev;
+
+    enum Rotation _rotation;
+    uint8_t _instance;
+    bool _force_external:1;
+};
+
+#endif  // AP_COMPASS_QMC5883P_ENABLED

libraries/AP_Compass/AP_Compass_config.h

@@ -112,6 +112,10 @@
 #define AP_COMPASS_MMC5XX3_ENABLED AP_COMPASS_I2C_BACKEND_DEFAULT_ENABLED
 #endif
 
+#ifndef AP_COMPASS_QMC5883P_ENABLED
+#define AP_COMPASS_QMC5883P_ENABLED AP_COMPASS_I2C_BACKEND_DEFAULT_ENABLED && (BOARD_FLASH_SIZE > 1024)
+#endif
+
 #ifndef AP_COMPASS_QMC5883L_ENABLED
 #define AP_COMPASS_QMC5883L_ENABLED AP_COMPASS_I2C_BACKEND_DEFAULT_ENABLED
 #endif