Roll Pitch Yaw

docs/IMU-QMI8658-QMC6310.md

@@ -55,7 +55,7 @@ Dependency pulled via PlatformIO:
 
   If `Δ` exceeds a small threshold (`0.15 g`), we wake the screen.
 
-### Debug Stream
+### Debug Stream & Fused Orientation (RPY)
 
 - The debug line (1 Hz) prints:
 
@@ -63,6 +63,8 @@ Dependency pulled via PlatformIO:
 
   This is also mirrored into the live data struct `g_qmi8658Live` that the UI reads.
 
+- An AHRS (Fusion library by Seb Madgwick) runs in the IMU thread using gyroscope + accelerometer and, when fresh, magnetometer from QMC6310. It outputs roll/pitch/yaw (ZYX, degrees) into `g_qmi8658Live.roll/pitch/yaw`. The QMI8658 UI screen displays “RPY r p y”.
+
 ---
 
 ## QMC6310 (I2C) – Implementation and Math
@@ -96,6 +98,26 @@ mz = rawZ − offsetZ
 
 This removes hard‑iron bias (DC offset) and is adequate for real‑time heading stabilization. For best results, slowly rotate the device on all axes for several seconds to let min/max settle.
 
+### Soft‑Iron Compensation (axis scaling)
+
+Ferric materials and PCB + enclosure can distort the local field, making the calibration cloud elliptical. We approximate this by computing per‑axis radii and scaling them toward the average radius:
+
+```
+R_x = (maxX − minX)/2
+R_y = (maxY − minY)/2
+R_z = (maxZ − minZ)/2
+R_avg = mean of available radii (ignore zeros)
+s_x = R_avg / R_x   (if R_x > 0 else 1)
+s_y = R_avg / R_y
+s_z = R_avg / R_z
+
+mx' = (rawX − offsetX) * s_x
+my' = (rawY − offsetY) * s_y
+mz' = (rawZ − offsetZ) * s_z
+```
+
+This improves the circularity of the cloud and reduces heading bias caused by anisotropy. For fully accurate compensation, an ellipsoid fit can be added later.
+
 Soft‑iron distortion (elliptical scaling) is NOT corrected here. A future enhancement can compute per‑axis scale from `(max−min)/2` or use an ellipsoid fit.
 
 ### Heading Computation
@@ -103,7 +125,7 @@ Soft‑iron distortion (elliptical scaling) is NOT corrected here. A future enha
 Raw 2‑D horizontal heading (no tilt compensation):
 
 ```
-heading_deg = atan2(my, mx) * 180/π
+heading_deg = atan2(my, mx) * 180/π     (Arduino‑style)
 heading_true = wrap_0_360( heading_deg + declination_deg + yaw_mount_offset )
 ```
 
@@ -115,6 +137,14 @@ Where:
 
 Screen orientation (0/90/180/270) is applied after heading is computed and normalized.
 
+Heading style and axis mapping are configurable via build flags:
+
+- `QMC6310_SWAP_XY` (0/1) – swap X and Y axes before heading.
+- `QMC6310_X_SIGN`, `QMC6310_Y_SIGN` (+1/−1) – flip axes if needed.
+- `QMC6310_HEADING_STYLE` (0/1)
+  - 0 → `atan2(my, mx)` (Arduino sketch style)
+  - 1 → `atan2(x, −y)` (Lewis He QST library `readPolar()` style)
+
 ### Tilt‑Compensated Heading (future option)
 
 If pitch/roll are available (from IMU), heading can be tilt‑compensated:
@@ -129,7 +159,7 @@ Where `φ` is roll and `θ` is pitch (radians), derived from accelerometer. This
 
 ### Live Data
 
-The magnetometer thread writes the latest raw XYZ, offsets and heading into `g_qmc6310Live` for the UI to display without touching hardware.
+The magnetometer thread writes the latest raw XYZ, offsets, µT (scaled), scale factors, and heading into `g_qmc6310Live` for the UI to display without touching hardware.
 
 ---
 
@@ -208,4 +238,3 @@ The values on the UI screens should match these, because both screens read from
 - If QMI8658 shows zeros on the UI screen, ensure `QMI8658_DEBUG_STREAM` is enabled or let the background IMU thread initialize first (it sets `g_qmi8658Live.initialized`).
 - If QMC6310 heading appears constrained or jumps, rotate the device slowly on all axes for 10–20 seconds to update min/max; verify you’re on the correct I2C port and address (`0x1C`).
 - If the I2C scan does not find the IMU on power‑on, check that the late‑rescan log appears; some boards power the sensor rail slightly later.
-

src/graphics/draw/DebugRenderer.cpp

