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

fix: Roll axis + yaw telemetry (issues #12, #13) #14

Merged
seb merged 1 commits from sl-firmware/fix-orientation-telemetry into main 2026-02-28 16:56:09 -05:00
Conversation 1 Commits 1 Files Changed 4 +63 -54
4 changed files with 63 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 6513b04e4e - Show all commits

6
include/mpu6000.h
View File

@ -5,8 +5,10 @@
#include <stdbool.h>
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;

7
src/main.c
View File

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

35
src/mpu6000.c
View File

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

69
ui/index.html
View File

@ -49,14 +49,14 @@
<h1>⚡ SALTYLAB <span id="state-badge" class="state-disarmed">DISARMED</span></h1>
<div class="stat"><span class="label">PITCH</span> <span class="val" id="v-pitch">--</span>°</div>
<div class="stat"><span class="label">ROLL</span> <span class="val" id="v-roll">--</span>°</div>
<div class="stat"><span class="label">YAW</span> <span class="val" id="v-yaw">--</span>°</div>
<div class="stat"><span class="label">MOTOR</span> <span class="val" id="v-motor">--</span></div>
<div class="stat"><span class="label">ACCEL</span> <span class="val" id="v-accel">--</span></div>
<div class="stat"><span class="label">GYRO</span> <span class="val" id="v-gyro">--</span></div>
<div class="stat"><span class="label">HZ</span> <span class="val" id="v-hz">--</span></div>
<div>
<button class="btn" id="connect-btn" onclick="toggleSerial()">CONNECT USB</button>
<button class="btn" id="arm-btn" onclick="toggleArm()">ARM</button>
<button class="btn" id="dfu-btn" onclick="enterDFU()">DFU</button>
<button class="btn" id="yaw-btn" onclick="resetYaw()" style="background:#335533;display:none">YAW RESET</button>
</div>
<div id="status">WebSerial ready</div>
</div>
@ -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();