IMU Integration (QMI8658 + QMC6310) for LILYGO T-Beam Supreme with Madgwick Filter

.vscode/settings.json

@@ -10,5 +10,62 @@
   },
   "[powershell]": {
     "editor.defaultFormatter": "ms-vscode.powershell"
-  }
-}
+  },
+  "C_Cpp_Runner.cCompilerPath": "gcc",
+  "C_Cpp_Runner.cppCompilerPath": "g++",
+  "C_Cpp_Runner.debuggerPath": "gdb",
+  "C_Cpp_Runner.cStandard": "",
+  "C_Cpp_Runner.cppStandard": "",
+  "C_Cpp_Runner.msvcBatchPath": "C:/Program Files/Microsoft Visual Studio/VR_NR/Community/VC/Auxiliary/Build/vcvarsall.bat",
+  "C_Cpp_Runner.useMsvc": false,
+  "C_Cpp_Runner.warnings": [
+    "-Wall",
+    "-Wextra",
+    "-Wpedantic",
+    "-Wshadow",
+    "-Wformat=2",
+    "-Wcast-align",
+    "-Wconversion",
+    "-Wsign-conversion",
+    "-Wnull-dereference"
+  ],
+  "C_Cpp_Runner.msvcWarnings": [
+    "/W4",
+    "/permissive-",
+    "/w14242",
+    "/w14287",
+    "/w14296",
+    "/w14311",
+    "/w14826",
+    "/w44062",
+    "/w44242",
+    "/w14905",
+    "/w14906",
+    "/w14263",
+    "/w44265",
+    "/w14928"
+  ],
+  "C_Cpp_Runner.enableWarnings": true,
+  "C_Cpp_Runner.warningsAsError": false,
+  "C_Cpp_Runner.compilerArgs": [],
+  "C_Cpp_Runner.linkerArgs": [],
+  "C_Cpp_Runner.includePaths": [],
+  "C_Cpp_Runner.includeSearch": [
+    "*",
+    "**/*"
+  ],
+  "C_Cpp_Runner.excludeSearch": [
+    "**/build",
+    "**/build/**",
+    "**/.*",
+    "**/.*/**",
+    "**/.vscode",
+    "**/.vscode/**"
+  ],
+  "C_Cpp_Runner.useAddressSanitizer": false,
+  "C_Cpp_Runner.useUndefinedSanitizer": false,
+  "C_Cpp_Runner.useLeakSanitizer": false,
+  "C_Cpp_Runner.showCompilationTime": false,
+  "C_Cpp_Runner.useLinkTimeOptimization": false,
+  "C_Cpp_Runner.msvcSecureNoWarnings": false
+}

.vscode/tasks.json

@@ -1,15 +1,26 @@
 {
-  "version": "2.0.0",
-  "tasks": [
-    {
-      "type": "PlatformIO",
-      "task": "Build",
-      "problemMatcher": ["$platformio"],
-      "group": {
-        "kind": "build",
-        "isDefault": true
-      },
-      "label": "PlatformIO: Build"
-    }
-  ]
-}
+	"version": "2.0.0",
+	"tasks": [
+		{
+			"type": "PlatformIO",
+			"task": "Build",
+			"problemMatcher": [
+				"$platformio"
+			],
+			"group": {
+				"kind": "build",
+				"isDefault": true
+			},
+			"label": "PlatformIO: Build"
+		},
+		{
+			"label": "PlatformIO: Build",
+			"type": "shell",
+			"command": "python -m platformio run -e tbeam-s3-core",
+			"problemMatcher": [
+				"$platformio"
+			],
+			"group": "build"
+		}
+	]
+}

docs/IMU-QMI8658-QMC6310.md

