diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp
index 5a0ef79dbe..18d2cc9136 100644
--- a/libraries/AP_Compass/AP_Compass.cpp
+++ b/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
diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h
index b798f31661..2c7b1ac741 100644
--- a/libraries/AP_Compass/AP_Compass.h
+++ b/libraries/AP_Compass/AP_Compass.h
@@ -481,7 +481,10 @@ private:
#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);
diff --git a/libraries/AP_Compass/AP_Compass_Backend.h b/libraries/AP_Compass/AP_Compass_Backend.h
index 779d63f385..79bfe1f66f 100644
--- a/libraries/AP_Compass/AP_Compass_Backend.h
+++ b/libraries/AP_Compass/AP_Compass_Backend.h
@@ -73,6 +73,7 @@ public:
DEVTYPE_MMC5983 = 0x13,
DEVTYPE_AK09918 = 0x14,
DEVTYPE_AK09915 = 0x15,
+ DEVTYPE_QMC5883P = 0x16,
};
#if AP_COMPASS_MSP_ENABLED
diff --git a/libraries/AP_Compass/AP_Compass_QMC5883P.cpp b/libraries/AP_Compass/AP_Compass_QMC5883P.cpp
new file mode 100644
index 0000000000..80d6e46f1c
--- /dev/null
+++ b/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 .
+ *
+ * Driver by Lokesh Ramina, Jan 2022
+ */
+#include "AP_Compass_QMC5883P.h"
+
+#include
+
+#include
+#include
+
+#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 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 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
diff --git a/libraries/AP_Compass/AP_Compass_QMC5883P.h b/libraries/AP_Compass/AP_Compass_QMC5883P.h
new file mode 100644
index 0000000000..4219640c7b
--- /dev/null
+++ b/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 .
+ *
+ * Driver by Lokesh Ramina, Jan 2022
+ */
+#pragma once
+
+#include
+#include
+#include
+
+#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 dev,
+ bool force_external,
+ enum Rotation rotation);
+
+ void read() override;
+
+ static constexpr const char *name = "QMC5883P";
+
+private:
+ AP_Compass_QMC5883P(AP_HAL::OwnPtr dev,
+ bool force_external,
+ enum Rotation rotation);
+
+ void _dump_registers();
+ bool _check_whoami();
+ void timer();
+ bool init();
+
+ AP_HAL::OwnPtr _dev;
+
+ enum Rotation _rotation;
+ uint8_t _instance;
+ bool _force_external:1;
+};
+
+#endif // AP_COMPASS_QMC5883P_ENABLED
diff --git a/libraries/AP_Compass/AP_Compass_config.h b/libraries/AP_Compass/AP_Compass_config.h
index 6bb8153b58..ce8989f2be 100644
--- a/libraries/AP_Compass/AP_Compass_config.h
+++ b/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