diff --git a/gui/admin.html b/gui/admin.html index 67d7910..3c7e2a8 100644 --- a/gui/admin.html +++ b/gui/admin.html @@ -138,6 +138,24 @@ RPY - +
+ + Heading roh + - +
+
+ + Heading korr. + - +
+
+ + Nord-Offset + - +
+
+ +
Hinweis @@ -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'); diff --git a/gui/index.html b/gui/index.html index 39343e1..e927417 100644 --- a/gui/index.html +++ b/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); diff --git a/scripts/exomy_admin_api.py b/scripts/exomy_admin_api.py index bccea69..bfb7fba 100644 --- a/scripts/exomy_admin_api.py +++ b/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,