@@ -711,7 +711,14 @@ void drawQMC6310Screen(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t
             snprintf(buf, sizeof(buf), "Head %3.0f  offX %.0f offY %.0f", g_qmc6310Live.heading, g_qmc6310Live.offX,
                      g_qmc6310Live.offY);
             display->drawString(x, getTextPositions(display)[line++], buf);
-            snprintf(buf, sizeof(buf), "rawX %d rawY %d", g_qmc6310Live.rawX, g_qmc6310Live.rawY);
+            snprintf(buf, sizeof(buf), "uT X %.1f  Y %.1f", g_qmc6310Live.uT_X, g_qmc6310Live.uT_Y);
+            display->drawString(x, getTextPositions(display)[line++], buf);
+            snprintf(buf, sizeof(buf), "scale X %.2f Y %.2f", g_qmc6310Live.scaleX, g_qmc6310Live.scaleY);
+            display->drawString(x, getTextPositions(display)[line++], buf);
+            // total field vs expected
+            float mag = sqrtf(g_qmc6310Live.uT_X * g_qmc6310Live.uT_X + g_qmc6310Live.uT_Y * g_qmc6310Live.uT_Y +
+                              g_qmc6310Live.uT_Z * g_qmc6310Live.uT_Z);
+            snprintf(buf, sizeof(buf), "|B| %.1f uT vs %.1f", mag, (float)QMC6310_EXPECTED_FIELD_uT);
             display->drawString(x, getTextPositions(display)[line++], buf);
         }
     } else {
@@ -733,6 +740,8 @@ void drawQMI8658Screen(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t
         display->drawString(x, getTextPositions(display)[line++], buf);
         snprintf(buf, sizeof(buf), "GYR %.2f %.2f %.2f", g_qmi8658Live.gyr.x, g_qmi8658Live.gyr.y, g_qmi8658Live.gyr.z);
         display->drawString(x, getTextPositions(display)[line++], buf);
+        snprintf(buf, sizeof(buf), "RPY %.1f %.1f %.1f", g_qmi8658Live.roll, g_qmi8658Live.pitch, g_qmi8658Live.yaw);
+        display->drawString(x, getTextPositions(display)[line++], buf);
     } else {
         display->drawString(x, getTextPositions(display)[line++], "No data");
     }

src/motion/QMC6310Sensor.cpp

@@ -74,9 +74,37 @@ int32_t QMC6310Sensor::runOnce()
 
         float mx = float(rx) - offsetX;
         float my = float(ry) - offsetY;
-        (void)rz; // mz is currently unused for heading
+        float mz = float(rz) - offsetZ;
 
-        float heading = atan2f(my, mx) * 180.0f / PI;
+        // Soft-iron scaling based on radii along axes
+        float radiusX = (maxX - minX) * 0.5f;
+        float radiusY = (maxY - minY) * 0.5f;
+        float radiusZ = (maxZ - minZ) * 0.5f;
+        float avgR = 0.0f; int rcount = 0;
+        if (radiusX > 1) { avgR += radiusX; rcount++; }
+        if (radiusY > 1) { avgR += radiusY; rcount++; }
+        if (radiusZ > 1) { avgR += radiusZ; rcount++; }
+        if (rcount > 0) avgR /= rcount; else avgR = 1.0f;
+        scaleX = (radiusX > 1) ? (avgR / radiusX) : 1.0f;
+        scaleY = (radiusY > 1) ? (avgR / radiusY) : 1.0f;
+        scaleZ = (radiusZ > 1) ? (avgR / radiusZ) : 1.0f;
+
+        mx *= scaleX; my *= scaleY; mz *= scaleZ;
+
+        // Axis mapping / sign
+        float hx = mx, hy = my;
+#if QMC6310_SWAP_XY
+        hx = my; hy = mx;
+#endif
+        hx *= (float)QMC6310_X_SIGN;
+        hy *= (float)QMC6310_Y_SIGN;
+
+        float heading;
+#if QMC6310_HEADING_STYLE == 1
+        heading = atan2f(hx, -hy) * 180.0f / PI; // QST library style
+#else
+        heading = atan2f(hy, hx) * 180.0f / PI;  // Arduino sketch style
+#endif
         heading += QMC6310_DECLINATION_DEG + QMC6310_YAW_MOUNT_OFFSET;
         while (heading < 0.0f) heading += 360.0f;
         while (heading >= 360.0f) heading -= 360.0f;
@@ -88,6 +116,14 @@ int32_t QMC6310Sensor::runOnce()
         g_qmc6310Live.offX = offsetX;
         g_qmc6310Live.offY = offsetY;
         g_qmc6310Live.offZ = offsetZ;
+        // Update live data (also publish scaled µT)
+        const float GAUSS_PER_LSB = QMC6310_SENS_GAUSS_PER_LSB;
+        g_qmc6310Live.uT_X = mx * GAUSS_PER_LSB * 100.0f;
+        g_qmc6310Live.uT_Y = my * GAUSS_PER_LSB * 100.0f;
+        g_qmc6310Live.uT_Z = mz * GAUSS_PER_LSB * 100.0f;
+        g_qmc6310Live.scaleX = scaleX;
+        g_qmc6310Live.scaleY = scaleY;
+        g_qmc6310Live.scaleZ = scaleZ;
         g_qmc6310Live.heading = heading;
         g_qmc6310Live.last_ms = millis();
 

src/motion/QMC6310Sensor.h

@@ -17,6 +17,29 @@
 #define QMC6310_YAW_MOUNT_OFFSET 0.0f
 #endif
 
+// Axis mapping and heading style controls
+#ifndef QMC6310_SWAP_XY
+#define QMC6310_SWAP_XY 0   // 0: normal, 1: swap X and Y
+#endif
+#ifndef QMC6310_X_SIGN
+#define QMC6310_X_SIGN 1    // +1 or -1 to flip X
+#endif
+#ifndef QMC6310_Y_SIGN
+#define QMC6310_Y_SIGN 1    // +1 or -1 to flip Y
+#endif
+#ifndef QMC6310_HEADING_STYLE
+#define QMC6310_HEADING_STYLE 0 // 0: atan2(my, mx); 1: atan2(x, -y) (QST library style)
+#endif
+
+// Sensitivity (Gauss/LSB) based on range; we set RANGE_2G in init()
+#ifndef QMC6310_SENS_GAUSS_PER_LSB
+#define QMC6310_SENS_GAUSS_PER_LSB 0.0066f
+#endif
+
+#ifndef QMC6310_EXPECTED_FIELD_uT
+#define QMC6310_EXPECTED_FIELD_uT 42.0f
+#endif
+
 class QMC6310Sensor : public MotionSensor
 {
   private:
@@ -27,6 +50,8 @@ class QMC6310Sensor : public MotionSensor
     float minY = 1e9f, maxY = -1e9f;
     float minZ = 1e9f, maxZ = -1e9f;
     float offsetX = 0.0f, offsetY = 0.0f, offsetZ = 0.0f;
+    // Soft-iron scale (computed from half-ranges)
+    float scaleX = 1.0f, scaleY = 1.0f, scaleZ = 1.0f;
 
   public:
     explicit QMC6310Sensor(ScanI2C::FoundDevice foundDevice);

src/motion/QMI8658Sensor.cpp

@@ -1,6 +1,7 @@
 #include "QMI8658Sensor.h"
 #include "NodeDB.h"
 #include "SensorLiveData.h"
+#include "Fusion/Fusion.h"
 
 #if !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C && __has_include(<SensorQMI8658.hpp>)
 
@@ -108,6 +109,49 @@ int32_t QMI8658Sensor::runOnce()
         LOG_DEBUG("QMI8658: ready=%d ACC[x=%.3f y=%.3f z=%.3f] m/s^2  GYR[x=%.3f y=%.3f z=%.3f] dps",
                   (int)ready, acc.x, acc.y, acc.z, gyr.x, gyr.y, gyr.z);
     }
+    // AHRS fusion (roll/pitch/yaw) using accelerometer, gyroscope and (optionally) magnetometer
+    {
+        static bool ahrsInit = false;
+        static FusionAhrs ahrs;
+        static uint32_t lastTime = 0;
+        const uint32_t now_ms = millis();
+        const float dt = (lastTime == 0) ? 0.01f : (now_ms - lastTime) / 1000.0f;
+        lastTime = now_ms;
+
+        if (!ahrsInit) {
+            FusionAhrsInitialise(&ahrs);
+            FusionAhrsSettings settings;
+            settings.convention = FusionConventionNed;   // NED frame
+            settings.gain = 0.5f;                        // fusion gain
+            settings.gyroscopeRange = 512.0f;            // dps range (matches config)
+            settings.accelerationRejection = 10.0f;      // deg
+            settings.magneticRejection = 10.0f;          // deg
+            settings.recoveryTriggerPeriod = 5;          // cycles
+            FusionAhrsSetSettings(&ahrs, &settings);
+            ahrsInit = true;
+        }
+
+        // Map live sensor values into Fusion vectors
+        FusionVector g = {.axis = {.x = g_qmi8658Live.gyr.x, .y = g_qmi8658Live.gyr.y, .z = g_qmi8658Live.gyr.z}};   // dps
+        FusionVector a = {.axis = {.x = g_qmi8658Live.acc.x, .y = g_qmi8658Live.acc.y, .z = g_qmi8658Live.acc.z}};   // m/s2
+
+        // Use magnetometer if recent (<= 200 ms old). Use µT values; the library normalises internally.
+        const bool magFresh = (now_ms - g_qmc6310Live.last_ms) <= 200 && g_qmc6310Live.initialized;
+        FusionVector m = {.axis = {.x = g_qmc6310Live.uT_X, .y = g_qmc6310Live.uT_Y, .z = g_qmc6310Live.uT_Z}};
+
+        if (magFresh) {
+            FusionAhrsUpdate(&ahrs, g, a, m, dt);
+        } else {
+            FusionAhrsUpdateNoMagnetometer(&ahrs, g, a, dt);
+        }
+
+        const FusionQuaternion q = FusionAhrsGetQuaternion(&ahrs);
+        const FusionEuler e = FusionQuaternionToEuler(q);
+        g_qmi8658Live.roll = e.angle.roll;
+        g_qmi8658Live.pitch = e.angle.pitch;
+        g_qmi8658Live.yaw = e.angle.yaw; // degrees
+    }
+
     return MOTION_SENSOR_CHECK_INTERVAL_MS;
 #endif
 
@@ -129,6 +173,46 @@ int32_t QMI8658Sensor::runOnce()
         }
     }
 
