IMU kann genullt werden

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,