621 lines
22 KiB
C#
621 lines
22 KiB
C#
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();
|
|
}
|
|
}
|
|
}
|
|
}
|