Supported Sensors Magnetometers: QMC5883P (#184)

* Add files via upload

* Add 'QMC5883P' to device choices in MagDevice

* Add MAG_QMC5883P to MagDevice.h enum

* Add support for MagQMC5338P device detection

* Add QMC5883P to supported magnetometers list

* Update MagDevice.cpp

* BusDevice.cpp

* Update MagQMC5338P.h

* Update Hardware.cpp

Author: hussam-qawaqzeh

README.md

@@ -90,7 +90,7 @@ After flashing you need to configure few things first:
 
  * Gyro: MPU6050, MPU6000, MPU6500, MPU9250, ICM20602, BMI160
  * Barometers: BMP180, BMP280, SPL06
- * Magnetometers: HMC5883, QMC5883, AK8963
+ * Magnetometers: HMC5883, QMC5883, AK8963, QMC5883P
  * Receivers: PPM, SBUS, IBUS, CRSF/ELRS
  * Esc Protocols: PWM, BRUSHED, ONESHOT125, ONESHOT42, MULTISHOT, DSHOT150, DSHOT300, DSHOT600
  * Other protocols: MSP, CLI, BLACKBOX, ESPNOW

lib/Espfc/src/Device/MagDevice.cpp

@@ -6,7 +6,7 @@ namespace Espfc::Device {
 
 const char ** MagDevice::getNames()
 {
-    static const char* devChoices[] = { PSTR("AUTO"), PSTR("NONE"), PSTR("HMC5883L"), PSTR("AK8975"), PSTR("AK8963"), PSTR("QMC5883L"),NULL };
+    static const char* devChoices[] = { PSTR("AUTO"), PSTR("NONE"), PSTR("HMC5883L"), PSTR("AK8975"), PSTR("AK8963"), PSTR("QMC5883L"),PSTR("QMC5883P"),NULL };
     return devChoices;
 }
 
@@ -16,4 +16,4 @@ const char * MagDevice::getName(DeviceType type)
     return getNames()[type];
 }
 
-}
+}

lib/Espfc/src/Device/MagDevice.h

@@ -12,6 +12,7 @@ enum MagDeviceType {
   MAG_AK8975  = 3,
   MAG_AK8963  = 4,
   MAG_QMC5883 = 5,
+  MAG_QMC5883P= 6,
   MAG_MAX
 };
 

lib/Espfc/src/Device/MagQMC5338P.h

