From 6513b04e4ea898aa7f9aefd81accb3f713b2bc60 Mon Sep 17 00:00:00 2001 From: sl-firmware Date: Sat, 28 Feb 2026 15:07:04 -0500 Subject: [PATCH] fix: correct roll axis mapping + add yaw telemetry (issues #12, #13) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Issue #12 — Roll displayed as pitch: - Firmware was sending r=pitch_rate (wrong). Changed to r=roll_deg*10. - mpu6000.c: add roll complementary filter (accel atan2(ay,az) + gyro gy integration, same COMP_ALPHA=0.98 as pitch). - IMUData: add roll and yaw fields. - UI: updateIMU() now uses data.r/10 for roll (not client-side filter that computed from ax/ay/az which firmware never sent). - Three.js: roll -> rotation.z (banking), pitch -> rotation.x (tipping) — axes were already correct, fix was the firmware data. Issue #13 — Add yaw telemetry: - Firmware: gyro Z integration (gz * dt) → s_yaw, sent as y=yaw_deg*10. Gyro-only, drifts — no magnetometer. - IMUData: yaw field added. - UI: yaw -> rotation.y (spinning on vertical axis). Displayed in HUD. - UI: YAW RESET button captures current yaw as new zero reference (client-side offset, no new firmware command needed). Closes #12, #13. Co-Authored-By: Claude Sonnet 4.6 --- include/mpu6000.h | 6 +++-- src/main.c | 7 ++--- src/mpu6000.c | 35 ++++++++++++++++-------- ui/index.html | 69 +++++++++++++++++++++-------------------------- 4 files changed, 63 insertions(+), 54 deletions(-) diff --git a/include/mpu6000.h b/include/mpu6000.h index 3f0bd4f..b19ea45 100644 --- a/include/mpu6000.h +++ b/include/mpu6000.h @@ -5,8 +5,10 @@ #include typedef struct { - float pitch; // degrees, filtered - float pitch_rate; // degrees/sec (raw gyro) + float pitch; // degrees, filtered (complementary filter) + float pitch_rate; // degrees/sec (raw gyro pitch axis) + float roll; // degrees, filtered (complementary filter) + float yaw; // degrees, gyro-integrated (drifts — no magnetometer) float accel_x; // g float accel_z; // g } IMUData; diff --git a/src/main.c b/src/main.c index be80029..e18f36f 100644 --- a/src/main.c +++ b/src/main.c @@ -198,13 +198,14 @@ int main(void) { if (imu_ret == 0) { float err = bal.setpoint - bal.pitch_deg; len = snprintf(buf, sizeof(buf), - "{\"p\":%d,\"r\":%d,\"e\":%d,\"ig\":%d,\"m\":%d,\"s\":%d}\n", + "{\"p\":%d,\"r\":%d,\"e\":%d,\"ig\":%d,\"m\":%d,\"s\":%d,\"y\":%d}\n", (int)(bal.pitch_deg * 10), /* pitch degrees x10 */ - (int)(imu.pitch_rate * 10), /* pitch rate °/s x10 */ + (int)(imu.roll * 10), /* roll degrees x10 */ (int)(err * 10), /* PID error x10 */ (int)(bal.integral * 10), /* integral x10 (windup monitor) */ (int)bal.motor_cmd, /* ESC command -1000..+1000 */ - (int)bal.state); + (int)bal.state, + (int)(imu.yaw * 10)); /* yaw degrees x10 (gyro-integrated, drifts) */ } else { len = snprintf(buf, sizeof(buf), "{\"err\":%d}\n", imu_ret); } diff --git a/src/mpu6000.c b/src/mpu6000.c index 9e700d4..eeba632 100644 --- a/src/mpu6000.c +++ b/src/mpu6000.c @@ -27,12 +27,16 @@ /* Filter state */ static float s_pitch = 0.0f; +static float s_roll = 0.0f; +static float s_yaw = 0.0f; static uint32_t s_last_tick = 0; bool mpu6000_init(void) { int ret = icm42688_init(); if (ret == 0) { s_pitch = 0.0f; + s_roll = 0.0f; + s_yaw = 0.0f; s_last_tick = HAL_GetTick(); } return (ret == 0); @@ -52,34 +56,43 @@ void mpu6000_read(IMUData *data) { /* Convert raw to physical units */ float ax = raw.ax * ACCEL_SCALE; /* g */ + float ay = raw.ay * ACCEL_SCALE; /* g */ float az = raw.az * ACCEL_SCALE; /* g */ /* - * Gyro pitch axis with CW270 alignment: pitch rate = gx. - * Sign may need inverting depending on mounting orientation — - * verify during bench test (positive nose-up should be positive). + * Gyro axes with CW270 alignment: + * pitch rate = gx, roll rate = gy, yaw rate = gz + * Signs may need inverting depending on mounting orientation. */ float gyro_pitch_rate = raw.gx * GYRO_SCALE; /* °/s */ + float gyro_roll_rate = raw.gy * GYRO_SCALE; /* °/s */ + float gyro_yaw_rate = raw.gz * GYRO_SCALE; /* °/s */ /* - * Accel pitch angle (degrees). - * CW270 alignment: pitch = atan2(ax, az). - * Valid while ax² + az² ≈ 1g (i.e., low linear acceleration). + * Accel-derived pitch and roll (degrees). + * CW270 alignment: pitch = atan2(ax, az), roll = atan2(ay, az). + * Valid while total accel ≈ 1g (low linear acceleration). */ float accel_pitch = atan2f(ax, az) * (180.0f / 3.14159265358979f); + float accel_roll = atan2f(ay, az) * (180.0f / 3.14159265358979f); /* - * Complementary filter: - * pitch = α * (pitch + ω*dt) + (1−α) * accel_pitch - * - * Gyro integration tracks fast dynamics; accel correction - * prevents long-term drift. + * Complementary filter for pitch and roll: + * angle = α * (angle + ω*dt) + (1−α) * accel_angle + * Gyro integration tracks fast dynamics; accel corrects drift. */ s_pitch = COMP_ALPHA * (s_pitch + gyro_pitch_rate * dt) + (1.0f - COMP_ALPHA) * accel_pitch; + s_roll = COMP_ALPHA * (s_roll + gyro_roll_rate * dt) + + (1.0f - COMP_ALPHA) * accel_roll; + + /* Yaw: pure gyro integration — no accel correction, drifts over time */ + s_yaw += gyro_yaw_rate * dt; data->pitch = s_pitch; data->pitch_rate = gyro_pitch_rate; + data->roll = s_roll; + data->yaw = s_yaw; data->accel_x = ax; data->accel_z = az; } diff --git a/ui/index.html b/ui/index.html index 83630c4..e3682c0 100644 --- a/ui/index.html +++ b/ui/index.html @@ -49,14 +49,14 @@

⚡ SALTYLAB DISARMED

PITCH --°
ROLL --°
+
YAW --°
MOTOR --
-
ACCEL --
-
GYRO --
HZ --
+
WebSerial ready
@@ -170,52 +170,34 @@ boardGroup.add(arrow); scene.add(boardGroup); // --- State --- -let targetPitch = 0, targetRoll = 0; -let currentPitch = 0, currentRoll = 0; - -// Complementary filter (client-side, for roll — firmware sends pitch) -let roll = 0, lastTime = 0; +let targetPitch = 0, targetRoll = 0, targetYaw = 0; +let currentPitch = 0, currentRoll = 0, currentYaw = 0; +let yawOffset = 0; // subtracted from firmware yaw for reset-to-zero const stateNames = ['DISARMED', 'ARMED', 'TILT FAULT']; const stateClasses = ['state-disarmed', 'state-armed', 'state-fault']; window.updateIMU = function(data) { - const ax = data.ax || 0, ay = data.ay || 0, az = data.az || 0; - const gx = data.gx || 0, gy = data.gy || 0, gz = data.gz || 0; - - // Firmware sends pitch as p (x10), motor as m, state as s - const fwPitch = (data.p !== undefined) ? data.p / 10.0 : null; + // Firmware sends: p=pitch×10, r=roll×10, y=yaw×10, m=motor, s=state + const pitch = (data.p !== undefined) ? data.p / 10.0 : 0; + const roll = (data.r !== undefined) ? data.r / 10.0 : 0; + const yawRaw = (data.y !== undefined) ? data.y / 10.0 : 0; + const yaw = yawRaw - yawOffset; const motorCmd = data.m || 0; - const state = data.s || 0; - - const now = performance.now(); - const dt = lastTime ? (now - lastTime) / 1000 : 0.02; - lastTime = now; - - // Use firmware pitch if available, otherwise compute from accel - let pitch; - if (fwPitch !== null) { - pitch = fwPitch; - } else { - pitch = Math.atan2(-ax, Math.sqrt(ay*ay + az*az)) * 180 / Math.PI; - } - - // Roll from client-side complementary filter - const accelRoll = Math.atan2(ay, az) * 180 / Math.PI; - if (Math.abs(roll) < 0.01) { - roll = accelRoll; - } else { - roll = 0.98 * (roll + (gx / 131.0) * dt) + 0.02 * accelRoll; - } + const state = data.s || 0; + // Three.js rotation targets (radians): + // pitch → rotation.x (tipping forward/back around left-right axis) + // roll → rotation.z (banking left/right around forward axis) + // yaw → rotation.y (spinning on vertical axis) targetPitch = pitch * Math.PI / 180; - targetRoll = roll * Math.PI / 180; + targetRoll = roll * Math.PI / 180; + targetYaw = yaw * Math.PI / 180; document.getElementById('v-pitch').textContent = pitch.toFixed(1); - document.getElementById('v-roll').textContent = roll.toFixed(1); + document.getElementById('v-roll').textContent = roll.toFixed(1); + document.getElementById('v-yaw').textContent = yaw.toFixed(1); document.getElementById('v-motor').textContent = motorCmd; - document.getElementById('v-accel').textContent = `${ax},${ay},${az}`; - document.getElementById('v-gyro').textContent = `${gx},${gy},${gz}`; // Pitch bar: center at 50%, ±90° document.getElementById('bar-pitch').style.width = ((pitch + 90) / 180 * 100) + '%'; @@ -253,13 +235,22 @@ window.updateIMU = function(data) { function animate() { requestAnimationFrame(animate); currentPitch += (targetPitch - currentPitch) * 0.15; - currentRoll += (targetRoll - currentRoll) * 0.15; + currentRoll += (targetRoll - currentRoll) * 0.15; + currentYaw += (targetYaw - currentYaw) * 0.15; boardGroup.rotation.x = currentPitch; boardGroup.rotation.z = currentRoll; + boardGroup.rotation.y = currentYaw; renderer.render(scene, camera); } animate(); +window.resetYaw = function() { + // Capture current firmware yaw as new zero reference + const currentFirmwareYaw = targetYaw * 180 / Math.PI + yawOffset; + yawOffset = currentFirmwareYaw; + targetYaw = 0; +}; + window.addEventListener('resize', () => { camera.aspect = window.innerWidth / window.innerHeight; camera.updateProjectionMatrix(); @@ -293,6 +284,7 @@ window.toggleSerial = async function() { document.getElementById('connect-btn').classList.remove('connected'); document.getElementById('arm-btn').style.display = 'none'; document.getElementById('dfu-btn').style.display = 'none'; + document.getElementById('yaw-btn').style.display = 'none'; document.getElementById('status').textContent = 'Disconnected'; return; } @@ -306,6 +298,7 @@ window.toggleSerial = async function() { document.getElementById('connect-btn').classList.add('connected'); document.getElementById('arm-btn').style.display = 'inline-block'; document.getElementById('dfu-btn').style.display = 'inline-block'; + document.getElementById('yaw-btn').style.display = 'inline-block'; document.getElementById('status').textContent = 'Connected — streaming'; readLoop(); -- 2.47.2