IMU jetzt im Fadenkreuz

2026-05-28 15:38:39 +02:00
parent cc44e05e57
commit e6ab4d7716
1 changed files with 172 additions and 0 deletions
@@ -123,6 +123,18 @@
            <path d="M58,74   L74,74   L74,58"   fill="none" stroke="rgba(0,255,145,.75)" stroke-width="1.5"/>
            <!-- pulsierender Ring -->
            <circle class="xh-pulse" cx="0" cy="0" r="6" fill="none" stroke="rgba(0,255,145,.7)" stroke-width="1"/>
+            <g id="imu-horizon-group" opacity="0">
+              <g id="imu-horizon-frame">
+                <line id="imu-horizon-main" x1="-28" y1="0" x2="28" y2="0" stroke="rgba(0,255,145,.82)" stroke-width="1.25"/>
+                <line x1="-9" y1="-9.9" x2="9" y2="-9.9" stroke="rgba(0,255,145,.36)" stroke-width=".9"/>
+                <line x1="-13" y1="-19.8" x2="13" y2="-19.8" stroke="rgba(0,255,145,.36)" stroke-width=".9"/>
+                <line x1="-9" y1="9.9" x2="9" y2="9.9" stroke="rgba(0,255,145,.36)" stroke-width=".9"/>
+                <line x1="-13" y1="19.8" x2="13" y2="19.8" stroke="rgba(0,255,145,.36)" stroke-width=".9"/>
+              </g>
+              <text id="imu-heading-text" x="0" y="-48" fill="rgba(0,255,145,.78)" font-size="8.5" text-anchor="middle">HDG 0°</text>
+              <text id="imu-roll-text" x="-56" y="52" fill="rgba(0,255,145,.78)" font-size="8.5" text-anchor="middle">R +0.0°</text>
+              <text id="imu-pitch-text" x="56" y="52" fill="rgba(0,255,145,.78)" font-size="8.5" text-anchor="middle">P +0.0°</text>
+            </g>
            <!-- Mittelpunkt -->
            <circle cx="0" cy="0" r="2.5" fill="rgba(0,255,145,.9)"/>
            <circle cx="0" cy="0" r="1"   fill="rgba(255,255,255,.7)"/>
@@ -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",