From e6ab4d7716f617bffd9f0fadb6fdf82481eec90c Mon Sep 17 00:00:00 2001 From: Eskimue Date: Thu, 28 May 2026 15:38:39 +0200 Subject: [PATCH] IMU jetzt im Fadenkreuz --- gui/index.html | 172 +++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 172 insertions(+) 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",