admin verbessert

gui/admin.html

@@ -816,6 +816,9 @@
   var roverCommandTopic = null;
   var motorCommandsTopic = null;
   var gpsFixTopic = null;
+  var imuDataTopic = null;
+  var imuMagTopic = null;
+  var imuStatusTopic = null;
   var gpsStatusTopic = null;
   var gpsDiagnosticsTopic = null;
   var pageConnection = {
@@ -930,6 +933,20 @@
     selectedSatellitePrn: null,
     plottedSatellites: []
   };
+  var imuRosState = {
+    state: 'unknown',
+    status: '-',
+    address: '0x4A',
+    acceleration: '--',
+    gyro: '--',
+    magnetic: '--',
+    quaternion: '--',
+    rollPitchYaw: '--',
+    error: '--',
+    lastDataAt: 0,
+    lastMagAt: 0,
+    lastStatusAt: 0
+  };
 
   function submitPassword() {
     var input = document.getElementById('auth_input');
@@ -982,14 +999,6 @@
     setServiceState('camera_service_state', '-');
     setServiceState('service_admin_api', '-');
     setServiceState('service_exomy', '-');
-    setStateText('imu_state', 'unknown', '-');
-    setText('imu_address', '--');
-    setText('imu_acceleration', '--');
-    setText('imu_gyro', '--');
-    setText('imu_magnetic', '--');
-    setText('imu_quaternion', '--');
-    setText('imu_roll_pitch_yaw', '--');
-    setText('imu_error', '--');
     var bar = document.getElementById('bar-temp-adm');
     if (bar) bar.style.width = '0%';
   }
@@ -1032,6 +1041,19 @@
     gpsDiagnosticsState.utcTime = null;
     gpsDiagnosticsState.lastFixAgeS = null;
     gpsDiagnosticsState.satellites = [];
+    imuRosState.state = 'unknown';
+    imuRosState.status = 'ROS getrennt';
+    imuRosState.address = '0x4A';
+    imuRosState.acceleration = '--';
+    imuRosState.gyro = '--';
+    imuRosState.magnetic = '--';
+    imuRosState.quaternion = '--';
+    imuRosState.rollPitchYaw = '--';
+    imuRosState.error = 'Keine ROS-Verbindung';
+    imuRosState.lastDataAt = 0;
+    imuRosState.lastMagAt = 0;
+    imuRosState.lastStatusAt = 0;
+    renderImuRosState();
     updateGpsDiagnosticsPanel();
   }
 
@@ -1213,6 +1235,81 @@
     node.classList.add('state_unknown');
   }
 