@@ -0,0 +1,240 @@
+# IMU and Magnetometer Integration (QMI8658 + QMC6310)
+
+This document explains the implementation work added for the LilyGo T-Beam S3 Supreme to support:
+
+- QMI8658 6‑axis IMU over SPI (accelerometer + gyroscope) with a debug stream and a UI page
+- QMC6310 3‑axis magnetometer over I2C with live hard‑iron calibration, heading computation, and a UI page
+- I2C scanner improvements for robust IMU detection
+- A small “live data” layer so UI screens do not reset sensors
+
+The focus is on the math used and how the code is wired together.
+
+---
+
+## Files and Components
+
+- QMI8658 (SPI) driver wrapper
+  - `src/motion/QMI8658Sensor.h/.cpp`
+- QMC6310 (I2C) driver wrapper
+  - `src/motion/QMC6310Sensor.h/.cpp`
+- Live data shared with UI (prevents sensor resets by UI)
+  - `src/motion/SensorLiveData.h/.cpp` (globals `g_qmi8658Live`, `g_qmc6310Live`)
+- I2C scanner and main wiring
+  - `src/detect/ScanI2CTwoWire.cpp`, `src/detect/ScanI2C.cpp`, `src/main.cpp`
+- UI Screens (added after the GPS screen)
+  - `src/graphics/draw/DebugRenderer.h/.cpp`
+  - `src/graphics/Screen.cpp`
+
+Dependency pulled via PlatformIO:
+
+- Lewis He SensorLib (provides `SensorQMI8658.hpp`, `SensorQMC6310.hpp`) pinned in `platformio.ini`.
+
+---
+
+## QMI8658 (SPI) – Implementation and Math
+
+### Bus + Initialization
+
+- On ESP32‑S3, we use HSPI for the IMU to avoid clashing with radio SPI:
+  - Pins (T‑Beam S3 Supreme): `MOSI=35`, `MISO=37`, `SCK=36`, `IMU_CS=34`.
+  - Code creates a local `SPIClass(HSPI)` and calls `begin(SCK, MISO, MOSI, -1)`; sets `IMU_CS` HIGH.
+- The driver is configured as:
+  - Accelerometer range: ±4 g, ODR ≈ 1000 Hz, LPF mode 0
+  - Gyroscope range: ±64 dps, ODR ≈ 897 Hz, LPF mode 3
+- The thread (`QMI8658Sensor`) runs continuously. When `QMI8658_DEBUG_STREAM` is enabled, it samples every pass but logs once per second.
+
+### Units
+
+- Accelerometer is reported in m/s² by the library. To measure total acceleration (for wake‑on‑motion), we compute:
+
+  ```
+  |a| = sqrt(ax² + ay² + az²)
+  |a|_g = |a| / 9.80665
+  Δ = |a|_g − 1.0
+  ```
+
+  If `Δ` exceeds a small threshold (`0.15 g`), we wake the screen.
+
+### Debug Stream & Fused Orientation (RPY)
+
+- The debug line (1 Hz) prints:
+
+  `QMI8658: ready=<0/1> ACC[x y z] m/s^2 GYR[x y z] dps`
+
+  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
+
+### Bus + Initialization
+
+- Address: `0x1C` on the sensors bus (Wire). The I2C scanner detects it and exposes it as `ScanI2C::QMC6310`.
+- Configuration via SensorLib wrapper:
+  - Mode: continuous
+  - Range: 2 G
+  - ODR: 50 Hz
+  - Oversample: 8×, Downsample: 1×
+
+### Hard‑Iron Calibration (live)
+
+We continuously track min/max per axis and compute offsets as the center:
+
+```
+minX = min(minX, rawX)   maxX = max(maxX, rawX)
+minY = min(minY, rawY)   maxY = max(maxY, rawY)
+minZ = min(minZ, rawZ)   maxZ = max(maxZ, rawZ)
+
+offsetX = (maxX + minX) / 2
+offsetY = (maxY + minY) / 2
+offsetZ = (maxZ + minZ) / 2
+
+mx = rawX − offsetX
+my = rawY − offsetY
+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
+
+Raw 2‑D horizontal heading (no tilt compensation):
+
+```
+heading_deg = atan2(my, mx) * 180/π     (Arduino‑style)
+heading_true = wrap_0_360( heading_deg + declination_deg + yaw_mount_offset )
+```
+
+Where:
+
+- `declination_deg` compensates for local magnetic declination (positive East, negative West). We support a build‑time macro `QMC6310_DECLINATION_DEG`.
+- `yaw_mount_offset` lets you nudge heading for how the board is mounted; build‑time macro `QMC6310_YAW_MOUNT_OFFSET`.
+- `wrap_0_360(θ)` folds θ into `[0, 360)` by repeated add/subtract 360.
+
+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:
+
+```
+mx' = mx*cos(θ) + mz*sin(θ)
+my' = mx*sin(φ)*sin(θ) + my*cos(φ) − mz*sin(φ)*cos(θ)
+heading = atan2(my', mx')
+```
+
+Where `φ` is roll and `θ` is pitch (radians), derived from accelerometer. This is not implemented yet but can be added.
+
+### Live Data
+
+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.
+
+---
+
+## I2C Scanner Improvements
+
+### Dual‑address detection for IMUs
+
+- QMI8658 is now probed at both `0x6B` and `0x6A`.
+- To avoid collisions with chargers on `0x6B`, we first check:
+  - BQ24295 ID via register `0x0A == 0xC0`
+  - BQ25896 via `0x14` (bits 1:0 == `0b10`)
+- If not a charger, we read `0x0F`:
+  - `0x6A` → classify as LSM6DS3
+  - otherwise → classify as QMI8658
+
+### IMU‑only late rescan
+
+- After a normal pass, if neither QMI8658 nor LSM6DS3 is found on a port, we wait 700 ms and probe just `0x6A/0x6B` again. This helps boards that power the IMU slightly late.
+
+---
+
+## UI Screens
+
+Two additional screens are inserted right after the GPS screen:
+
+1) QMC6310 screen
+   - Shows `Heading`, `offX/offY`, and `rawX/rawY` (1 Hz)
+   - Data source: `g_qmc6310Live`
+
+2) QMI8658 screen
+   - Shows `ACC x/y/z` (m/s²) and `GYR x/y/z` (dps) (1 Hz)
+   - Data source: `g_qmi8658Live`
+
+Because these screens read the live data structs, they do NOT call `begin()` on sensors (which would reset them). This resolved the issue where the screen showed all zeros after switching.
+
+---
+
+## Build Flags and Configuration
+
+- Global debug stream (QMI8658):
+  - `-D QMI8658_DEBUG_STREAM` (enabled in the T‑Beam S3 Core variant)
+  - When enabled, the main will also start a parallel IMU debug thread even if an I2C accelerometer/magnetometer is present.
+
+- Declination and mount offset for QMC6310 heading (optional):
+  - `-D QMC6310_DECLINATION_DEG=<deg>` (e.g., `-0.25` for ≈ 0°15′ W)
+  - `-D QMC6310_YAW_MOUNT_OFFSET=<deg>` (tune to match a known reference)
+
+- SensorLib dependency (Lewis He):
+  - Pinned in `platformio.ini` under `[arduino_base]`:
+    `https://github.com/lewisxhe/SensorLib/archive/769b48472278aeaa62d5d0526eccceb74abe649a.zip`
+
+---
+
+## Example Logs
+
+```
+QMC6310: head=137.5 off[x=-12900 y=9352 z=12106] raw[x=-12990 y=9435 z=12134]
+QMI8658: ready=1 ACC[x=-0.782 y=0.048 z=0.539] m/s^2  GYR[x=11.742 y=4.570 z=1.836] dps
+```
+
+The values on the UI screens should match these, because both screens read from the live data updated by the threads.
+
+---
+
+## Known Limitations and Next Steps
+
+- QMC6310 uses live hard‑iron calibration only; no soft‑iron compensation yet. We can add per‑axis scale from `(max−min)/2` or use an ellipsoid fit for better accuracy.
+- Heading is not tilt‑compensated; adding pitch/roll from the IMU and applying the standard compensation will stabilize heading during motion.
+- We currently do not persist calibration offsets across boots; adding storage (NVS) would improve user experience.
+- The QMI8658 debug stream is designed for development and can be disabled via build flag to reduce log noise.
+
+---
+
+## Troubleshooting
+
+- 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.

platformio.ini

@@ -25,6 +25,7 @@ build_flags = -Wno-missing-field-initializers
 	-Wno-format
 	-Isrc -Isrc/mesh -Isrc/mesh/generated -Isrc/gps -Isrc/buzz -Wl,-Map,"${platformio.build_dir}"/output.map
 	-DUSE_THREAD_NAMES
+	-DQMI8658_DEBUG_STREAM
 	-DTINYGPS_OPTION_NO_CUSTOM_FIELDS
 	-DPB_ENABLE_MALLOC=1
 	-DRADIOLIB_EXCLUDE_CC1101=1
@@ -90,6 +91,8 @@ lib_deps =
 	${env.lib_deps}
 	# renovate: datasource=custom.pio depName=NonBlockingRTTTL packageName=end2endzone/library/NonBlockingRTTTL
 	end2endzone/[email protected]
+	# lewisxhe SensorLib (QMI8658)
+	https://github.com/lewisxhe/SensorLib/archive/769b48472278aeaa62d5d0526eccceb74abe649a.zip
 build_flags = ${env.build_flags} -Os
 build_src_filter = ${env.build_src_filter} -<platform/portduino/> -<graphics/niche/>