IMU visualization

source/imu.cpp

@@ -1,8 +1,12 @@
 #include "imu.h"
 #include "Wire.h"
+#include <math.h>
 
 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)
@@ -25,8 +29,10 @@ 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<BIAS_SAMPLES; i++) {
     sx += imu.readFloatGyroX(); sy += imu.readFloatGyroY(); sz += imu.readFloatGyroZ();
+    sax += imu.readFloatAccelX(); say += imu.readFloatAccelY();
     digitalWrite(LED_GREEN, (i%20 < 10)); delay(5);  // green flutter during calibration
   }
   biasGX = (float)(sx/BIAS_SAMPLES);
@@ -35,6 +41,22 @@ void calibrateGyroBias() {
   calTempC = readIMUTemp();
   angleX = angleY = accumX = accumY = 0.0f;
 
+  // Roll compensation: compute device yaw on the table from gravity's horizontal components.
+  // ax/ay are small when flat; their ratio gives the rotation angle θ.
+  // Firmware maps: screenX ← -gz, screenY ← -gy.
+  // With device rotated θ CW: screenX ← -(gz·cosθ + gy·sinθ), screenY ← -(gy·cosθ - gz·sinθ).
+  float ax_avg = (float)(sax / BIAS_SAMPLES);
+  float ay_avg = (float)(say / BIAS_SAMPLES);
+  float norm = sqrtf(ax_avg*ax_avg + ay_avg*ay_avg);
+  if (norm > 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);