+  function formatImuVector(values, unit, decimals) {
+    if (!Array.isArray(values) || values.length < 3) return '--';
+    var digits = typeof decimals === 'number' ? decimals : 2;
+    return 'X ' + Number(values[0]).toFixed(digits) +
+      ' | Y ' + Number(values[1]).toFixed(digits) +
+      ' | Z ' + Number(values[2]).toFixed(digits) + ' ' + unit;
+  }
+
+  function formatImuQuaternion(values) {
+    if (!Array.isArray(values) || values.length < 4) return '--';
+    return 'I ' + Number(values[0]).toFixed(3) +
+      ' | J ' + Number(values[1]).toFixed(3) +
+      ' | K ' + Number(values[2]).toFixed(3) +
+      ' | R ' + Number(values[3]).toFixed(3);
+  }
+
+  function quaternionToEulerDeg(values) {
+    if (!Array.isArray(values) || values.length < 4) return null;
+    var x = Number(values[0]);
+    var y = Number(values[1]);
+    var z = Number(values[2]);
+    var w = Number(values[3]);
+    var sinrCosp = 2 * (w * x + y * z);
+    var cosrCosp = 1 - 2 * (x * x + y * y);
+    var roll = Math.atan2(sinrCosp, cosrCosp);
+    var sinp = 2 * (w * y - z * x);
+    var pitch = Math.abs(sinp) >= 1 ? Math.sign(sinp) * (Math.PI / 2) : Math.asin(sinp);
+    var sinyCosp = 2 * (w * z + x * y);
+    var cosyCosp = 1 - 2 * (y * y + z * z);
+    var yaw = Math.atan2(sinyCosp, cosyCosp);
+    return [
+      roll * 180 / Math.PI,
+      pitch * 180 / Math.PI,
+      yaw * 180 / Math.PI
+    ];
+  }
+
+  function formatRollPitchYaw(values) {
+    if (!Array.isArray(values) || values.length < 3) return '--';
+    return 'Roll ' + Number(values[0]).toFixed(1) + '° | ' +
+      'Pitch ' + Number(values[1]).toFixed(1) + '° | ' +
+      'Yaw ' + Number(values[2]).toFixed(1) + '°';
+  }
+
+  function renderImuRosState() {
+    var now = Date.now();
+    var dataFresh = imuRosState.lastDataAt && (now - imuRosState.lastDataAt) < 3000;
+    var magFresh = imuRosState.lastMagAt && (now - imuRosState.lastMagAt) < 3000;
+    var statusFresh = imuRosState.lastStatusAt && (now - imuRosState.lastStatusAt) < 5000;
+    var state = imuRosState.state;
+    var status = imuRosState.status;
+    var error = imuRosState.error;
+
+    if (!pageConnection.rosOk) {
+      state = 'unknown';
+      status = 'ROS getrennt';
+      error = 'Keine ROS-Verbindung';
+    } else if (!dataFresh) {
+      state = 'unknown';
+      status = 'Warte auf IMU-Daten';
+      if (!error || error === '--') error = 'Noch keine frischen IMU-Daten';
+    } else if (!statusFresh && (!status || status === '-')) {
+      status = 'Verbunden';
+    }
+
+    setStateText('imu_state', state, status || '-');
+    setText('imu_address', imuRosState.address);
+    setText('imu_acceleration', dataFresh ? imuRosState.acceleration : '--');
+    setText('imu_gyro', dataFresh ? imuRosState.gyro : '--');
+    setText('imu_magnetic', magFresh ? imuRosState.magnetic : '--');
+    setText('imu_quaternion', dataFresh ? imuRosState.quaternion : '--');
+    setText('imu_roll_pitch_yaw', dataFresh ? imuRosState.rollPitchYaw : '--');
+    setText('imu_error', error || '-');
+  }
+
   function formatNumber(value, digits) {
     return Number(value || 0).toLocaleString('de-DE', {
       minimumFractionDigits: digits,
@@ -2432,6 +2529,12 @@
       ros = null;
       roverCommandTopic = null;
       motorCommandsTopic = null;
+      gpsFixTopic = null;
+      gpsStatusTopic = null;
+      gpsDiagnosticsTopic = null;
+      imuDataTopic = null;
+      imuMagTopic = null;
+      imuStatusTopic = null;
       window.setTimeout(connectDriveEstimatorRos, 3000);
     });
     ros.on('error', function() {
@@ -2482,6 +2585,69 @@
       messageType: 'std_msgs/String'
     });
     gpsDiagnosticsTopic.subscribe(handleGpsDiagnostics);
+
+    imuDataTopic = new ROSLIB.Topic({
+      ros: ros,
+      name: '/imu/data',
+      messageType: 'sensor_msgs/Imu'
+    });
+    imuDataTopic.subscribe(function(message) {
+      var linearAcceleration = message.linear_acceleration || {};
+      var angularVelocity = message.angular_velocity || {};
+      var orientation = message.orientation || {};
+      var quaternion = [
+        Number(orientation.x || 0),
+        Number(orientation.y || 0),
+        Number(orientation.z || 0),
+        Number(orientation.w || 1)
+      ];
+      imuRosState.state = 'active';
+      imuRosState.address = '0x4A';
+      imuRosState.acceleration = formatImuVector([
+        Number(linearAcceleration.x || 0),
+        Number(linearAcceleration.y || 0),
+        Number(linearAcceleration.z || 0)
+      ], 'm/s^2', 2);
+      imuRosState.gyro = formatImuVector([
+        Number(angularVelocity.x || 0),
+        Number(angularVelocity.y || 0),
+        Number(angularVelocity.z || 0)
+      ], 'rad/s', 3);
+      imuRosState.quaternion = formatImuQuaternion(quaternion);
+      imuRosState.rollPitchYaw = formatRollPitchYaw(quaternionToEulerDeg(quaternion));
+      imuRosState.lastDataAt = Date.now();
+      if (!imuRosState.error || imuRosState.error === 'Keine ROS-Verbindung' || imuRosState.error === 'Noch keine frischen IMU-Daten') {
+        imuRosState.error = '';
+      }
+    });
+
+    imuMagTopic = new ROSLIB.Topic({
+      ros: ros,
+      name: '/imu/mag',
+      messageType: 'sensor_msgs/MagneticField'
+    });
+    imuMagTopic.subscribe(function(message) {
+      var magneticField = message.magnetic_field || {};
+      imuRosState.magnetic = formatImuVector([
+        Number(magneticField.x || 0) * 1000000,
+        Number(magneticField.y || 0) * 1000000,
+        Number(magneticField.z || 0) * 1000000
+      ], 'uT', 2);
+      imuRosState.lastMagAt = Date.now();
+    });
+
+    imuStatusTopic = new ROSLIB.Topic({
+      ros: ros,
+      name: '/imu/status',
+      messageType: 'std_msgs/String'
+    });
+    imuStatusTopic.subscribe(function(message) {
+      var text = String((message && message.data) || '').trim();
+      imuRosState.status = text || 'Verbunden';
+      imuRosState.state = text.indexOf('Fehler') === 0 ? 'unknown' : 'active';
+      imuRosState.error = text.indexOf('Fehler') === 0 ? text : '';
+      imuRosState.lastStatusAt = Date.now();
+    });
   }
 
   function temperatureToWidth(value) {
@@ -2494,7 +2660,6 @@
   function renderStatus(data) {
     var system = data.system || {};
     var services = data.services || {};
-    var imu = data.imu || {};
     setAdminStatus(data.status || 'Bereit');
     setText('info_ips', system.ips);
     setText('info_wifi', system.wifi);
@@ -2530,14 +2695,6 @@
     setServiceState('camera_service_state', services.camera);
     setServiceState('service_admin_api', services.admin_api);
     setServiceState('service_exomy', services.exomy);
-    setStateText('imu_state', imu.state || 'unknown', imu.status || '-');
-    setText('imu_address', imu.address);
-    setText('imu_acceleration', imu.acceleration);
-    setText('imu_gyro', imu.gyro);
-    setText('imu_magnetic', imu.magnetic);
-    setText('imu_quaternion', imu.quaternion);
-    setText('imu_roll_pitch_yaw', imu.roll_pitch_yaw);
-    setText('imu_error', imu.error || '-');
   }
 
   async function fetchStatus() {
@@ -3109,6 +3266,7 @@
     renderWaypointList();
     applyWaypointPlanningState();
     renderDriveEstimator();
+    renderImuRosState();
     updateGpsDiagnosticsPanel();
     refreshCameraPreview();
     window.setInterval(fetchStatus, 10000);
@@ -3116,6 +3274,7 @@
     window.setInterval(fetchDelay, 5000);
     window.setInterval(fetchSpeedLimit, 5000);
     window.setInterval(fetchMotorTestStatus, 4000);
+    window.setInterval(renderImuRosState, 750);
     window.setInterval(renderDriveEstimator, 250);
     window.setInterval(updatePageConnectionState, 1000);
   });

