IMU eingebaut

scripts/exomy_admin_api.py

@@ -14,6 +14,7 @@ import subprocess
 import termios
 import threading
 import time
+import warnings
 from zoneinfo import ZoneInfo
 
 PORT = 8082
@@ -66,6 +67,11 @@ CAMERA_PROFILE_MAP = {profile['id']: profile for profile in CAMERA_PROFILES}
 DEFAULT_CAMERA_PROFILE_ID = '1296x972'
 CAMERA_FPS_OPTIONS = [1, 2, 3, 4, 5, 7, 10, 20]
 DEFAULT_CAMERA_FPS = 10
+IMU_I2C_ADDRESS = '0x4A'
+IMU_LOCK = threading.Lock()
+IMU_SENSOR = None
+IMU_LAST_ERROR = None
+IMU_WORKAROUND_APPLIED = False
 
 
 class ReusableThreadingTCPServer(socketserver.ThreadingTCPServer):
@@ -77,6 +83,130 @@ def run_command(command):
     return result.stdout.strip(), result.stderr.strip(), result.returncode
 
 
+def _format_vector(values, unit, decimals=2):
+    if not values:
+        return 'unbekannt'
+    return 'X {0:.{d}f} | Y {1:.{d}f} | Z {2:.{d}f} {u}'.format(
+        values[0], values[1], values[2], d=decimals, u=unit
+    )
+
+
+def _format_quaternion(values):
+    if not values:
+        return 'unbekannt'
+    return 'I {0:.3f} | J {1:.3f} | K {2:.3f} | R {3:.3f}'.format(
+        values[0], values[1], values[2], values[3]
+    )
+
+
+def _apply_bno08x_workaround(module):
+    global IMU_WORKAROUND_APPLIED
+
+    if IMU_WORKAROUND_APPLIED:
+        return
+
+    original_handle_packet = module.BNO08X._handle_packet
+
+    def safe_handle_packet(self, packet):
+        try:
+            return original_handle_packet(self, packet)
+        except KeyError as exc:
+            # Some BNO085 firmwares emit undocumented reports that the Adafruit
+            # library cannot decode yet. Ignore those packets and keep known
+            # sensor reports flowing.
+            if len(exc.args) == 1 and isinstance(exc.args[0], int):
+                return
+            raise
+
+    module.BNO08X._handle_packet = safe_handle_packet
+    IMU_WORKAROUND_APPLIED = True
+
+
+def _create_imu_sensor():
+    global IMU_LAST_ERROR
+
+    try:
+        with warnings.catch_warnings():
+            warnings.filterwarnings(
+                'ignore',
+                message='I2C frequency is not settable in python, ignoring!',
+                category=RuntimeWarning,
+            )
+            import adafruit_bno08x
+            import board
+            import busio
+            from adafruit_bno08x import (
+                BNO_REPORT_ACCELEROMETER,
+                BNO_REPORT_GYROSCOPE,
+                BNO_REPORT_MAGNETOMETER,
+                BNO_REPORT_ROTATION_VECTOR,
+            )
+            from adafruit_bno08x.i2c import BNO08X_I2C
+
+            _apply_bno08x_workaround(adafruit_bno08x)
+
+            i2c = busio.I2C(board.SCL, board.SDA, frequency=400000)
+            sensor = BNO08X_I2C(i2c)
+            for feature in (
+                BNO_REPORT_ACCELEROMETER,
+                BNO_REPORT_GYROSCOPE,
+                BNO_REPORT_MAGNETOMETER,
+                BNO_REPORT_ROTATION_VECTOR,
+            ):
+                sensor.enable_feature(feature)
+            time.sleep(0.2)
+            IMU_LAST_ERROR = None
+            return sensor
+    except Exception as exc:
+        IMU_LAST_ERROR = str(exc)
+        return None
+
+
+def read_imu_status():
+    global IMU_SENSOR, IMU_LAST_ERROR
+
+    with IMU_LOCK:
+        if IMU_SENSOR is None:
+            IMU_SENSOR = _create_imu_sensor()
+
+        if IMU_SENSOR is None:
+            return {
+                'state': 'unknown',
+                'status': 'Nicht bereit',
+                'address': IMU_I2C_ADDRESS,
+                'acceleration': 'unbekannt',
+                'gyro': 'unbekannt',
+                'magnetic': 'unbekannt',
+                'quaternion': 'unbekannt',
+                'error': IMU_LAST_ERROR or 'BNO085 nicht verfugbar',
+            }
+
+        try:
+            return {
+                'state': 'active',
+                'status': 'Verbunden',
+                'address': IMU_I2C_ADDRESS,
+                'acceleration': _format_vector(IMU_SENSOR.acceleration, 'm/s^2'),
+                'gyro': _format_vector(IMU_SENSOR.gyro, 'rad/s', 3),
+                'magnetic': _format_vector(IMU_SENSOR.magnetic, 'uT'),
+                'quaternion': _format_quaternion(IMU_SENSOR.quaternion),
+                'error': '',
+            }
+        except Exception as exc:
+            IMU_SENSOR = None
+            IMU_LAST_ERROR = str(exc)
+            return {
+                'state': 'unknown',
+                'status': 'Lesefehler',
+                'address': IMU_I2C_ADDRESS,
+                'acceleration': 'unbekannt',
+                'gyro': 'unbekannt',
+                'magnetic': 'unbekannt',
+                'quaternion': 'unbekannt',
+                'error': IMU_LAST_ERROR,
+            }
+
+
 def read_cpu_temperature():
     try:
         with open('/sys/class/thermal/thermal_zone0/temp', 'r', encoding='utf-8') as handle:
@@ -523,6 +653,7 @@ def collect_status():
     wifi_details = get_wifi_details()
     time_details = get_system_time_details()
     container_details = get_container_details(EXOMY_CONTAINER)
+    imu_details = read_imu_status()
     return {
         'status': 'Bereit',
         'system': {
@@ -563,7 +694,8 @@ def collect_status():
             'exomy': get_container_state(EXOMY_CONTAINER),
             'exomy_uptime': container_details['uptime'],
             'exomy_restarts': container_details['restarts'],
-        }
+        },
+        'imu': imu_details,
     }