Initial commit
This commit is contained in:
@@ -0,0 +1,863 @@
|
||||
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;
|
||||
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 (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);
|
||||
|
||||
// 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 = "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();
|
||||
|
||||
if (_printDataEnabled)
|
||||
{
|
||||
_printDataTimer?.Dispose();
|
||||
_printDataTimer = new Timer(_ =>
|
||||
{
|
||||
try
|
||||
{
|
||||
var acc = CachedAcceleration.Accel.Linear;
|
||||
var gyro = CachedAngularVelocity.Vector;
|
||||
var ori = CachedOrientation.Vector;
|
||||
var temp = CachedTemperature;
|
||||
var t = ((IInertialMeasurementUnit)this).LastUpdateTime;
|
||||
var tempStr = temp.HasValue ? temp.Value.ToString("F2") : "NA";
|
||||
|
||||
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}) " +
|
||||
$"temp={tempStr}");
|
||||
}
|
||||
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 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();
|
||||
|
||||
_printDataTimer?.Dispose();
|
||||
_printDataTimer = null;
|
||||
|
||||
await Task.CompletedTask;
|
||||
}
|
||||
|
||||
protected override async Task OnResetAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
// Huy event va disconnect
|
||||
IMU.DataReceived -= IMU_DataReceived;
|
||||
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);
|
||||
_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();
|
||||
_printDataTimer?.Dispose();
|
||||
_printDataTimer = null;
|
||||
}
|
||||
base.Dispose(disposing);
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user