QUYVN/BQP
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update imu, lidar

Browse Source
This commit is contained in:
QUYVN
2026-07-14 15:15:34 +07:00
parent dddf3fc594
commit 921bb91084
6 changed files with 3197 additions and 868 deletions
Download Patch File Download Diff File Expand all files Collapse all files

59
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/WheeltecIMU/WheeltecReader.cs
View File

@@ -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();