update imu, lidar
This commit is contained in:
@@ -0,0 +1,503 @@
|
||||
using System;
|
||||
using System.IO;
|
||||
using System.IO.Ports;
|
||||
using System.Threading;
|
||||
|
||||
namespace RobotNet10.RobotApp.Drivers.HfiA9IMU
|
||||
{
|
||||
/// <summary>
|
||||
/// Doc du lieu tu IMU HandsFree HFI-A9 qua serial (CP210x USB-UART, 921600 baud).
|
||||
/// Giao thuc: frame = 0xAA 0x55 [LEN] [payload LEN bytes] [CRC16-Modbus lo, hi]
|
||||
/// CRC tinh tren [LEN] + payload.
|
||||
/// LEN 0x2C: 10 float LE tai payload offset 4:
|
||||
/// [0]=timestamp, [1:4]=gyro (rad/s), [4:7]=accel (g), [7:10]=mag
|
||||
/// LEN 0x14: 4 float LE tai payload offset 4: [1:4]=roll/pitch/yaw (do)
|
||||
/// </summary>
|
||||
public class HfiA9Reader : IDisposable
|
||||
{
|
||||
// Frame lon nhat: 0x2C + 5 = 49 bytes, dung 256 bytes de dam bao an toan
|
||||
private const int FRAME_BUFFER_SIZE = 256;
|
||||
private readonly byte[] _frameBuffer = new byte[FRAME_BUFFER_SIZE];
|
||||
private int _frameBufferLength = 0;
|
||||
|
||||
// Non-volatile field de dung voi Volatile.Write lam memory barrier
|
||||
private int _memoryBarrier = 0;
|
||||
|
||||
// Thread doc du lieu tu SerialPort voi priority cao
|
||||
private Thread? _readingThread;
|
||||
private volatile bool _shouldRead = false;
|
||||
private CancellationTokenSource? _readingCts;
|
||||
|
||||
// Event de thong bao khi co du lieu moi duoc decode (fire sau frame Euler
|
||||
// de moi event tuong ung mot chu ky mau day du: gyro/accel/mag + euler)
|
||||
public event EventHandler? DataReceived;
|
||||
|
||||
// Event bao trang thai ket noi thay doi (true = vua mo port, false = mat ket noi)
|
||||
// de device class bao cho DeviceBase cap nhat state machine / UI
|
||||
public event EventHandler<bool>? ConnectionStateChanged;
|
||||
|
||||
// Properties - lock-free voi memory barriers (volatile khong ho tro double)
|
||||
public double Roll { get; private set; }
|
||||
public double Pitch { get; private set; }
|
||||
public double Yaw { get; private set; }
|
||||
|
||||
public double Gx { get; private set; }
|
||||
public double Gy { get; private set; }
|
||||
public double Gz { get; private set; }
|
||||
|
||||
public double AccX { get; private set; }
|
||||
public double AccY { get; private set; }
|
||||
public double AccZ { get; private set; }
|
||||
|
||||
public double MagX { get; private set; }
|
||||
public double MagY { get; private set; }
|
||||
public double MagZ { get; private set; }
|
||||
|
||||
const byte FRAME_HEAD_1 = 0xAA;
|
||||
const byte FRAME_HEAD_2 = 0x55;
|
||||
|
||||
// LEN byte cua tung loai goi
|
||||
const byte LEN_IMU = 0x2C; // 44: timestamp + gyro + accel + mag (10 float)
|
||||
const byte LEN_EULER = 0x14; // 20: timestamp + roll/pitch/yaw (4 float)
|
||||
|
||||
// HFI-A9 xuat accel theo don vi g; nhan -9.8 theo quy uoc truc cua driver hang
|
||||
// (dung yen, Z huong len -> AccZ ~ +9.8 m/s², cung quy uoc voi Wheeltec N100)
|
||||
private const double ACCEL_SCALE = -9.8;
|
||||
private const double DEG_TO_RAD = Math.PI / 180.0;
|
||||
|
||||
private SerialPort? serial;
|
||||
|
||||
// Luu thong so port de tao lai SerialPort khi reconnect
|
||||
private readonly string _portName;
|
||||
private readonly int _baudRate;
|
||||
private readonly int _timeOut;
|
||||
|
||||
// Thoi gian backoff giua cac lan thu reconnect (ms)
|
||||
private const int RECONNECT_BACKOFF_MS = 1000;
|
||||
|
||||
// Watchdog: neu qua khoang thoi gian nay khong co frame hop le -> coi nhu mat ket noi
|
||||
// va trigger reconnect. Dung cho truong hop USB "chet mem" khong ne'm exception.
|
||||
private const long DATA_TIMEOUT_TICKS = 3 * TimeSpan.TicksPerSecond;
|
||||
private long _lastFrameTicks;
|
||||
|
||||
// Kiem tra dinh ky file port con ton tai khong (rut USB -> /dev/ttyUSBx bien mat)
|
||||
// de phat hien disconnect nhanh hon watchdog
|
||||
private const long PORT_CHECK_INTERVAL_TICKS = TimeSpan.TicksPerSecond / 2;
|
||||
private long _lastPortCheckTicks;
|
||||
|
||||
public bool IsConnected => serial != null && serial.IsOpen;
|
||||
|
||||
/// <summary>
|
||||
/// Co frame hop le trong DATA_TIMEOUT gan nhat khong — dung de phan biet
|
||||
/// "port mo nhung khong co du lieu" (vd ModemManager dang giu port sau khi cam lai)
|
||||
/// </summary>
|
||||
public bool HasRecentData => DateTime.UtcNow.Ticks - Volatile.Read(ref _lastFrameTicks) < DATA_TIMEOUT_TICKS;
|
||||
|
||||
public HfiA9Reader(string portName, int baudRate, int timeOut)
|
||||
{
|
||||
_portName = portName;
|
||||
_baudRate = baudRate;
|
||||
_timeOut = timeOut;
|
||||
|
||||
serial = CreateSerialPort();
|
||||
}
|
||||
|
||||
private SerialPort CreateSerialPort()
|
||||
{
|
||||
return new SerialPort()
|
||||
{
|
||||
PortName = _portName,
|
||||
BaudRate = _baudRate,
|
||||
ReadTimeout = _timeOut,
|
||||
Parity = Parity.None,
|
||||
StopBits = StopBits.One,
|
||||
DataBits = 8,
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Snapshot structure de lay tat ca du lieu cung luc mot cach thread-safe
|
||||
/// </summary>
|
||||
public struct DataSnapshot
|
||||
{
|
||||
public double AccX, AccY, AccZ;
|
||||
public double Gx, Gy, Gz;
|
||||
public double MagX, MagY, MagZ;
|
||||
public double Roll, Pitch, Yaw;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Lay snapshot cua tat ca du lieu hien tai mot cach thread-safe
|
||||
/// </summary>
|
||||
public DataSnapshot GetSnapshot()
|
||||
{
|
||||
Volatile.Read(ref _memoryBarrier);
|
||||
return new DataSnapshot
|
||||
{
|
||||
AccX = AccX,
|
||||
AccY = AccY,
|
||||
AccZ = AccZ,
|
||||
Gx = Gx,
|
||||
Gy = Gy,
|
||||
Gz = Gz,
|
||||
MagX = MagX,
|
||||
MagY = MagY,
|
||||
MagZ = MagZ,
|
||||
Roll = Roll,
|
||||
Pitch = Pitch,
|
||||
Yaw = Yaw,
|
||||
};
|
||||
}
|
||||
|
||||
public void Connect()
|
||||
{
|
||||
_frameBufferLength = 0;
|
||||
Array.Clear(_frameBuffer, 0, FRAME_BUFFER_SIZE);
|
||||
|
||||
// Reading thread se tu mo port trong outer loop va tu reconnect khi mat ket noi
|
||||
StartReadingThread();
|
||||
}
|
||||
|
||||
private void StartReadingThread()
|
||||
{
|
||||
if (_readingThread != null && _readingThread.IsAlive)
|
||||
return;
|
||||
|
||||
_shouldRead = true;
|
||||
|
||||
_readingCts?.Dispose();
|
||||
_readingCts = new CancellationTokenSource();
|
||||
|
||||
_readingThread = new Thread(() => ReadingThreadLoop(_readingCts.Token))
|
||||
{
|
||||
Name = "HfiA9IMU-Reading",
|
||||
IsBackground = false,
|
||||
Priority = ThreadPriority.Highest
|
||||
};
|
||||
|
||||
_readingThread.Start();
|
||||
}
|
||||
|
||||
private void StopReadingThread()
|
||||
{
|
||||
_shouldRead = false;
|
||||
|
||||
_readingCts?.Cancel();
|
||||
|
||||
if (_readingThread != null)
|
||||
{
|
||||
if (!_readingThread.Join(1000))
|
||||
{
|
||||
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] Reading thread did not stop gracefully");
|
||||
}
|
||||
_readingThread = null;
|
||||
}
|
||||
|
||||
_readingCts?.Dispose();
|
||||
_readingCts = null;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Reading thread loop - outer loop xu ly reconnect, inner loop doc du lieu
|
||||
/// </summary>
|
||||
private void ReadingThreadLoop(CancellationToken cancellationToken)
|
||||
{
|
||||
Thread.BeginThreadAffinity();
|
||||
byte[] readBuffer = new byte[256];
|
||||
|
||||
while (_shouldRead && !cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
try
|
||||
{
|
||||
if (serial == null || !serial.IsOpen)
|
||||
{
|
||||
SafeCloseSerial();
|
||||
serial = CreateSerialPort();
|
||||
serial.Open();
|
||||
_frameBufferLength = 0;
|
||||
serial.DiscardInBuffer();
|
||||
_lastFrameTicks = DateTime.UtcNow.Ticks;
|
||||
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] Connected to {_portName}");
|
||||
RaiseConnectionStateChanged(true);
|
||||
}
|
||||
|
||||
InnerReadLoop(readBuffer, cancellationToken);
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
break;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] IMU disconnected: {ex.Message}");
|
||||
SafeCloseSerial();
|
||||
RaiseConnectionStateChanged(false);
|
||||
}
|
||||
|
||||
if (_shouldRead && !cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
cancellationToken.WaitHandle.WaitOne(RECONNECT_BACKOFF_MS);
|
||||
}
|
||||
}
|
||||
|
||||
SafeCloseSerial();
|
||||
Thread.EndThreadAffinity();
|
||||
}
|
||||
|
||||
private void InnerReadLoop(byte[] readBuffer, CancellationToken cancellationToken)
|
||||
{
|
||||
while (_shouldRead && !cancellationToken.IsCancellationRequested
|
||||
&& serial != null && serial.IsOpen)
|
||||
{
|
||||
// Watchdog phai kiem tra MOI vong lap: khi rut USB tren Linux,
|
||||
// BytesToRead co the van > 0 nhung Read() tra ve 0 (EOF) lien tuc,
|
||||
// neu chi kiem tra trong nhanh bytesToRead <= 0 se khong bao gio phat hien
|
||||
long nowTicks = DateTime.UtcNow.Ticks;
|
||||
if (nowTicks - _lastFrameTicks > DATA_TIMEOUT_TICKS)
|
||||
{
|
||||
throw new TimeoutException($"No IMU frame for > {DATA_TIMEOUT_TICKS / TimeSpan.TicksPerSecond}s");
|
||||
}
|
||||
|
||||
// Rut USB -> device file bien mat: phat hien nhanh hon watchdog
|
||||
if (nowTicks - _lastPortCheckTicks > PORT_CHECK_INTERVAL_TICKS)
|
||||
{
|
||||
_lastPortCheckTicks = nowTicks;
|
||||
if (!File.Exists(_portName))
|
||||
{
|
||||
throw new IOException($"Serial port {_portName} no longer exists (USB unplugged?)");
|
||||
}
|
||||
}
|
||||
|
||||
int bytesToRead = serial.BytesToRead;
|
||||
if (bytesToRead <= 0)
|
||||
{
|
||||
Thread.Sleep(1);
|
||||
continue;
|
||||
}
|
||||
|
||||
int bytesRead = serial.Read(readBuffer, 0, Math.Min(bytesToRead, readBuffer.Length));
|
||||
if (bytesRead <= 0)
|
||||
{
|
||||
Thread.Sleep(1);
|
||||
continue;
|
||||
}
|
||||
|
||||
ProcessIncomingData(readBuffer, bytesRead);
|
||||
}
|
||||
}
|
||||
|
||||
private void SafeCloseSerial()
|
||||
{
|
||||
if (serial == null) return;
|
||||
|
||||
try
|
||||
{
|
||||
serial.Close();
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] Error closing serial: {ex.Message}");
|
||||
}
|
||||
|
||||
try { serial.Dispose(); }
|
||||
catch { }
|
||||
|
||||
serial = null;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Them du lieu moi vao frame buffer va parse cac frame hoan chinh
|
||||
/// </summary>
|
||||
private void ProcessIncomingData(byte[] data, int length)
|
||||
{
|
||||
int dataOffset = 0;
|
||||
|
||||
while (dataOffset < length)
|
||||
{
|
||||
ProcessCompleteFramesInBuffer();
|
||||
|
||||
int availableSpace = FRAME_BUFFER_SIZE - _frameBufferLength;
|
||||
|
||||
if (availableSpace == 0)
|
||||
{
|
||||
// Buffer day ma khong co frame hop le -> du lieu rac, xoa het
|
||||
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] Buffer full without valid frame, clearing buffer");
|
||||
_frameBufferLength = 0;
|
||||
availableSpace = FRAME_BUFFER_SIZE;
|
||||
}
|
||||
|
||||
int bytesToAdd = Math.Min(length - dataOffset, availableSpace);
|
||||
Array.Copy(data, dataOffset, _frameBuffer, _frameBufferLength, bytesToAdd);
|
||||
_frameBufferLength += bytesToAdd;
|
||||
dataOffset += bytesToAdd;
|
||||
|
||||
ProcessCompleteFramesInBuffer();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Xu ly tat ca cac frame hoan chinh trong buffer hien tai
|
||||
/// </summary>
|
||||
private void ProcessCompleteFramesInBuffer()
|
||||
{
|
||||
while (_frameBufferLength > 0)
|
||||
{
|
||||
ReadOnlySpan<byte> bufferSpan = new(_frameBuffer, 0, _frameBufferLength);
|
||||
int headIndex = bufferSpan.IndexOf(FRAME_HEAD_1);
|
||||
|
||||
if (headIndex < 0)
|
||||
{
|
||||
_frameBufferLength = 0;
|
||||
break;
|
||||
}
|
||||
|
||||
if (headIndex > 0)
|
||||
{
|
||||
ShiftBuffer(headIndex);
|
||||
continue;
|
||||
}
|
||||
|
||||
// Can toi thieu 3 bytes: AA 55 LEN
|
||||
if (_frameBufferLength < 3)
|
||||
break;
|
||||
|
||||
if (_frameBuffer[1] != FRAME_HEAD_2)
|
||||
{
|
||||
ShiftBuffer(1);
|
||||
continue;
|
||||
}
|
||||
|
||||
byte payloadLength = _frameBuffer[2];
|
||||
if (payloadLength != LEN_IMU && payloadLength != LEN_EULER)
|
||||
{
|
||||
ShiftBuffer(1);
|
||||
continue;
|
||||
}
|
||||
|
||||
int totalFrameLength = payloadLength + 5;
|
||||
if (_frameBufferLength < totalFrameLength)
|
||||
break;
|
||||
|
||||
// CRC16-Modbus tren [LEN + payload], luu little-endian sau payload
|
||||
ushort crcCalc = Crc16Modbus(_frameBuffer.AsSpan(2, 1 + payloadLength));
|
||||
ushort crcRecv = (ushort)(_frameBuffer[3 + payloadLength] | (_frameBuffer[4 + payloadLength] << 8));
|
||||
if (crcCalc != crcRecv)
|
||||
{
|
||||
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] CRC16 error: recv={crcRecv:X4}, calc={crcCalc:X4}");
|
||||
ShiftBuffer(2);
|
||||
continue;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
DecodeFrame(payloadLength, _frameBuffer.AsSpan(3, payloadLength));
|
||||
_lastFrameTicks = DateTime.UtcNow.Ticks;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] Error parsing frame: {ex.Message}");
|
||||
}
|
||||
|
||||
ShiftBuffer(totalFrameLength);
|
||||
}
|
||||
}
|
||||
|
||||
private void ShiftBuffer(int count)
|
||||
{
|
||||
int remaining = _frameBufferLength - count;
|
||||
if (remaining > 0)
|
||||
{
|
||||
Array.Copy(_frameBuffer, count, _frameBuffer, 0, remaining);
|
||||
}
|
||||
_frameBufferLength = Math.Max(0, remaining);
|
||||
}
|
||||
|
||||
private void DecodeFrame(byte payloadLength, ReadOnlySpan<byte> payload)
|
||||
{
|
||||
if (payloadLength == LEN_IMU)
|
||||
{
|
||||
// float [0] la timestamp noi bo, bo qua
|
||||
Gx = BitConverter.ToSingle(payload.Slice(8, 4));
|
||||
Gy = BitConverter.ToSingle(payload.Slice(12, 4));
|
||||
Gz = BitConverter.ToSingle(payload.Slice(16, 4));
|
||||
AccX = BitConverter.ToSingle(payload.Slice(20, 4)) * ACCEL_SCALE;
|
||||
AccY = BitConverter.ToSingle(payload.Slice(24, 4)) * ACCEL_SCALE;
|
||||
AccZ = BitConverter.ToSingle(payload.Slice(28, 4)) * ACCEL_SCALE;
|
||||
MagX = BitConverter.ToSingle(payload.Slice(32, 4));
|
||||
MagY = BitConverter.ToSingle(payload.Slice(36, 4));
|
||||
MagZ = BitConverter.ToSingle(payload.Slice(40, 4));
|
||||
Volatile.Write(ref _memoryBarrier, 0);
|
||||
}
|
||||
else if (payloadLength == LEN_EULER)
|
||||
{
|
||||
Roll = BitConverter.ToSingle(payload.Slice(8, 4)) * DEG_TO_RAD;
|
||||
Pitch = BitConverter.ToSingle(payload.Slice(12, 4)) * DEG_TO_RAD;
|
||||
Yaw = BitConverter.ToSingle(payload.Slice(16, 4)) * DEG_TO_RAD;
|
||||
Volatile.Write(ref _memoryBarrier, 0);
|
||||
|
||||
// Frame Euler den sau frame IMU trong moi chu ky -> fire event mot lan/chu ky
|
||||
try
|
||||
{
|
||||
DataReceived?.Invoke(this, EventArgs.Empty);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] DataReceived subscriber error: {ex.Message}");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private void RaiseConnectionStateChanged(bool connected)
|
||||
{
|
||||
try
|
||||
{
|
||||
ConnectionStateChanged?.Invoke(this, connected);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] ConnectionStateChanged subscriber error: {ex.Message}");
|
||||
}
|
||||
}
|
||||
|
||||
private static ushort Crc16Modbus(ReadOnlySpan<byte> data)
|
||||
{
|
||||
ushort crc = 0xFFFF;
|
||||
foreach (byte b in data)
|
||||
{
|
||||
crc ^= b;
|
||||
for (int i = 0; i < 8; i++)
|
||||
{
|
||||
if ((crc & 1) != 0)
|
||||
crc = (ushort)((crc >> 1) ^ 0xA001);
|
||||
else
|
||||
crc >>= 1;
|
||||
}
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
public void Disconnect()
|
||||
{
|
||||
StopReadingThread();
|
||||
|
||||
SafeCloseSerial();
|
||||
|
||||
_frameBufferLength = 0;
|
||||
Array.Clear(_frameBuffer, 0, FRAME_BUFFER_SIZE);
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
Dispose(true);
|
||||
GC.SuppressFinalize(this);
|
||||
}
|
||||
|
||||
protected virtual void Dispose(bool disposing)
|
||||
{
|
||||
if (disposing)
|
||||
{
|
||||
Disconnect();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user