diff --git a/gui/index.html b/gui/index.html
index 1d55031..263d811 100644
--- a/gui/index.html
+++ b/gui/index.html
@@ -123,6 +123,18 @@
+
+
+
+
+
+
+
+
+ HDG 0°
+ R +0.0°
+ P +0.0°
+
@@ -241,6 +253,8 @@ var motorCommandsListener = null;
var gpsFixListener = null;
var gpsStatusListener = null;
var gpsDiagnosticsListener = null;
+var imuDataListener = null;
+var imuStatusListener = null;
var rosReconnectTimer = null;
var rosDisconnectTimer = null;
var cameraReconnectTimer = null;
@@ -278,6 +292,13 @@ var pageConnection = {
gpsLastAt: 0,
state: "offline"
};
+var imuOverlayState = {
+ status: "Offline",
+ rollDeg: 0,
+ pitchDeg: 0,
+ yawDeg: 0,
+ lastUpdateAt: 0
+};
function setText(id, text) {
var el = document.getElementById(id);
@@ -297,6 +318,74 @@ function formatSpeedCms(speedMps, digits) {
return formatNumber(Number(speedMps || 0) * 100, digits);
}
+function formatSignedNumber(value, digits) {
+ var number = Number(value || 0);
+ var text = number.toLocaleString("de-DE", {
+ minimumFractionDigits: digits,
+ maximumFractionDigits: digits
+ });
+ return (number >= 0 ? "+" : "") + text;
+}
+
+function normalizeHeadingDeg(value) {
+ var heading = Number(value || 0) % 360;
+ if (heading < 0) heading += 360;
+ return heading;
+}
+
+function updateImuOverlaySvg() {
+ var group = document.getElementById("imu-horizon-group");
+ var frame = document.getElementById("imu-horizon-frame");
+ var headingText = document.getElementById("imu-heading-text");
+ var rollText = document.getElementById("imu-roll-text");
+ var pitchText = document.getElementById("imu-pitch-text");
+ if (!group || !frame || !headingText || !rollText || !pitchText) return;
+
+ var ageMs = Date.now() - Number(imuOverlayState.lastUpdateAt || 0);
+ if (!imuOverlayState.lastUpdateAt || ageMs > 3000) {
+ group.setAttribute("opacity", "0");
+ return;
+ }
+
+ var rollDeg = Number(imuOverlayState.rollDeg || 0);
+ var pitchDeg = Number(imuOverlayState.pitchDeg || 0);
+ var headingDeg = normalizeHeadingDeg(imuOverlayState.yawDeg);
+ var pitchOffset = Math.max(-26, Math.min(26, pitchDeg * 1.1));
+
+ group.setAttribute("opacity", "1");
+ frame.setAttribute("transform", "rotate(" + (-rollDeg).toFixed(2) + ") translate(0 " + pitchOffset.toFixed(2) + ")");
+ headingText.textContent = "HDG " + Math.round(headingDeg) + "°";
+ rollText.textContent = "R " + formatSignedNumber(rollDeg, 1) + "°";
+ pitchText.textContent = "P " + formatSignedNumber(pitchDeg, 1) + "°";
+}
+
+function quaternionToEulerDeg(quaternion) {
+ if (!quaternion || quaternion.length < 4) {
+ return { rollDeg: 0, pitchDeg: 0, yawDeg: 0 };
+ }
+ var x = Number(quaternion[0] || 0);
+ var y = Number(quaternion[1] || 0);
+ var z = Number(quaternion[2] || 0);
+ var w = Number(quaternion[3] || 1);
+
+ var sinrCosp = 2 * (w * x + y * z);
+ var cosrCosp = 1 - 2 * (x * x + y * y);
+ var roll = Math.atan2(sinrCosp, cosrCosp);
+
+ var sinp = 2 * (w * y - z * x);
+ var pitch = Math.abs(sinp) >= 1 ? Math.sign(sinp) * (Math.PI / 2) : Math.asin(sinp);
+
+ var sinyCosp = 2 * (w * z + x * y);
+ var cosyCosp = 1 - 2 * (y * y + z * z);
+ var yaw = Math.atan2(sinyCosp, cosyCosp);
+
+ return {
+ rollDeg: roll * 180 / Math.PI,
+ pitchDeg: pitch * 180 / Math.PI,
+ yawDeg: yaw * 180 / Math.PI
+ };
+}
+
function cloneCalibrationPoints(points) {
return (points || []).map(function (item) {
return {
@@ -981,6 +1070,53 @@ function drawOverlayTextBlock(context, lines, x, y, align, lineHeight, color) {
context.restore();
}
+function drawImuHorizonOverlay(context, canvasWidth, canvasHeight, size) {
+ var ageMs = Date.now() - Number(imuOverlayState.lastUpdateAt || 0);
+ if (!imuOverlayState.lastUpdateAt || ageMs > 3000) return;
+
+ var cx = canvasWidth / 2;
+ var cy = canvasHeight / 2;
+ var unit = size / 180;
+ var rollRad = Number(imuOverlayState.rollDeg || 0) * Math.PI / 180;
+ var pitchOffsetPx = Math.max(-26, Math.min(26, Number(imuOverlayState.pitchDeg || 0) * 0.9 * unit));
+ var headingDeg = normalizeHeadingDeg(imuOverlayState.yawDeg);
+
+ context.save();
+ context.translate(cx, cy);
+ context.rotate(-rollRad);
+ context.translate(0, pitchOffsetPx);
+
+ context.strokeStyle = "rgba(0,255,145,.82)";
+ context.lineWidth = 1.25 * unit;
+ context.beginPath();
+ context.moveTo(-28 * unit, 0);
+ context.lineTo(28 * unit, 0);
+ context.stroke();
+
+ context.strokeStyle = "rgba(0,255,145,.36)";
+ context.lineWidth = 0.9 * unit;
+ [-18, -9, 9, 18].forEach(function(stepDeg) {
+ var y = stepDeg * 1.1 * unit;
+ var span = Math.abs(stepDeg) === 18 ? 13 : 9;
+ context.beginPath();
+ context.moveTo(-span * unit, y);
+ context.lineTo(span * unit, y);
+ context.stroke();
+ });
+
+ context.restore();
+
+ context.save();
+ context.font = (8.5 * unit) + 'px "IBM Plex Mono", "Roboto Mono", monospace';
+ context.fillStyle = "rgba(0,255,145,.78)";
+ context.textAlign = "center";
+ context.textBaseline = "middle";
+ context.fillText("HDG " + Math.round(headingDeg) + "°", cx, cy - (48 * unit));
+ context.fillText("R " + formatSignedNumber(imuOverlayState.rollDeg, 1) + "°", cx - (56 * unit), cy + (52 * unit));
+ context.fillText("P " + formatSignedNumber(imuOverlayState.pitchDeg, 1) + "°", cx + (56 * unit), cy + (52 * unit));
+ context.restore();
+}
+
function drawCrosshairOverlayToCanvas(context, canvasWidth, canvasHeight, size) {
var cx = canvasWidth / 2;
var cy = canvasHeight / 2;
@@ -1052,6 +1188,8 @@ function drawCrosshairOverlayToCanvas(context, canvasWidth, canvasHeight, size)
context.fillStyle = "rgba(255,255,255,.7)";
context.fill();
context.restore();
+
+ drawImuHorizonOverlay(context, canvasWidth, canvasHeight, size);
}
function drawVignetteAndScanlines(context, canvasWidth, canvasHeight) {
@@ -1335,10 +1473,12 @@ window.addEventListener("load", function () {
updateModeDisplay();
updateMotorDisplay();
updateInputDisplay();
+ updateImuOverlaySvg();
updateAxesDisplay(0, 0);
updateControlLocks();
updateClocks();
window.setInterval(updateClocks, 1000);
+ window.setInterval(updateImuOverlaySvg, 750);
fetchSystemStatus();
refreshRosSocketState();
setPageConnectionState();
@@ -1486,6 +1626,38 @@ window.addEventListener("load", function () {
updateGpsOverlayFromDiagnostics(message.data);
});
+ imuDataListener = new ROSLIB.Topic({
+ ros: ros,
+ name: "/imu/data",
+ messageType: "sensor_msgs/Imu"
+ });
+ imuDataListener.subscribe(function (message) {
+ clearPendingRosDisconnect();
+ markRosFresh();
+ setRosStatus("Verbunden");
+ var orientation = message.orientation || {};
+ var euler = quaternionToEulerDeg([
+ Number(orientation.x || 0),
+ Number(orientation.y || 0),
+ Number(orientation.z || 0),
+ Number(orientation.w || 1)
+ ]);
+ imuOverlayState.rollDeg = euler.rollDeg;
+ imuOverlayState.pitchDeg = euler.pitchDeg;
+ imuOverlayState.yawDeg = euler.yawDeg;
+ imuOverlayState.lastUpdateAt = Date.now();
+ updateImuOverlaySvg();
+ });
+
+ imuStatusListener = new ROSLIB.Topic({
+ ros: ros,
+ name: "/imu/status",
+ messageType: "std_msgs/String"
+ });
+ imuStatusListener.subscribe(function (message) {
+ imuOverlayState.status = String((message && message.data) || "").trim() || "Verbunden";
+ });
+
var joySubscriber = new ROSLIB.Topic({
ros: ros,
name: "/joy",