@@ -0,0 +1,123 @@
+#ifndef _ESPFC_DEVICE_MAG_QMC5338P_H_
+#define _ESPFC_DEVICE_MAG_QMC5338P_H_
+
+#include "MagDevice.h"
+#include "BusDevice.h"
+
+// address for QMC5883P
+#define QMC5883P_ADDRESS            0x2C
+#define QMC5883P_DEFAULT_ADDRESS    0x2C
+
+// QMC5883P records according to Adafruit specifications
+#define QMC5883P_REG_CHIPID         0x00
+#define QMC5883P_REG_XOUT_LSB       0x01
+#define QMC5883P_REG_XOUT_MSB       0x02
+#define QMC5883P_REG_YOUT_LSB       0x03
+#define QMC5883P_REG_YOUT_MSB       0x04
+#define QMC5883P_REG_ZOUT_LSB       0x05
+#define QMC5883P_REG_ZOUT_MSB       0x06
+#define QMC5883P_REG_STATUS         0x09
+#define QMC5883P_REG_CONTROL1       0x0A
+#define QMC5883P_REG_CONTROL2       0x0B
+
+// Range values (as defined in Adafruit lib)
+#define QMC5883P_RANGE_30G          0x00
+#define QMC5883P_RANGE_12G          0x01
+#define QMC5883P_RANGE_8G           0x02
+#define QMC5883P_RANGE_2G           0x03
+
+// Mode values
+#define QMC5883P_MODE_CONTINUOUS    0x03  // Continuous measurement mode
+
+namespace Espfc {
+namespace Device {
+
+class MagQMC5338P : public MagDevice 
+{
+public:
+    int begin(BusDevice* bus) override 
+    {
+        return begin(bus, QMC5883P_DEFAULT_ADDRESS);
+    }
+
+    int begin(BusDevice* bus, uint8_t addr) override {
+        setBus(bus, addr);
+
+        if (!testConnection()) return 0;
+
+        // We use ±8G (0x02) as the default range for the drone.
+        _currentRange = QMC5883P_RANGE_8G;
+        setMode(_currentRange);
+
+        // Continuous operating mode
+        uint8_t ctrl1 = (QMC5883P_MODE_CONTINUOUS) |        // bits [1:0]
+                        (0x02 << 2) |                        // ODR = 100Hz (0x02)
+                        (0x03 << 4) |                        // OSR = 1 (0x03) ← faster
+                        (0x00 << 6);                         // DSR = 1
+        _bus->writeByte(_addr, QMC5883P_REG_CONTROL1, ctrl1);
+
+        // Initial reading
+        uint8_t buffer[6];
+        _bus->read(_addr, QMC5883P_REG_XOUT_LSB, 6, buffer);
+
+        return 1;
+    }
+
+    int readMag(VectorInt16& v) override {
+        uint8_t buffer[6];
+        if (_bus->read(_addr, QMC5883P_REG_XOUT_LSB, 6, buffer) != 6) {
+            return 0;
+        }
+
+        // in QMC5883P: X = [MSB=0x02, LSB=0x01] → buffer[1], buffer[0]
+        v.x = (int16_t)((buffer[1] << 8) | buffer[0]);
+        v.y = (int16_t)((buffer[3] << 8) | buffer[2]);
+        v.z = (int16_t)((buffer[5] << 8) | buffer[4]);
+
+        return 1;
+    }
+
+    const VectorFloat convert(const VectorInt16& v) const override {
+        float lsb_per_gauss;
+        switch (_currentRange) {
+            case QMC5883P_RANGE_30G: lsb_per_gauss = 1000.0f;   break;
+            case QMC5883P_RANGE_12G: lsb_per_gauss = 2500.0f;   break;
+            case QMC5883P_RANGE_8G:  lsb_per_gauss = 3750.0f;   break;
+            case QMC5883P_RANGE_2G:  lsb_per_gauss = 15000.0f;  break;
+            default: lsb_per_gauss = 3750.0f; // fallback to 8G
+        }
+        float scale = 1.0f / lsb_per_gauss;
+        return VectorFloat{ v.x * scale, v.y * scale, v.z * scale };
+    }
+
+    int getRate() const override {
+        return 100; // Based on ODR = 100Hz
+    }
+
+    virtual MagDeviceType getType() const override {
+        return MAG_QMC5883P;
+    }
+
+    void setMode(uint8_t range) {
+        _currentRange = range;
+        // Set the bits to [3:2] in CONTROL2
+        uint8_t ctrl2 = (range << 2);
+        _bus->writeByte(_addr, QMC5883P_REG_CONTROL2, ctrl2);
+    }
+
+    bool testConnection() override {
+        uint8_t chip_id;
+        if (_bus->read(_addr, QMC5883P_REG_CHIPID, 1, &chip_id) != 1) {
+            return false;
+        }
+        return chip_id == 0x80; 
+    }
+
+private:
+    uint8_t _currentRange = QMC5883P_RANGE_8G;
+};
+
+} 
+} 
+
+#endif

lib/Espfc/src/Hardware.cpp

@@ -8,6 +8,7 @@
 #include "Device/GyroBMI160.h"
 #include "Device/MagHMC5338L.h"
 #include "Device/MagQMC5338L.h"
+#include "Device/MagQMC5338P.h"
 #include "Device/MagAK8963.h"
 #include "Device/BaroDevice.h"
 #include "Device/BaroBMP085.h"
@@ -40,6 +41,7 @@ namespace {
   static Espfc::Device::GyroBMI160 bmi160;
   static Espfc::Device::MagHMC5338L hmc5883l;
   static Espfc::Device::MagQMC5338L qmc5883l;
+  static Espfc::Device::MagQMC5338P qmc5883p;
   static Espfc::Device::MagAK8963 ak8963;
   static Espfc::Device::BaroBMP085 bmp085;
   static Espfc::Device::BaroBMP280 bmp280;
@@ -128,13 +130,15 @@ void Hardware::detectMag()
     if(!detectedMag && detectDevice(ak8963, i2cBus)) detectedMag = &ak8963;
     if(!detectedMag && detectDevice(hmc5883l, i2cBus)) detectedMag = &hmc5883l;
     if(!detectedMag && detectDevice(qmc5883l, i2cBus)) detectedMag = &qmc5883l;
+    if(!detectedMag && detectDevice(qmc5883p, i2cBus)) detectedMag = &qmc5883p;
   }
 #endif
   if(gyroSlaveBus.getBus())
   {
     if(!detectedMag && detectDevice(ak8963, gyroSlaveBus)) detectedMag = &ak8963;
     if(!detectedMag && detectDevice(hmc5883l, gyroSlaveBus)) detectedMag = &hmc5883l;
     if(!detectedMag && detectDevice(qmc5883l, gyroSlaveBus)) detectedMag = &qmc5883l;
+    if(!detectedMag && detectDevice(qmc5883p, gyroSlaveBus)) detectedMag = &qmc5883p;
 
   }
   _model.state.mag.dev = detectedMag;