From b3cc319d5efb4e56ffa53a73ea1a06db10f4287d Mon Sep 17 00:00:00 2001 From: Nik Rozman Date: Sun, 1 Mar 2026 22:12:23 +0100 Subject: [PATCH] Attempt to fix roll correction --- source/config.h | 4 ++-- source/imu.cpp | 21 --------------------- source/imu.h | 5 ----- source/main.cpp | 33 ++++++++++++--------------------- web/app.js | 22 +++++++++++----------- 5 files changed, 25 insertions(+), 60 deletions(-) diff --git a/source/config.h b/source/config.h index dca06ba..5c23ae2 100644 --- a/source/config.h +++ b/source/config.h @@ -122,8 +122,8 @@ extern TelemetryPacket telem; // ─── ImuPacket (14 bytes) ───────────────────────────────────────────────────── #ifdef FEATURE_IMU_STREAM struct __attribute__((packed)) ImuPacket { - int16_t gyroY_mDPS; // [0] - int16_t gyroZ_mDPS; // [2] + int16_t gyroX_mDPS; // [0] pitch axis (nod up/down → cursor Y) + int16_t gyroZ_mDPS; // [2] yaw axis (pan left/right → cursor X) int16_t accelX_mg; // [4] int16_t accelY_mg; // [6] int16_t accelZ_mg; // [8] diff --git a/source/imu.cpp b/source/imu.cpp index 25eb215..74e24ab 100644 --- a/source/imu.cpp +++ b/source/imu.cpp @@ -4,9 +4,6 @@ LSM6DS3 imu(I2C_MODE, 0x6A); -float rollSin = 0.0f; // identity: no rotation -float rollCos = 1.0f; - // ─── I2C helpers ────────────────────────────────────────────────────────────── void imuWriteReg(uint8_t reg, uint8_t val) { // LSM6DS3 is on Wire1 (internal I2C, SDA=P0.17, SCL=P0.16), NOT Wire (external pins 4/5) @@ -29,10 +26,8 @@ float readIMUTemp() { void calibrateGyroBias() { Serial.println("[CAL] Hold still..."); double sx=0, sy=0, sz=0; - double sax=0, say=0; for (int i=0; i 0.05f) { // only update if we can see meaningful tilt (>~3°) - rollSin = ax_avg / norm; - rollCos = -ay_avg / norm; // negative: gravity pulls in -Y when flat and nominal - } else { - rollSin = 0.0f; - rollCos = 1.0f; - } - Serial.print("[CAL] roll="); Serial.print(atan2f(rollSin, rollCos)*(180.f/PI), 1); Serial.println("deg"); - #ifdef FEATURE_TELEMETRY statRecalCount++; float bxr = biasGX*(PI/180.f), byr = biasGY*(PI/180.f), bzr = biasGZ*(PI/180.f); diff --git a/source/imu.h b/source/imu.h index c997db6..74aea5b 100644 --- a/source/imu.h +++ b/source/imu.h @@ -4,11 +4,6 @@ extern LSM6DS3 imu; -// Roll-compensation rotation matrix coefficients (computed in calibrateGyroBias). -// Rotate device-frame [gz, gy] → world-frame [screenX, screenY]. -extern float rollSin; // sin of device yaw on table -extern float rollCos; // cos of device yaw on table - void imuWriteReg(uint8_t reg, uint8_t val); uint8_t imuReadReg(uint8_t reg); float readIMUTemp(); diff --git a/source/main.cpp b/source/main.cpp index 33969fd..0677a68 100644 --- a/source/main.cpp +++ b/source/main.cpp @@ -327,15 +327,15 @@ void loop() { #endif // Gyro reads with optional temperature compensation - float gx, gy, gz; + float gx, gz; #ifdef FEATURE_TEMP_COMPENSATION float correction = TEMP_COMP_COEFF_DPS_C * (cachedTempC - calTempC); gx = (imu.readFloatGyroX() - biasGX - correction) * (PI/180.0f); - gy = (imu.readFloatGyroY() - biasGY - correction) * (PI/180.0f); + (void)(imu.readFloatGyroY()); // GY unused — read to keep SPI/I2C sequence consistent gz = (imu.readFloatGyroZ() - biasGZ - correction) * (PI/180.0f); #else gx = (imu.readFloatGyroX() - biasGX) * (PI/180.0f); - gy = (imu.readFloatGyroY() - biasGY) * (PI/180.0f); + (void)(imu.readFloatGyroY()); // GY unused — read to keep SPI/I2C sequence consistent gz = (imu.readFloatGyroZ() - biasGZ) * (PI/180.0f); #endif @@ -343,14 +343,16 @@ void loop() { float ay = imu.readFloatAccelY(); float az = imu.readFloatAccelZ(); - // Complementary filter + // Complementary filter — gx=pitch axis, gz=yaw axis on this board layout angleX = ALPHA*(angleX + gx*dt) + (1.0f - ALPHA)*atan2f(ax, sqrtf(ay*ay + az*az)); - angleY = ALPHA*(angleY + gy*dt) + (1.0f - ALPHA)*atan2f(ay, sqrtf(ax*ax + az*az)); + angleY = ALPHA*(angleY + gz*dt) + (1.0f - ALPHA)*atan2f(ay, sqrtf(ax*ax + az*az)); - float fGy = (fabsf(gy) > cfg.deadZone) ? gy : 0.0f; + // Pan (cursor X) = gyro Z (yaw). Nod (cursor Y) = gyro X (pitch). + // Confirmed by serial diagnostics: GZ fires on left/right pan, GX fires on up/down nod. + float fGx = (fabsf(gx) > cfg.deadZone) ? gx : 0.0f; float fGz = (fabsf(gz) > cfg.deadZone) ? gz : 0.0f; - bool moving = (fGy != 0.0f || fGz != 0.0f); + bool moving = (fGx != 0.0f || fGz != 0.0f); if (moving) { idleFrames = 0; idleStartMs = 0; } else { idleFrames++; if (idleStartMs == 0) idleStartMs = now; } bool idle = (idleFrames >= IDLE_FRAMES); @@ -369,11 +371,8 @@ void loop() { accumX = accumY = 0.0f; flags |= 0x01; } else { - // World-frame gyro: rotate device axes by boot-pose yaw - float wGz = fGz * rollCos + fGy * rollSin; - float wGy = fGy * rollCos - fGz * rollSin; - float rawX = applyAcceleration(applyCurve(-wGz * cfg.sensitivity * dt)); - float rawY = applyAcceleration(applyCurve(-wGy * cfg.sensitivity * dt)); + float rawX = applyAcceleration(applyCurve(-fGz * cfg.sensitivity * dt)); + float rawY = applyAcceleration(applyCurve(-fGx * cfg.sensitivity * dt)); if (cfg.axisFlip & 0x01) rawX = -rawX; if (cfg.axisFlip & 0x02) rawY = -rawY; accumX += rawX; accumY += rawY; @@ -392,7 +391,7 @@ void loop() { // Backing off — host TX buffer congested, skip to avoid 100ms block } else { ImuPacket pkt; - pkt.gyroY_mDPS = (int16_t)constrain(gy*(180.f/PI)*1000.f, -32000, 32000); + pkt.gyroX_mDPS = (int16_t)constrain(gx*(180.f/PI)*1000.f, -32000, 32000); pkt.gyroZ_mDPS = (int16_t)constrain(gz*(180.f/PI)*1000.f, -32000, 32000); pkt.accelX_mg = (int16_t)constrain(ax*1000.f, -32000, 32000); pkt.accelY_mg = (int16_t)constrain(ay*1000.f, -32000, 32000); @@ -426,12 +425,4 @@ void loop() { } } #endif - - #ifdef DEBUG - Serial.print("T="); Serial.print(cachedTempC,1); - Serial.print(" gy="); Serial.print(gy,3); - Serial.print(" gz="); Serial.print(gz,3); - Serial.print(" mx="); Serial.print(moveX); - Serial.print(" my="); Serial.println(moveY); - #endif } diff --git a/web/app.js b/web/app.js index 2337cb3..81b051c 100644 --- a/web/app.js +++ b/web/app.js @@ -488,7 +488,7 @@ function onDisconnected() { // ── IMU Stream + Visualiser ────────────────────────────────────────────────── // ImuPacket (14 bytes LE): -// int16 gyroY_mDPS [0], int16 gyroZ_mDPS [2] +// int16 gyroX_mDPS [0], int16 gyroZ_mDPS [2] // int16 accelX_mg [4], int16 accelY_mg [6], int16 accelZ_mg [8] // int8 moveX [10], int8 moveY [11], uint8 flags [12], uint8 pad [13] const canvas = document.getElementById('vizCanvas'); @@ -593,10 +593,10 @@ function parseImuStream(dv) { return; } - let gyroY, gyroZ, accelX, accelY, accelZ, moveX, moveY, flags; + let gyroX, gyroZ, accelX, accelY, accelZ, moveX, moveY, flags; try { - gyroY = view.getInt16(0, true); - gyroZ = view.getInt16(2, true); + gyroX = view.getInt16(0, true); // GX = pitch axis (nod → cursor Y) + gyroZ = view.getInt16(2, true); // GZ = yaw axis (pan → cursor X) accelX = view.getInt16(4, true); accelY = view.getInt16(6, true); accelZ = view.getInt16(8, true); @@ -608,9 +608,9 @@ function parseImuStream(dv) { const single = !!(flags & 0x02); const dbl = !!(flags & 0x04); - // Axis bars: show raw gyro (firmware convention: Z→screen-X, Y→screen-Y) + // Axis bars: show raw gyro (firmware convention: Z→screen-X, X→screen-Y) updateAxisBar('gy', -gyroZ, 30000); - updateAxisBar('gz', -gyroY, 30000); + updateAxisBar('gz', -gyroX, 30000); if (!idle) { // moveX/moveY are already roll-corrected by firmware — use them directly @@ -626,7 +626,7 @@ function parseImuStream(dv) { if (dbl) flashTap('Right'); drawViz(idle); - orientFeedIMU(accelX, accelY, accelZ, gyroY, gyroZ); + orientFeedIMU(accelX, accelY, accelZ, gyroX, gyroZ); } function updateAxisBar(axis, val, max) { @@ -743,7 +743,7 @@ function orientUpdateColors() { if (orientEdges) orientEdges.material.color.setHex(c); } -function orientFeedIMU(ax, ay, az, gyY_mDPS, gyZ_mDPS) { +function orientFeedIMU(ax, ay, az, gyX_mDPS, gyZ_mDPS) { if (!orientRenderer) return; const now = Date.now(); const dt = orientLastT ? Math.min((now - orientLastT) / 1000, 0.1) : 0.05; @@ -769,9 +769,9 @@ function orientFeedIMU(ax, ay, az, gyY_mDPS, gyZ_mDPS) { qAccel.copy(orientQ); } - // Gyro integration — firmware sends gyroY (pitch) and gyroZ (yaw), mDPS - // Map to Three.js axes: gyroZ→world Y, gyroY→world X - const gyRad = gyY_mDPS * (Math.PI / 180) / 1000; + // Gyro integration — firmware sends gyroX (pitch) and gyroZ (yaw), mDPS + // Map to Three.js axes: gyroZ→world Y, gyroX→world X + const gyRad = gyX_mDPS * (Math.PI / 180) / 1000; const gzRad = gyZ_mDPS * (Math.PI / 180) / 1000; const dq = new THREE.Quaternion( gyRad * dt * 0.5, // x