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,