Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/WheeltecIMU/WheeltecReader.cs

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