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

Initial commit

Browse Source
This commit is contained in:
QUYVN
2026-07-03 16:31:37 +07:00
commit 899c7c637d
1939 changed files with 641750 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

159
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/WheeltecIMU/CRCTable.cs Normal file
View File

@@ -0,0 +1,159 @@
namespace RobotNet10.RobotApp.Drivers.WheeltecIMU
{
public class CRCTable
{
public static readonly byte[] CRC8Table =
{
0, 94, 188, 226, 97, 63, 221, 131, 194, 156, 126, 32, 163, 253, 31, 65,
157, 195, 33, 127, 252, 162, 64, 30, 95, 1, 227, 189, 62, 96, 130, 220,
35, 125, 159, 193, 66, 28, 254, 160, 225, 191, 93, 3, 128, 222, 60, 98,
190, 224, 2, 92, 223, 129, 99, 61, 124, 34, 192, 158, 29, 67, 161, 255,
70, 24, 250, 164, 39, 121, 155, 197, 132, 218, 56, 102, 229, 187, 89, 7,
219, 133, 103, 57, 186, 228, 6, 88, 25, 71, 165, 251, 120, 38, 196, 154,
101, 59, 217, 135, 4, 90, 184, 230, 167, 249, 27, 69, 198, 152, 122, 36,
248, 166, 68, 26, 153, 199, 37, 123, 58, 100, 134, 216, 91, 5, 231, 185,
140, 210, 48, 110, 237, 179, 81, 15, 78, 16, 242, 172, 47, 113, 147, 205,
17, 79, 173, 243, 112, 46, 204, 146, 211, 141, 111, 49, 178, 236, 14, 80,
175, 241, 19, 77, 206, 144, 114, 44, 109, 51, 209, 143, 12, 82, 176, 238,
50, 108, 142, 208, 83, 13, 239, 177, 240, 174, 76, 18, 145, 207, 45, 115,
202, 148, 118, 40, 171, 245, 23, 73, 8, 86, 180, 234, 105, 55, 213, 139,
87, 9, 235, 181, 54, 104, 138, 212, 149, 203, 41, 119, 244, 170, 72, 22,
233, 183, 85, 11, 136, 214, 52, 106, 43, 117, 151, 201, 74, 20, 246, 168,
116, 42, 200, 150, 21, 75, 169, 247, 182, 232, 10, 84, 215, 137, 107, 53
};
public static readonly ushort[] CRC16Table =
{
0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7,
0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF,
0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6,
0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE,
0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485,
0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D,
0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4,
0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC,
0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823,
0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B,
0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12,
0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A,
0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41,
0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49,
0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70,
0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78,
0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F,
0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067,
0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E,
0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256,
0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D,
0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C,
0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634,
0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB,
0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3,
0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A,
0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92,
0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9,
0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1,
0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8,
0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0
};
public static readonly uint[] CRC32Table =
{
0x00000000, 0x77073096, 0xee0e612c, 0x990951ba,
0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3,
0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988,
0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91,
0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de,
0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec,
0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5,
0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172,
0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,
0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940,
0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116,
0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f,
0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924,
0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,
0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a,
0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818,
0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01,
0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e,
0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457,
0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c,
0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2,
0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb,
0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0,
0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9,
0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086,
0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4,
0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad,
0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a,
0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683,
0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8,
0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe,
0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7,
0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc,
0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5,
0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252,
0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60,
0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79,
0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236,
0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f,
0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04,
0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a,
0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713,
0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38,
0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21,
0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e,
0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c,
0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45,
0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2,
0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db,
0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0,
0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6,
0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf,
0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94,
0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d
};
public static byte CRC8_Table(byte[] buffer, int counter)
{
return CRC8_Table(buffer.AsSpan(0, counter));
}
public static byte CRC8_Table(ReadOnlySpan<byte> buffer)
{
byte crc8 = 0;
foreach (byte value in buffer)
{
byte new_index = (byte)(crc8 ^ value);
crc8 = CRC8Table[new_index];
}
return crc8;
}
// Fixed CRC16_Table method - accepts byte[] and int counter parameter
public static ushort CRC16_Table(byte[] buffer, int counter)
{
return CRC16_Table(buffer.AsSpan(0, counter));
}
public static ushort CRC16_Table(ReadOnlySpan<byte> buffer)
{
ushort crc16 = 0;
foreach (byte value in buffer)
{
crc16 = (ushort)(CRC16Table[((crc16 >> 8) ^ value) & 0xFF] ^ (crc16 << 8));
}
return crc16;
}
}
}

19
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/WheeltecIMU/README.md Normal file
View File

