seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 5 Actions Packages Projects Releases Wiki Activity

fix: correct IMU axis mapping for CW270 mount orientation (issue #15)

Browse Source 
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 <noreply@anthropic.com>
This commit is contained in:
sl-firmware 2026-02-28 17:23:02 -05:00
parent fb3c8c863a
commit 93d50054a2
2 changed files with 29 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
src/mpu6000.c
View File

@ -60,21 +60,29 @@ void mpu6000_read(IMUData *data) {
float az = raw.az * ACCEL_SCALE; /* g */ float az = raw.az * ACCEL_SCALE; /* g */
/* /*
* Gyro axes with CW270 alignment: * CW270 alignment transform (Betaflight convention, R_CW270):
* pitch rate = gx, roll rate = gy, yaw rate = gz *
* Signs may need inverting depending on mounting orientation. * 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_pitch_rate = -raw.gx * GYRO_SCALE; /* °/s board_gy = -sensor_gx */
float gyro_roll_rate = raw.gy * GYRO_SCALE; /* °/s */ float gyro_roll_rate = raw.gy * GYRO_SCALE; /* °/s board_gx = sensor_gy */
float gyro_yaw_rate = raw.gz * GYRO_SCALE; /* °/s */ float gyro_yaw_rate = raw.gz * GYRO_SCALE; /* °/s unchanged */
/* /*
* Accel-derived pitch and roll (degrees). * Accel-derived angles after CW270 transform.
* CW270 alignment: pitch = atan2(ax, az), roll = atan2(ay, az). * 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). * Valid while total accel ≈ 1g (low linear acceleration).
*/ */
float accel_pitch = atan2f(ax, az) * (180.0f / 3.14159265358979f); float accel_pitch = atan2f( ay, az) * (180.0f / 3.14159265358979f);
float accel_roll = atan2f(ay, az) * (180.0f / 3.14159265358979f); float accel_roll = atan2f(-ax, az) * (180.0f / 3.14159265358979f);
/* /*
* Complementary filter for pitch and roll: * Complementary filter for pitch and roll:

19
ui/index.html
View File

@ -186,13 +186,15 @@ window.updateIMU = function(data) {
const motorCmd = data.m || 0; const motorCmd = data.m || 0;
const state = data.s || 0; const state = data.s || 0;
// Three.js rotation targets (radians): // Three.js rotation targets (radians) — CW270 IMU mount on MAMBA F722S:
// pitch → rotation.x (tipping forward/back around left-right axis) // pitch → rotation.x positive = nose up (Three.js +x rotates -Z end upward ✓)
// roll → rotation.z (banking left/right around forward axis) // roll → -rotation.z positive = right bank (Three.js +z is CCW from camera = left bank,
// yaw → rotation.y (spinning on vertical axis) // 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; targetPitch = pitch * Math.PI / 180;
targetRoll = roll * Math.PI / 180; targetRoll = -roll * Math.PI / 180; // negate: Three.js +z = left bank, we want right bank+
targetYaw = yaw * Math.PI / 180; targetYaw = -yaw * Math.PI / 180; // negate: Three.js +y = CCW, sensor gz+ = CW
document.getElementById('v-pitch').textContent = pitch.toFixed(1); document.getElementById('v-pitch').textContent = pitch.toFixed(1);
document.getElementById('v-roll').textContent = roll.toFixed(1); document.getElementById('v-roll').textContent = roll.toFixed(1);
@ -245,8 +247,9 @@ function animate() {
animate(); animate();
window.resetYaw = function() { window.resetYaw = function() {
// Capture current firmware yaw as new zero reference // Capture current raw firmware yaw (before negate) as new zero reference.
const currentFirmwareYaw = targetYaw * 180 / Math.PI + yawOffset; // targetYaw = -(yawRaw - yawOffset) * pi/180, so yawRaw = yawOffset - targetYaw*180/pi
const currentFirmwareYaw = yawOffset - targetYaw * 180 / Math.PI;
yawOffset = currentFirmwareYaw; yawOffset = currentFirmwareYaw;
targetYaw = 0; targetYaw = 0;
}; };