+    // When not streaming, we still update AHRS at the same cadence
+    {
+        static bool ahrsInit = false;
+        static FusionAhrs ahrs;
+        static uint32_t lastTime = 0;
+        const uint32_t now_ms = millis();
+        const float dt = (lastTime == 0) ? 0.01f : (now_ms - lastTime) / 1000.0f;
+        lastTime = now_ms;
+
+        if (!ahrsInit) {
+            FusionAhrsInitialise(&ahrs);
+            FusionAhrsSettings settings;
+            settings.convention = FusionConventionNed;
+            settings.gain = 0.5f;
+            settings.gyroscopeRange = 512.0f;
+            settings.accelerationRejection = 10.0f;
+            settings.magneticRejection = 10.0f;
+            settings.recoveryTriggerPeriod = 5;
+            FusionAhrsSetSettings(&ahrs, &settings);
+            ahrsInit = true;
+        }
+
+        FusionVector g = {.axis = {.x = g_qmi8658Live.gyr.x, .y = g_qmi8658Live.gyr.y, .z = g_qmi8658Live.gyr.z}};
+        FusionVector a = {.axis = {.x = g_qmi8658Live.acc.x, .y = g_qmi8658Live.acc.y, .z = g_qmi8658Live.acc.z}};
+        const bool magFresh = (now_ms - g_qmc6310Live.last_ms) <= 200 && g_qmc6310Live.initialized;
+        FusionVector m = {.axis = {.x = g_qmc6310Live.uT_X, .y = g_qmc6310Live.uT_Y, .z = g_qmc6310Live.uT_Z}};
+
+        if (magFresh) {
+            FusionAhrsUpdate(&ahrs, g, a, m, dt);
+        } else {
+            FusionAhrsUpdateNoMagnetometer(&ahrs, g, a, dt);
+        }
+
+        const FusionQuaternion q = FusionAhrsGetQuaternion(&ahrs);
+        const FusionEuler e = FusionQuaternionToEuler(q);
+        g_qmi8658Live.roll = e.angle.roll;
+        g_qmi8658Live.pitch = e.angle.pitch;
+        g_qmi8658Live.yaw = e.angle.yaw;
+    }
+
     return MOTION_SENSOR_CHECK_INTERVAL_MS;
 }
 