src/imu_node.py

@@ -26,6 +26,7 @@ ROSBRIDGE_PORT = int(os.environ.get('IMU_ROSBRIDGE_PORT', '9090'))
 
 RUNNING = True
 WORKAROUND_APPLIED = False
+LAST_ERROR_TEXT = None
 
 
 def handle_shutdown(signum, frame):
@@ -40,6 +41,7 @@ def apply_bno08x_workaround():
     if WORKAROUND_APPLIED:
         return
 
+    adafruit_bno08x._dbg = lambda *args, **kwargs: None
     original_handle_packet = adafruit_bno08x.BNO08X._handle_packet
 
     def safe_handle_packet(self, packet):
@@ -49,6 +51,10 @@ def apply_bno08x_workaround():
             if len(exc.args) == 1 and isinstance(exc.args[0], int):
                 return
             raise
+        except IndexError as exc:
+            if 'list assignment index out of range' in str(exc):
+                return
+            raise
 
     adafruit_bno08x.BNO08X._handle_packet = safe_handle_packet
     WORKAROUND_APPLIED = True
@@ -107,6 +113,27 @@ def publish_status(topic, text):
     topic.publish(roslibpy.Message({'data': text}))
 
 
+def report_error(status_topic, text):
+    global LAST_ERROR_TEXT
+
+    if text == LAST_ERROR_TEXT:
+        return
+
+    LAST_ERROR_TEXT = text
+    print(text)
+    sys.stdout.flush()
+    if status_topic is not None:
+        try:
+            publish_status(status_topic, text)
+        except Exception:
+            pass
+
+
+def clear_error_state():
+    global LAST_ERROR_TEXT
+    LAST_ERROR_TEXT = None
+
+
 def publish_measurements(imu_topic, mag_topic, status_topic, sensor):
     stamp = ros_time_now()
     acceleration = sensor.acceleration
@@ -174,22 +201,21 @@ def main():
 
             if sensor is None:
                 sensor = create_sensor()
+                clear_error_state()
                 publish_status(status_topic, 'BNO085 initialisiert auf {0}'.format(ADDRESS))
                 print('BNO085 initialisiert')
                 sys.stdout.flush()
 
             publish_measurements(imu_topic, mag_topic, status_topic, sensor)
+            clear_error_state()
             time.sleep(sleep_seconds)
         except Exception as exc:
             message = 'IMU Fehler: {0}'.format(exc)
-            print(message)
+            report_error(status_topic if ros_client is not None and ros_client.is_connected else None, message)
-            sys.stdout.flush()
-            if status_topic is not None and ros_client is not None and ros_client.is_connected:
-                try:
-                    publish_status(status_topic, message)
-                except Exception:
-                    pass
             sensor = None
+            if isinstance(exc, OSError) and getattr(exc, 'errno', None) == 5:
+                time.sleep(1.5)
+            else:
                 time.sleep(1.0)
 
     if ros_client is not None: