sl-firmware
93d50054a2
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>
372 lines
14 KiB
HTML
372 lines
14 KiB
HTML
<!DOCTYPE html>
|
||
<html lang="en">
|
||
<head>
|
||
<meta charset="UTF-8">
|
||
<meta name="viewport" content="width=device-width, initial-scale=1.0">
|
||
<title>SaltyLab Balance Bot</title>
|
||
<style>
|
||
* { margin: 0; padding: 0; box-sizing: border-box; }
|
||
body { background: #0a0a0f; color: #e0e0e0; font-family: 'SF Mono', 'Fira Code', monospace; overflow: hidden; }
|
||
#hud { position: fixed; top: 20px; left: 20px; z-index: 10; }
|
||
#hud h1 { font-size: 18px; color: #ff4444; margin-bottom: 8px; letter-spacing: 2px; }
|
||
.stat { font-size: 13px; line-height: 1.8; color: #888; }
|
||
.stat .val { color: #0ff; font-weight: bold; }
|
||
.stat .label { color: #666; width: 80px; display: inline-block; }
|
||
.btn {
|
||
margin-top: 8px; margin-right: 6px; padding: 10px 20px;
|
||
color: #fff; border: none;
|
||
font-family: inherit; font-size: 13px; cursor: pointer;
|
||
border-radius: 4px; letter-spacing: 1px;
|
||
}
|
||
#connect-btn { background: #ff4444; }
|
||
#connect-btn:hover { background: #ff6666; }
|
||
#connect-btn.connected { background: #00cc66; }
|
||
#arm-btn { background: #cc8800; display: none; }
|
||
#arm-btn:hover { background: #ddaa22; }
|
||
#arm-btn.armed { background: #ff2222; }
|
||
#dfu-btn { background: #555; display: none; }
|
||
#dfu-btn:hover { background: #777; }
|
||
#status { margin-top: 8px; font-size: 12px; color: #666; }
|
||
#state-badge {
|
||
display: inline-block; padding: 2px 8px; border-radius: 3px;
|
||
font-size: 11px; font-weight: bold; margin-left: 8px;
|
||
}
|
||
.state-disarmed { background: #333; color: #888; }
|
||
.state-armed { background: #442200; color: #ffaa00; }
|
||
.state-fault { background: #440000; color: #ff4444; }
|
||
#horizon { position: fixed; bottom: 20px; left: 20px; z-index: 10; }
|
||
.bar-container { margin: 4px 0; }
|
||
.bar-label { font-size: 11px; color: #666; width: 50px; display: inline-block; }
|
||
.bar-bg { display: inline-block; width: 200px; height: 8px; background: #1a1a2e; border-radius: 4px; vertical-align: middle; }
|
||
.bar-fill { height: 100%; border-radius: 4px; transition: width 0.05s; }
|
||
.bar-pitch .bar-fill { background: #ff4444; }
|
||
.bar-motor .bar-fill { background: #44ff44; }
|
||
canvas { display: block; }
|
||
</style>
|
||
</head>
|
||
<body>
|
||
<div id="hud">
|
||
<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">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>
|
||
|
||
<div id="horizon">
|
||
<div class="bar-container bar-pitch">
|
||
<span class="bar-label">PITCH</span>
|
||
<span class="bar-bg"><span class="bar-fill" id="bar-pitch" style="width:50%"></span></span>
|
||
</div>
|
||
<div class="bar-container bar-motor">
|
||
<span class="bar-label">MOTOR</span>
|
||
<span class="bar-bg"><span class="bar-fill" id="bar-motor" style="width:50%"></span></span>
|
||
</div>
|
||
</div>
|
||
|
||
<script type="importmap">
|
||
{ "imports": { "three": "https://cdn.jsdelivr.net/npm/three@0.170.0/build/three.module.js" } }
|
||
</script>
|
||
<script type="module">
|
||
import * as THREE from 'three';
|
||
|
||
// --- Three.js scene ---
|
||
const scene = new THREE.Scene();
|
||
const camera = new THREE.PerspectiveCamera(50, window.innerWidth / window.innerHeight, 0.1, 100);
|
||
camera.position.set(0, 2, 5);
|
||
camera.lookAt(0, 0, 0);
|
||
|
||
const renderer = new THREE.WebGLRenderer({ antialias: true, alpha: true });
|
||
renderer.setSize(window.innerWidth, window.innerHeight);
|
||
renderer.setPixelRatio(window.devicePixelRatio);
|
||
renderer.setClearColor(0x0a0a0f);
|
||
document.body.appendChild(renderer.domElement);
|
||
|
||
// Lighting
|
||
scene.add(new THREE.AmbientLight(0x404040, 2));
|
||
const dirLight = new THREE.DirectionalLight(0xffffff, 3);
|
||
dirLight.position.set(5, 10, 7);
|
||
scene.add(dirLight);
|
||
const pointLight = new THREE.PointLight(0xff4444, 2, 20);
|
||
pointLight.position.set(-3, 3, 3);
|
||
scene.add(pointLight);
|
||
|
||
// Grid
|
||
scene.add(new THREE.GridHelper(10, 20, 0x222244, 0x111122));
|
||
|
||
// FC Board model
|
||
const boardGroup = new THREE.Group();
|
||
|
||
// PCB
|
||
boardGroup.add(new THREE.Mesh(
|
||
new THREE.BoxGeometry(1.6, 0.08, 1.6),
|
||
new THREE.MeshPhongMaterial({ color: 0x1a1a1a, specular: 0x333333 })
|
||
));
|
||
|
||
// Copper traces
|
||
const traceMat = new THREE.MeshPhongMaterial({ color: 0xcc8833, specular: 0xffaa44 });
|
||
for (let i = -0.6; i <= 0.6; i += 0.3) {
|
||
const trace = new THREE.Mesh(new THREE.BoxGeometry(1.4, 0.005, 0.02), traceMat);
|
||
trace.position.set(0, 0.043, i);
|
||
boardGroup.add(trace);
|
||
}
|
||
|
||
// MCU chip
|
||
const mcu = new THREE.Mesh(
|
||
new THREE.BoxGeometry(0.5, 0.06, 0.5),
|
||
new THREE.MeshPhongMaterial({ color: 0x222222, specular: 0x444444 })
|
||
);
|
||
mcu.position.set(0, 0.07, 0);
|
||
boardGroup.add(mcu);
|
||
|
||
// IMU chip
|
||
const imuChip = new THREE.Mesh(
|
||
new THREE.BoxGeometry(0.2, 0.04, 0.2),
|
||
new THREE.MeshPhongMaterial({ color: 0x331111, specular: 0x442222 })
|
||
);
|
||
imuChip.position.set(-0.4, 0.06, -0.3);
|
||
boardGroup.add(imuChip);
|
||
|
||
// USB connector
|
||
const usb = new THREE.Mesh(
|
||
new THREE.BoxGeometry(0.35, 0.1, 0.15),
|
||
new THREE.MeshPhongMaterial({ color: 0x888888, specular: 0xaaaaaa })
|
||
);
|
||
usb.position.set(0, 0.06, -0.85);
|
||
boardGroup.add(usb);
|
||
|
||
// Status LED
|
||
const ledMat = new THREE.MeshBasicMaterial({ color: 0xff0000 });
|
||
const led = new THREE.Mesh(new THREE.SphereGeometry(0.03, 8, 8), ledMat);
|
||
led.position.set(0.5, 0.06, 0.5);
|
||
boardGroup.add(led);
|
||
|
||
// Mounting holes
|
||
const holeMat = new THREE.MeshPhongMaterial({ color: 0x444444 });
|
||
[[-0.55, -0.55], [-0.55, 0.55], [0.55, -0.55], [0.55, 0.55]].forEach(([x, z]) => {
|
||
const ring = new THREE.Mesh(new THREE.TorusGeometry(0.06, 0.015, 8, 16), holeMat);
|
||
ring.rotation.x = Math.PI / 2;
|
||
ring.position.set(x, 0.05, z);
|
||
boardGroup.add(ring);
|
||
});
|
||
|
||
// Forward arrow
|
||
const arrow = new THREE.Mesh(
|
||
new THREE.ConeGeometry(0.08, 0.2, 8),
|
||
new THREE.MeshBasicMaterial({ color: 0xff4444, transparent: true, opacity: 0.8 })
|
||
);
|
||
arrow.rotation.x = -Math.PI / 2;
|
||
arrow.position.set(0, 0.06, -0.65);
|
||
boardGroup.add(arrow);
|
||
|
||
scene.add(boardGroup);
|
||
|
||
// --- State ---
|
||
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) {
|
||
// 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;
|
||
|
||
// 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; // 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);
|
||
document.getElementById('v-yaw').textContent = yaw.toFixed(1);
|
||
document.getElementById('v-motor').textContent = motorCmd;
|
||
|
||
// Pitch bar: center at 50%, ±90°
|
||
document.getElementById('bar-pitch').style.width = ((pitch + 90) / 180 * 100) + '%';
|
||
// Motor bar: center at 50%, ±1000
|
||
document.getElementById('bar-motor').style.width = ((motorCmd + 1000) / 2000 * 100) + '%';
|
||
|
||
// State badge
|
||
const badge = document.getElementById('state-badge');
|
||
badge.textContent = stateNames[state] || 'UNKNOWN';
|
||
badge.className = stateClasses[state] || 'state-disarmed';
|
||
|
||
// Arm button state
|
||
const armBtn = document.getElementById('arm-btn');
|
||
if (state === 1) {
|
||
armBtn.textContent = 'DISARM';
|
||
armBtn.classList.add('armed');
|
||
} else {
|
||
armBtn.textContent = 'ARM';
|
||
armBtn.classList.remove('armed');
|
||
}
|
||
|
||
// LED color based on state
|
||
if (state === 1) {
|
||
ledMat.color.setRGB(0, 1, 0); // green when armed
|
||
} else if (state === 2) {
|
||
const blink = Math.sin(Date.now() / 100) > 0 ? 1 : 0;
|
||
ledMat.color.setRGB(blink, 0, 0); // fast red blink on fault
|
||
} else {
|
||
const intensity = 0.5 + 0.5 * Math.sin(Date.now() / 500);
|
||
ledMat.color.setRGB(intensity, 0, 0); // slow red pulse disarmed
|
||
}
|
||
};
|
||
|
||
// Animation
|
||
function animate() {
|
||
requestAnimationFrame(animate);
|
||
currentPitch += (targetPitch - currentPitch) * 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 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;
|
||
};
|
||
|
||
window.addEventListener('resize', () => {
|
||
camera.aspect = window.innerWidth / window.innerHeight;
|
||
camera.updateProjectionMatrix();
|
||
renderer.setSize(window.innerWidth, window.innerHeight);
|
||
});
|
||
|
||
// --- WebSerial ---
|
||
let port = null, reader = null, writer = null, lineBuffer = '', opening = false;
|
||
let msgCount = 0, lastHzTime = Date.now();
|
||
let currentState = 0;
|
||
|
||
async function sendCmd(cmd) {
|
||
if (!port?.writable) return;
|
||
try {
|
||
const w = port.writable.getWriter();
|
||
await w.write(new TextEncoder().encode(cmd));
|
||
w.releaseLock();
|
||
} catch(e) {
|
||
console.error('Send failed:', e);
|
||
}
|
||
}
|
||
|
||
window.toggleSerial = async function() {
|
||
if (opening) return;
|
||
if (port) {
|
||
try { await sendCmd('S'); } catch(e) {} // Stop streaming before disconnect
|
||
try { reader?.cancel(); } catch(e) {}
|
||
try { await port.close(); } catch(e) {}
|
||
port = null; reader = null;
|
||
document.getElementById('connect-btn').textContent = 'CONNECT USB';
|
||
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;
|
||
}
|
||
try {
|
||
opening = true;
|
||
port = await navigator.serial.requestPort();
|
||
await port.open({ baudRate: 115200 });
|
||
|
||
opening = false;
|
||
document.getElementById('connect-btn').textContent = 'DISCONNECT';
|
||
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();
|
||
} catch (e) {
|
||
opening = false;
|
||
port = null; writer = null;
|
||
document.getElementById('status').textContent = 'Error: ' + e.message;
|
||
}
|
||
};
|
||
|
||
window.toggleArm = async function() {
|
||
if (currentState === 1) {
|
||
await sendCmd('D');
|
||
} else {
|
||
await sendCmd('A');
|
||
}
|
||
};
|
||
|
||
window.enterDFU = async function() {
|
||
if (!confirm('Reboot to DFU mode? You will lose connection.')) return;
|
||
await sendCmd('R');
|
||
document.getElementById('status').textContent = 'Rebooting to DFU...';
|
||
};
|
||
|
||
async function readLoop() {
|
||
const decoder = new TextDecoderStream();
|
||
port.readable.pipeTo(decoder.writable);
|
||
reader = decoder.readable.getReader();
|
||
try {
|
||
while (true) {
|
||
const { value, done } = await reader.read();
|
||
if (done) break;
|
||
lineBuffer += value;
|
||
const lines = lineBuffer.split('\n');
|
||
lineBuffer = lines.pop();
|
||
for (const line of lines) {
|
||
if (!line.trim()) continue;
|
||
try {
|
||
const data = JSON.parse(line);
|
||
if (data.init) {
|
||
document.getElementById('status').textContent =
|
||
`Connected — IMU:${data.imu === 0 ? 'OK' : 'ERR'} WHO:0x${(data.who||0).toString(16)}`;
|
||
continue;
|
||
}
|
||
if (data.err !== undefined) {
|
||
document.getElementById('status').textContent = `IMU error: ${data.err}`;
|
||
continue;
|
||
}
|
||
currentState = data.s || 0;
|
||
window.updateIMU(data);
|
||
msgCount++;
|
||
const now = Date.now();
|
||
if (now - lastHzTime >= 1000) {
|
||
document.getElementById('v-hz').textContent = msgCount;
|
||
msgCount = 0;
|
||
lastHzTime = now;
|
||
}
|
||
} catch(e) {}
|
||
}
|
||
}
|
||
} catch (e) {
|
||
document.getElementById('status').textContent = 'Read error: ' + e.message;
|
||
}
|
||
}
|
||
</script>
|
||
</body>
|
||
</html>
|