nikrozman/air-mouse
1
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects 2 Releases Activity

Attempt to fix roll correction

Browse Source
This commit is contained in:
Nik Rozman
2026-03-01 22:12:23 +01:00
parent 9e481096be
commit b3cc319d5e
5 changed files with 25 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
source/config.h
View File

@@ -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]

21
source/imu.cpp
View File

@@ -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<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);
@@ -41,22 +36,6 @@ 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);

5
source/imu.h
View File

@@ -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();

33
source/main.cpp
View File

@@ -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
}