From b4ea4799a966681cfea2c1d0aef65d34160cdc67 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Thomas=20M=C3=BCller?= Date: Fri, 29 May 2026 08:52:41 +0200 Subject: [PATCH] IMU kann genullt werden --- gui/admin.html | 55 ++++++++++++++++++++++ gui/index.html | 15 ++++-- scripts/exomy_admin_api.py | 93 ++++++++++++++++++++++++++++++++++++++ 3 files changed, 160 insertions(+), 3 deletions(-) diff --git a/gui/admin.html b/gui/admin.html index a169217..9f21097 100644 --- a/gui/admin.html +++ b/gui/admin.html @@ -153,8 +153,24 @@ Nord-Offset - +
+ + Roll korr. + - +
+
+ + Pitch korr. + - +
+
+ + Neig.-Offset + - +
+
@@ -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'); diff --git a/gui/index.html b/gui/index.html index cb3d119..ecd2628 100644 --- a/gui/index.html +++ b/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.pitchDeg = euler.pitchDeg; + imuOverlayState.rollDeg = euler.rollDeg + imuOverlayState.rollOffsetDeg; + imuOverlayState.pitchDeg = euler.pitchDeg + imuOverlayState.pitchOffsetDeg; imuOverlayState.yawDeg = euler.yawDeg; imuOverlayState.lastUpdateAt = Date.now(); updateImuOverlaySvg(); diff --git a/scripts/exomy_admin_api.py b/scripts/exomy_admin_api.py index bfb7fba..2d12ea1 100644 --- a/scripts/exomy_admin_api.py +++ b/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,