823 lines
36 KiB
C#
823 lines
36 KiB
C#
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.WheeltecIMU
|
|
{
|
|
[Device(DeviceType.Imu, "WheeltecN100IMU", "WheeltecN100IMU", "1.0.0", Description = "IMU Simulation Driver")]
|
|
public class WheeltecN100IMU : DeviceBase, IInertialMeasurementUnit
|
|
{
|
|
private readonly WheeltecReader IMU;
|
|
|
|
// Cached data
|
|
private AccelStamped _cachedAcceleration;
|
|
private Vector3Stamped _cachedAngularVelocity;
|
|
private Vector3Stamped _cachedMagnetometer;
|
|
private Vector3Stamped _cachedOrientation;
|
|
private QuaternionStamped _cachedQuaternion;
|
|
private double _cachedTemperature = 25.0;
|
|
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;
|
|
|
|
// 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 (firmware drift ~0.009 rad/s)
|
|
// 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 WheeltecN100IMU(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 WheeltecReader(_portName, _baudRate, _timeOut);
|
|
|
|
// 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 = "100"
|
|
};
|
|
|
|
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"
|
|
};
|
|
|
|
yield return new PropertyDescription("Temperature", "Temperature (°C)", "Nhiet do cam bien (°C)")
|
|
{
|
|
DataType = "number",
|
|
IsReadOnly = true,
|
|
DisplayOrder = 6,
|
|
Category = "Du lieu",
|
|
DefaultValue = "25"
|
|
};
|
|
}
|
|
|
|
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();
|
|
|
|
// 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 WheeltecReader
|
|
IMU.DataReceived += IMU_DataReceived;
|
|
|
|
// 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;
|
|
|
|
// Disconnect IMU de dung processing thread va serial port
|
|
IMU.Disconnect();
|
|
|
|
await Task.CompletedTask;
|
|
}
|
|
|
|
protected override async Task OnResetAsync(CancellationToken cancellationToken)
|
|
{
|
|
// Huy event va disconnect
|
|
IMU.DataReceived -= IMU_DataReceived;
|
|
IMU.Disconnect();
|
|
|
|
// 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);
|
|
_cachedTemperature = 25.0;
|
|
_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;
|
|
|
|
// 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)
|
|
{
|
|
return Task.FromResult(IMU.IsConnected);
|
|
}
|
|
|
|
/// <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 WheeltecReader.
|
|
/// 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} [WheeltecN100IMU] 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;
|
|
}
|
|
|
|
// FIXED: 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} [WheeltecN100IMU] 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
|
|
// Firmware AHRS Yaw drift ~0.009 rad/s do tich phan gyro noi bo khong chinh xac
|
|
// CalibratedGz sau khi tru bias chi con ~0.00006 rad/s trung binh → giam drift 150 lan
|
|
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} | " +
|
|
$"Temp={snapshot.Temp:F2}");
|
|
}
|
|
|
|
// 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} [WheeltecN100IMU] 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
|
|
// Struct assignments are NOT atomic — without lock, readers could get partially updated data
|
|
lock (_dataLock)
|
|
{
|
|
previousAcceleration = _cachedAcceleration;
|
|
previousOrientation = _cachedOrientation;
|
|
|
|
_cachedAcceleration = newAcceleration;
|
|
_cachedAngularVelocity = newAngularVelocity;
|
|
_cachedMagnetometer = newMagnetometer;
|
|
_cachedOrientation = newOrientation;
|
|
_cachedQuaternion = newQuaternion;
|
|
_cachedTemperature = snapshot.Temp;
|
|
_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 temp = _cachedTemperature;
|
|
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}");
|
|
SetProperty("Temperature", temp.ToString("F2"));
|
|
}
|
|
|
|
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
|
|
{
|
|
get { lock (_dataLock) { return _cachedTemperature; } }
|
|
}
|
|
|
|
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.Disconnect();
|
|
IMU.Dispose();
|
|
}
|
|
base.Dispose(disposing);
|
|
}
|
|
}
|
|
}
|