IMU Norden kann kalibriert werden

gui/admin.html

@@ -138,6 +138,24 @@
             <span class="sr-key">RPY</span>
             <strong id="imu_roll_pitch_yaw" class="sr-val">-</strong>
           </div>
+          <div class="sr">
+            <span class="sr-led led-blue"></span>
+            <span class="sr-key">Heading roh</span>
+            <strong id="imu_heading_raw" class="sr-val">-</strong>
+          </div>
+          <div class="sr">
+            <span class="sr-led led-ok"></span>
+            <span class="sr-key">Heading korr.</span>
+            <strong id="imu_heading_corrected" class="sr-val">-</strong>
+          </div>
+          <div class="sr">
+            <span class="sr-led led-off"></span>
+            <span class="sr-key">Nord-Offset</span>
+            <strong id="imu_heading_offset" class="sr-val">-</strong>
+          </div>
+          <div class="act-grid">
+            <button class="act-btn subtle full" onclick="setImuNorth()">Aktuelle Richtung als Norden setzen</button>
+          </div>
           <div class="sr">
             <span class="sr-led led-off"></span>
             <span class="sr-key">Hinweis</span>
@@ -942,6 +960,9 @@
     magnetic: '--',
     quaternion: '--',
     rollPitchYaw: '--',
+    headingRawDeg: null,
+    headingCorrectedDeg: null,
+    headingOffsetDeg: 0,
     error: '--',
     lastDataAt: 0,
     lastMagAt: 0,
@@ -1049,6 +1070,8 @@
     imuRosState.magnetic = '--';
     imuRosState.quaternion = '--';
     imuRosState.rollPitchYaw = '--';
+    imuRosState.headingRawDeg = null;
+    imuRosState.headingCorrectedDeg = null;
     imuRosState.error = 'Keine ROS-Verbindung';
     imuRosState.lastDataAt = 0;
     imuRosState.lastMagAt = 0;
@@ -1279,6 +1302,23 @@
       'Yaw ' + Number(values[2]).toFixed(1) + '°';
   }
 
+  function normalizeHeadingDeg(value) {
+    var heading = Number(value || 0) % 360;
+    if (heading < 0) heading += 360;
+    return heading;
+  }
+
+  function formatHeadingDeg(value) {
+    if (typeof value !== 'number' || isNaN(value)) return '--';
+    return normalizeHeadingDeg(value).toFixed(1).replace('.', ',') + '°';
+  }
+
+  function formatHeadingOffsetDeg(value) {
+    if (typeof value !== 'number' || isNaN(value)) return '--';
+    var prefix = value >= 0 ? '+' : '';
+    return prefix + Number(value).toFixed(1).replace('.', ',') + '°';
+  }
+
   function renderImuRosState() {
     var now = Date.now();
     var dataFresh = imuRosState.lastDataAt && (now - imuRosState.lastDataAt) < 3000;
@@ -1307,6 +1347,9 @@
     setText('imu_magnetic', magFresh ? imuRosState.magnetic : '--');
     setText('imu_quaternion', dataFresh ? imuRosState.quaternion : '--');
     setText('imu_roll_pitch_yaw', dataFresh ? imuRosState.rollPitchYaw : '--');
+    setText('imu_heading_raw', dataFresh ? formatHeadingDeg(imuRosState.headingRawDeg) : '--');
+    setText('imu_heading_corrected', dataFresh ? formatHeadingDeg(imuRosState.headingCorrectedDeg) : '--');
+    setText('imu_heading_offset', formatHeadingOffsetDeg(imuRosState.headingOffsetDeg));
     setText('imu_error', error || '-');
   }
 
@@ -2614,7 +2657,12 @@
         Number(angularVelocity.z || 0)
       ], 'rad/s', 3);
       imuRosState.quaternion = formatImuQuaternion(quaternion);
-      imuRosState.rollPitchYaw = formatRollPitchYaw(quaternionToEulerDeg(quaternion));
+      var eulerDeg = quaternionToEulerDeg(quaternion);
+      imuRosState.rollPitchYaw = formatRollPitchYaw(eulerDeg);
+      imuRosState.headingRawDeg = Array.isArray(eulerDeg) ? normalizeHeadingDeg(eulerDeg[2]) : null;
+      imuRosState.headingCorrectedDeg = Array.isArray(eulerDeg)
+        ? normalizeHeadingDeg(eulerDeg[2] + Number(imuRosState.headingOffsetDeg || 0))
+        : null;
       imuRosState.lastDataAt = Date.now();
       if (!imuRosState.error || imuRosState.error === 'Keine ROS-Verbindung' || imuRosState.error === 'Noch keine frischen IMU-Daten') {
         imuRosState.error = '';
@@ -2660,6 +2708,10 @@
   function renderStatus(data) {
     var system = data.system || {};
     var services = data.services || {};
+    var imuHeading = data.imu_heading || {};
+    if (typeof imuHeading.offset_deg === 'number') imuRosState.headingOffsetDeg = imuHeading.offset_deg;
+    if (typeof imuHeading.raw_heading_deg === 'number') imuRosState.headingRawDeg = imuHeading.raw_heading_deg;
+    if (typeof imuHeading.corrected_heading_deg === 'number') imuRosState.headingCorrectedDeg = imuHeading.corrected_heading_deg;
     setAdminStatus(data.status || 'Bereit');
     setText('info_ips', system.ips);
     setText('info_wifi', system.wifi);
@@ -2695,6 +2747,7 @@
     setServiceState('camera_service_state', services.camera);
     setServiceState('service_admin_api', services.admin_api);
     setServiceState('service_exomy', services.exomy);
+    renderImuRosState();
   }
 
   async function fetchStatus() {
@@ -2733,6 +2786,26 @@
     }
   }
 