src/motion/SensorLiveData.h

@@ -13,6 +13,10 @@ struct QMI8658LiveData {
     bool ready = false;
     Vec3f acc;  // m/s^2
     Vec3f gyr;  // dps
+    // Fused orientation (degrees), using Fusion AHRS with QMC6310 magnetometer when available
+    float roll = 0.0f;
+    float pitch = 0.0f;
+    float yaw = 0.0f;
     uint32_t last_ms = 0;
 };
 
@@ -21,9 +25,12 @@ struct QMC6310LiveData {
     int16_t rawX = 0, rawY = 0, rawZ = 0;
     float offX = 0, offY = 0, offZ = 0;
     float heading = 0; // degrees 0..360
+    // Scaled field strength in microtesla (after hard/soft iron corrections)
+    float uT_X = 0, uT_Y = 0, uT_Z = 0;
+    // Soft-iron scale factors applied to calibrated axes
+    float scaleX = 1.0f, scaleY = 1.0f, scaleZ = 1.0f;
     uint32_t last_ms = 0;
 };
 
 extern QMI8658LiveData g_qmi8658Live;
 extern QMC6310LiveData g_qmc6310Live;
-

variants/esp32s3/tbeam-s3-core/platformio.ini

@@ -14,3 +14,5 @@ build_flags =
   -I variants/esp32s3/tbeam-s3-core
   -D PCF8563_RTC=0x51 ;Putting definitions in variant.h does not compile correctly
   -D QMI8658_DEBUG_STREAM
+  -D QMC6310_DECLINATION_DEG=-0.0167
+  -D QMC6310_EXPECTED_FIELD_uT=41.9154