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();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user