update imu, lidar

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/WheeltecIMU/WheeltecReader.cs

@@ -1,5 +1,6 @@
 using System;
 using System.Collections.Generic;
+using System.IO;
 using System.IO.Ports;
 using System.Threading;
 
@@ -24,6 +25,10 @@ namespace RobotNet10.RobotApp.Drivers.WheeltecIMU
         // Event de thong bao khi co du lieu moi duoc decode
         public event EventHandler? DataReceived;
 
+        // Event bao trang thai ket noi thay doi (true = vua mo port, false = mat ket noi)
+        // de device class bao cho DeviceBase cap nhat state machine / UI
+        public event EventHandler<bool>? ConnectionStateChanged;
+
         // Properties - lock-free voi memory barriers (volatile khong ho tro double)
         public double Roll { get; private set; }
         public double Pitch { get; private set; }
@@ -85,8 +90,19 @@ namespace RobotNet10.RobotApp.Drivers.WheeltecIMU
         private const long DATA_TIMEOUT_TICKS = 3 * TimeSpan.TicksPerSecond;
         private long _lastFrameTicks;
 
+        // Kiem tra dinh ky file port con ton tai khong (rut USB -> /dev/ttyUSBx bien mat)
+        // de phat hien disconnect nhanh hon watchdog
+        private const long PORT_CHECK_INTERVAL_TICKS = TimeSpan.TicksPerSecond / 2;
+        private long _lastPortCheckTicks;
+
         public bool IsConnected => serial != null && serial.IsOpen;
 
+        /// <summary>
+        /// Co frame hop le trong DATA_TIMEOUT gan nhat khong — dung de phan biet
+        /// "port mo nhung khong co du lieu" (vd ModemManager dang giu port sau khi cam lai)
+        /// </summary>
+        public bool HasRecentData => DateTime.UtcNow.Ticks - Volatile.Read(ref _lastFrameTicks) < DATA_TIMEOUT_TICKS;
+
         public WheeltecReader(string portName, int baudRate, int timeOut)
         {
             _portName = portName;
@@ -226,6 +242,7 @@ namespace RobotNet10.RobotApp.Drivers.WheeltecIMU
                         serial.DiscardInBuffer();
                         _lastFrameTicks = DateTime.UtcNow.Ticks;
                         Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [WheeltecReader] Connected to {_portName}");
+                        RaiseConnectionStateChanged(true);
                     }
 
                     InnerReadLoop(readBuffer, cancellationToken);
@@ -238,6 +255,7 @@ namespace RobotNet10.RobotApp.Drivers.WheeltecIMU
                 {
                     Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [WheeltecReader] IMU disconnected: {ex.Message}");
                     SafeCloseSerial();
+                    RaiseConnectionStateChanged(false);
                 }
 
                 if (_shouldRead && !cancellationToken.IsCancellationRequested)
@@ -259,19 +277,38 @@ namespace RobotNet10.RobotApp.Drivers.WheeltecIMU
             while (_shouldRead && !cancellationToken.IsCancellationRequested
                    && serial != null && serial.IsOpen)
             {
+                // Watchdog phai kiem tra MOI vong lap: khi rut USB tren Linux,
+                // BytesToRead co the van > 0 nhung Read() tra ve 0 (EOF) lien tuc,
+                // neu chi kiem tra trong nhanh bytesToRead <= 0 se khong bao gio phat hien
+                long nowTicks = DateTime.UtcNow.Ticks;
+                if (nowTicks - _lastFrameTicks > DATA_TIMEOUT_TICKS)
+                {
+                    throw new TimeoutException($"No IMU frame for > {DATA_TIMEOUT_TICKS / TimeSpan.TicksPerSecond}s");
+                }
+
+                // Rut USB -> device file bien mat: phat hien nhanh hon watchdog
+                if (nowTicks - _lastPortCheckTicks > PORT_CHECK_INTERVAL_TICKS)
+                {
+                    _lastPortCheckTicks = nowTicks;
+                    if (!File.Exists(_portName))
+                    {
+                        throw new IOException($"Serial port {_portName} no longer exists (USB unplugged?)");
+                    }
+                }
+
                 int bytesToRead = serial.BytesToRead;
                 if (bytesToRead <= 0)
                 {
-                    if (DateTime.UtcNow.Ticks - _lastFrameTicks > DATA_TIMEOUT_TICKS)
-                    {
-                        throw new TimeoutException($"No IMU frame for > {DATA_TIMEOUT_TICKS / TimeSpan.TicksPerSecond}s");
-                    }
                     Thread.Sleep(1);
                     continue;
                 }
 
                 int bytesRead = serial.Read(readBuffer, 0, Math.Min(bytesToRead, readBuffer.Length));
-                if (bytesRead <= 0) continue;
+                if (bytesRead <= 0)
+                {
+                    Thread.Sleep(1);
+                    continue;
+                }
 
                 ProcessIncomingData(readBuffer, bytesRead);
             }
@@ -482,6 +519,18 @@ namespace RobotNet10.RobotApp.Drivers.WheeltecIMU
             _frameBufferLength = remainingAfterHead;
         }
 
+        private void RaiseConnectionStateChanged(bool connected)
+        {
+            try
+            {
+                ConnectionStateChanged?.Invoke(this, connected);
+            }
+            catch (Exception ex)
+            {
+                Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [WheeltecReader] ConnectionStateChanged subscriber error: {ex.Message}");
+            }
+        }
+
         public void Disconnect()
         {
             StopReadingThread();