IMU kann genullt werden

gui/admin.html

@@ -153,8 +153,24 @@
             <span class="sr-key">Nord-Offset</span>
             <strong id="imu_heading_offset" class="sr-val">-</strong>
           </div>
+          <div class="sr">
+            <span class="sr-led led-ok"></span>
+            <span class="sr-key">Roll korr.</span>
+            <strong id="imu_roll_corrected" class="sr-val">-</strong>
+          </div>
+          <div class="sr">
+            <span class="sr-led led-ok"></span>
+            <span class="sr-key">Pitch korr.</span>
+            <strong id="imu_pitch_corrected" class="sr-val">-</strong>
+          </div>
+          <div class="sr">
+            <span class="sr-led led-off"></span>
+            <span class="sr-key">Neig.-Offset</span>
+            <strong id="imu_tilt_offset" class="sr-val">-</strong>
+          </div>
           <div class="act-grid">
             <button class="act-btn subtle full" onclick="setImuNorth()">Aktuelle Richtung als Norden setzen</button>
+            <button class="act-btn subtle full" onclick="setImuLevel()">Aktuelle Neigung als Null setzen</button>
           </div>
           <div class="sr">
             <span class="sr-led led-off"></span>
@@ -963,6 +979,10 @@
     headingRawDeg: null,
     headingCorrectedDeg: null,
     headingOffsetDeg: 0,
+    rollRawDeg: null,
+    pitchRawDeg: null,
+    rollOffsetDeg: 0,
+    pitchOffsetDeg: 0,
     error: '--',
     lastDataAt: 0,
     lastMagAt: 0,
@@ -1072,6 +1092,8 @@
     imuRosState.rollPitchYaw = '--';
     imuRosState.headingRawDeg = null;
     imuRosState.headingCorrectedDeg = null;
+    imuRosState.rollRawDeg = null;
+    imuRosState.pitchRawDeg = null;
     imuRosState.error = 'Keine ROS-Verbindung';
     imuRosState.lastDataAt = 0;
     imuRosState.lastMagAt = 0;
@@ -1350,6 +1372,11 @@
     setText('imu_heading_raw', dataFresh ? formatHeadingDeg(imuRosState.headingRawDeg) : '--');
     setText('imu_heading_corrected', dataFresh ? formatHeadingDeg(imuRosState.headingCorrectedDeg) : '--');
     setText('imu_heading_offset', formatHeadingOffsetDeg(imuRosState.headingOffsetDeg));
+    var corrRoll = dataFresh && imuRosState.rollRawDeg !== null ? imuRosState.rollRawDeg + imuRosState.rollOffsetDeg : null;
+    var corrPitch = dataFresh && imuRosState.pitchRawDeg !== null ? imuRosState.pitchRawDeg + imuRosState.pitchOffsetDeg : null;
+    setText('imu_roll_corrected', corrRoll !== null ? corrRoll.toFixed(1) + '°' : '--');
+    setText('imu_pitch_corrected', corrPitch !== null ? corrPitch.toFixed(1) + '°' : '--');
+    setText('imu_tilt_offset', 'R ' + imuRosState.rollOffsetDeg.toFixed(1) + '° | P ' + imuRosState.pitchOffsetDeg.toFixed(1) + '°');
     setText('imu_error', error || '-');
   }
 
@@ -2663,6 +2690,8 @@
       imuRosState.headingCorrectedDeg = Array.isArray(eulerDeg)
         ? normalizeHeadingDeg(eulerDeg[2] + Number(imuRosState.headingOffsetDeg || 0))
         : null;
+      imuRosState.rollRawDeg = Array.isArray(eulerDeg) ? eulerDeg[0] : null;
+      imuRosState.pitchRawDeg = Array.isArray(eulerDeg) ? eulerDeg[1] : null;
       imuRosState.lastDataAt = Date.now();
       if (!imuRosState.error || imuRosState.error === 'Keine ROS-Verbindung' || imuRosState.error === 'Noch keine frischen IMU-Daten') {
         imuRosState.error = '';
@@ -2712,6 +2741,11 @@
     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;
+    var imuTilt = data.imu_tilt || {};
+    if (typeof imuTilt.roll_offset_deg === 'number') imuRosState.rollOffsetDeg = imuTilt.roll_offset_deg;
+    if (typeof imuTilt.pitch_offset_deg === 'number') imuRosState.pitchOffsetDeg = imuTilt.pitch_offset_deg;
+    if (typeof imuTilt.raw_roll_deg === 'number') imuRosState.rollRawDeg = imuTilt.raw_roll_deg;
+    if (typeof imuTilt.raw_pitch_deg === 'number') imuRosState.pitchRawDeg = imuTilt.raw_pitch_deg;
     setAdminStatus(data.status || 'Bereit');
     setText('info_ips', system.ips);
     setText('info_wifi', system.wifi);
@@ -2806,6 +2840,27 @@
     }
   }
 
+  async function setImuLevel() {
+    if (!window.confirm('Rover jetzt wirklich waagerecht aufgestellt?')) return;
+    setAdminStatus('Speichere Neigungsoffset...');
+    try {
+      var response = await fetch(adminApiBase + '/api/imu/set-level', { method: 'POST' });
+      var data = await response.json();
+      if (!response.ok) throw new Error(data.status || ('status ' + response.status));
+      var imuTilt = data.imu_tilt || {};
+      if (typeof imuTilt.roll_offset_deg === 'number') imuRosState.rollOffsetDeg = imuTilt.roll_offset_deg;
+      if (typeof imuTilt.pitch_offset_deg === 'number') imuRosState.pitchOffsetDeg = imuTilt.pitch_offset_deg;
+      if (typeof imuTilt.raw_roll_deg === 'number') imuRosState.rollRawDeg = imuTilt.raw_roll_deg;
+      if (typeof imuTilt.raw_pitch_deg === 'number') imuRosState.pitchRawDeg = imuTilt.raw_pitch_deg;
+      renderImuRosState();
+      setAdminStatus(data.status || 'Neigung genullt');
+      window.setTimeout(fetchStatus, 500);
+    } catch (error) {
+      setAdminStatus('Neigungskalibrierung fehlgeschlagen');
+      alert(error.message || 'Neigung konnte nicht genullt werden.');
+    }
+  }
+
   async function fetchDelay() {
     try {
       var response = await fetch(adminApiBase + '/api/delay');

gui/index.html

@@ -324,6 +324,8 @@ var imuOverlayState = {
   pitchDeg: 0,
   yawDeg: 0,
   yawOffsetDeg: 0,
+  rollOffsetDeg: 0,
+  pitchOffsetDeg: 0,
   lastUpdateAt: 0
 };
 
@@ -992,12 +994,19 @@ function updateServiceState(payload) {
   var system = payload.system || {};
   var services = payload.services || {};
   var imuHeading = payload.imu_heading || {};
+  var imuTilt = payload.imu_tilt || {};
   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();
   }
+  if (typeof imuTilt.roll_offset_deg === "number" && !isNaN(imuTilt.roll_offset_deg)) {
+    imuOverlayState.rollOffsetDeg = Number(imuTilt.roll_offset_deg);
+  }
+  if (typeof imuTilt.pitch_offset_deg === "number" && !isNaN(imuTilt.pitch_offset_deg)) {
+    imuOverlayState.pitchOffsetDeg = Number(imuTilt.pitch_offset_deg);
+  }
+  updateImuOverlaySvg();
   updateUndervoltageDisplay(system.undervoltage || "unbekannt", system.undervoltage_state || "unknown");
   if (cameraServiceState === "active" && lastCameraServiceState && lastCameraServiceState !== "active") {
     setText("cam-state", "Verbinde...");
@@ -1709,8 +1718,8 @@ window.addEventListener("load", function () {
       Number(orientation.z || 0),
       Number(orientation.w || 1)
     ]);
-    imuOverlayState.rollDeg = euler.rollDeg;
+    imuOverlayState.rollDeg = euler.rollDeg + imuOverlayState.rollOffsetDeg;
-    imuOverlayState.pitchDeg = euler.pitchDeg;
+    imuOverlayState.pitchDeg = euler.pitchDeg + imuOverlayState.pitchOffsetDeg;
     imuOverlayState.yawDeg = euler.yawDeg;
     imuOverlayState.lastUpdateAt = Date.now();
     updateImuOverlaySvg();

scripts/exomy_admin_api.py

@@ -32,6 +32,9 @@ 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'
 )
+IMU_TILT_CALIBRATION_FILE = os.path.join(
+    os.path.dirname(__file__), '..', 'config', 'imu_tilt_calibration.json'
+)
 MOTOR_TEST_LOCK = threading.Lock()
 GPS_PORT_PATTERNS = [
     '/dev/serial/by-id/*',
@@ -91,6 +94,8 @@ IMU_ROS_CACHE = {
     'magnetic': 'unbekannt',
     'quaternion': 'unbekannt',
     'roll_pitch_yaw': 'unbekannt',
+    'roll_deg': None,
+    'pitch_deg': None,
     'error': 'Noch keine IMU-Daten aus ROS',
     'heading_deg': None,
     'updated_at': 0.0,
@@ -195,6 +200,8 @@ def _on_imu_data_message(message):
         ), 'rad/s', 3),
         quaternion=_format_quaternion(quaternion),
         roll_pitch_yaw=_format_euler_deg(euler_deg),
+        roll_deg=None if not euler_deg else euler_deg[0],
+        pitch_deg=None if not euler_deg else euler_deg[1],
         heading_deg=None if not euler_deg else _normalize_heading_deg(euler_deg[2]),
         error='',
     )
@@ -856,6 +863,7 @@ def collect_status():
     container_details = get_container_details(EXOMY_CONTAINER)
     imu_details = read_imu_ros_status()
     imu_heading = get_imu_heading_status()
+    imu_tilt = get_imu_tilt_status()
     return {
         'status': 'Bereit',
         'system': {
@@ -899,6 +907,7 @@ def collect_status():
         },
         'imu': imu_details,
         'imu_heading': imu_heading,
+        'imu_tilt': imu_tilt,
     }
 
 
@@ -1142,6 +1151,85 @@ def calibrate_imu_heading_to_north():
     }
 
 