@@ -0,0 +1,19 @@
1.Config USB
$sudo chmod 666 /dev/ttyUSB0
$sudo nano /etc/udev/rules.d/99-usb-serial.rules
``SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6001", MODE="0666", GROUP="dialout"``
2. Kill process robotnet10
$ps aux | grep -i robotnet | grep -v grep
$lsof /dev/ttyUSB0 2>&1 || echo "Port is free"
$kill 2381 4242 8011
$kill -9 2381 4242 8011
$lsof /dev/ttyUSB0
$./run.sh
3. HTML test
# Option A: Dùng default browser
xdg-open /home/robotics/sonvh/odometry_comparison_test.html
# Option B: Dùng specific browser
firefox /home/robotics/sonvh/odometry_comparison_test.html
# hoặc
google-chrome /home/robotics/sonvh/odometry_comparison_test.html

863
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/WheeltecIMU/WheeltecN100IMU.cs Normal file
View File

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

620
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/WheeltecIMU/WheeltecReader.cs Normal file
View File

@@ -0,0 +1,620 @@
using System;
using System.Collections.Generic;
using System.IO.Ports;
using System.Threading;
namespace RobotNet10.RobotApp.Drivers.WheeltecIMU
{
public class WheeltecReader : IDisposable
{
// Buffer nho de tich luy du lieu cho den khi co du mot frame hoan chinh
// Frame lon nhat: INSGPS = 8 + 84 = 92 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
public event EventHandler? DataReceived;
// 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 uint Time_stamp { 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; }
public double Temp { get; private set; }
public double Rollspeed { get; private set; }
public double Pitchspeed { get; private set; }
public double Yawspeed { get; private set; }
const byte FRAME_HEAD = 0xFC;
const byte FRAME_END = 0xFD;
// Loai goi
const byte TYPE_IMU = 0x40;
const byte TYPE_AHRS = 0x41;
const byte TYPE_INSGPS = 0x42;
const byte TYPE_GROUND = 0xF0;
// Chieu dai payload
const byte IMU_LEN = 0x38; // 56
const byte AHRS_LEN = 0x30; // 48
const byte INSGPS_LEN = 0x54; // 84
// Dictionary de map datatype -> expectedLength
private static readonly Dictionary<byte, byte> DataTypeLengthMap = new()
{
{ TYPE_IMU, IMU_LEN },
{ TYPE_AHRS, AHRS_LEN },
{ TYPE_INSGPS, INSGPS_LEN }
};
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;
public bool IsConnected => serial != null && serial.IsOpen;
public WheeltecReader(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;
public double Temp;
}
/// <summary>
/// Lay snapshot cua tat ca du lieu hien tai mot cach thread-safe
/// Dam bao tat ca cac gia tri deu tu cung mot thoi diem
/// </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,
Temp = Temp
};
}
public void Connect()
{
// Reset frame buffer khi ket noi moi
_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();
}
/// <summary>
/// Khoi dong reading thread voi priority cao de doc du lieu tu SerialPort
/// </summary>
private void StartReadingThread()
{
if (_readingThread != null && _readingThread.IsAlive)
return;
_shouldRead = true;
// Tao moi CancellationTokenSource cho thread moi
_readingCts?.Dispose();
_readingCts = new CancellationTokenSource();
_readingThread = new Thread(() => ReadingThreadLoop(_readingCts.Token))
{
Name = "WheeltecIMU-Reading",
IsBackground = false,
Priority = ThreadPriority.Highest
};
_readingThread.Start();
}
/// <summary>
/// Dung reading thread
/// </summary>
private void StopReadingThread()
{
_shouldRead = false;
_readingCts?.Cancel();
if (_readingThread != null)
{
if (!_readingThread.Join(1000))
{
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [WheeltecReader] Reading thread did not stop gracefully");
}
_readingThread = null;
}
// Dispose CancellationTokenSource sau khi thread da dung
_readingCts?.Dispose();
_readingCts = null;
}
/// <summary>
/// Reading thread loop - outer loop xu ly reconnect, inner loop doc du lieu
/// Moi exception tu SerialPort deu duoc bat de tranh crash thread va tu dong
/// reconnect sau RECONNECT_BACKOFF_MS
/// </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} [WheeltecReader] Connected to {_portName}");
}
InnerReadLoop(readBuffer, cancellationToken);
}
catch (OperationCanceledException)
{
break;
}
catch (Exception ex)
{
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [WheeltecReader] IMU disconnected: {ex.Message}");
SafeCloseSerial();
}
if (_shouldRead && !cancellationToken.IsCancellationRequested)
{
cancellationToken.WaitHandle.WaitOne(RECONNECT_BACKOFF_MS);
}
}
SafeCloseSerial();
Thread.EndThreadAffinity();
}
/// <summary>
/// Inner loop doc du lieu tu serial port. Thoat khi port dong hoac co exception
/// de outer loop xu ly reconnect
/// </summary>
private void InnerReadLoop(byte[] readBuffer, CancellationToken cancellationToken)
{
while (_shouldRead && !cancellationToken.IsCancellationRequested
&& serial != null && serial.IsOpen)
{
int bytesToRead = serial.BytesToRead;
if (bytesToRead <= 0)
{
if (DateTime.UtcNow.Ticks - _lastFrameTicks > DATA_TIMEOUT_TICKS)
{
throw new TimeoutException($"No IMU frame for > {DATA_TIMEOUT_TICKS / TimeSpan.TicksPerSecond}s");
}
Thread.Sleep(1);
continue;
}
int bytesRead = serial.Read(readBuffer, 0, Math.Min(bytesToRead, readBuffer.Length));
if (bytesRead <= 0) continue;
ProcessIncomingData(readBuffer, bytesRead);
}
}
/// <summary>
/// Dong va dispose SerialPort an toan, set serial=null de lan sau tao moi.
/// SerialPort sau IOException thuong khong Open() lai duoc tren Linux nen phai tao moi.
/// </summary>
private void SafeCloseSerial()
{
if (serial == null) return;
try
{
serial.Close();
}
catch (Exception ex)
{
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [WheeltecReader] Error closing serial: {ex.Message}");
}
try { serial.Dispose(); }
catch { }
serial = null;
}
/// <summary>
/// Xu ly du lieu moi nhan duoc tu serial port
/// Them vao frame buffer va tim, 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)
{
RemoveIncompleteFrameAtStart();
availableSpace = FRAME_BUFFER_SIZE - _frameBufferLength;
if (availableSpace == 0)
{
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [WheeltecReader] Buffer still full after removing incomplete 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);
if (headIndex < 0)
{
_frameBufferLength = 0;
break;
}
if (headIndex > 0)
{
int remainingBytes = _frameBufferLength - headIndex;
Array.Copy(_frameBuffer, headIndex, _frameBuffer, 0, remainingBytes);
_frameBufferLength = remainingBytes;
continue;
}
if (_frameBufferLength < 8)
{
break;
}
byte datatype = _frameBuffer[1];
byte payloadLength = _frameBuffer[2];
if (!DataTypeLengthMap.TryGetValue(datatype, out byte expectedLength))
{
int remainingBytes = _frameBufferLength - 1;
Array.Copy(_frameBuffer, 1, _frameBuffer, 0, remainingBytes);
_frameBufferLength = remainingBytes;
continue;
}
if (payloadLength != expectedLength)
{
int remainingBytes = _frameBufferLength - 1;
Array.Copy(_frameBuffer, 1, _frameBuffer, 0, remainingBytes);
_frameBufferLength = remainingBytes;
continue;
}
int totalFrameLength = 8 + payloadLength;
if (_frameBufferLength < totalFrameLength)
{
break;
}
if (_frameBuffer[7 + payloadLength] != FRAME_END)
{
int remainingBytes = _frameBufferLength - 1;
Array.Copy(_frameBuffer, 1, _frameBuffer, 0, remainingBytes);
_frameBufferLength = remainingBytes;
continue;
}
byte[] frame = new byte[totalFrameLength];
Array.Copy(_frameBuffer, 0, frame, 0, totalFrameLength);
int remainingAfterFrame = _frameBufferLength - totalFrameLength;
if (remainingAfterFrame > 0)
{
Array.Copy(_frameBuffer, totalFrameLength, _frameBuffer, 0, remainingAfterFrame);
}
_frameBufferLength = remainingAfterFrame;
try
{
if (ParseFrame(frame))
{
_lastFrameTicks = DateTime.UtcNow.Ticks;
try
{
DataReceived?.Invoke(this, EventArgs.Empty);
}
catch (Exception ex)
{
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [WheeltecReader] DataReceived subscriber error: {ex.Message}");
}
}
}
catch (Exception ex)
{
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [WheeltecReader] Error parsing frame: {ex.Message}");
}
}
}
/// <summary>
/// Loai bo frame thieu o dau buffer va tim frame head tiep theo
/// </summary>
private void RemoveIncompleteFrameAtStart()
{
if (_frameBufferLength == 0)
return;
ReadOnlySpan<byte> bufferSpan = new(_frameBuffer, 0, _frameBufferLength);
int headIndex = bufferSpan.IndexOf(FRAME_HEAD);
if (headIndex < 0)
{
_frameBufferLength = 0;
return;
}
if (headIndex == 0)
{
if (_frameBufferLength < 8)
return;
byte datatype = _frameBuffer[1];
if (!DataTypeLengthMap.TryGetValue(datatype, out byte expectedLength))
{
int remainingAfterSkip = _frameBufferLength - 1;
Array.Copy(_frameBuffer, 1, _frameBuffer, 0, remainingAfterSkip);
_frameBufferLength = remainingAfterSkip;
return;
}
byte payloadLength = _frameBuffer[2];
if (payloadLength != expectedLength)
{
int remainingAfterSkip = _frameBufferLength - 1;
Array.Copy(_frameBuffer, 1, _frameBuffer, 0, remainingAfterSkip);
_frameBufferLength = remainingAfterSkip;
return;
}
int totalFrameLength = 8 + payloadLength;
if (_frameBufferLength < totalFrameLength)
return;
return;
}
int remainingAfterHead = _frameBufferLength - headIndex;
Array.Copy(_frameBuffer, headIndex, _frameBuffer, 0, remainingAfterHead);
_frameBufferLength = remainingAfterHead;
}
public void Disconnect()
{
StopReadingThread();
SafeCloseSerial();
_frameBufferLength = 0;
Array.Clear(_frameBuffer, 0, FRAME_BUFFER_SIZE);
}
private void DecodeIMU(byte[] payload)
{
Gx = BitConverter.ToSingle(payload, 0);
Gy = BitConverter.ToSingle(payload, 4);
Gz = BitConverter.ToSingle(payload, 8);
AccX = BitConverter.ToSingle(payload, 12);
AccY = BitConverter.ToSingle(payload, 16);
AccZ = BitConverter.ToSingle(payload, 20);
MagX = BitConverter.ToSingle(payload, 24);
MagY = BitConverter.ToSingle(payload, 28);
MagZ = BitConverter.ToSingle(payload, 32);
Temp = BitConverter.ToSingle(payload, 36);
Time_stamp = BitConverter.ToUInt32(payload, 40);
Volatile.Write(ref _memoryBarrier, 0);
}
private void DecodeAHRS(byte[] payload)
{
double rollspeed = BitConverter.ToSingle(payload, 0);
double pitchspeed = BitConverter.ToSingle(payload, 4);
double yawspeed = BitConverter.ToSingle(payload, 8);
double roll = BitConverter.ToSingle(payload, 12);
double pitch = BitConverter.ToSingle(payload, 16);
double yaw = BitConverter.ToSingle(payload, 20);
Rollspeed = rollspeed;
Pitchspeed = pitchspeed;
Yawspeed = yawspeed;
Roll = roll;
Pitch = pitch;
Yaw = yaw;
Volatile.Write(ref _memoryBarrier, 0);
}
private void DecodeINSGPS(byte[] payload)
{
double latitude = BitConverter.ToDouble(payload, 0);
double longitude = BitConverter.ToDouble(payload, 8);
double altitude = BitConverter.ToSingle(payload, 16);
double vn = BitConverter.ToSingle(payload, 20);
double ve = BitConverter.ToSingle(payload, 24);
double vd = BitConverter.ToSingle(payload, 28);
double roll = BitConverter.ToSingle(payload, 32);
double pitch = BitConverter.ToSingle(payload, 36);
double yaw = BitConverter.ToSingle(payload, 40);
double qw = BitConverter.ToSingle(payload, 44);
double qx = BitConverter.ToSingle(payload, 48);
double qy = BitConverter.ToSingle(payload, 52);
double qz = BitConverter.ToSingle(payload, 56);
}
/// <summary>
/// Parse mot frame hoan chinh tu buffer
/// </summary>
private bool ParseFrame(byte[] frame)
{
if (frame.Length < 8)
return false;
byte head = frame[0];
if (head != FRAME_HEAD)
return false;
byte datatype = frame[1];
byte length = frame[2];
byte sn = frame[3];
byte crc8 = frame[4];
byte crc16_h = frame[5];
byte crc16_l = frame[6];
ushort head_crc16 = (ushort)(crc16_l + (crc16_h << 8));
Span<byte> header = [head, datatype, length, sn];
byte crc8_calc = CRCTable.CRC8_Table(header);
if (crc8_calc != crc8)
{
throw new Exception($"CRC8 header error: recv={crc8:X2}, calc={crc8_calc:X2}");
}
if (frame[7 + length] != FRAME_END)
{
throw new Exception($"Frame end error: {BitConverter.ToString(frame)}");
}
ReadOnlySpan<byte> payload = frame.AsSpan(7, length);
ushort crc16_calc = CRCTable.CRC16_Table(payload);
if (crc16_calc != head_crc16)
{
throw new Exception($"CRC16 payload error: recv={head_crc16:X4}, calc={crc16_calc:X4}");
}
switch (datatype)
{
case TYPE_AHRS:
DecodeAHRS([..payload]);
break;
case TYPE_IMU:
DecodeIMU([..payload]);
break;
case TYPE_INSGPS:
DecodeINSGPS([..payload]);
break;
}
return true;
}
public void Dispose()
{
Dispose(true);
GC.SuppressFinalize(this);
}
protected virtual void Dispose(bool disposing)
{
if (disposing)
{
Disconnect();
}
}
}
}