+  async function setImuNorth() {
+    if (!window.confirm('Rover jetzt wirklich sauber nach Norden ausgerichtet?')) return;
+    setAdminStatus('Speichere Nordrichtung...');
+    try {
+      var response = await fetch(adminApiBase + '/api/imu/set-north', { method: 'POST' });
+      var data = await response.json();
+      if (!response.ok) throw new Error(data.status || ('status ' + response.status));
+      var imuHeading = data.imu_heading || {};
+      if (typeof imuHeading.offset_deg === 'number') imuRosState.headingOffsetDeg = imuHeading.offset_deg;
+      if (typeof imuHeading.raw_heading_deg === 'number') imuRosState.headingRawDeg = imuHeading.raw_heading_deg;
+      if (typeof imuHeading.corrected_heading_deg === 'number') imuRosState.headingCorrectedDeg = imuHeading.corrected_heading_deg;
+      renderImuRosState();
+      setAdminStatus(data.status || 'Nordrichtung gespeichert');
+      window.setTimeout(fetchStatus, 500);
+    } catch (error) {
+      setAdminStatus('Nord-Kalibrierung fehlgeschlagen');
+      alert(error.message || 'Nordrichtung konnte nicht gespeichert werden.');
+    }
+  }
+
   async function fetchDelay() {
     try {
       var response = await fetch(adminApiBase + '/api/delay');

gui/index.html

@@ -323,6 +323,7 @@ var imuOverlayState = {
   rollDeg: 0,
   pitchDeg: 0,
   yawDeg: 0,
+  yawOffsetDeg: 0,
   lastUpdateAt: 0
 };
 
@@ -364,6 +365,10 @@ function normalizeHeadingDeg(value) {
   return heading;
 }
 
+function applyImuHeadingOffset(yawDeg) {
+  return normalizeHeadingDeg(Number(yawDeg || 0) + Number(imuOverlayState.yawOffsetDeg || 0));
+}
+
 function updateImuOverlaySvg() {
   var group = document.getElementById("imu-horizon-group");
   var frame = document.getElementById("imu-horizon-frame");
@@ -381,7 +386,7 @@ function updateImuOverlaySvg() {
 
   var rollDeg = Number(imuOverlayState.rollDeg || 0);
   var pitchDeg = Number(imuOverlayState.pitchDeg || 0);
-  var headingDeg = normalizeHeadingDeg(imuOverlayState.yawDeg);
+  var headingDeg = applyImuHeadingOffset(imuOverlayState.yawDeg);
   var pitchOffset = Math.max(-55, Math.min(55, pitchDeg * 2.2));
 
   group.setAttribute("opacity", "1");
@@ -986,8 +991,13 @@ function updateUndervoltageDisplay(text, state) {
 function updateServiceState(payload) {
   var system = payload.system || {};
   var services = payload.services || {};
+  var imuHeading = payload.imu_heading || {};
   var cameraServiceState = String(services.camera || "");
   var cameraSettingsVersion = Number(services.camera_settings_version || 0);
+  if (typeof imuHeading.offset_deg === "number" && !isNaN(imuHeading.offset_deg)) {
+    imuOverlayState.yawOffsetDeg = Number(imuHeading.offset_deg);
+    updateImuOverlaySvg();
+  }
   updateUndervoltageDisplay(system.undervoltage || "unbekannt", system.undervoltage_state || "unknown");
   if (cameraServiceState === "active" && lastCameraServiceState && lastCameraServiceState !== "active") {
     setText("cam-state", "Verbinde...");
@@ -1114,7 +1124,7 @@ function drawImuHorizonOverlay(context, canvasWidth, canvasHeight, size) {
   var unit = size / 220;
   var rollRad = Number(imuOverlayState.rollDeg || 0) * Math.PI / 180;
   var pitchOffsetPx = Math.max(-55, Math.min(55, Number(imuOverlayState.pitchDeg || 0) * 2.2)) * unit;
-  var headingDeg = normalizeHeadingDeg(imuOverlayState.yawDeg);
+  var headingDeg = applyImuHeadingOffset(imuOverlayState.yawDeg);
   var rollDeg = Number(imuOverlayState.rollDeg || 0);
   var pitchDeg = Number(imuOverlayState.pitchDeg || 0);
 

scripts/exomy_admin_api.py

@@ -29,6 +29,9 @@ ROUTE_LIBRARY_FILE = os.path.join(
     os.path.dirname(__file__), '..', 'config', 'route_library.json'
 )
 CAMERA_RUNTIME_SETTINGS_FILE = '/tmp/exomy_camera_settings.json'
+IMU_HEADING_CALIBRATION_FILE = os.path.join(
+    os.path.dirname(__file__), '..', 'config', 'imu_heading_calibration.json'
+)
 MOTOR_TEST_LOCK = threading.Lock()
 GPS_PORT_PATTERNS = [
     '/dev/serial/by-id/*',
@@ -89,6 +92,7 @@ IMU_ROS_CACHE = {
     'quaternion': 'unbekannt',
     'roll_pitch_yaw': 'unbekannt',
     'error': 'Noch keine IMU-Daten aus ROS',
+    'heading_deg': None,
     'updated_at': 0.0,
 }
 
@@ -153,6 +157,13 @@ def _quaternion_to_euler_deg(values):
     )
 
 
+def _normalize_heading_deg(value):
+    heading = float(value) % 360.0
+    if heading < 0.0:
+        heading += 360.0
+    return heading
+
+
 def _imu_ros_cache_update(**kwargs):
     with IMU_ROS_LOCK:
         IMU_ROS_CACHE.update(kwargs)
@@ -169,6 +180,7 @@ def _on_imu_data_message(message):
         float(orientation.get('z', 0.0)),
         float(orientation.get('w', 1.0)),
     )
+    euler_deg = _quaternion_to_euler_deg(quaternion)
     _imu_ros_cache_update(
         state='active',
         acceleration=_format_vector((
@@ -182,7 +194,8 @@ def _on_imu_data_message(message):
             float(angular_velocity.get('z', 0.0)),
         ), 'rad/s', 3),
         quaternion=_format_quaternion(quaternion),
-        roll_pitch_yaw=_format_euler_deg(_quaternion_to_euler_deg(quaternion)),
+        roll_pitch_yaw=_format_euler_deg(euler_deg),
+        heading_deg=None if not euler_deg else _normalize_heading_deg(euler_deg[2]),
         error='',
     )
 
@@ -842,6 +855,7 @@ def collect_status():
     time_details = get_system_time_details()
     container_details = get_container_details(EXOMY_CONTAINER)
     imu_details = read_imu_ros_status()
+    imu_heading = get_imu_heading_status()
     return {
         'status': 'Bereit',
         'system': {
@@ -884,6 +898,7 @@ def collect_status():
             'exomy_restarts': container_details['restarts'],
         },
         'imu': imu_details,
+        'imu_heading': imu_heading,
     }
 
 
@@ -1021,6 +1036,52 @@ def read_camera_settings_version():
         return 0
 
 
+def get_default_imu_heading_calibration():
+    return {
+        'offset_deg': 0.0,
+        'updated_at': None,
+        'source': 'default',
+    }
+
+
+def read_imu_heading_calibration():
+    data = get_default_imu_heading_calibration()
+    try:
+        with open(IMU_HEADING_CALIBRATION_FILE, 'r', encoding='utf-8') as handle:
+            payload = json.load(handle)
+    except (FileNotFoundError, json.JSONDecodeError, OSError, ValueError, TypeError):
+        return data
+
+    try:
+        offset_deg = float(payload.get('offset_deg', 0.0))
+    except (TypeError, ValueError):
+        return data
+
+    data['offset_deg'] = offset_deg
+    data['updated_at'] = payload.get('updated_at')
+    data['source'] = 'saved'
+    return data
+
+
+def write_imu_heading_calibration(offset_deg):
+    payload = {
+        'offset_deg': float(offset_deg),
+        'updated_at': time.strftime('%Y-%m-%d %H:%M:%S'),
+    }
+    os.makedirs(os.path.dirname(IMU_HEADING_CALIBRATION_FILE), exist_ok=True)
+    with open(IMU_HEADING_CALIBRATION_FILE, 'w', encoding='utf-8') as handle:
+        json.dump(payload, handle, ensure_ascii=False, indent=2)
+        handle.write('\n')
+    payload['source'] = 'saved'
+    return payload
+
+
+def get_corrected_heading_deg(raw_heading_deg, offset_deg):
+    if raw_heading_deg is None:
+        return None
+    return _normalize_heading_deg(float(raw_heading_deg) + float(offset_deg))
+
+
 def get_camera_config():
     profile = read_camera_settings()
     return {
@@ -1036,6 +1097,51 @@ def get_camera_config():
     }
 
 
+def get_imu_heading_status():
+    calibration = read_imu_heading_calibration()
+    with IMU_ROS_LOCK:
+        raw_heading_deg = IMU_ROS_CACHE.get('heading_deg')
+
+    corrected_heading_deg = get_corrected_heading_deg(raw_heading_deg, calibration['offset_deg'])
+    return {
+        'offset_deg': calibration['offset_deg'],
+        'raw_heading_deg': raw_heading_deg,
+        'corrected_heading_deg': corrected_heading_deg,
+        'updated_at': calibration['updated_at'],
+        'source': calibration['source'],
+    }
+
+
+def set_imu_heading_offset(offset_deg):
+    payload = write_imu_heading_calibration(offset_deg)
+    corrected_heading_deg = None
+    with IMU_ROS_LOCK:
+        raw_heading_deg = IMU_ROS_CACHE.get('heading_deg')
+        if raw_heading_deg is not None:
+            corrected_heading_deg = get_corrected_heading_deg(raw_heading_deg, payload['offset_deg'])
+    return {
+        'offset_deg': payload['offset_deg'],
+        'raw_heading_deg': raw_heading_deg,
+        'corrected_heading_deg': corrected_heading_deg,
+        'updated_at': payload['updated_at'],
+        'source': payload['source'],
+    }
+
+
+def calibrate_imu_heading_to_north():
+    with IMU_ROS_LOCK:
+        raw_heading_deg = IMU_ROS_CACHE.get('heading_deg')
+
+    if raw_heading_deg is None:
+        return 409, {'status': 'Noch keine frischen IMU-Daten für Nord-Kalibrierung'}
+
+    calibration = set_imu_heading_offset(-float(raw_heading_deg))
+    return 200, {
+        'status': 'Nordrichtung gespeichert',
+        'imu_heading': calibration,
+    }
+
+
 def set_camera_config(profile_id=None, fps=None):
     current = read_camera_settings()
     if profile_id is None:
@@ -1805,7 +1911,7 @@ class Handler(http.server.BaseHTTPRequestHandler):
 
     def do_POST(self):
         raw_body = b'{}'
-        if self.path in ('/api/routes/save', '/api/routes/delete'):
+        if self.path in ('/api/routes/save', '/api/routes/delete', '/api/imu/heading-offset'):
             try:
                 content_length = int(self.headers.get('Content-Length', '0'))
             except ValueError:
@@ -1918,6 +2024,25 @@ class Handler(http.server.BaseHTTPRequestHandler):
             self.write_json(status_code, response)
             return
 
+        if self.path == '/api/imu/heading-offset':
+            try:
+                payload = json.loads(raw_body.decode('utf-8'))
+            except (UnicodeDecodeError, json.JSONDecodeError):
+                self.write_json(400, {'status': 'Ungültige Anfrage'})
+                return
+
+            try:
+                offset_deg = float(payload.get('offset_deg', 0.0))
+            except (TypeError, ValueError):
+                self.write_json(400, {'status': 'Ungültiger Heading-Offset'})
+                return
+
+            self.write_json(200, {
+                'status': 'Heading-Offset gespeichert',
+                'imu_heading': set_imu_heading_offset(offset_deg),
+            })
+            return
+
         if self.path == '/api/motor-test/drive-neutral/preview':
             try:
                 content_length = int(self.headers.get('Content-Length', '0'))
@@ -2067,6 +2192,11 @@ class Handler(http.server.BaseHTTPRequestHandler):
                 self.write_json(500, {'status': str(exc)})
             return
 
+        if self.path == '/api/imu/set-north':
+            status_code, response = calibrate_imu_heading_to_north()
+            self.write_json(status_code, response)
+            return
+
         actions = {
             '/api/cold-start-gps': {
                 'handler': cold_start_gps_receiver,