+def get_default_imu_tilt_calibration():
+    return {
+        'roll_offset_deg': 0.0,
+        'pitch_offset_deg': 0.0,
+        'updated_at': None,
+        'source': 'default',
+    }
+
+
+def read_imu_tilt_calibration():
+    data = get_default_imu_tilt_calibration()
+    try:
+        with open(IMU_TILT_CALIBRATION_FILE, 'r', encoding='utf-8') as handle:
+            payload = json.load(handle)
+        data['roll_offset_deg'] = float(payload.get('roll_offset_deg', 0.0))
+        data['pitch_offset_deg'] = float(payload.get('pitch_offset_deg', 0.0))
+        data['updated_at'] = payload.get('updated_at')
+        data['source'] = payload.get('source', 'file')
+    except (OSError, ValueError, KeyError):
+        pass
+    return data
+
+
+def write_imu_tilt_calibration(roll_offset_deg, pitch_offset_deg):
+    payload = {
+        'roll_offset_deg': float(roll_offset_deg),
+        'pitch_offset_deg': float(pitch_offset_deg),
+        'updated_at': time.strftime('%Y-%m-%dT%H:%M:%S'),
+        'source': 'admin',
+    }
+    os.makedirs(os.path.dirname(IMU_TILT_CALIBRATION_FILE), exist_ok=True)
+    with open(IMU_TILT_CALIBRATION_FILE, 'w', encoding='utf-8') as handle:
+        json.dump(payload, handle, indent=2)
+    return payload
+
+
+def get_imu_tilt_status():
+    calibration = read_imu_tilt_calibration()
+    with IMU_ROS_LOCK:
+        raw_roll_deg = IMU_ROS_CACHE.get('roll_deg')
+        raw_pitch_deg = IMU_ROS_CACHE.get('pitch_deg')
+    corrected_roll = None if raw_roll_deg is None else raw_roll_deg + calibration['roll_offset_deg']
+    corrected_pitch = None if raw_pitch_deg is None else raw_pitch_deg + calibration['pitch_offset_deg']
+    return {
+        'roll_offset_deg': calibration['roll_offset_deg'],
+        'pitch_offset_deg': calibration['pitch_offset_deg'],
+        'raw_roll_deg': raw_roll_deg,
+        'raw_pitch_deg': raw_pitch_deg,
+        'corrected_roll_deg': corrected_roll,
+        'corrected_pitch_deg': corrected_pitch,
+        'updated_at': calibration['updated_at'],
+        'source': calibration['source'],
+    }
+
+
+def zero_imu_tilt():
+    with IMU_ROS_LOCK:
+        raw_roll_deg = IMU_ROS_CACHE.get('roll_deg')
+        raw_pitch_deg = IMU_ROS_CACHE.get('pitch_deg')
+
+    if raw_roll_deg is None or raw_pitch_deg is None:
+        return 409, {'status': 'Noch keine frischen IMU-Daten für Neigungskalibrierung'}
+
+    payload = write_imu_tilt_calibration(-float(raw_roll_deg), -float(raw_pitch_deg))
+    return 200, {
+        'status': 'Neigung genullt',
+        'imu_tilt': {
+            'roll_offset_deg': payload['roll_offset_deg'],
+            'pitch_offset_deg': payload['pitch_offset_deg'],
+            'raw_roll_deg': raw_roll_deg,
+            'raw_pitch_deg': raw_pitch_deg,
+            'corrected_roll_deg': 0.0,
+            'corrected_pitch_deg': 0.0,
+            'updated_at': payload['updated_at'],
+            'source': payload['source'],
+        },
+    }
+
+
 def set_camera_config(profile_id=None, fps=None):
     current = read_camera_settings()
     if profile_id is None:
@@ -2197,6 +2285,11 @@ class Handler(http.server.BaseHTTPRequestHandler):
             self.write_json(status_code, response)
             return
 
+        if self.path == '/api/imu/set-level':
+            status_code, response = zero_imu_tilt()
+            self.write_json(status_code, response)
+            return
+
         actions = {
             '/api/cold-start-gps': {
                 'handler': cold_start_gps_receiver,