update imu, lidar
This commit is contained in:
@@ -0,0 +1,859 @@
|
|||||||
|
using System.Diagnostics;
|
||||||
|
using System.Threading;
|
||||||
|
using RobotNet10.RobotApp.Client.Shared.Devices;
|
||||||
|
using RobotNet10.RobotApp.Devices;
|
||||||
|
using RobotNet10.Shared;
|
||||||
|
using RobotNet10.Shared.Geometry;
|
||||||
|
using RobotNet10.Shared.Sensor;
|
||||||
|
|
||||||
|
namespace RobotNet10.RobotApp.Drivers.HfiA9IMU
|
||||||
|
{
|
||||||
|
[Device(DeviceType.Imu, "HfiA9IMU", "HfiA9IMU", "1.0.0", Description = "HandsFree HFI-A9 IMU Driver")]
|
||||||
|
public class HfiA9IMU : DeviceBase, IInertialMeasurementUnit
|
||||||
|
{
|
||||||
|
private readonly HfiA9Reader IMU;
|
||||||
|
|
||||||
|
// Cached data
|
||||||
|
private AccelStamped _cachedAcceleration;
|
||||||
|
private Vector3Stamped _cachedAngularVelocity;
|
||||||
|
private Vector3Stamped _cachedMagnetometer;
|
||||||
|
private Vector3Stamped _cachedOrientation;
|
||||||
|
private QuaternionStamped _cachedQuaternion;
|
||||||
|
private bool _isCalibrated = false;
|
||||||
|
private DateTime _lastUpdateTime = DateTime.UtcNow;
|
||||||
|
private double _sampleRate = 0.0;
|
||||||
|
private readonly string _portName;
|
||||||
|
private readonly int _timeOut;
|
||||||
|
private readonly int _baudRate;
|
||||||
|
private readonly bool _printDataEnabled;
|
||||||
|
private readonly TimeSpan _printDataInterval;
|
||||||
|
private Timer? _printDataTimer;
|
||||||
|
|
||||||
|
// Sample rate calculation
|
||||||
|
private int _sampleCount = 0;
|
||||||
|
private DateTime _sampleRateStartTime = DateTime.UtcNow;
|
||||||
|
private readonly TimeSpan _sampleRateWindow = TimeSpan.FromSeconds(1.0);
|
||||||
|
|
||||||
|
// Calibration: 2 giay dau sau lan nhan du lieu dau tien de thu thap bias (robot dung yen tuyet doi)
|
||||||
|
private static readonly TimeSpan CalibrationDuration = TimeSpan.FromSeconds(2.0);
|
||||||
|
private static readonly TimeSpan CalibrationWaitTimeout = TimeSpan.FromSeconds(5.0);
|
||||||
|
private const double GravityMps2 = 9.81;
|
||||||
|
private const double GravityToleranceMps2 = 2.0;
|
||||||
|
|
||||||
|
// IMU outlier validation thresholds
|
||||||
|
private const double MaxValidAccelerationMps2 = 50.0;
|
||||||
|
private const double MaxValidAngularVelocityRadS = 20.0;
|
||||||
|
|
||||||
|
// Thread-safety: Lock for cached sensor data (struct assignments are NOT atomic)
|
||||||
|
private readonly object _dataLock = new();
|
||||||
|
|
||||||
|
// High-precision timestamp using Stopwatch (DateTime.UtcNow has ~10-15ms precision)
|
||||||
|
private readonly Stopwatch _highPrecisionTimer = Stopwatch.StartNew();
|
||||||
|
private DateTime _timerStartUtc = DateTime.UtcNow;
|
||||||
|
private DateTime? _calibrationStartUtc;
|
||||||
|
private bool _calibrationDone;
|
||||||
|
private TaskCompletionSource<bool>? _calibrationCompletedTcs;
|
||||||
|
private double _calibrationSumAccX, _calibrationSumAccY, _calibrationSumAccZ;
|
||||||
|
private double _calibrationSumGx, _calibrationSumGy, _calibrationSumGz;
|
||||||
|
private double _calibrationSumRoll, _calibrationSumPitch, _calibrationSumYaw;
|
||||||
|
private int _calibrationCount;
|
||||||
|
private double _accBiasX, _accBiasY, _accBiasZ;
|
||||||
|
private double _gyroBiasX, _gyroBiasY, _gyroBiasZ;
|
||||||
|
private double _orientationBiasRoll, _orientationBiasPitch, _orientationBiasYaw;
|
||||||
|
|
||||||
|
// Yaw integration: tich phan CalibratedGz thay vi dung firmware AHRS Yaw
|
||||||
|
// Roll/Pitch van dung firmware AHRS vi co gravity reference (khong drift)
|
||||||
|
private double _integratedYaw;
|
||||||
|
private DateTime _lastIntegrationTime;
|
||||||
|
|
||||||
|
// Diagnostic: log drift moi 5 giay
|
||||||
|
private DateTime _lastDiagnosticLog = DateTime.MinValue;
|
||||||
|
private static readonly TimeSpan DiagnosticLogInterval = TimeSpan.FromSeconds(5.0);
|
||||||
|
|
||||||
|
// Events (interface)
|
||||||
|
public event EventHandler<AccelerationChangedEventArgs>? AccelerationChanged;
|
||||||
|
public event EventHandler<AngularVelocityChangedEventArgs>? AngularVelocityChanged;
|
||||||
|
public event EventHandler<MagnetometerChangedEventArgs>? MagnetometerChanged;
|
||||||
|
public event EventHandler<OrientationChangedEventArgs>? OrientationChanged;
|
||||||
|
|
||||||
|
// Event thong nhat cho SensorPipeline
|
||||||
|
public event EventHandler<ImuDataChangedEventArgs>? ImuDataChanged;
|
||||||
|
|
||||||
|
public HfiA9IMU(string deviceId, string deviceName, IConfigurationSection connection)
|
||||||
|
: base(deviceId, deviceName, DeviceType.Imu)
|
||||||
|
{
|
||||||
|
_portName = connection.GetValue<string>("Port") ?? throw new Exception("Port is required");
|
||||||
|
_baudRate = connection.GetValue<int?>("BaudRate") ?? throw new Exception("BaudRate is required");
|
||||||
|
_timeOut = connection.GetValue<int?>("TimeOut") ?? throw new Exception("Timeout is required");
|
||||||
|
|
||||||
|
IMU = new HfiA9Reader(_portName, _baudRate, _timeOut);
|
||||||
|
|
||||||
|
// Continuous IMU data printing (optional)
|
||||||
|
_printDataEnabled = connection.GetValue<bool?>("DebugEnabled") ?? false;
|
||||||
|
_printDataInterval = TimeSpan.FromMilliseconds(connection.GetValue<int?>("PrintDataIntervalMs") ?? 200);
|
||||||
|
|
||||||
|
// Doc cau hinh neu co
|
||||||
|
var autoReconnectEnabled = connection.GetValue<bool?>("AutoReconnectEnabled");
|
||||||
|
var reconnectDelayMs = connection.GetValue<int?>("ReconnectDelayMs");
|
||||||
|
var maxReconnectAttempts = connection.GetValue<int?>("MaxReconnectAttempts");
|
||||||
|
|
||||||
|
if (autoReconnectEnabled.HasValue)
|
||||||
|
AutoReconnectEnabled = autoReconnectEnabled.Value;
|
||||||
|
else
|
||||||
|
AutoReconnectEnabled = true;
|
||||||
|
|
||||||
|
if (reconnectDelayMs.HasValue)
|
||||||
|
ReconnectDelayMs = reconnectDelayMs.Value;
|
||||||
|
|
||||||
|
if (maxReconnectAttempts.HasValue)
|
||||||
|
MaxReconnectAttempts = maxReconnectAttempts.Value;
|
||||||
|
|
||||||
|
_cachedAcceleration = CreateAccelStamped(0, 0, 9.81, DateTime.UtcNow);
|
||||||
|
_cachedAngularVelocity = CreateVector3Stamped(0, 0, 0, DateTime.UtcNow);
|
||||||
|
_cachedMagnetometer = CreateVector3Stamped(0, 0, 0, DateTime.UtcNow);
|
||||||
|
_cachedOrientation = CreateVector3Stamped(0, 0, 0, DateTime.UtcNow);
|
||||||
|
_cachedQuaternion = CreateQuaternionStamped(1, 0, 0, 0, DateTime.UtcNow);
|
||||||
|
|
||||||
|
// Khoi tao gia tri properties
|
||||||
|
UpdateProperties();
|
||||||
|
}
|
||||||
|
|
||||||
|
protected override IEnumerable<PropertyDescription> CreatePropertyDescriptions()
|
||||||
|
{
|
||||||
|
yield return new PropertyDescription("IsCalibrated", "Calibrated", "Trang thai calibrate")
|
||||||
|
{
|
||||||
|
DataType = "boolean",
|
||||||
|
IsReadOnly = true,
|
||||||
|
DisplayOrder = 1,
|
||||||
|
Category = "Trang thai",
|
||||||
|
DefaultValue = "true"
|
||||||
|
};
|
||||||
|
|
||||||
|
yield return new PropertyDescription("SampleRate", "Sample Rate (Hz)", "Tan so lay mau (Hz)")
|
||||||
|
{
|
||||||
|
DataType = "number",
|
||||||
|
IsReadOnly = true,
|
||||||
|
DisplayOrder = 2,
|
||||||
|
Category = "Cau hinh",
|
||||||
|
DefaultValue = "150"
|
||||||
|
};
|
||||||
|
|
||||||
|
yield return new PropertyDescription("Acceleration", "Acceleration (m/s²)", "Gia toc 3 truc (m/s²)")
|
||||||
|
{
|
||||||
|
DataType = "string",
|
||||||
|
IsReadOnly = true,
|
||||||
|
DisplayOrder = 3,
|
||||||
|
Category = "Du lieu",
|
||||||
|
DefaultValue = "0, 0, 9.81"
|
||||||
|
};
|
||||||
|
|
||||||
|
yield return new PropertyDescription("AngularVelocity", "Angular Velocity (rad/s)", "Van toc goc 3 truc (rad/s)")
|
||||||
|
{
|
||||||
|
DataType = "string",
|
||||||
|
IsReadOnly = true,
|
||||||
|
DisplayOrder = 4,
|
||||||
|
Category = "Du lieu",
|
||||||
|
DefaultValue = "0, 0, 0"
|
||||||
|
};
|
||||||
|
|
||||||
|
yield return new PropertyDescription("Orientation", "Orientation (rad)", "Huong Euler angles (rad)")
|
||||||
|
{
|
||||||
|
DataType = "string",
|
||||||
|
IsReadOnly = true,
|
||||||
|
DisplayOrder = 5,
|
||||||
|
Category = "Du lieu",
|
||||||
|
DefaultValue = "0, 0, 0"
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
protected override async Task OnInitializeAsync(CancellationToken cancellationToken)
|
||||||
|
{
|
||||||
|
IMU.Connect();
|
||||||
|
await Task.CompletedTask;
|
||||||
|
}
|
||||||
|
|
||||||
|
protected override async Task OnConnectAsync(CancellationToken cancellationToken)
|
||||||
|
{
|
||||||
|
// Dam bao port da mo (sau Disconnect can Connect lai)
|
||||||
|
if (!IMU.IsConnected)
|
||||||
|
IMU.Connect();
|
||||||
|
|
||||||
|
if (_printDataEnabled)
|
||||||
|
{
|
||||||
|
_printDataTimer?.Dispose();
|
||||||
|
_printDataTimer = new Timer(_ =>
|
||||||
|
{
|
||||||
|
try
|
||||||
|
{
|
||||||
|
var acc = CachedAcceleration.Accel.Linear;
|
||||||
|
var gyro = CachedAngularVelocity.Vector;
|
||||||
|
var ori = CachedOrientation.Vector;
|
||||||
|
var t = ((IInertialMeasurementUnit)this).LastUpdateTime;
|
||||||
|
|
||||||
|
Console.WriteLine(
|
||||||
|
$"{DateTime.Now:HH:mm:ss.ffffff} [IMU_DATA] " +
|
||||||
|
$"t={t:HH:mm:ss.fff} " +
|
||||||
|
$"acc=({acc.X:F3},{acc.Y:F3},{acc.Z:F3}) " +
|
||||||
|
$"gyro=({gyro.X:F5},{gyro.Y:F5},{gyro.Z:F5}) " +
|
||||||
|
$"rpy=({ori.X:F5},{ori.Y:F5},{ori.Z:F5})");
|
||||||
|
}
|
||||||
|
catch { }
|
||||||
|
}, null, dueTime: TimeSpan.Zero, period: _printDataInterval);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Reset high-precision timer for accurate timestamps
|
||||||
|
_timerStartUtc = DateTime.UtcNow;
|
||||||
|
_highPrecisionTimer.Restart();
|
||||||
|
|
||||||
|
// Reset calibration de moi lan connect thu thap lai 2 giay dau
|
||||||
|
ResetCalibrationState();
|
||||||
|
_calibrationCompletedTcs = new TaskCompletionSource<bool>();
|
||||||
|
|
||||||
|
// Dang ky event handler cho DataReceived tu HfiA9Reader
|
||||||
|
IMU.DataReceived += IMU_DataReceived;
|
||||||
|
IMU.ConnectionStateChanged += IMU_ConnectionStateChanged;
|
||||||
|
|
||||||
|
// Doi xu ly _calibrationDone (toi da CalibrationWaitTimeout), de connect chi hoan tat sau khi da calibrate
|
||||||
|
try
|
||||||
|
{
|
||||||
|
await Task.WhenAny(
|
||||||
|
_calibrationCompletedTcs.Task,
|
||||||
|
Task.Delay(CalibrationWaitTimeout, cancellationToken)).ConfigureAwait(false);
|
||||||
|
}
|
||||||
|
catch (OperationCanceledException)
|
||||||
|
{
|
||||||
|
// cancellationToken bi huy
|
||||||
|
}
|
||||||
|
_calibrationCompletedTcs = null;
|
||||||
|
}
|
||||||
|
|
||||||
|
protected override async Task OnDisconnectAsync(CancellationToken cancellationToken)
|
||||||
|
{
|
||||||
|
// Huy dang ky event handler
|
||||||
|
IMU.DataReceived -= IMU_DataReceived;
|
||||||
|
IMU.ConnectionStateChanged -= IMU_ConnectionStateChanged;
|
||||||
|
|
||||||
|
// Disconnect IMU de dung processing thread va serial port
|
||||||
|
IMU.Disconnect();
|
||||||
|
|
||||||
|
_printDataTimer?.Dispose();
|
||||||
|
_printDataTimer = null;
|
||||||
|
|
||||||
|
await Task.CompletedTask;
|
||||||
|
}
|
||||||
|
|
||||||
|
protected override async Task OnResetAsync(CancellationToken cancellationToken)
|
||||||
|
{
|
||||||
|
// Huy event va disconnect
|
||||||
|
IMU.DataReceived -= IMU_DataReceived;
|
||||||
|
IMU.ConnectionStateChanged -= IMU_ConnectionStateChanged;
|
||||||
|
IMU.Disconnect();
|
||||||
|
|
||||||
|
_printDataTimer?.Dispose();
|
||||||
|
_printDataTimer = null;
|
||||||
|
|
||||||
|
// Reset high-precision timer
|
||||||
|
_timerStartUtc = DateTime.UtcNow;
|
||||||
|
_highPrecisionTimer.Restart();
|
||||||
|
|
||||||
|
lock (_dataLock)
|
||||||
|
{
|
||||||
|
_cachedAcceleration = CreateAccelStamped(0, 0, 9.81, DateTime.UtcNow);
|
||||||
|
_cachedAngularVelocity = CreateVector3Stamped(0, 0, 0, DateTime.UtcNow);
|
||||||
|
_cachedMagnetometer = CreateVector3Stamped(0, 0, 0, DateTime.UtcNow);
|
||||||
|
_cachedOrientation = CreateVector3Stamped(0, 0, 0, DateTime.UtcNow);
|
||||||
|
_cachedQuaternion = CreateQuaternionStamped(1, 0, 0, 0, DateTime.UtcNow);
|
||||||
|
_lastUpdateTime = DateTime.UtcNow;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Reset calibration de sau khi connect lai thu thap 2 giay dau
|
||||||
|
ResetCalibrationState();
|
||||||
|
|
||||||
|
// Reset sample rate
|
||||||
|
_sampleCount = 0;
|
||||||
|
_sampleRate = 0.0;
|
||||||
|
_sampleRateStartTime = DateTime.UtcNow;
|
||||||
|
|
||||||
|
// Reconnect va bat dau calibration lai
|
||||||
|
IMU.Connect();
|
||||||
|
_calibrationCompletedTcs = new TaskCompletionSource<bool>();
|
||||||
|
IMU.DataReceived += IMU_DataReceived;
|
||||||
|
IMU.ConnectionStateChanged += IMU_ConnectionStateChanged;
|
||||||
|
|
||||||
|
// Doi calibration hoan tat
|
||||||
|
try
|
||||||
|
{
|
||||||
|
await Task.WhenAny(
|
||||||
|
_calibrationCompletedTcs.Task,
|
||||||
|
Task.Delay(CalibrationWaitTimeout, cancellationToken)).ConfigureAwait(false);
|
||||||
|
}
|
||||||
|
catch (OperationCanceledException)
|
||||||
|
{
|
||||||
|
// cancellationToken bi huy
|
||||||
|
}
|
||||||
|
_calibrationCompletedTcs = null;
|
||||||
|
UpdateProperties();
|
||||||
|
}
|
||||||
|
|
||||||
|
protected override Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
|
||||||
|
{
|
||||||
|
// Port mo CHUA du de coi la connected: sau khi cam lai USB, port co the mo
|
||||||
|
// nhung khong co du lieu (ModemManager giu port) -> yeu cau co frame gan day
|
||||||
|
return Task.FromResult(IMU.IsConnected && IMU.HasRecentData);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Reader tu reconnect ben trong; bao cho DeviceBase de state machine / UI
|
||||||
|
/// cap nhat dung trang thai (Connected/Disconnected/Reconnecting)
|
||||||
|
/// </summary>
|
||||||
|
private void IMU_ConnectionStateChanged(object? sender, bool connected)
|
||||||
|
{
|
||||||
|
_ = CheckConnectionAsync();
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Reset toan bo trang thai calibration ve gia tri ban dau
|
||||||
|
/// </summary>
|
||||||
|
private void ResetCalibrationState()
|
||||||
|
{
|
||||||
|
_calibrationStartUtc = null;
|
||||||
|
_calibrationDone = false;
|
||||||
|
_isCalibrated = false;
|
||||||
|
_calibrationSumAccX = _calibrationSumAccY = _calibrationSumAccZ = 0;
|
||||||
|
_calibrationSumGx = _calibrationSumGy = _calibrationSumGz = 0;
|
||||||
|
_calibrationSumRoll = _calibrationSumPitch = _calibrationSumYaw = 0;
|
||||||
|
_calibrationCount = 0;
|
||||||
|
_accBiasX = _accBiasY = _accBiasZ = 0;
|
||||||
|
_gyroBiasX = _gyroBiasY = _gyroBiasZ = 0;
|
||||||
|
_orientationBiasRoll = _orientationBiasPitch = _orientationBiasYaw = 0;
|
||||||
|
_integratedYaw = 0;
|
||||||
|
_lastIntegrationTime = DateTime.MinValue;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Event handler cho DataReceived tu HfiA9Reader.
|
||||||
|
/// 2 giay dau ke tu lan nhan du lieu dau tien: chi thu thap mau de tinh bias (robot dung yen tuyet doi).
|
||||||
|
/// Sau 2 giay: ap dung bias de calibrate, roi moi cap nhat _cached*, fire events, _sampleCount.
|
||||||
|
/// </summary>
|
||||||
|
private void IMU_DataReceived(object? sender, EventArgs e)
|
||||||
|
{
|
||||||
|
try
|
||||||
|
{
|
||||||
|
var snapshot = IMU.GetSnapshot();
|
||||||
|
// Use high-precision timer instead of DateTime.UtcNow (which has ~10-15ms precision)
|
||||||
|
var timestamp = _timerStartUtc + _highPrecisionTimer.Elapsed;
|
||||||
|
|
||||||
|
// Bat dau cua so calibration khi nhan du lieu lan dau
|
||||||
|
if (_calibrationStartUtc == null)
|
||||||
|
{
|
||||||
|
_calibrationStartUtc = timestamp;
|
||||||
|
_calibrationSumAccX = _calibrationSumAccY = _calibrationSumAccZ = 0;
|
||||||
|
_calibrationSumGx = _calibrationSumGy = _calibrationSumGz = 0;
|
||||||
|
_calibrationSumRoll = _calibrationSumPitch = _calibrationSumYaw = 0;
|
||||||
|
_calibrationCount = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!_calibrationDone)
|
||||||
|
{
|
||||||
|
var calElapsed = timestamp - _calibrationStartUtc.Value;
|
||||||
|
if (calElapsed < CalibrationDuration)
|
||||||
|
{
|
||||||
|
// Trong 2 giay dau: chi tich luy mau, khong cap nhat cache / fire events / sampleCount
|
||||||
|
_calibrationSumAccX += snapshot.AccX;
|
||||||
|
_calibrationSumAccY += snapshot.AccY;
|
||||||
|
_calibrationSumAccZ += snapshot.AccZ;
|
||||||
|
_calibrationSumGx += snapshot.Gx;
|
||||||
|
_calibrationSumGy += snapshot.Gy;
|
||||||
|
_calibrationSumGz += snapshot.Gz;
|
||||||
|
_calibrationSumRoll += snapshot.Roll;
|
||||||
|
_calibrationSumPitch += snapshot.Pitch;
|
||||||
|
_calibrationSumYaw += snapshot.Yaw;
|
||||||
|
_calibrationCount++;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Het 2 giay: tinh bias va danh dau da calibrate
|
||||||
|
if (_calibrationCount > 0)
|
||||||
|
{
|
||||||
|
double n = _calibrationCount;
|
||||||
|
double meanAccX = _calibrationSumAccX / n;
|
||||||
|
double meanAccY = _calibrationSumAccY / n;
|
||||||
|
double meanAccZ = _calibrationSumAccZ / n;
|
||||||
|
|
||||||
|
// Validate gravity magnitude — neu lech qua xa 9.81 thi robot bi rung/di chuyen
|
||||||
|
double gravityMagnitude = Math.Sqrt(meanAccX * meanAccX + meanAccY * meanAccY + meanAccZ * meanAccZ);
|
||||||
|
if (Math.Abs(gravityMagnitude - GravityMps2) > GravityToleranceMps2)
|
||||||
|
{
|
||||||
|
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9IMU] Calibration REJECTED: " +
|
||||||
|
$"GravityMag={gravityMagnitude:F4} (expected ~{GravityMps2}, tolerance ±{GravityToleranceMps2}). " +
|
||||||
|
$"Robot co the dang rung/di chuyen. Thu lai...");
|
||||||
|
_calibrationStartUtc = null;
|
||||||
|
_calibrationCount = 0;
|
||||||
|
_calibrationSumAccX = _calibrationSumAccY = _calibrationSumAccZ = 0;
|
||||||
|
_calibrationSumGx = _calibrationSumGy = _calibrationSumGz = 0;
|
||||||
|
_calibrationSumRoll = _calibrationSumPitch = _calibrationSumYaw = 0;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Khong tru gravity khoi acceleration data!
|
||||||
|
// ImuTracker trong Cartographer CAN gravity de estimate orientation.
|
||||||
|
// Khi dung yen (Z up): acc ~ (0, 0, +9.81) — day la luc phan ung tu mat dat
|
||||||
|
//
|
||||||
|
// Chi calibrate bias nho (sensor offset) cho X va Y.
|
||||||
|
// Voi Z: tinh bias = meanAccZ - expected_gravity
|
||||||
|
double expectedGravityZ = meanAccZ > 0 ? GravityMps2 : -GravityMps2;
|
||||||
|
|
||||||
|
// Bias X,Y: offset khi dung yen (nen ~ 0 neu robot dat phang)
|
||||||
|
_accBiasX = meanAccX;
|
||||||
|
_accBiasY = meanAccY;
|
||||||
|
// Bias Z: chi tru phan offset, GIU NGUYEN gravity
|
||||||
|
_accBiasZ = meanAccZ - expectedGravityZ;
|
||||||
|
|
||||||
|
// Gyro bias: dung — khi dung yen angular velocity = 0
|
||||||
|
_gyroBiasX = _calibrationSumGx / n;
|
||||||
|
_gyroBiasY = _calibrationSumGy / n;
|
||||||
|
_gyroBiasZ = _calibrationSumGz / n;
|
||||||
|
|
||||||
|
// Orientation bias: giu lai cho display purposes
|
||||||
|
_orientationBiasRoll = _calibrationSumRoll / n;
|
||||||
|
_orientationBiasPitch = _calibrationSumPitch / n;
|
||||||
|
_orientationBiasYaw = _calibrationSumYaw / n;
|
||||||
|
|
||||||
|
_isCalibrated = true;
|
||||||
|
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9IMU] Calibration done (n={_calibrationCount}): " +
|
||||||
|
$"MeanAcc=({meanAccX:F4},{meanAccY:F4},{meanAccZ:F4}), GravityMag={gravityMagnitude:F4}, " +
|
||||||
|
$"AccBias=({_accBiasX:F4},{_accBiasY:F4},{_accBiasZ:F4}), " +
|
||||||
|
$"GyroBias=({_gyroBiasX:F6},{_gyroBiasY:F6},{_gyroBiasZ:F6})");
|
||||||
|
}
|
||||||
|
_calibrationDone = true;
|
||||||
|
_calibrationCompletedTcs?.TrySetResult(true);
|
||||||
|
_calibrationCompletedTcs = null;
|
||||||
|
// Khoi tao yaw integration tu thoi diem calibration xong
|
||||||
|
_integratedYaw = 0;
|
||||||
|
_lastIntegrationTime = timestamp;
|
||||||
|
// Bat dau dem sample rate tu sau calibration
|
||||||
|
_sampleRateStartTime = timestamp;
|
||||||
|
_sampleCount = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Ap dung bias: du lieu da calibrate (sau 2 giay moi chay toi day)
|
||||||
|
double accX = snapshot.AccX - _accBiasX;
|
||||||
|
double accY = snapshot.AccY - _accBiasY;
|
||||||
|
double accZ = snapshot.AccZ - _accBiasZ;
|
||||||
|
double gx = snapshot.Gx - _gyroBiasX;
|
||||||
|
double gy = snapshot.Gy - _gyroBiasY;
|
||||||
|
double gz = snapshot.Gz - _gyroBiasZ;
|
||||||
|
double roll = snapshot.Roll - _orientationBiasRoll;
|
||||||
|
double pitch = snapshot.Pitch - _orientationBiasPitch;
|
||||||
|
|
||||||
|
// Tich phan CalibratedGz de tinh Yaw thay vi dung firmware AHRS Yaw
|
||||||
|
double yaw;
|
||||||
|
if (_lastIntegrationTime != DateTime.MinValue)
|
||||||
|
{
|
||||||
|
double dt = (timestamp - _lastIntegrationTime).TotalSeconds;
|
||||||
|
_integratedYaw += gz * dt;
|
||||||
|
yaw = _integratedYaw;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
yaw = 0;
|
||||||
|
}
|
||||||
|
_lastIntegrationTime = timestamp;
|
||||||
|
|
||||||
|
// Diagnostic log moi 5 giay: theo doi drift
|
||||||
|
if (timestamp - _lastDiagnosticLog >= DiagnosticLogInterval)
|
||||||
|
{
|
||||||
|
_lastDiagnosticLog = timestamp;
|
||||||
|
var elapsedSec = (timestamp - _timerStartUtc).TotalSeconds;
|
||||||
|
double firmwareYaw = snapshot.Yaw - _orientationBiasYaw;
|
||||||
|
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [IMU_DIAG] t={elapsedSec:F1}s | " +
|
||||||
|
$"CalibratedGz={gz:F6} | " +
|
||||||
|
$"IntegratedYaw={yaw:F6} FirmwareYaw={firmwareYaw:F6}");
|
||||||
|
}
|
||||||
|
|
||||||
|
// Outlier validation: reject data with unrealistic values
|
||||||
|
// This prevents ImuTracker corruption from EMI spikes or communication errors
|
||||||
|
double accMagnitude = Math.Sqrt(accX * accX + accY * accY + accZ * accZ);
|
||||||
|
double gyroMagnitude = Math.Sqrt(gx * gx + gy * gy + gz * gz);
|
||||||
|
if (accMagnitude > MaxValidAccelerationMps2 || gyroMagnitude > MaxValidAngularVelocityRadS)
|
||||||
|
{
|
||||||
|
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9IMU] OUTLIER REJECTED: " +
|
||||||
|
$"accMag={accMagnitude:F2} m/s² (max={MaxValidAccelerationMps2}), " +
|
||||||
|
$"gyroMag={gyroMagnitude:F2} rad/s (max={MaxValidAngularVelocityRadS})");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Quaternion tu goc Euler da calibrate
|
||||||
|
var cosRoll = Math.Cos(roll / 2);
|
||||||
|
var sinRoll = Math.Sin(roll / 2);
|
||||||
|
var cosPitch = Math.Cos(pitch / 2);
|
||||||
|
var sinPitch = Math.Sin(pitch / 2);
|
||||||
|
var cosYaw = Math.Cos(yaw / 2);
|
||||||
|
var sinYaw = Math.Sin(yaw / 2);
|
||||||
|
var qw = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
|
||||||
|
var qx = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
|
||||||
|
var qy = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
|
||||||
|
var qz = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
|
||||||
|
|
||||||
|
var newAcceleration = CreateAccelStamped(accX, accY, accZ, timestamp);
|
||||||
|
var newAngularVelocity = CreateVector3Stamped(gx, gy, gz, timestamp);
|
||||||
|
var newMagnetometer = CreateVector3Stamped(snapshot.MagX, snapshot.MagY, snapshot.MagZ, timestamp);
|
||||||
|
var newOrientation = CreateVector3Stamped(roll, pitch, yaw, timestamp);
|
||||||
|
var newQuaternion = CreateQuaternionStamped(qw, qx, qy, qz, timestamp);
|
||||||
|
|
||||||
|
AccelStamped previousAcceleration;
|
||||||
|
Vector3Stamped previousOrientation;
|
||||||
|
|
||||||
|
// Thread-safe update of cached data using lock
|
||||||
|
lock (_dataLock)
|
||||||
|
{
|
||||||
|
previousAcceleration = _cachedAcceleration;
|
||||||
|
previousOrientation = _cachedOrientation;
|
||||||
|
|
||||||
|
_cachedAcceleration = newAcceleration;
|
||||||
|
_cachedAngularVelocity = newAngularVelocity;
|
||||||
|
_cachedMagnetometer = newMagnetometer;
|
||||||
|
_cachedOrientation = newOrientation;
|
||||||
|
_cachedQuaternion = newQuaternion;
|
||||||
|
_lastUpdateTime = timestamp;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Fire unified event (SensorPipeline)
|
||||||
|
ImuDataChanged?.Invoke(this, new ImuDataChangedEventArgs(
|
||||||
|
newAcceleration,
|
||||||
|
newAngularVelocity,
|
||||||
|
newMagnetometer,
|
||||||
|
newOrientation,
|
||||||
|
timestamp));
|
||||||
|
|
||||||
|
// Fire individual events (interface — XlocIntegrationService, etc.)
|
||||||
|
if (Math.Abs(previousAcceleration.Accel.Linear.X - newAcceleration.Accel.Linear.X) > 0.1 ||
|
||||||
|
Math.Abs(previousAcceleration.Accel.Linear.Y - newAcceleration.Accel.Linear.Y) > 0.1 ||
|
||||||
|
Math.Abs(previousAcceleration.Accel.Linear.Z - newAcceleration.Accel.Linear.Z) > 0.1)
|
||||||
|
{
|
||||||
|
AccelerationChanged?.Invoke(this, new AccelerationChangedEventArgs(newAcceleration));
|
||||||
|
}
|
||||||
|
|
||||||
|
// Xloc requires a continuous IMU stream even when the robot is stationary.
|
||||||
|
// Emit angular velocity updates every sample instead of threshold-based changes.
|
||||||
|
AngularVelocityChanged?.Invoke(this, new AngularVelocityChangedEventArgs(newAngularVelocity));
|
||||||
|
|
||||||
|
MagnetometerChanged?.Invoke(this, new MagnetometerChangedEventArgs(newMagnetometer));
|
||||||
|
|
||||||
|
if (Math.Abs(previousOrientation.Vector.X - newOrientation.Vector.X) > 0.01 ||
|
||||||
|
Math.Abs(previousOrientation.Vector.Y - newOrientation.Vector.Y) > 0.01 ||
|
||||||
|
Math.Abs(previousOrientation.Vector.Z - newOrientation.Vector.Z) > 0.01)
|
||||||
|
{
|
||||||
|
OrientationChanged?.Invoke(this, new OrientationChangedEventArgs(newOrientation));
|
||||||
|
}
|
||||||
|
|
||||||
|
_sampleCount++;
|
||||||
|
var elapsed = timestamp - _sampleRateStartTime;
|
||||||
|
if (elapsed >= _sampleRateWindow)
|
||||||
|
{
|
||||||
|
_ = Task.Run(() =>
|
||||||
|
{
|
||||||
|
_sampleRate = _sampleCount / elapsed.TotalSeconds;
|
||||||
|
_sampleCount = 0;
|
||||||
|
_sampleRateStartTime = timestamp;
|
||||||
|
UpdateProperties();
|
||||||
|
});
|
||||||
|
}
|
||||||
|
}
|
||||||
|
catch (Exception ex)
|
||||||
|
{
|
||||||
|
LastError = ex;
|
||||||
|
OnErrorOccurred(ex, "Error updating data from IMU");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Cap nhat properties hien thi
|
||||||
|
/// </summary>
|
||||||
|
private void UpdateProperties()
|
||||||
|
{
|
||||||
|
var accel = _cachedAcceleration;
|
||||||
|
var angularVel = _cachedAngularVelocity;
|
||||||
|
var orientation = _cachedOrientation;
|
||||||
|
var sampleRate = _sampleRate;
|
||||||
|
var isCalibrated = _isCalibrated;
|
||||||
|
|
||||||
|
SetProperty("IsCalibrated", isCalibrated.ToString());
|
||||||
|
SetProperty("SampleRate", sampleRate.ToString("F1"));
|
||||||
|
SetProperty("Acceleration", $"{accel.Accel.Linear.X:F2}, {accel.Accel.Linear.Y:F2}, {accel.Accel.Linear.Z:F2}");
|
||||||
|
SetProperty("AngularVelocity", $"{angularVel.Vector.X:F3}, {angularVel.Vector.Y:F3}, {angularVel.Vector.Z:F3}");
|
||||||
|
SetProperty("Orientation", $"{orientation.Vector.X:F3}, {orientation.Vector.Y:F3}, {orientation.Vector.Z:F3}");
|
||||||
|
}
|
||||||
|
|
||||||
|
private static AccelStamped CreateAccelStamped(double x, double y, double z, DateTime timestamp)
|
||||||
|
{
|
||||||
|
return new AccelStamped
|
||||||
|
{
|
||||||
|
Header = new Header
|
||||||
|
{
|
||||||
|
Stamp = timestamp,
|
||||||
|
FrameId = "imu_frame"
|
||||||
|
},
|
||||||
|
Accel = new Accel
|
||||||
|
{
|
||||||
|
Linear = new Vector3(x, y, z),
|
||||||
|
Angular = new Vector3(0, 0, 0)
|
||||||
|
}
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
private static Vector3Stamped CreateVector3Stamped(double x, double y, double z, DateTime timestamp)
|
||||||
|
{
|
||||||
|
return new Vector3Stamped
|
||||||
|
{
|
||||||
|
Header = new Header
|
||||||
|
{
|
||||||
|
Stamp = timestamp,
|
||||||
|
FrameId = "imu_frame"
|
||||||
|
},
|
||||||
|
Vector = new Vector3(x, y, z)
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
private static QuaternionStamped CreateQuaternionStamped(double w, double x, double y, double z, DateTime timestamp)
|
||||||
|
{
|
||||||
|
return new QuaternionStamped
|
||||||
|
{
|
||||||
|
Header = new Header
|
||||||
|
{
|
||||||
|
Stamp = timestamp,
|
||||||
|
FrameId = "imu_frame"
|
||||||
|
},
|
||||||
|
Quaternion = new Quaternion(x, y, z, w)
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
#region IInertialMeasurementUnit Implementation
|
||||||
|
|
||||||
|
bool IInertialMeasurementUnit.IsConnected => base.IsConnected;
|
||||||
|
|
||||||
|
public bool IsCalibrated
|
||||||
|
{
|
||||||
|
get { return _isCalibrated; }
|
||||||
|
}
|
||||||
|
|
||||||
|
public double SampleRate
|
||||||
|
{
|
||||||
|
get
|
||||||
|
{
|
||||||
|
Thread.MemoryBarrier();
|
||||||
|
return _sampleRate;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public AccelStamped CachedAcceleration
|
||||||
|
{
|
||||||
|
get { lock (_dataLock) { return _cachedAcceleration; } }
|
||||||
|
}
|
||||||
|
|
||||||
|
public Vector3Stamped CachedAngularVelocity
|
||||||
|
{
|
||||||
|
get { lock (_dataLock) { return _cachedAngularVelocity; } }
|
||||||
|
}
|
||||||
|
|
||||||
|
public Vector3Stamped? CachedMagnetometer
|
||||||
|
{
|
||||||
|
get { lock (_dataLock) { return _cachedMagnetometer; } }
|
||||||
|
}
|
||||||
|
|
||||||
|
public Vector3Stamped CachedOrientation
|
||||||
|
{
|
||||||
|
get { lock (_dataLock) { return _cachedOrientation; } }
|
||||||
|
}
|
||||||
|
|
||||||
|
public QuaternionStamped? CachedQuaternion
|
||||||
|
{
|
||||||
|
get { lock (_dataLock) { return _cachedQuaternion; } }
|
||||||
|
}
|
||||||
|
|
||||||
|
public double? CachedTemperature
|
||||||
|
{
|
||||||
|
// HFI-A9 khong co cam bien nhiet do
|
||||||
|
get { return null; }
|
||||||
|
}
|
||||||
|
|
||||||
|
DateTime IInertialMeasurementUnit.LastUpdateTime
|
||||||
|
{
|
||||||
|
get { lock (_dataLock) { return _lastUpdateTime; } }
|
||||||
|
}
|
||||||
|
|
||||||
|
public async Task<AccelStamped> ReadAccelerationAsync(CancellationToken cancellationToken = default)
|
||||||
|
{
|
||||||
|
await Task.CompletedTask;
|
||||||
|
return CachedAcceleration;
|
||||||
|
}
|
||||||
|
|
||||||
|
public async Task<Vector3Stamped> ReadAngularVelocityAsync(CancellationToken cancellationToken = default)
|
||||||
|
{
|
||||||
|
await Task.CompletedTask;
|
||||||
|
return CachedAngularVelocity;
|
||||||
|
}
|
||||||
|
|
||||||
|
public async Task<Vector3Stamped?> ReadMagnetometerAsync(CancellationToken cancellationToken = default)
|
||||||
|
{
|
||||||
|
await Task.CompletedTask;
|
||||||
|
return CachedMagnetometer;
|
||||||
|
}
|
||||||
|
|
||||||
|
public async Task<Vector3Stamped> ReadOrientationAsync(CancellationToken cancellationToken = default)
|
||||||
|
{
|
||||||
|
await Task.CompletedTask;
|
||||||
|
return CachedOrientation;
|
||||||
|
}
|
||||||
|
|
||||||
|
public async Task<QuaternionStamped?> ReadQuaternionAsync(CancellationToken cancellationToken = default)
|
||||||
|
{
|
||||||
|
await Task.CompletedTask;
|
||||||
|
return CachedQuaternion;
|
||||||
|
}
|
||||||
|
|
||||||
|
public async Task<Imu> ReadAllDataAsync(CancellationToken cancellationToken = default)
|
||||||
|
{
|
||||||
|
await Task.CompletedTask;
|
||||||
|
lock (_dataLock)
|
||||||
|
{
|
||||||
|
return CreateImuFromCachedData();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private Imu CreateImuFromCachedData()
|
||||||
|
{
|
||||||
|
var timestamp = _lastUpdateTime != default ? _lastUpdateTime : DateTime.UtcNow;
|
||||||
|
|
||||||
|
var orientation = _cachedQuaternion.Quaternion;
|
||||||
|
|
||||||
|
var orientationCovariance = new double[Imu.OrientationCovarianceSize];
|
||||||
|
var angularVelocityCovariance = new double[Imu.AngularVelocityCovarianceSize];
|
||||||
|
var linearAccelerationCovariance = new double[Imu.LinearAccelerationCovarianceSize];
|
||||||
|
|
||||||
|
double gyroVariance = 1e-4;
|
||||||
|
angularVelocityCovariance[0] = gyroVariance;
|
||||||
|
angularVelocityCovariance[4] = gyroVariance;
|
||||||
|
angularVelocityCovariance[8] = gyroVariance;
|
||||||
|
|
||||||
|
double accelVariance = 1e-3;
|
||||||
|
linearAccelerationCovariance[0] = accelVariance;
|
||||||
|
linearAccelerationCovariance[4] = accelVariance;
|
||||||
|
linearAccelerationCovariance[8] = accelVariance;
|
||||||
|
|
||||||
|
orientationCovariance[0] = 0.001;
|
||||||
|
orientationCovariance[4] = 0.001;
|
||||||
|
orientationCovariance[8] = 0.002;
|
||||||
|
|
||||||
|
return new Imu(
|
||||||
|
header: new Header
|
||||||
|
{
|
||||||
|
Stamp = timestamp,
|
||||||
|
FrameId = "imu_frame"
|
||||||
|
},
|
||||||
|
orientation: orientation,
|
||||||
|
orientationCovariance: orientationCovariance,
|
||||||
|
angularVelocity: _cachedAngularVelocity.Vector,
|
||||||
|
angularVelocityCovariance: angularVelocityCovariance,
|
||||||
|
linearAcceleration: _cachedAcceleration.Accel.Linear,
|
||||||
|
linearAccelerationCovariance: linearAccelerationCovariance
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
public async Task<double?> ReadTemperatureAsync(CancellationToken cancellationToken = default)
|
||||||
|
{
|
||||||
|
await Task.CompletedTask;
|
||||||
|
return CachedTemperature;
|
||||||
|
}
|
||||||
|
|
||||||
|
public async Task CalibrateAsync(CancellationToken cancellationToken = default)
|
||||||
|
{
|
||||||
|
// Huy event, reset calibration, dang ky lai event va doi calib
|
||||||
|
IMU.DataReceived -= IMU_DataReceived;
|
||||||
|
ResetCalibrationState();
|
||||||
|
|
||||||
|
_timerStartUtc = DateTime.UtcNow;
|
||||||
|
_highPrecisionTimer.Restart();
|
||||||
|
|
||||||
|
_calibrationCompletedTcs = new TaskCompletionSource<bool>();
|
||||||
|
IMU.DataReceived += IMU_DataReceived;
|
||||||
|
|
||||||
|
try
|
||||||
|
{
|
||||||
|
await Task.WhenAny(
|
||||||
|
_calibrationCompletedTcs.Task,
|
||||||
|
Task.Delay(CalibrationWaitTimeout, cancellationToken)).ConfigureAwait(false);
|
||||||
|
}
|
||||||
|
catch (OperationCanceledException)
|
||||||
|
{
|
||||||
|
// cancellationToken bi huy
|
||||||
|
}
|
||||||
|
_calibrationCompletedTcs = null;
|
||||||
|
UpdateProperties();
|
||||||
|
}
|
||||||
|
|
||||||
|
public async Task CalibrateMagnetometerAsync(CancellationToken cancellationToken = default)
|
||||||
|
{
|
||||||
|
await Task.Delay(2000, cancellationToken);
|
||||||
|
}
|
||||||
|
|
||||||
|
public async Task ResetCalibrationAsync(CancellationToken cancellationToken = default)
|
||||||
|
{
|
||||||
|
// Huy event, reset state, dang ky lai va doi calib moi
|
||||||
|
IMU.DataReceived -= IMU_DataReceived;
|
||||||
|
ResetCalibrationState();
|
||||||
|
|
||||||
|
_timerStartUtc = DateTime.UtcNow;
|
||||||
|
_highPrecisionTimer.Restart();
|
||||||
|
|
||||||
|
_calibrationCompletedTcs = new TaskCompletionSource<bool>();
|
||||||
|
IMU.DataReceived += IMU_DataReceived;
|
||||||
|
|
||||||
|
try
|
||||||
|
{
|
||||||
|
await Task.WhenAny(
|
||||||
|
_calibrationCompletedTcs.Task,
|
||||||
|
Task.Delay(CalibrationWaitTimeout, cancellationToken)).ConfigureAwait(false);
|
||||||
|
}
|
||||||
|
catch (OperationCanceledException)
|
||||||
|
{
|
||||||
|
// cancellationToken bi huy
|
||||||
|
}
|
||||||
|
_calibrationCompletedTcs = null;
|
||||||
|
UpdateProperties();
|
||||||
|
}
|
||||||
|
|
||||||
|
public async Task SetSampleRateAsync(double sampleRate, CancellationToken cancellationToken = default)
|
||||||
|
{
|
||||||
|
await Task.CompletedTask;
|
||||||
|
_sampleRate = Math.Max(1, Math.Min(1000, sampleRate));
|
||||||
|
UpdateProperties();
|
||||||
|
}
|
||||||
|
|
||||||
|
public async Task SetAccelerometerRangeAsync(double range, CancellationToken cancellationToken = default)
|
||||||
|
{
|
||||||
|
await Task.CompletedTask;
|
||||||
|
}
|
||||||
|
|
||||||
|
public async Task SetGyroscopeRangeAsync(double range, CancellationToken cancellationToken = default)
|
||||||
|
{
|
||||||
|
await Task.CompletedTask;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
protected override void Dispose(bool disposing)
|
||||||
|
{
|
||||||
|
if (disposing)
|
||||||
|
{
|
||||||
|
IMU.DataReceived -= IMU_DataReceived;
|
||||||
|
IMU.ConnectionStateChanged -= IMU_ConnectionStateChanged;
|
||||||
|
IMU.Disconnect();
|
||||||
|
IMU.Dispose();
|
||||||
|
_printDataTimer?.Dispose();
|
||||||
|
_printDataTimer = null;
|
||||||
|
}
|
||||||
|
base.Dispose(disposing);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -0,0 +1,503 @@
|
|||||||
|
using System;
|
||||||
|
using System.IO;
|
||||||
|
using System.IO.Ports;
|
||||||
|
using System.Threading;
|
||||||
|
|
||||||
|
namespace RobotNet10.RobotApp.Drivers.HfiA9IMU
|
||||||
|
{
|
||||||
|
/// <summary>
|
||||||
|
/// Doc du lieu tu IMU HandsFree HFI-A9 qua serial (CP210x USB-UART, 921600 baud).
|
||||||
|
/// Giao thuc: frame = 0xAA 0x55 [LEN] [payload LEN bytes] [CRC16-Modbus lo, hi]
|
||||||
|
/// CRC tinh tren [LEN] + payload.
|
||||||
|
/// LEN 0x2C: 10 float LE tai payload offset 4:
|
||||||
|
/// [0]=timestamp, [1:4]=gyro (rad/s), [4:7]=accel (g), [7:10]=mag
|
||||||
|
/// LEN 0x14: 4 float LE tai payload offset 4: [1:4]=roll/pitch/yaw (do)
|
||||||
|
/// </summary>
|
||||||
|
public class HfiA9Reader : IDisposable
|
||||||
|
{
|
||||||
|
// Frame lon nhat: 0x2C + 5 = 49 bytes, dung 256 bytes de dam bao an toan
|
||||||
|
private const int FRAME_BUFFER_SIZE = 256;
|
||||||
|
private readonly byte[] _frameBuffer = new byte[FRAME_BUFFER_SIZE];
|
||||||
|
private int _frameBufferLength = 0;
|
||||||
|
|
||||||
|
// Non-volatile field de dung voi Volatile.Write lam memory barrier
|
||||||
|
private int _memoryBarrier = 0;
|
||||||
|
|
||||||
|
// Thread doc du lieu tu SerialPort voi priority cao
|
||||||
|
private Thread? _readingThread;
|
||||||
|
private volatile bool _shouldRead = false;
|
||||||
|
private CancellationTokenSource? _readingCts;
|
||||||
|
|
||||||
|
// Event de thong bao khi co du lieu moi duoc decode (fire sau frame Euler
|
||||||
|
// de moi event tuong ung mot chu ky mau day du: gyro/accel/mag + euler)
|
||||||
|
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; }
|
||||||
|
public double Yaw { get; private set; }
|
||||||
|
|
||||||
|
public double Gx { get; private set; }
|
||||||
|
public double Gy { get; private set; }
|
||||||
|
public double Gz { get; private set; }
|
||||||
|
|
||||||
|
public double AccX { get; private set; }
|
||||||
|
public double AccY { get; private set; }
|
||||||
|
public double AccZ { get; private set; }
|
||||||
|
|
||||||
|
public double MagX { get; private set; }
|
||||||
|
public double MagY { get; private set; }
|
||||||
|
public double MagZ { get; private set; }
|
||||||
|
|
||||||
|
const byte FRAME_HEAD_1 = 0xAA;
|
||||||
|
const byte FRAME_HEAD_2 = 0x55;
|
||||||
|
|
||||||
|
// LEN byte cua tung loai goi
|
||||||
|
const byte LEN_IMU = 0x2C; // 44: timestamp + gyro + accel + mag (10 float)
|
||||||
|
const byte LEN_EULER = 0x14; // 20: timestamp + roll/pitch/yaw (4 float)
|
||||||
|
|
||||||
|
// HFI-A9 xuat accel theo don vi g; nhan -9.8 theo quy uoc truc cua driver hang
|
||||||
|
// (dung yen, Z huong len -> AccZ ~ +9.8 m/s², cung quy uoc voi Wheeltec N100)
|
||||||
|
private const double ACCEL_SCALE = -9.8;
|
||||||
|
private const double DEG_TO_RAD = Math.PI / 180.0;
|
||||||
|
|
||||||
|
private SerialPort? serial;
|
||||||
|
|
||||||
|
// Luu thong so port de tao lai SerialPort khi reconnect
|
||||||
|
private readonly string _portName;
|
||||||
|
private readonly int _baudRate;
|
||||||
|
private readonly int _timeOut;
|
||||||
|
|
||||||
|
// Thoi gian backoff giua cac lan thu reconnect (ms)
|
||||||
|
private const int RECONNECT_BACKOFF_MS = 1000;
|
||||||
|
|
||||||
|
// Watchdog: neu qua khoang thoi gian nay khong co frame hop le -> coi nhu mat ket noi
|
||||||
|
// va trigger reconnect. Dung cho truong hop USB "chet mem" khong ne'm exception.
|
||||||
|
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 HfiA9Reader(string portName, int baudRate, int timeOut)
|
||||||
|
{
|
||||||
|
_portName = portName;
|
||||||
|
_baudRate = baudRate;
|
||||||
|
_timeOut = timeOut;
|
||||||
|
|
||||||
|
serial = CreateSerialPort();
|
||||||
|
}
|
||||||
|
|
||||||
|
private SerialPort CreateSerialPort()
|
||||||
|
{
|
||||||
|
return new SerialPort()
|
||||||
|
{
|
||||||
|
PortName = _portName,
|
||||||
|
BaudRate = _baudRate,
|
||||||
|
ReadTimeout = _timeOut,
|
||||||
|
Parity = Parity.None,
|
||||||
|
StopBits = StopBits.One,
|
||||||
|
DataBits = 8,
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Snapshot structure de lay tat ca du lieu cung luc mot cach thread-safe
|
||||||
|
/// </summary>
|
||||||
|
public struct DataSnapshot
|
||||||
|
{
|
||||||
|
public double AccX, AccY, AccZ;
|
||||||
|
public double Gx, Gy, Gz;
|
||||||
|
public double MagX, MagY, MagZ;
|
||||||
|
public double Roll, Pitch, Yaw;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Lay snapshot cua tat ca du lieu hien tai mot cach thread-safe
|
||||||
|
/// </summary>
|
||||||
|
public DataSnapshot GetSnapshot()
|
||||||
|
{
|
||||||
|
Volatile.Read(ref _memoryBarrier);
|
||||||
|
return new DataSnapshot
|
||||||
|
{
|
||||||
|
AccX = AccX,
|
||||||
|
AccY = AccY,
|
||||||
|
AccZ = AccZ,
|
||||||
|
Gx = Gx,
|
||||||
|
Gy = Gy,
|
||||||
|
Gz = Gz,
|
||||||
|
MagX = MagX,
|
||||||
|
MagY = MagY,
|
||||||
|
MagZ = MagZ,
|
||||||
|
Roll = Roll,
|
||||||
|
Pitch = Pitch,
|
||||||
|
Yaw = Yaw,
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Connect()
|
||||||
|
{
|
||||||
|
_frameBufferLength = 0;
|
||||||
|
Array.Clear(_frameBuffer, 0, FRAME_BUFFER_SIZE);
|
||||||
|
|
||||||
|
// Reading thread se tu mo port trong outer loop va tu reconnect khi mat ket noi
|
||||||
|
StartReadingThread();
|
||||||
|
}
|
||||||
|
|
||||||
|
private void StartReadingThread()
|
||||||
|
{
|
||||||
|
if (_readingThread != null && _readingThread.IsAlive)
|
||||||
|
return;
|
||||||
|
|
||||||
|
_shouldRead = true;
|
||||||
|
|
||||||
|
_readingCts?.Dispose();
|
||||||
|
_readingCts = new CancellationTokenSource();
|
||||||
|
|
||||||
|
_readingThread = new Thread(() => ReadingThreadLoop(_readingCts.Token))
|
||||||
|
{
|
||||||
|
Name = "HfiA9IMU-Reading",
|
||||||
|
IsBackground = false,
|
||||||
|
Priority = ThreadPriority.Highest
|
||||||
|
};
|
||||||
|
|
||||||
|
_readingThread.Start();
|
||||||
|
}
|
||||||
|
|
||||||
|
private void StopReadingThread()
|
||||||
|
{
|
||||||
|
_shouldRead = false;
|
||||||
|
|
||||||
|
_readingCts?.Cancel();
|
||||||
|
|
||||||
|
if (_readingThread != null)
|
||||||
|
{
|
||||||
|
if (!_readingThread.Join(1000))
|
||||||
|
{
|
||||||
|
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] Reading thread did not stop gracefully");
|
||||||
|
}
|
||||||
|
_readingThread = null;
|
||||||
|
}
|
||||||
|
|
||||||
|
_readingCts?.Dispose();
|
||||||
|
_readingCts = null;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Reading thread loop - outer loop xu ly reconnect, inner loop doc du lieu
|
||||||
|
/// </summary>
|
||||||
|
private void ReadingThreadLoop(CancellationToken cancellationToken)
|
||||||
|
{
|
||||||
|
Thread.BeginThreadAffinity();
|
||||||
|
byte[] readBuffer = new byte[256];
|
||||||
|
|
||||||
|
while (_shouldRead && !cancellationToken.IsCancellationRequested)
|
||||||
|
{
|
||||||
|
try
|
||||||
|
{
|
||||||
|
if (serial == null || !serial.IsOpen)
|
||||||
|
{
|
||||||
|
SafeCloseSerial();
|
||||||
|
serial = CreateSerialPort();
|
||||||
|
serial.Open();
|
||||||
|
_frameBufferLength = 0;
|
||||||
|
serial.DiscardInBuffer();
|
||||||
|
_lastFrameTicks = DateTime.UtcNow.Ticks;
|
||||||
|
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] Connected to {_portName}");
|
||||||
|
RaiseConnectionStateChanged(true);
|
||||||
|
}
|
||||||
|
|
||||||
|
InnerReadLoop(readBuffer, cancellationToken);
|
||||||
|
}
|
||||||
|
catch (OperationCanceledException)
|
||||||
|
{
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
catch (Exception ex)
|
||||||
|
{
|
||||||
|
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] IMU disconnected: {ex.Message}");
|
||||||
|
SafeCloseSerial();
|
||||||
|
RaiseConnectionStateChanged(false);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (_shouldRead && !cancellationToken.IsCancellationRequested)
|
||||||
|
{
|
||||||
|
cancellationToken.WaitHandle.WaitOne(RECONNECT_BACKOFF_MS);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
SafeCloseSerial();
|
||||||
|
Thread.EndThreadAffinity();
|
||||||
|
}
|
||||||
|
|
||||||
|
private void InnerReadLoop(byte[] readBuffer, CancellationToken cancellationToken)
|
||||||
|
{
|
||||||
|
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)
|
||||||
|
{
|
||||||
|
Thread.Sleep(1);
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
int bytesRead = serial.Read(readBuffer, 0, Math.Min(bytesToRead, readBuffer.Length));
|
||||||
|
if (bytesRead <= 0)
|
||||||
|
{
|
||||||
|
Thread.Sleep(1);
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
ProcessIncomingData(readBuffer, bytesRead);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private void SafeCloseSerial()
|
||||||
|
{
|
||||||
|
if (serial == null) return;
|
||||||
|
|
||||||
|
try
|
||||||
|
{
|
||||||
|
serial.Close();
|
||||||
|
}
|
||||||
|
catch (Exception ex)
|
||||||
|
{
|
||||||
|
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] Error closing serial: {ex.Message}");
|
||||||
|
}
|
||||||
|
|
||||||
|
try { serial.Dispose(); }
|
||||||
|
catch { }
|
||||||
|
|
||||||
|
serial = null;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Them du lieu moi vao frame buffer va parse cac frame hoan chinh
|
||||||
|
/// </summary>
|
||||||
|
private void ProcessIncomingData(byte[] data, int length)
|
||||||
|
{
|
||||||
|
int dataOffset = 0;
|
||||||
|
|
||||||
|
while (dataOffset < length)
|
||||||
|
{
|
||||||
|
ProcessCompleteFramesInBuffer();
|
||||||
|
|
||||||
|
int availableSpace = FRAME_BUFFER_SIZE - _frameBufferLength;
|
||||||
|
|
||||||
|
if (availableSpace == 0)
|
||||||
|
{
|
||||||
|
// Buffer day ma khong co frame hop le -> du lieu rac, xoa het
|
||||||
|
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] Buffer full without valid frame, clearing buffer");
|
||||||
|
_frameBufferLength = 0;
|
||||||
|
availableSpace = FRAME_BUFFER_SIZE;
|
||||||
|
}
|
||||||
|
|
||||||
|
int bytesToAdd = Math.Min(length - dataOffset, availableSpace);
|
||||||
|
Array.Copy(data, dataOffset, _frameBuffer, _frameBufferLength, bytesToAdd);
|
||||||
|
_frameBufferLength += bytesToAdd;
|
||||||
|
dataOffset += bytesToAdd;
|
||||||
|
|
||||||
|
ProcessCompleteFramesInBuffer();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Xu ly tat ca cac frame hoan chinh trong buffer hien tai
|
||||||
|
/// </summary>
|
||||||
|
private void ProcessCompleteFramesInBuffer()
|
||||||
|
{
|
||||||
|
while (_frameBufferLength > 0)
|
||||||
|
{
|
||||||
|
ReadOnlySpan<byte> bufferSpan = new(_frameBuffer, 0, _frameBufferLength);
|
||||||
|
int headIndex = bufferSpan.IndexOf(FRAME_HEAD_1);
|
||||||
|
|
||||||
|
if (headIndex < 0)
|
||||||
|
{
|
||||||
|
_frameBufferLength = 0;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (headIndex > 0)
|
||||||
|
{
|
||||||
|
ShiftBuffer(headIndex);
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Can toi thieu 3 bytes: AA 55 LEN
|
||||||
|
if (_frameBufferLength < 3)
|
||||||
|
break;
|
||||||
|
|
||||||
|
if (_frameBuffer[1] != FRAME_HEAD_2)
|
||||||
|
{
|
||||||
|
ShiftBuffer(1);
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
byte payloadLength = _frameBuffer[2];
|
||||||
|
if (payloadLength != LEN_IMU && payloadLength != LEN_EULER)
|
||||||
|
{
|
||||||
|
ShiftBuffer(1);
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
int totalFrameLength = payloadLength + 5;
|
||||||
|
if (_frameBufferLength < totalFrameLength)
|
||||||
|
break;
|
||||||
|
|
||||||
|
// CRC16-Modbus tren [LEN + payload], luu little-endian sau payload
|
||||||
|
ushort crcCalc = Crc16Modbus(_frameBuffer.AsSpan(2, 1 + payloadLength));
|
||||||
|
ushort crcRecv = (ushort)(_frameBuffer[3 + payloadLength] | (_frameBuffer[4 + payloadLength] << 8));
|
||||||
|
if (crcCalc != crcRecv)
|
||||||
|
{
|
||||||
|
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] CRC16 error: recv={crcRecv:X4}, calc={crcCalc:X4}");
|
||||||
|
ShiftBuffer(2);
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
try
|
||||||
|
{
|
||||||
|
DecodeFrame(payloadLength, _frameBuffer.AsSpan(3, payloadLength));
|
||||||
|
_lastFrameTicks = DateTime.UtcNow.Ticks;
|
||||||
|
}
|
||||||
|
catch (Exception ex)
|
||||||
|
{
|
||||||
|
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] Error parsing frame: {ex.Message}");
|
||||||
|
}
|
||||||
|
|
||||||
|
ShiftBuffer(totalFrameLength);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private void ShiftBuffer(int count)
|
||||||
|
{
|
||||||
|
int remaining = _frameBufferLength - count;
|
||||||
|
if (remaining > 0)
|
||||||
|
{
|
||||||
|
Array.Copy(_frameBuffer, count, _frameBuffer, 0, remaining);
|
||||||
|
}
|
||||||
|
_frameBufferLength = Math.Max(0, remaining);
|
||||||
|
}
|
||||||
|
|
||||||
|
private void DecodeFrame(byte payloadLength, ReadOnlySpan<byte> payload)
|
||||||
|
{
|
||||||
|
if (payloadLength == LEN_IMU)
|
||||||
|
{
|
||||||
|
// float [0] la timestamp noi bo, bo qua
|
||||||
|
Gx = BitConverter.ToSingle(payload.Slice(8, 4));
|
||||||
|
Gy = BitConverter.ToSingle(payload.Slice(12, 4));
|
||||||
|
Gz = BitConverter.ToSingle(payload.Slice(16, 4));
|
||||||
|
AccX = BitConverter.ToSingle(payload.Slice(20, 4)) * ACCEL_SCALE;
|
||||||
|
AccY = BitConverter.ToSingle(payload.Slice(24, 4)) * ACCEL_SCALE;
|
||||||
|
AccZ = BitConverter.ToSingle(payload.Slice(28, 4)) * ACCEL_SCALE;
|
||||||
|
MagX = BitConverter.ToSingle(payload.Slice(32, 4));
|
||||||
|
MagY = BitConverter.ToSingle(payload.Slice(36, 4));
|
||||||
|
MagZ = BitConverter.ToSingle(payload.Slice(40, 4));
|
||||||
|
Volatile.Write(ref _memoryBarrier, 0);
|
||||||
|
}
|
||||||
|
else if (payloadLength == LEN_EULER)
|
||||||
|
{
|
||||||
|
Roll = BitConverter.ToSingle(payload.Slice(8, 4)) * DEG_TO_RAD;
|
||||||
|
Pitch = BitConverter.ToSingle(payload.Slice(12, 4)) * DEG_TO_RAD;
|
||||||
|
Yaw = BitConverter.ToSingle(payload.Slice(16, 4)) * DEG_TO_RAD;
|
||||||
|
Volatile.Write(ref _memoryBarrier, 0);
|
||||||
|
|
||||||
|
// Frame Euler den sau frame IMU trong moi chu ky -> fire event mot lan/chu ky
|
||||||
|
try
|
||||||
|
{
|
||||||
|
DataReceived?.Invoke(this, EventArgs.Empty);
|
||||||
|
}
|
||||||
|
catch (Exception ex)
|
||||||
|
{
|
||||||
|
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] DataReceived subscriber error: {ex.Message}");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private void RaiseConnectionStateChanged(bool connected)
|
||||||
|
{
|
||||||
|
try
|
||||||
|
{
|
||||||
|
ConnectionStateChanged?.Invoke(this, connected);
|
||||||
|
}
|
||||||
|
catch (Exception ex)
|
||||||
|
{
|
||||||
|
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] ConnectionStateChanged subscriber error: {ex.Message}");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private static ushort Crc16Modbus(ReadOnlySpan<byte> data)
|
||||||
|
{
|
||||||
|
ushort crc = 0xFFFF;
|
||||||
|
foreach (byte b in data)
|
||||||
|
{
|
||||||
|
crc ^= b;
|
||||||
|
for (int i = 0; i < 8; i++)
|
||||||
|
{
|
||||||
|
if ((crc & 1) != 0)
|
||||||
|
crc = (ushort)((crc >> 1) ^ 0xA001);
|
||||||
|
else
|
||||||
|
crc >>= 1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return crc;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Disconnect()
|
||||||
|
{
|
||||||
|
StopReadingThread();
|
||||||
|
|
||||||
|
SafeCloseSerial();
|
||||||
|
|
||||||
|
_frameBufferLength = 0;
|
||||||
|
Array.Clear(_frameBuffer, 0, FRAME_BUFFER_SIZE);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Dispose()
|
||||||
|
{
|
||||||
|
Dispose(true);
|
||||||
|
GC.SuppressFinalize(this);
|
||||||
|
}
|
||||||
|
|
||||||
|
protected virtual void Dispose(bool disposing)
|
||||||
|
{
|
||||||
|
if (disposing)
|
||||||
|
{
|
||||||
|
Disconnect();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -0,0 +1,867 @@
|
|||||||
|
using System.Diagnostics;
|
||||||
|
using System.Net.Sockets;
|
||||||
|
using RobotNet10.RobotApp.Client.Shared.Devices;
|
||||||
|
using RobotNet10.RobotApp.Devices;
|
||||||
|
using RobotNet10.Shared;
|
||||||
|
using RobotNet10.Shared.Sensor;
|
||||||
|
|
||||||
|
namespace RobotNet10.RobotApp.Drivers.Hinson;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Configuration cho Hinson FE-35 LiDAR Driver
|
||||||
|
/// </summary>
|
||||||
|
public class HinsonFE35LidarDriverConfig
|
||||||
|
{
|
||||||
|
/// <summary>
|
||||||
|
/// Địa chỉ IP của LiDAR - mặc định: 192.168.1.88 (theo tài liệu Hinson FE)
|
||||||
|
/// </summary>
|
||||||
|
public string IpAddress { get; set; } = "192.168.1.88";
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Port của LiDAR - mặc định: 8080
|
||||||
|
/// </summary>
|
||||||
|
public int Port { get; set; } = 8080;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Sử dụng UDP thay vì TCP - mặc định: false (TCP)
|
||||||
|
/// </summary>
|
||||||
|
public bool UseUdp { get; set; } = false;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Frame ID cho scan data (ROS-style message headers)
|
||||||
|
/// </summary>
|
||||||
|
public string FrameId { get; set; } = "laser";
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Tầm quét tối thiểu (mét) - mặc định: 0.05 m
|
||||||
|
/// </summary>
|
||||||
|
public double MinRangeM { get; set; } = 0.05;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Tầm quét tối đa (mét) - mặc định: 35.0 m (FE-35FB)
|
||||||
|
/// </summary>
|
||||||
|
public double MaxRangeM { get; set; } = 35.0;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Góc offset (độ) cộng thêm vào dữ liệu scan.
|
||||||
|
/// Lưu ý: góc 0° của LiDAR Hinson FE là hướng chính sau, chiều dương ngược kim đồng hồ.
|
||||||
|
/// </summary>
|
||||||
|
public double AngleOffsetDeg { get; set; } = 0.0;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// LiDAR gắn ngược (upside down) - đảo góc quét (180° - angle) như Olei driver
|
||||||
|
/// </summary>
|
||||||
|
public bool Inverted { get; set; } = false;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Timeout không nhận được dữ liệu thì coi như mất kết nối (milliseconds)
|
||||||
|
/// </summary>
|
||||||
|
public int DataTimeoutMs { get; set; } = 2000;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Có gửi lệnh cấu hình tham số ("SCtrl") xuống LiDAR khi kết nối hay không.
|
||||||
|
/// false: giữ nguyên cấu hình hiện tại trong LiDAR
|
||||||
|
/// </summary>
|
||||||
|
public bool ChangeParam { get; set; } = false;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Tần số quay (Hz) - hợp lệ: 12 (12.5Hz), 25, 50. Chỉ dùng khi ChangeParam = true
|
||||||
|
/// </summary>
|
||||||
|
public int SpinFrequencyHz { get; set; } = 25;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Độ phân giải góc (độ) - hợp lệ: "0.025", "0.050", "0.100", "0.200", "0.250", "0.500".
|
||||||
|
/// Chỉ dùng khi ChangeParam = true
|
||||||
|
/// </summary>
|
||||||
|
public string AngleIncrementDeg { get; set; } = "0.100";
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Mức lọc nhiễu 0~3 - chỉ dùng khi ChangeParam = true
|
||||||
|
/// </summary>
|
||||||
|
public int NoiseFilterLevel { get; set; } = 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Driver cho Hinson (兴颂/HINS) FE-35FB-01000 2D LiDAR
|
||||||
|
/// Implements DeviceBase và ILidar interface
|
||||||
|
/// Protocol: TCP/UDP - tham khảo Hinson_FE_ROS_driver_v1.2 và Hinson_FE35使用手册V1.0
|
||||||
|
///
|
||||||
|
/// Luồng hoạt động:
|
||||||
|
/// 1. Kết nối TCP (hoặc UDP) tới LiDAR (mặc định 192.168.1.88:8080)
|
||||||
|
/// 2. (Tuỳ chọn) Gửi lệnh cấu hình "SCtrl" (12 byte, CRC16-Modbus)
|
||||||
|
/// 3. Gửi lệnh bắt đầu đo "RAuto" + 0x01 0x87 0x80 (8 byte)
|
||||||
|
/// 4. Nhận các frame dữ liệu header "HISN":
|
||||||
|
/// - Header 16 byte: [0..3]="HISN", các trường uint16 big-endian:
|
||||||
|
/// start_angle, end_angle (độ), data_size, data_position, measure_size, time
|
||||||
|
/// - Body: data_size điểm × 4 byte little-endian (distance mm, intensity)
|
||||||
|
/// 5. Ghép các frame thành vòng quét 360° hoàn chỉnh khi
|
||||||
|
/// end_angle == 360 và data_position == measure_size
|
||||||
|
/// </summary>
|
||||||
|
[Device(DeviceType.Lidar, "Hinson", "HinsonFE35LidarDriver", "1.0.0",
|
||||||
|
Description = "Hinson FE-35FB-01000 2D LiDAR - TCP/UDP Protocol")]
|
||||||
|
public class HinsonFE35LidarDriver : DeviceBase, ILidar
|
||||||
|
{
|
||||||
|
private readonly HinsonFE35LidarDriverConfig _config = new();
|
||||||
|
|
||||||
|
// Lệnh bắt đầu đo: "RAuto" + 0x01 + CRC (theo hins::kStartCapture)
|
||||||
|
private static readonly byte[] StartCaptureCommand = [0x52, 0x41, 0x75, 0x74, 0x6F, 0x01, 0x87, 0x80];
|
||||||
|
|
||||||
|
// Frame header dữ liệu quét: "HISN"
|
||||||
|
private static readonly byte[] RangeFrameHead = [0x48, 0x49, 0x53, 0x4E];
|
||||||
|
|
||||||
|
// Frame header dữ liệu vùng an toàn (area/obstacle): "WSimu" - 13 byte, bỏ qua
|
||||||
|
private static readonly byte[] AreaFrameHead = [0x57, 0x53, 0x69, 0x6D, 0x75];
|
||||||
|
|
||||||
|
private const int RANGE_FRAME_HEADER_SIZE = 16;
|
||||||
|
private const int AREA_FRAME_SIZE = 13;
|
||||||
|
private const int BYTES_PER_POINT = 4;
|
||||||
|
|
||||||
|
// Giá trị distance (mm) lớn hơn ngưỡng này là không hợp lệ (theo hins::kMaxDistance)
|
||||||
|
private const int MAX_DISTANCE_RAW_MM = 50000;
|
||||||
|
private const double DEFAULT_ACCURACY_M = 0.03; // ±30 mm theo datasheet FE series
|
||||||
|
|
||||||
|
// Connection
|
||||||
|
private TcpClient? _tcpClient;
|
||||||
|
private NetworkStream? _tcpStream;
|
||||||
|
private UdpClient? _udpClient;
|
||||||
|
private CancellationTokenSource? _receiveCts;
|
||||||
|
private Task? _receiveTask;
|
||||||
|
|
||||||
|
// Receive buffer (dồn dữ liệu TCP stream, tách frame)
|
||||||
|
private readonly byte[] _rxBuffer = new byte[131072];
|
||||||
|
private int _rxLength;
|
||||||
|
|
||||||
|
// Scan accumulation (một vòng 360°)
|
||||||
|
private double[]? _scanRangesRaw; // distance mm, -1 = chưa có dữ liệu
|
||||||
|
private double[]? _scanIntensities;
|
||||||
|
private double _angleIncrementDeg;
|
||||||
|
private DateTime _currentScanStartTime = DateTime.UtcNow;
|
||||||
|
private uint _scanSequenceNumber;
|
||||||
|
private long _lastDataReceivedTicks;
|
||||||
|
|
||||||
|
// Cached measurements
|
||||||
|
private LaserScan? _currentMeasurementData;
|
||||||
|
private DateTime? _lastScanDataTimestamp;
|
||||||
|
private readonly Lock _scanLock = new();
|
||||||
|
|
||||||
|
// Statistics
|
||||||
|
private long _framesReceived;
|
||||||
|
private long _scansGenerated;
|
||||||
|
|
||||||
|
// Scan frequency calculation
|
||||||
|
private readonly Stopwatch _scanFrequencyStopwatch = Stopwatch.StartNew();
|
||||||
|
private long _scansInCurrentSecond;
|
||||||
|
private readonly Stopwatch _propertyUpdateStopwatch = Stopwatch.StartNew();
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Constructor with configuration
|
||||||
|
/// </summary>
|
||||||
|
public HinsonFE35LidarDriver(
|
||||||
|
string deviceId,
|
||||||
|
string deviceName, IConfigurationSection configuration)
|
||||||
|
: base(deviceId, deviceName, DeviceType.Lidar)
|
||||||
|
{
|
||||||
|
configuration.Bind(_config);
|
||||||
|
Description = "Hinson FE-35FB-01000 2D LiDAR - TCP/UDP Protocol";
|
||||||
|
}
|
||||||
|
|
||||||
|
#region ILidar Implementation
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Current measurement data (scan points)
|
||||||
|
/// </summary>
|
||||||
|
public LaserScan? CurrentMeasurementData => _currentMeasurementData;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Timestamp of the most recent scan data
|
||||||
|
/// </summary>
|
||||||
|
public DateTime? LastScanDataTimestamp => _lastScanDataTimestamp;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Góc quét tối thiểu (radian)
|
||||||
|
/// </summary>
|
||||||
|
public double MinAngleRad => 0.0;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Góc quét tối đa (radian) - LiDAR quét đủ 360°
|
||||||
|
/// </summary>
|
||||||
|
public double MaxAngleRad => 2.0 * Math.PI;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Tầm quét tối thiểu (mét)
|
||||||
|
/// </summary>
|
||||||
|
public double MinRangeM => _config.MinRangeM;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Tầm quét tối đa (mét)
|
||||||
|
/// </summary>
|
||||||
|
public double MaxRangeM => _config.MaxRangeM;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Độ phân giải góc (radian) - tính từ dữ liệu thực tế
|
||||||
|
/// </summary>
|
||||||
|
public double? AngularResolutionRad =>
|
||||||
|
_angleIncrementDeg > 0 ? _angleIncrementDeg * Math.PI / 180.0 : null;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Tần số quét (Hz) - đo từ tốc độ sinh LaserScan thực tế (12.5/25/50 Hz)
|
||||||
|
/// </summary>
|
||||||
|
public double? ScanFrequencyHz { get; private set; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Field of View (radians)
|
||||||
|
/// </summary>
|
||||||
|
public double FieldOfViewRad => MaxAngleRad - MinAngleRad;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Hỗ trợ đo intensity
|
||||||
|
/// </summary>
|
||||||
|
public bool SupportsIntensity => true;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Độ chính xác đo khoảng cách (mét)
|
||||||
|
/// </summary>
|
||||||
|
public double? AccuracyM => DEFAULT_ACCURACY_M;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Event raised when new scan data is received
|
||||||
|
/// </summary>
|
||||||
|
public event EventHandler<LidarScanDataEventArgs>? ScanDataReceived;
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
#region DeviceBase Implementation
|
||||||
|
|
||||||
|
protected override Task OnInitializeAsync(CancellationToken cancellationToken)
|
||||||
|
{
|
||||||
|
SetProperty("IpAddress", _config.IpAddress);
|
||||||
|
SetProperty("Port", _config.Port.ToString());
|
||||||
|
SetProperty("Transport", _config.UseUdp ? "UDP" : "TCP");
|
||||||
|
SetProperty("FrameId", _config.FrameId);
|
||||||
|
SetProperty("MinRange", $"{_config.MinRangeM:F2} m");
|
||||||
|
SetProperty("MaxRange", $"{_config.MaxRangeM:F2} m");
|
||||||
|
SetProperty("AngularResolution", "N/A");
|
||||||
|
SetProperty("ScanFrequency", "N/A");
|
||||||
|
SetProperty("ConnectionStatus", "Not connected");
|
||||||
|
|
||||||
|
return Task.CompletedTask;
|
||||||
|
}
|
||||||
|
|
||||||
|
protected override async Task OnConnectAsync(CancellationToken cancellationToken)
|
||||||
|
{
|
||||||
|
// Dừng receive loop cũ và đóng kết nối cũ nếu có (trường hợp reconnect)
|
||||||
|
await StopReceiveLoopAsync();
|
||||||
|
CloseConnection();
|
||||||
|
|
||||||
|
if (_config.UseUdp)
|
||||||
|
{
|
||||||
|
_udpClient = new UdpClient();
|
||||||
|
_udpClient.Connect(_config.IpAddress, _config.Port);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
_tcpClient = new TcpClient
|
||||||
|
{
|
||||||
|
NoDelay = true,
|
||||||
|
ReceiveTimeout = _config.DataTimeoutMs
|
||||||
|
};
|
||||||
|
await _tcpClient.ConnectAsync(_config.IpAddress, _config.Port, cancellationToken);
|
||||||
|
_tcpStream = _tcpClient.GetStream();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Gửi lệnh cấu hình tham số nếu được yêu cầu
|
||||||
|
if (_config.ChangeParam)
|
||||||
|
{
|
||||||
|
var paramCommand = BuildParamCommand(
|
||||||
|
_config.SpinFrequencyHz, _config.AngleIncrementDeg, _config.NoiseFilterLevel);
|
||||||
|
await SendAsync(paramCommand, cancellationToken);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Gửi lệnh bắt đầu đo
|
||||||
|
await SendAsync(StartCaptureCommand, cancellationToken);
|
||||||
|
|
||||||
|
// Reset trạng thái nhận dữ liệu
|
||||||
|
_rxLength = 0;
|
||||||
|
ResetScanAccumulation();
|
||||||
|
Interlocked.Exchange(ref _lastDataReceivedTicks, DateTime.UtcNow.Ticks);
|
||||||
|
|
||||||
|
// Bắt đầu vòng lặp nhận dữ liệu
|
||||||
|
_receiveCts = new CancellationTokenSource();
|
||||||
|
_receiveTask = Task.Run(() => ReceiveLoopAsync(_receiveCts.Token), CancellationToken.None);
|
||||||
|
|
||||||
|
SetProperty("ConnectionStatus", "Connected");
|
||||||
|
}
|
||||||
|
|
||||||
|
protected override async Task OnDisconnectAsync(CancellationToken cancellationToken)
|
||||||
|
{
|
||||||
|
await StopReceiveLoopAsync();
|
||||||
|
CloseConnection();
|
||||||
|
SetProperty("ConnectionStatus", "Disconnected");
|
||||||
|
}
|
||||||
|
|
||||||
|
protected override Task OnResetAsync(CancellationToken cancellationToken)
|
||||||
|
{
|
||||||
|
Interlocked.Exchange(ref _framesReceived, 0);
|
||||||
|
Interlocked.Exchange(ref _scansGenerated, 0);
|
||||||
|
Interlocked.Exchange(ref _scansInCurrentSecond, 0);
|
||||||
|
_scanFrequencyStopwatch.Restart();
|
||||||
|
ScanFrequencyHz = null;
|
||||||
|
|
||||||
|
SetProperty("FramesReceived", "0");
|
||||||
|
SetProperty("ScansGenerated", "0");
|
||||||
|
SetProperty("ScanFrequency", "N/A");
|
||||||
|
|
||||||
|
return Task.CompletedTask;
|
||||||
|
}
|
||||||
|
|
||||||
|
protected override async Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
|
||||||
|
{
|
||||||
|
await Task.Delay(500, cancellationToken);
|
||||||
|
|
||||||
|
if (_receiveTask == null || _receiveTask.IsCompleted)
|
||||||
|
return false;
|
||||||
|
|
||||||
|
if (!_config.UseUdp && (_tcpClient == null || !_tcpClient.Connected))
|
||||||
|
return false;
|
||||||
|
|
||||||
|
// Kiểm tra dữ liệu có đang về hay không
|
||||||
|
var lastDataTicks = Interlocked.Read(ref _lastDataReceivedTicks);
|
||||||
|
var elapsed = DateTime.UtcNow - new DateTime(lastDataTicks, DateTimeKind.Utc);
|
||||||
|
return elapsed.TotalMilliseconds <= _config.DataTimeoutMs;
|
||||||
|
}
|
||||||
|
|
||||||
|
protected override List<PropertyDescription> CreatePropertyDescriptions()
|
||||||
|
{
|
||||||
|
return
|
||||||
|
[
|
||||||
|
new PropertyDescription("IpAddress", "IP Address", "LiDAR IP address"),
|
||||||
|
new PropertyDescription("Port", "Port", "LiDAR TCP/UDP port"),
|
||||||
|
new PropertyDescription("Transport", "Transport", "TCP or UDP"),
|
||||||
|
new PropertyDescription("FrameId", "Frame ID", "ROS-style frame identifier"),
|
||||||
|
new PropertyDescription("MinRange", "Min Range", "Minimum measurement range"),
|
||||||
|
new PropertyDescription("MaxRange", "Max Range", "Maximum measurement range"),
|
||||||
|
new PropertyDescription("AngularResolution", "Angular Resolution", "Angle between scan points"),
|
||||||
|
new PropertyDescription("ScanFrequency", "Scan Frequency", "Actual scan rate (Hz)"),
|
||||||
|
new PropertyDescription("ConnectionStatus", "Connection Status", "Socket connection status"),
|
||||||
|
new PropertyDescription("FramesReceived", "Frames Received", "Number of data frames received"),
|
||||||
|
new PropertyDescription("ScansGenerated", "Scans Generated", "Number of full 360° scans generated"),
|
||||||
|
new PropertyDescription("LastScanTime", "Last Scan Time", "Timestamp of last scan"),
|
||||||
|
];
|
||||||
|
}
|
||||||
|
|
||||||
|
protected override void Dispose(bool disposing)
|
||||||
|
{
|
||||||
|
if (disposing)
|
||||||
|
{
|
||||||
|
try
|
||||||
|
{
|
||||||
|
_receiveCts?.Cancel();
|
||||||
|
}
|
||||||
|
catch (ObjectDisposedException)
|
||||||
|
{
|
||||||
|
// Already disposed, ignore
|
||||||
|
}
|
||||||
|
CloseConnection();
|
||||||
|
_receiveCts?.Dispose();
|
||||||
|
_receiveCts = null;
|
||||||
|
}
|
||||||
|
|
||||||
|
base.Dispose(disposing);
|
||||||
|
}
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
#region Connection Helpers
|
||||||
|
|
||||||
|
private async Task SendAsync(byte[] data, CancellationToken cancellationToken)
|
||||||
|
{
|
||||||
|
if (_config.UseUdp)
|
||||||
|
{
|
||||||
|
if (_udpClient == null)
|
||||||
|
throw new InvalidOperationException("UDP client not connected");
|
||||||
|
await _udpClient.SendAsync(data, cancellationToken);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
if (_tcpStream == null)
|
||||||
|
throw new InvalidOperationException("TCP stream not connected");
|
||||||
|
await _tcpStream.WriteAsync(data, cancellationToken);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private async Task StopReceiveLoopAsync()
|
||||||
|
{
|
||||||
|
try
|
||||||
|
{
|
||||||
|
_receiveCts?.Cancel();
|
||||||
|
}
|
||||||
|
catch (ObjectDisposedException)
|
||||||
|
{
|
||||||
|
// Already disposed, ignore
|
||||||
|
}
|
||||||
|
|
||||||
|
if (_receiveTask != null)
|
||||||
|
{
|
||||||
|
try
|
||||||
|
{
|
||||||
|
await _receiveTask.WaitAsync(TimeSpan.FromSeconds(3));
|
||||||
|
}
|
||||||
|
catch (Exception)
|
||||||
|
{
|
||||||
|
// Timeout hoặc task lỗi - bỏ qua, socket sẽ bị đóng bên dưới
|
||||||
|
}
|
||||||
|
_receiveTask = null;
|
||||||
|
}
|
||||||
|
|
||||||
|
_receiveCts?.Dispose();
|
||||||
|
_receiveCts = null;
|
||||||
|
}
|
||||||
|
|
||||||
|
private void CloseConnection()
|
||||||
|
{
|
||||||
|
try
|
||||||
|
{
|
||||||
|
_tcpStream?.Close();
|
||||||
|
_tcpClient?.Close();
|
||||||
|
_udpClient?.Close();
|
||||||
|
}
|
||||||
|
catch (Exception)
|
||||||
|
{
|
||||||
|
// Ignore errors during close
|
||||||
|
}
|
||||||
|
finally
|
||||||
|
{
|
||||||
|
_tcpStream = null;
|
||||||
|
_tcpClient = null;
|
||||||
|
_udpClient = null;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Vòng lặp nhận dữ liệu từ LiDAR và parse frame
|
||||||
|
/// </summary>
|
||||||
|
private async Task ReceiveLoopAsync(CancellationToken cancellationToken)
|
||||||
|
{
|
||||||
|
var buffer = new byte[8192];
|
||||||
|
|
||||||
|
// ReceiveTimeout của TcpClient KHÔNG áp dụng cho async read: nếu rút cáp mạng
|
||||||
|
// (không có TCP RST) thì ReadAsync treo vô hạn. Phải tự đặt timeout bằng WaitAsync
|
||||||
|
// để phát hiện mất dữ liệu và trigger reconnect.
|
||||||
|
var readTimeout = TimeSpan.FromMilliseconds(Math.Max(_config.DataTimeoutMs, 500));
|
||||||
|
|
||||||
|
try
|
||||||
|
{
|
||||||
|
while (!cancellationToken.IsCancellationRequested)
|
||||||
|
{
|
||||||
|
int bytesRead;
|
||||||
|
if (_config.UseUdp)
|
||||||
|
{
|
||||||
|
var result = await _udpClient!.ReceiveAsync(cancellationToken)
|
||||||
|
.AsTask().WaitAsync(readTimeout, cancellationToken);
|
||||||
|
bytesRead = result.Buffer.Length;
|
||||||
|
AppendToRxBuffer(result.Buffer, bytesRead);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
bytesRead = await _tcpStream!.ReadAsync(buffer, cancellationToken)
|
||||||
|
.AsTask().WaitAsync(readTimeout, cancellationToken);
|
||||||
|
if (bytesRead == 0)
|
||||||
|
throw new IOException("LiDAR closed the connection");
|
||||||
|
AppendToRxBuffer(buffer, bytesRead);
|
||||||
|
}
|
||||||
|
|
||||||
|
Interlocked.Exchange(ref _lastDataReceivedTicks, DateTime.UtcNow.Ticks);
|
||||||
|
|
||||||
|
// Parse tất cả frame hoàn chỉnh trong buffer
|
||||||
|
while (TryParseNextFrame()) { }
|
||||||
|
}
|
||||||
|
}
|
||||||
|
catch (OperationCanceledException)
|
||||||
|
{
|
||||||
|
// Normal shutdown
|
||||||
|
}
|
||||||
|
catch (Exception ex)
|
||||||
|
{
|
||||||
|
if (!cancellationToken.IsCancellationRequested)
|
||||||
|
{
|
||||||
|
var reason = ex is TimeoutException
|
||||||
|
? $"No data from LiDAR for > {readTimeout.TotalMilliseconds:F0} ms (cable unplugged?)"
|
||||||
|
: ex.Message;
|
||||||
|
OnErrorOccurred(new Exception($"Hinson LiDAR receive loop error: {reason}", ex));
|
||||||
|
SetProperty("ConnectionStatus", "Connection lost");
|
||||||
|
|
||||||
|
// Báo cho DeviceBase để state machine chuyển Disconnected và
|
||||||
|
// chạy AutoReconnectLoop (gọi lại OnConnectAsync mở lại socket)
|
||||||
|
_ = CheckConnectionAsync();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
#region Protocol Parsing
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Dồn dữ liệu mới vào cuối rx buffer. Nếu tràn buffer (dữ liệu rác) thì reset.
|
||||||
|
/// </summary>
|
||||||
|
private void AppendToRxBuffer(byte[] data, int count)
|
||||||
|
{
|
||||||
|
if (_rxLength + count > _rxBuffer.Length)
|
||||||
|
{
|
||||||
|
// Buffer đầy mà không tách được frame nào - dữ liệu hỏng, bỏ hết làm lại
|
||||||
|
_rxLength = 0;
|
||||||
|
if (count > _rxBuffer.Length)
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
Array.Copy(data, 0, _rxBuffer, _rxLength, count);
|
||||||
|
_rxLength += count;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Xoá byteCount byte đầu của rx buffer
|
||||||
|
/// </summary>
|
||||||
|
private void ConsumeRxBuffer(int byteCount)
|
||||||
|
{
|
||||||
|
if (byteCount >= _rxLength)
|
||||||
|
{
|
||||||
|
_rxLength = 0;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
Array.Copy(_rxBuffer, byteCount, _rxBuffer, 0, _rxLength - byteCount);
|
||||||
|
_rxLength -= byteCount;
|
||||||
|
}
|
||||||
|
|
||||||
|
private static bool MatchAt(byte[] buffer, int index, byte[] pattern)
|
||||||
|
{
|
||||||
|
for (int i = 0; i < pattern.Length; i++)
|
||||||
|
{
|
||||||
|
if (buffer[index + i] != pattern[i])
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Tìm và xử lý frame kế tiếp trong rx buffer.
|
||||||
|
/// Trả về true nếu đã xử lý được một frame (cần gọi lại để xử lý tiếp).
|
||||||
|
/// </summary>
|
||||||
|
private bool TryParseNextFrame()
|
||||||
|
{
|
||||||
|
if (_rxLength < AreaFrameHead.Length)
|
||||||
|
return false;
|
||||||
|
|
||||||
|
// Tìm frame header ("HISN" - dữ liệu quét, "WSimu" - dữ liệu vùng an toàn)
|
||||||
|
int headIndex = -1;
|
||||||
|
bool isAreaFrame = false;
|
||||||
|
int searchEnd = _rxLength - AreaFrameHead.Length;
|
||||||
|
for (int i = 0; i <= searchEnd; i++)
|
||||||
|
{
|
||||||
|
if (MatchAt(_rxBuffer, i, RangeFrameHead))
|
||||||
|
{
|
||||||
|
headIndex = i;
|
||||||
|
isAreaFrame = false;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
if (MatchAt(_rxBuffer, i, AreaFrameHead))
|
||||||
|
{
|
||||||
|
headIndex = i;
|
||||||
|
isAreaFrame = true;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (headIndex < 0)
|
||||||
|
{
|
||||||
|
// Không có header - giữ lại vài byte cuối phòng header bị cắt giữa 2 lần nhận
|
||||||
|
if (_rxLength > AreaFrameHead.Length)
|
||||||
|
ConsumeRxBuffer(_rxLength - AreaFrameHead.Length);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Bỏ dữ liệu rác trước header
|
||||||
|
if (headIndex > 0)
|
||||||
|
ConsumeRxBuffer(headIndex);
|
||||||
|
|
||||||
|
if (isAreaFrame)
|
||||||
|
{
|
||||||
|
// Frame vùng an toàn (obstacle area) - không dùng, bỏ qua
|
||||||
|
if (_rxLength < AREA_FRAME_SIZE)
|
||||||
|
return false;
|
||||||
|
ConsumeRxBuffer(AREA_FRAME_SIZE);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
return TryParseRangeFrame();
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Parse frame dữ liệu quét "HISN" ở đầu rx buffer
|
||||||
|
/// </summary>
|
||||||
|
private bool TryParseRangeFrame()
|
||||||
|
{
|
||||||
|
if (_rxLength < RANGE_FRAME_HEADER_SIZE)
|
||||||
|
return false;
|
||||||
|
|
||||||
|
// Header: các trường uint16 big-endian
|
||||||
|
int startAngle = (_rxBuffer[4] << 8) | _rxBuffer[5]; // độ
|
||||||
|
int endAngle = (_rxBuffer[6] << 8) | _rxBuffer[7]; // độ
|
||||||
|
int dataSize = (_rxBuffer[8] << 8) | _rxBuffer[9]; // số điểm trong frame này
|
||||||
|
int dataPosition = (_rxBuffer[10] << 8) | _rxBuffer[11]; // vị trí điểm hiện tại trong khối
|
||||||
|
int measureSize = (_rxBuffer[12] << 8) | _rxBuffer[13]; // tổng số điểm của khối góc
|
||||||
|
|
||||||
|
// Theo ROS driver: data_size không được vượt quá measure_size
|
||||||
|
if (dataSize > measureSize)
|
||||||
|
dataSize = measureSize;
|
||||||
|
|
||||||
|
int frameSize = RANGE_FRAME_HEADER_SIZE + dataSize * BYTES_PER_POINT;
|
||||||
|
if (_rxLength < frameSize)
|
||||||
|
return false; // Chưa nhận đủ frame
|
||||||
|
|
||||||
|
// Validate header - loại frame lỗi
|
||||||
|
if (measureSize <= 0 || endAngle <= startAngle || endAngle > 360)
|
||||||
|
{
|
||||||
|
ConsumeRxBuffer(RANGE_FRAME_HEADER_SIZE);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
Interlocked.Increment(ref _framesReceived);
|
||||||
|
|
||||||
|
// Độ phân giải góc và tổng số điểm một vòng quét
|
||||||
|
double angleIncrementDeg = (double)(endAngle - startAngle) / measureSize;
|
||||||
|
int totalPoints = (int)Math.Round(360.0 / angleIncrementDeg);
|
||||||
|
|
||||||
|
if (totalPoints <= 0 || totalPoints > 40000)
|
||||||
|
{
|
||||||
|
ConsumeRxBuffer(frameSize);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Cấp lại buffer tích luỹ khi độ phân giải thay đổi
|
||||||
|
if (_scanRangesRaw == null || _scanRangesRaw.Length != totalPoints)
|
||||||
|
{
|
||||||
|
_scanRangesRaw = new double[totalPoints];
|
||||||
|
_scanIntensities = new double[totalPoints];
|
||||||
|
Array.Fill(_scanRangesRaw, -1.0);
|
||||||
|
_currentScanStartTime = DateTime.UtcNow;
|
||||||
|
}
|
||||||
|
_angleIncrementDeg = angleIncrementDeg;
|
||||||
|
|
||||||
|
// Index của điểm đầu tiên trong frame này (theo công thức của ROS driver)
|
||||||
|
int beginPointIndex = (int)(startAngle / angleIncrementDeg) + dataPosition - dataSize;
|
||||||
|
|
||||||
|
for (int i = 0; i < dataSize; i++)
|
||||||
|
{
|
||||||
|
int offset = RANGE_FRAME_HEADER_SIZE + i * BYTES_PER_POINT;
|
||||||
|
|
||||||
|
// Distance và intensity: uint16 little-endian
|
||||||
|
int distanceMm = _rxBuffer[offset] | (_rxBuffer[offset + 1] << 8);
|
||||||
|
int intensity = _rxBuffer[offset + 2] | (_rxBuffer[offset + 3] << 8);
|
||||||
|
|
||||||
|
int index = beginPointIndex + i;
|
||||||
|
if (index < 0 || index >= totalPoints)
|
||||||
|
continue;
|
||||||
|
|
||||||
|
_scanRangesRaw![index] = distanceMm;
|
||||||
|
_scanIntensities![index] = intensity;
|
||||||
|
}
|
||||||
|
|
||||||
|
ConsumeRxBuffer(frameSize);
|
||||||
|
|
||||||
|
// Hoàn thành một vòng quét 360°
|
||||||
|
if (endAngle == 360 && dataPosition == measureSize)
|
||||||
|
{
|
||||||
|
PublishCompletedScan(totalPoints);
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
#region Scan Publishing
|
||||||
|
|
||||||
|
private void ResetScanAccumulation()
|
||||||
|
{
|
||||||
|
_scanRangesRaw = null;
|
||||||
|
_scanIntensities = null;
|
||||||
|
_angleIncrementDeg = 0.0;
|
||||||
|
_currentScanStartTime = DateTime.UtcNow;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Build LaserScan từ dữ liệu một vòng quét hoàn chỉnh và fire event
|
||||||
|
/// </summary>
|
||||||
|
private void PublishCompletedScan(int totalPoints)
|
||||||
|
{
|
||||||
|
if (_scanRangesRaw == null || _scanIntensities == null)
|
||||||
|
return;
|
||||||
|
|
||||||
|
var scanStartTime = _currentScanStartTime;
|
||||||
|
var header = new Header(
|
||||||
|
seq: _scanSequenceNumber++,
|
||||||
|
stamp: scanStartTime,
|
||||||
|
frameId: _config.FrameId
|
||||||
|
);
|
||||||
|
|
||||||
|
double angleIncrementRad = 2.0 * Math.PI / totalPoints;
|
||||||
|
|
||||||
|
double[] ranges = new double[totalPoints];
|
||||||
|
double[] intensities = new double[totalPoints];
|
||||||
|
|
||||||
|
// Offset index: xoay dữ liệu theo AngleOffsetDeg và tuỳ chọn Inverted
|
||||||
|
double offsetDeg = _config.AngleOffsetDeg + (_config.Inverted ? 180.0 : 0.0);
|
||||||
|
int indexOffset = (int)Math.Round(offsetDeg / 360.0 * totalPoints);
|
||||||
|
|
||||||
|
for (int i = 0; i < totalPoints; i++)
|
||||||
|
{
|
||||||
|
int srcIndex = i - indexOffset;
|
||||||
|
srcIndex %= totalPoints;
|
||||||
|
if (srcIndex < 0)
|
||||||
|
srcIndex += totalPoints;
|
||||||
|
|
||||||
|
double distanceRawMm = _scanRangesRaw[srcIndex];
|
||||||
|
double distanceM = distanceRawMm / 1000.0;
|
||||||
|
|
||||||
|
if (distanceRawMm <= 0 || distanceRawMm > MAX_DISTANCE_RAW_MM ||
|
||||||
|
distanceM < _config.MinRangeM || distanceM > _config.MaxRangeM)
|
||||||
|
{
|
||||||
|
ranges[i] = -1.0; // No detection (JSON-safe, giống Olei driver)
|
||||||
|
intensities[i] = 0.0;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
ranges[i] = distanceM;
|
||||||
|
intensities[i] = _scanIntensities[srcIndex];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Chuẩn bị buffer cho vòng quét kế tiếp
|
||||||
|
Array.Fill(_scanRangesRaw, -1.0);
|
||||||
|
Array.Fill(_scanIntensities, 0.0);
|
||||||
|
_currentScanStartTime = DateTime.UtcNow;
|
||||||
|
|
||||||
|
Interlocked.Increment(ref _scansGenerated);
|
||||||
|
Interlocked.Increment(ref _scansInCurrentSecond);
|
||||||
|
UpdateScanFrequency();
|
||||||
|
|
||||||
|
double scanTime = ScanFrequencyHz.HasValue && ScanFrequencyHz.Value > 0
|
||||||
|
? 1.0 / ScanFrequencyHz.Value
|
||||||
|
: 0.04; // Default 25 Hz
|
||||||
|
|
||||||
|
var scan = new LaserScan
|
||||||
|
{
|
||||||
|
Header = header,
|
||||||
|
AngleMin = 0.0,
|
||||||
|
AngleMax = 2.0 * Math.PI,
|
||||||
|
AngleIncrement = angleIncrementRad,
|
||||||
|
TimeIncrement = scanTime / totalPoints,
|
||||||
|
ScanTime = scanTime,
|
||||||
|
RangeMin = _config.MinRangeM,
|
||||||
|
RangeMax = _config.MaxRangeM,
|
||||||
|
Ranges = ranges,
|
||||||
|
Intensities = intensities
|
||||||
|
};
|
||||||
|
|
||||||
|
lock (_scanLock)
|
||||||
|
{
|
||||||
|
_currentMeasurementData = scan;
|
||||||
|
_lastScanDataTimestamp = scan.Header.Stamp;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Cập nhật UI properties tối đa 1 lần/giây
|
||||||
|
if (_propertyUpdateStopwatch.ElapsedMilliseconds > 1000)
|
||||||
|
{
|
||||||
|
_propertyUpdateStopwatch.Restart();
|
||||||
|
SetProperty("LastScanTime", scan.Header.Stamp.ToString("HH:mm:ss.fff"));
|
||||||
|
SetProperty("ScanFrequency", ScanFrequencyHz.HasValue ? $"{ScanFrequencyHz.Value:F2} Hz" : "N/A");
|
||||||
|
SetProperty("AngularResolution", $"{_angleIncrementDeg:F3}°");
|
||||||
|
SetProperty("FramesReceived", Interlocked.Read(ref _framesReceived).ToString());
|
||||||
|
SetProperty("ScansGenerated", Interlocked.Read(ref _scansGenerated).ToString());
|
||||||
|
}
|
||||||
|
|
||||||
|
ScanDataReceived?.Invoke(this, new LidarScanDataEventArgs(scan.Header.Stamp, scan));
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Cập nhật tần số quét dựa trên số LaserScan sinh ra mỗi giây
|
||||||
|
/// </summary>
|
||||||
|
private void UpdateScanFrequency()
|
||||||
|
{
|
||||||
|
if (_scanFrequencyStopwatch.ElapsedMilliseconds >= 1000)
|
||||||
|
{
|
||||||
|
long scanCount = Interlocked.Exchange(ref _scansInCurrentSecond, 0);
|
||||||
|
double elapsedSeconds = _scanFrequencyStopwatch.ElapsedMilliseconds / 1000.0;
|
||||||
|
ScanFrequencyHz = scanCount / elapsedSeconds;
|
||||||
|
_scanFrequencyStopwatch.Restart();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
#region Command Builders
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Build lệnh cấu hình tham số LiDAR "SCtrl" (12 byte, CRC16-Modbus ở 2 byte cuối)
|
||||||
|
/// </summary>
|
||||||
|
private static byte[] BuildParamCommand(int spinFrequencyHz, string angleIncrementDeg, int noiseFilterLevel)
|
||||||
|
{
|
||||||
|
var command = new byte[12];
|
||||||
|
command[0] = (byte)'S';
|
||||||
|
command[1] = (byte)'C';
|
||||||
|
command[2] = (byte)'t';
|
||||||
|
command[3] = (byte)'r';
|
||||||
|
command[4] = (byte)'l';
|
||||||
|
|
||||||
|
command[5] = 0x00; // run_state: 0x00 = run, 0x01 = stop
|
||||||
|
command[6] = 0x00; // reserved
|
||||||
|
|
||||||
|
command[7] = spinFrequencyHz switch
|
||||||
|
{
|
||||||
|
12 => 0x00, // 12.5 Hz
|
||||||
|
25 => 0x01,
|
||||||
|
50 => 0x02,
|
||||||
|
_ => 0x00
|
||||||
|
};
|
||||||
|
|
||||||
|
command[8] = angleIncrementDeg switch
|
||||||
|
{
|
||||||
|
"0.025" => 0x00,
|
||||||
|
"0.050" => 0x01,
|
||||||
|
"0.100" => 0x02,
|
||||||
|
"0.200" => 0x03,
|
||||||
|
"0.250" => 0x04,
|
||||||
|
"0.500" => 0x05,
|
||||||
|
_ => 0x02
|
||||||
|
};
|
||||||
|
|
||||||
|
command[9] = (byte)Math.Clamp(noiseFilterLevel, 0, 3);
|
||||||
|
|
||||||
|
// CRC16-Modbus trên 10 byte đầu, low byte trước
|
||||||
|
ushort crc = Crc16Modbus(command, 10);
|
||||||
|
command[10] = (byte)(crc & 0x00FF);
|
||||||
|
command[11] = (byte)((crc & 0xFF00) >> 8);
|
||||||
|
|
||||||
|
return command;
|
||||||
|
}
|
||||||
|
|
||||||
|
private static ushort Crc16Modbus(byte[] data, int length)
|
||||||
|
{
|
||||||
|
ushort crc = 0xFFFF;
|
||||||
|
for (int i = 0; i < length; i++)
|
||||||
|
{
|
||||||
|
crc ^= data[i];
|
||||||
|
for (int j = 0; j < 8; j++)
|
||||||
|
{
|
||||||
|
if ((crc & 0x0001) != 0)
|
||||||
|
crc = (ushort)((crc >> 1) ^ 0xA001);
|
||||||
|
else
|
||||||
|
crc >>= 1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return crc;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
}
|
||||||
File diff suppressed because it is too large
Load Diff
@@ -1,5 +1,6 @@
|
|||||||
using System;
|
using System;
|
||||||
using System.Collections.Generic;
|
using System.Collections.Generic;
|
||||||
|
using System.IO;
|
||||||
using System.IO.Ports;
|
using System.IO.Ports;
|
||||||
using System.Threading;
|
using System.Threading;
|
||||||
|
|
||||||
@@ -24,6 +25,10 @@ namespace RobotNet10.RobotApp.Drivers.WheeltecIMU
|
|||||||
// Event de thong bao khi co du lieu moi duoc decode
|
// Event de thong bao khi co du lieu moi duoc decode
|
||||||
public event EventHandler? DataReceived;
|
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)
|
// Properties - lock-free voi memory barriers (volatile khong ho tro double)
|
||||||
public double Roll { get; private set; }
|
public double Roll { get; private set; }
|
||||||
public double Pitch { 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 const long DATA_TIMEOUT_TICKS = 3 * TimeSpan.TicksPerSecond;
|
||||||
private long _lastFrameTicks;
|
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;
|
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)
|
public WheeltecReader(string portName, int baudRate, int timeOut)
|
||||||
{
|
{
|
||||||
_portName = portName;
|
_portName = portName;
|
||||||
@@ -226,6 +242,7 @@ namespace RobotNet10.RobotApp.Drivers.WheeltecIMU
|
|||||||
serial.DiscardInBuffer();
|
serial.DiscardInBuffer();
|
||||||
_lastFrameTicks = DateTime.UtcNow.Ticks;
|
_lastFrameTicks = DateTime.UtcNow.Ticks;
|
||||||
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [WheeltecReader] Connected to {_portName}");
|
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [WheeltecReader] Connected to {_portName}");
|
||||||
|
RaiseConnectionStateChanged(true);
|
||||||
}
|
}
|
||||||
|
|
||||||
InnerReadLoop(readBuffer, cancellationToken);
|
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}");
|
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [WheeltecReader] IMU disconnected: {ex.Message}");
|
||||||
SafeCloseSerial();
|
SafeCloseSerial();
|
||||||
|
RaiseConnectionStateChanged(false);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (_shouldRead && !cancellationToken.IsCancellationRequested)
|
if (_shouldRead && !cancellationToken.IsCancellationRequested)
|
||||||
@@ -259,19 +277,38 @@ namespace RobotNet10.RobotApp.Drivers.WheeltecIMU
|
|||||||
while (_shouldRead && !cancellationToken.IsCancellationRequested
|
while (_shouldRead && !cancellationToken.IsCancellationRequested
|
||||||
&& serial != null && serial.IsOpen)
|
&& 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;
|
int bytesToRead = serial.BytesToRead;
|
||||||
if (bytesToRead <= 0)
|
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);
|
Thread.Sleep(1);
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
int bytesRead = serial.Read(readBuffer, 0, Math.Min(bytesToRead, readBuffer.Length));
|
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);
|
ProcessIncomingData(readBuffer, bytesRead);
|
||||||
}
|
}
|
||||||
@@ -482,6 +519,18 @@ namespace RobotNet10.RobotApp.Drivers.WheeltecIMU
|
|||||||
_frameBufferLength = remainingAfterHead;
|
_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()
|
public void Disconnect()
|
||||||
{
|
{
|
||||||
StopReadingThread();
|
StopReadingThread();
|
||||||
|
|||||||
@@ -461,6 +461,21 @@
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
},
|
},
|
||||||
|
"HinsonLidar":
|
||||||
|
{
|
||||||
|
"Enabled": true,
|
||||||
|
"DeviceId": "lidar-hinson-front",
|
||||||
|
"DeviceName": "Hinson FE-35FB-01000",
|
||||||
|
"DeviceType": "Lidar",
|
||||||
|
"DriverName": "HinsonFE35LidarDriver",
|
||||||
|
"DriverVersion": "1.0.0",
|
||||||
|
"Connection": {
|
||||||
|
"IpAddress": "192.168.100.88",
|
||||||
|
"Port": 8080,
|
||||||
|
"FrameId": "hinson-front",
|
||||||
|
"MaxRangeM": 35.0
|
||||||
|
}
|
||||||
|
},
|
||||||
"SickLidar01": {
|
"SickLidar01": {
|
||||||
"DeviceId": "scan_1",
|
"DeviceId": "scan_1",
|
||||||
"Enabled": false,
|
"Enabled": false,
|
||||||
@@ -680,6 +695,22 @@
|
|||||||
"PrintDataIntervalMs": 100
|
"PrintDataIntervalMs": 100
|
||||||
}
|
}
|
||||||
},
|
},
|
||||||
|
"HfiA9IMU": {
|
||||||
|
"DeviceId": "imu",
|
||||||
|
"Enabled": true,
|
||||||
|
"DeviceName": "HandsFree HFI-A9 IMU",
|
||||||
|
"DeviceType": "IMU",
|
||||||
|
"DriverName": "HfiA9IMU",
|
||||||
|
"DriverVersion": "1.0.0",
|
||||||
|
"Description": "IMU",
|
||||||
|
"Connection": {
|
||||||
|
"Port": "/dev/serial/by-id/usb-Silicon_Labs_HandsFree_IMU_USB_to_UART_Bridge_Controller_0001-if00-port0",
|
||||||
|
"BaudRate": 921600,
|
||||||
|
"Timeout": 2000,
|
||||||
|
"DebugEnabled": true,
|
||||||
|
"PrintDataIntervalMs": 100
|
||||||
|
}
|
||||||
|
},
|
||||||
"HikVisionQr": {
|
"HikVisionQr": {
|
||||||
"DeviceId": "hik-qr-001",
|
"DeviceId": "hik-qr-001",
|
||||||
"Enabled": false,
|
"Enabled": false,
|
||||||
|
|||||||
Reference in New Issue
Block a user