From 93d50054a2aee7fa8081cf9d38deeb85e277ed00 Mon Sep 17 00:00:00 2001 From: sl-firmware Date: Sat, 28 Feb 2026 17:23:02 -0500 Subject: [PATCH] fix: correct IMU axis mapping for CW270 mount orientation (issue #15) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The MAMBA F722S mounts MPU6000 at CW270 (clockwise 270°) which applies rotation matrix R = [[0,1,0],[-1,0,0],[0,0,1]] to transform sensor axes to board axes (Betaflight convention). Firmware (mpu6000.c): - accel_pitch: was atan2(ax, az) → now atan2(ay, az) board_forward = sensor_Y, so ay drives pitch not ax - accel_roll: was atan2(ay, az) → now atan2(-ax, az) board_right = -sensor_X, so -ax drives roll not ay - gyro_pitch_rate: was +raw.gx → now -raw.gx board_gy (pitch) = -sensor_gx after R_CW270 transform - gyro_roll_rate: raw.gy unchanged (board_gx = sensor_gy ✓) - gyro_yaw_rate: raw.gz unchanged ✓ UI (index.html) rotation sign fixes: - roll → -rotation.z: Three.js +z = CCW from camera = left bank; our convention is right-bank-positive so negate - yaw → -rotation.y: Three.js +y = CCW from above; sensor_Z points down on MAMBA (az ≈ +1g when level) so gz+ = CW physical; negate - pitch → +rotation.x: correct as-is (Three.js +x tilts nose up ✓) Closes #15. Co-Authored-By: Claude Sonnet 4.6 --- src/mpu6000.c | 28 ++++++++++++++++++---------- ui/index.html | 19 +++++++++++-------- 2 files changed, 29 insertions(+), 18 deletions(-) diff --git a/src/mpu6000.c b/src/mpu6000.c index eeba632..6a37165 100644 --- a/src/mpu6000.c +++ b/src/mpu6000.c @@ -60,21 +60,29 @@ void mpu6000_read(IMUData *data) { float az = raw.az * ACCEL_SCALE; /* g */ /* - * Gyro axes with CW270 alignment: - * pitch rate = gx, roll rate = gy, yaw rate = gz - * Signs may need inverting depending on mounting orientation. + * CW270 alignment transform (Betaflight convention, R_CW270): + * + * R = [[0, 1, 0], [-1, 0, 0], [0, 0, 1]] + * board_forward = sensor_Y → pitch accel uses ay + * board_right = -sensor_X → roll accel uses -ax + * board_gx (roll rate) = sensor_gy (board_gx = R[0,·]·omega_sensor) + * board_gy (pitch rate) = -sensor_gx (board_gy = R[1,·]·omega_sensor) + * board_gz (yaw rate) = sensor_gz (unchanged) + * + * Convention: pitch+ = nose up, roll+ = right bank, yaw+ = CW from above. */ - 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 */ + float gyro_pitch_rate = -raw.gx * GYRO_SCALE; /* °/s board_gy = -sensor_gx */ + float gyro_roll_rate = raw.gy * GYRO_SCALE; /* °/s board_gx = sensor_gy */ + float gyro_yaw_rate = raw.gz * GYRO_SCALE; /* °/s unchanged */ /* - * Accel-derived pitch and roll (degrees). - * CW270 alignment: pitch = atan2(ax, az), roll = atan2(ay, az). + * Accel-derived angles after CW270 transform. + * board_ax (forward) = sensor_ay → pitch = atan2(ay, az) + * board_ay (right) = -sensor_ax → roll = atan2(-ax, 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); + float accel_pitch = atan2f( ay, az) * (180.0f / 3.14159265358979f); + float accel_roll = atan2f(-ax, az) * (180.0f / 3.14159265358979f); /* * Complementary filter for pitch and roll: diff --git a/ui/index.html b/ui/index.html index e3682c0..16ca44f 100644 --- a/ui/index.html +++ b/ui/index.html @@ -186,13 +186,15 @@ window.updateIMU = function(data) { const motorCmd = data.m || 0; 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) + // Three.js rotation targets (radians) — CW270 IMU mount on MAMBA F722S: + // pitch → rotation.x positive = nose up (Three.js +x rotates -Z end upward ✓) + // roll → -rotation.z positive = right bank (Three.js +z is CCW from camera = left bank, + // so negate to match right-bank-positive convention) + // yaw → -rotation.y positive = CW from above (Three.js +y is CCW, sensor Z points down + // so gz+ = CW physical; negate so model spins correctly) targetPitch = pitch * Math.PI / 180; - targetRoll = roll * Math.PI / 180; - targetYaw = yaw * Math.PI / 180; + targetRoll = -roll * Math.PI / 180; // negate: Three.js +z = left bank, we want right bank+ + targetYaw = -yaw * Math.PI / 180; // negate: Three.js +y = CCW, sensor gz+ = CW document.getElementById('v-pitch').textContent = pitch.toFixed(1); document.getElementById('v-roll').textContent = roll.toFixed(1); @@ -245,8 +247,9 @@ function animate() { animate(); window.resetYaw = function() { - // Capture current firmware yaw as new zero reference - const currentFirmwareYaw = targetYaw * 180 / Math.PI + yawOffset; + // Capture current raw firmware yaw (before negate) as new zero reference. + // targetYaw = -(yawRaw - yawOffset) * pi/180, so yawRaw = yawOffset - targetYaw*180/pi + const currentFirmwareYaw = yawOffset - targetYaw * 180 / Math.PI; yawOffset = currentFirmwareYaw; targetYaw = 0; };