Eskimue/ExoMy_Cuno
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

IMU Norden kann kalibriert werden

Browse Source
This commit is contained in:
Eskimue
2026-05-28 19:52:01 +02:00
parent 948ced194b
commit 9609bb282f
3 changed files with 218 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
gui/admin.html
+74 -1
View File
@@ -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
+12 -2
View File
@@ -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
+132 -2
View File
@@ -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,