Enable Set/Reset (write 0x29 to Z_MSB)

lib/Espfc/src/Device/MagQMC5338P.h

@@ -4,11 +4,9 @@
 #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
@@ -20,14 +18,12 @@
 #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
+#define QMC5883P_MODE_CONTINUOUS    0x03
 
 namespace Espfc {
 namespace Device {
@@ -45,18 +41,22 @@ class MagQMC5338P : public MagDevice
 
         if (!testConnection()) return 0;
 
-        // We use ±8G (0x02) as the default range for the drone.
-        _currentRange = QMC5883P_RANGE_8G;
-        setMode(_currentRange);
+        //  Step 1: Enable Set/Reset (write 0x29 to Z_MSB)
+        _bus->writeByte(_addr, QMC5883P_REG_ZOUT_MSB, 0x29);
 
-        // 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
+        //  Step 2: Set Range in CONTROL2[3:2] (Adafruit style)
+        _currentRange = QMC5883P_RANGE_8G;
+        setMode(_currentRange); // This will write to CONTROL2
+
+        //  Step 3: Configure CONTROL1
+        uint8_t ctrl1 = 
+            QMC5883P_MODE_CONTINUOUS |  // [1:0]
+            (0x02 << 2) |               // ODR = 100Hz [3:2]
+            (0x03 << 4) |               // OSR = 1 [5:4]
+            (0x00 << 6);                // DSR = 1 [7:6]
         _bus->writeByte(_addr, QMC5883P_REG_CONTROL1, ctrl1);
 
-        // Initial reading
+        // Initial read
         uint8_t buffer[6];
         _bus->read(_addr, QMC5883P_REG_XOUT_LSB, 6, buffer);
 
@@ -69,7 +69,7 @@ class MagQMC5338P : public MagDevice
             return 0;
         }
 
-        // in QMC5883P: X = [MSB=0x02, LSB=0x01] → buffer[1], buffer[0]
+        //  Read raw values EXACTLY like Adafruit (no sign flip yet)
         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]);
@@ -78,29 +78,31 @@ class MagQMC5338P : public MagDevice
     }
 
     const VectorFloat convert(const VectorInt16& v) const override {
+        //  Use Adafruit's conversion factors (based on actual range)
         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
+            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;
         }
         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
+        return 100;
     }
 
     virtual MagDeviceType getType() const override {
         return MAG_QMC5883P;
     }
 
+    //  CORRECT setMode: writes range to CONTROL2[3:2]
     void setMode(uint8_t range) {
         _currentRange = range;
-        // Set the bits to [3:2] in CONTROL2
+        // Range is in bits [3:2] of CONTROL2
         uint8_t ctrl2 = (range << 2);
         _bus->writeByte(_addr, QMC5883P_REG_CONTROL2, ctrl2);
     }
@@ -121,3 +123,6 @@ class MagQMC5338P : public MagDevice
 } 
 
 #endif
+
+
+