From b602bca6f2eb917d15878fa58120e90bdcf075ff Mon Sep 17 00:00:00 2001 From: MATBckh22 Date: Mon, 22 Sep 2025 11:06:19 +0800 Subject: [PATCH] Roll Pitch Yaw --- docs/IMU-QMI8658-QMC6310.md | 37 +++++++- src/graphics/draw/DebugRenderer.cpp | 11 ++- src/motion/QMC6310Sensor.cpp | 40 ++++++++- src/motion/QMC6310Sensor.h | 25 ++++++ src/motion/QMI8658Sensor.cpp | 84 +++++++++++++++++++ src/motion/SensorLiveData.h | 9 +- variants/esp32s3/tbeam-s3-core/platformio.ini | 2 + 7 files changed, 200 insertions(+), 8 deletions(-) diff --git a/docs/IMU-QMI8658-QMC6310.md b/docs/IMU-QMI8658-QMC6310.md index 16f127557..700317a80 100644 --- a/docs/IMU-QMI8658-QMC6310.md +++ b/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. - diff --git a/src/graphics/draw/DebugRenderer.cpp b/src/graphics/draw/DebugRenderer.cpp index 85d9b1c59..756a446a1 100644 --- a/src/graphics/draw/DebugRenderer.cpp +++ b/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"); } diff --git a/src/motion/QMC6310Sensor.cpp b/src/motion/QMC6310Sensor.cpp index bdb6ace1e..8215c9c46 100644 --- a/src/motion/QMC6310Sensor.cpp +++ b/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(); diff --git a/src/motion/QMC6310Sensor.h b/src/motion/QMC6310Sensor.h index 7353d4d20..920eb1584 100644 --- a/src/motion/QMC6310Sensor.h +++ b/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); diff --git a/src/motion/QMI8658Sensor.cpp b/src/motion/QMI8658Sensor.cpp index 3fbb63db5..597773f1b 100644 --- a/src/motion/QMI8658Sensor.cpp +++ b/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() @@ -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; } diff --git a/src/motion/SensorLiveData.h b/src/motion/SensorLiveData.h index 7e0609b57..b05aa5afe 100644 --- a/src/motion/SensorLiveData.h +++ b/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; - diff --git a/variants/esp32s3/tbeam-s3-core/platformio.ini b/variants/esp32s3/tbeam-s3-core/platformio.ini index e1ea8930d..399999583 100644 --- a/variants/esp32s3/tbeam-s3-core/platformio.ini +++ b/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