518 lines
21 KiB
C#
518 lines
21 KiB
C#
using System;
|
|
using System.IO.Ports;
|
|
using System.Text;
|
|
|
|
namespace ImuReader
|
|
{
|
|
public class IMUFrame
|
|
{
|
|
public float Gx, Gy, Gz;
|
|
public float Ax, Ay, Az;
|
|
public float Mx, My, Mz;
|
|
public float Roll, Pitch, Yaw;
|
|
public float Temp, Counter;
|
|
}
|
|
|
|
public class AHRSFrame
|
|
{
|
|
public float Roll, Pitch, Yaw;
|
|
public float Q0, Q1, Q2, Q3;
|
|
public float GbX, GbY, GbZ;
|
|
public float Counter;
|
|
}
|
|
|
|
public class INSGPSFrame
|
|
{
|
|
public double Lat, Lon, Alt;
|
|
public float Vx, Vy, Vz;
|
|
public float Roll, Pitch, Yaw;
|
|
public int Status;
|
|
public float Counter;
|
|
}
|
|
|
|
internal class Program
|
|
{
|
|
// Frame định nghĩa
|
|
const byte FRAME_HEAD = 0xFC;
|
|
const byte FRAME_END = 0xFD;
|
|
|
|
// Loại gói
|
|
const byte TYPE_IMU = 0x40;
|
|
const byte TYPE_AHRS = 0x41;
|
|
const byte TYPE_INSGPS = 0x42;
|
|
const byte TYPE_GROUND = 0xF0;
|
|
|
|
// Chiều dài payload
|
|
const int IMU_LEN = 56; // 0x38
|
|
const int AHRS_LEN = 48; // 0x30
|
|
const int INSGPS_LEN = 84; // 0x54
|
|
|
|
static bool if_debug_ = true;
|
|
static bool first_sn_ = false;
|
|
static int read_sn_ = -1;
|
|
static int sn_lost_ = 0;
|
|
|
|
static void Main(string[] args)
|
|
{
|
|
Console.OutputEncoding = Encoding.UTF8;
|
|
|
|
SerialPort serial = new SerialPort()
|
|
{
|
|
PortName = "COM5", // chỉnh lại cho đúng cổng
|
|
BaudRate = 921600,
|
|
ReadTimeout = 2000,
|
|
Parity = Parity.None,
|
|
StopBits = StopBits.One,
|
|
DataBits = 8,
|
|
};
|
|
|
|
try
|
|
{
|
|
serial.Open();
|
|
Console.WriteLine("[INFO] Serial port opened.");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Console.WriteLine("[ERROR] Cannot open port: " + ex.Message);
|
|
return;
|
|
}
|
|
|
|
while (serial.IsOpen)
|
|
{
|
|
try
|
|
{
|
|
// B1: Tìm header (đồng bộ)
|
|
int headInt = serial.ReadByte();
|
|
if (headInt < 0) { Console.WriteLine("[WARN] ReadByte returned -1"); continue; }
|
|
byte head = (byte)headInt;
|
|
if (head != FRAME_HEAD)
|
|
{
|
|
if (if_debug_) Console.WriteLine($"[DEBUG] Bỏ byte: 0x{head:X2}");
|
|
continue;
|
|
}
|
|
|
|
// B2: đọc datatype, length
|
|
int dt = serial.ReadByte();
|
|
if (dt < 0) { Console.WriteLine("[WARN] datatype timeout"); continue; }
|
|
byte datatype = (byte)dt;
|
|
|
|
int ln = serial.ReadByte();
|
|
if (ln < 0) { Console.WriteLine("[WARN] length timeout"); continue; }
|
|
byte length = (byte)ln;
|
|
|
|
if (if_debug_) Console.WriteLine($"[DEBUG] Datatype: 0x{datatype:X2}, Length: {length}");
|
|
|
|
// Kiểm tra loại hợp lệ
|
|
if (datatype != TYPE_IMU && datatype != TYPE_AHRS && datatype != TYPE_INSGPS && datatype != TYPE_GROUND)
|
|
{
|
|
Console.WriteLine("[WARN] datatype error");
|
|
continue;
|
|
}
|
|
|
|
// Kiểm tra độ dài hợp lệ cho từng type
|
|
if (datatype == TYPE_IMU && length != IMU_LEN) { Console.WriteLine($"[ERROR] IMU length error expected {IMU_LEN} got {length}"); continue; }
|
|
if (datatype == TYPE_AHRS && length != AHRS_LEN) { Console.WriteLine($"[ERROR] AHRS length error expected {AHRS_LEN} got {length}"); continue; }
|
|
if (datatype == TYPE_INSGPS && length != INSGPS_LEN) { Console.WriteLine($"[ERROR] INSGPS length error expected {INSGPS_LEN} got {length}"); continue; }
|
|
|
|
// Nếu là GROUND hoặc code 0x50 -> bỏ qua frame này (như code gốc)
|
|
if (datatype == TYPE_GROUND || length == 0x50)
|
|
{
|
|
// read sn
|
|
int gsn = serial.ReadByte();
|
|
if (gsn < 0) { Console.WriteLine("[WARN] ground sn timeout"); continue; }
|
|
byte groundSn = (byte)gsn;
|
|
|
|
// update sn lost similar to C++ logic
|
|
if (!first_sn_)
|
|
{
|
|
read_sn_ = (groundSn - 1) & 0xFF;
|
|
first_sn_ = true;
|
|
}
|
|
int expected = (read_sn_ + 1) & 0xFF;
|
|
if (groundSn != expected)
|
|
{
|
|
int lost = groundSn > expected ? groundSn - expected : 256 - (expected - groundSn);
|
|
sn_lost_ += lost;
|
|
Console.WriteLine($"[WARN] ground SN lost {lost}, total lost {sn_lost_}");
|
|
}
|
|
read_sn_ = groundSn;
|
|
|
|
// đọc phần còn lại và bỏ qua
|
|
int bytesToRead = (length + 4);
|
|
if (bytesToRead > 500) bytesToRead = 500;
|
|
byte[] ground_ignore = new byte[bytesToRead];
|
|
int got = serial.Read(ground_ignore, 0, bytesToRead);
|
|
if (if_debug_) Console.WriteLine($"[DEBUG] Bỏ {got} byte ground");
|
|
continue;
|
|
}
|
|
|
|
// B3: đọc sn, crc8, crc16_h, crc16_l
|
|
int sn_b = serial.ReadByte();
|
|
int crc8_b = serial.ReadByte();
|
|
int crc16_h_b = serial.ReadByte();
|
|
int crc16_l_b = serial.ReadByte();
|
|
if (sn_b < 0 || crc8_b < 0 || crc16_h_b < 0 || crc16_l_b < 0)
|
|
{
|
|
Console.WriteLine("[WARN] header tail bytes timeout");
|
|
continue;
|
|
}
|
|
|
|
byte sn = (byte)sn_b;
|
|
byte crc8_recv = (byte)crc8_b;
|
|
byte crc16_h = (byte)crc16_h_b;
|
|
byte crc16_l = (byte)crc16_l_b;
|
|
|
|
// Tạo header array để check CRC8 (4 byte: head, datatype, length, sn)
|
|
byte[] header4 = new byte[4] { head, datatype, length, sn };
|
|
byte crc8_calc = CRC8_Table(header4, 4);
|
|
if (crc8_calc != crc8_recv)
|
|
{
|
|
Console.WriteLine($"[WARN] header_crc8 error calc=0x{crc8_calc:X2} recv=0x{crc8_recv:X2}");
|
|
continue;
|
|
}
|
|
|
|
// SN handling (check lost)
|
|
if (!first_sn_)
|
|
{
|
|
read_sn_ = (sn - 1) & 0xFF;
|
|
first_sn_ = true;
|
|
}
|
|
int expectedSn = (read_sn_ + 1) & 0xFF;
|
|
if (sn != expectedSn)
|
|
{
|
|
int lost = sn > expectedSn ? sn - expectedSn : 256 - (expectedSn - sn);
|
|
sn_lost_ += lost;
|
|
Console.WriteLine($"[WARN] SN lost {lost}, total lost {sn_lost_}");
|
|
}
|
|
read_sn_ = sn;
|
|
|
|
// B4: đọc payload + end
|
|
int payloadLen = length;
|
|
byte[] payload_plus_end = new byte[payloadLen + 1];
|
|
int readPayload = 0;
|
|
while (readPayload < payload_plus_end.Length)
|
|
{
|
|
int r = serial.Read(payload_plus_end, readPayload, payload_plus_end.Length - readPayload);
|
|
if (r <= 0) break;
|
|
readPayload += r;
|
|
}
|
|
if (readPayload != payload_plus_end.Length)
|
|
{
|
|
Console.WriteLine($"[WARN] Thiếu payload, đọc được {readPayload}/{payload_plus_end.Length}");
|
|
continue;
|
|
}
|
|
|
|
if (payload_plus_end[payloadLen] != FRAME_END)
|
|
{
|
|
Console.WriteLine($"[WARN] frame_end sai: 0x{payload_plus_end[payloadLen]:X2}");
|
|
continue;
|
|
}
|
|
|
|
// B5: check CRC16 (CRC16 trong header: high then low)
|
|
ushort crc16_recv = (ushort)((crc16_h << 8) | crc16_l);
|
|
ushort crc16_calc = CRC16_Table(payload_plus_end, payloadLen); // chỉ tính trên payload (không bao gồm end)
|
|
if (crc16_calc != crc16_recv)
|
|
{
|
|
Console.WriteLine($"[WARN] CRC16 mismatch calc=0x{crc16_calc:X4} recv=0x{crc16_recv:X4}");
|
|
continue;
|
|
}
|
|
|
|
// B6: parse theo loại
|
|
if (datatype == TYPE_IMU)
|
|
{
|
|
IMUFrame imu = ParseIMU(payload_plus_end);
|
|
PrintIMU(imu);
|
|
}
|
|
else if (datatype == TYPE_AHRS)
|
|
{
|
|
AHRSFrame ahrs = ParseAHRS(payload_plus_end);
|
|
PrintAHRS(ahrs);
|
|
}
|
|
else if (datatype == TYPE_INSGPS)
|
|
{
|
|
INSGPSFrame gps = ParseINSGPS(payload_plus_end);
|
|
PrintINSGPS(gps);
|
|
}
|
|
}
|
|
catch (TimeoutException)
|
|
{
|
|
Console.WriteLine("[WARN] Timeout khi đọc serial.");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Console.WriteLine($"[ERROR] {ex.Message}");
|
|
}
|
|
}
|
|
}
|
|
|
|
static IMUFrame ParseIMU(byte[] payloadPlusEnd)
|
|
{
|
|
IMUFrame imu = new IMUFrame();
|
|
// payloadPlusEnd length = IMU_LEN + 1, but BitConverter offsets on payload start at 0
|
|
imu.Gx = BitConverter.ToSingle(payloadPlusEnd, 0);
|
|
imu.Gy = BitConverter.ToSingle(payloadPlusEnd, 4);
|
|
imu.Gz = BitConverter.ToSingle(payloadPlusEnd, 8);
|
|
|
|
imu.Ax = BitConverter.ToSingle(payloadPlusEnd, 12);
|
|
imu.Ay = BitConverter.ToSingle(payloadPlusEnd, 16);
|
|
imu.Az = BitConverter.ToSingle(payloadPlusEnd, 20);
|
|
|
|
imu.Mx = BitConverter.ToSingle(payloadPlusEnd, 24);
|
|
imu.My = BitConverter.ToSingle(payloadPlusEnd, 28);
|
|
imu.Mz = BitConverter.ToSingle(payloadPlusEnd, 32);
|
|
|
|
imu.Roll = BitConverter.ToSingle(payloadPlusEnd, 36);
|
|
imu.Pitch = BitConverter.ToSingle(payloadPlusEnd, 40);
|
|
imu.Yaw = BitConverter.ToSingle(payloadPlusEnd, 44);
|
|
|
|
imu.Temp = BitConverter.ToSingle(payloadPlusEnd, 48);
|
|
imu.Counter = BitConverter.ToSingle(payloadPlusEnd, 52);
|
|
|
|
return imu;
|
|
}
|
|
|
|
static void PrintIMU(IMUFrame imu)
|
|
{
|
|
Console.WriteLine("=== IMU Data ===");
|
|
Console.WriteLine($"Gyro : {imu.Gx:F3}, {imu.Gy:F3}, {imu.Gz:F3}");
|
|
Console.WriteLine($"Accel: {imu.Ax:F3}, {imu.Ay:F3}, {imu.Az:F3}");
|
|
Console.WriteLine($"Mag : {imu.Mx:F3}, {imu.My:F3}, {imu.Mz:F3}");
|
|
Console.WriteLine($"Euler: R={imu.Roll:F3}, P={imu.Pitch:F3}, Y={imu.Yaw:F3}");
|
|
Console.WriteLine($"Temp : {imu.Temp:F3}, Counter={imu.Counter:F0}");
|
|
Console.WriteLine("-------------------------------");
|
|
}
|
|
|
|
static AHRSFrame ParseAHRS(byte[] payloadPlusEnd)
|
|
{
|
|
AHRSFrame ahrs = new AHRSFrame();
|
|
|
|
ahrs.Roll = BitConverter.ToSingle(payloadPlusEnd, 0);
|
|
ahrs.Pitch = BitConverter.ToSingle(payloadPlusEnd, 4);
|
|
ahrs.Yaw = BitConverter.ToSingle(payloadPlusEnd, 8);
|
|
|
|
ahrs.Q0 = BitConverter.ToSingle(payloadPlusEnd, 12);
|
|
ahrs.Q1 = BitConverter.ToSingle(payloadPlusEnd, 16);
|
|
ahrs.Q2 = BitConverter.ToSingle(payloadPlusEnd, 20);
|
|
ahrs.Q3 = BitConverter.ToSingle(payloadPlusEnd, 24);
|
|
|
|
ahrs.GbX = BitConverter.ToSingle(payloadPlusEnd, 28);
|
|
ahrs.GbY = BitConverter.ToSingle(payloadPlusEnd, 32);
|
|
ahrs.GbZ = BitConverter.ToSingle(payloadPlusEnd, 36);
|
|
|
|
ahrs.Counter = BitConverter.ToSingle(payloadPlusEnd, 40);
|
|
|
|
return ahrs;
|
|
}
|
|
|
|
static void PrintAHRS(AHRSFrame ahrs)
|
|
{
|
|
Console.WriteLine("=== AHRS Data ===");
|
|
Console.WriteLine($"Euler: R={ahrs.Roll:F3}, P={ahrs.Pitch:F3}, Y={ahrs.Yaw:F3}");
|
|
Console.WriteLine($"Quat : {ahrs.Q0:F3}, {ahrs.Q1:F3}, {ahrs.Q2:F3}, {ahrs.Q3:F3}");
|
|
Console.WriteLine($"GyroBias: {ahrs.GbX:F3}, {ahrs.GbY:F3}, {ahrs.GbZ:F3}");
|
|
Console.WriteLine($"Counter: {ahrs.Counter:F0}");
|
|
Console.WriteLine("-------------------------------");
|
|
}
|
|
|
|
static INSGPSFrame ParseINSGPS(byte[] payloadPlusEnd)
|
|
{
|
|
INSGPSFrame gps = new INSGPSFrame();
|
|
|
|
gps.Lat = BitConverter.ToDouble(payloadPlusEnd, 0);
|
|
gps.Lon = BitConverter.ToDouble(payloadPlusEnd, 8);
|
|
gps.Alt = BitConverter.ToDouble(payloadPlusEnd, 16);
|
|
|
|
gps.Vx = BitConverter.ToSingle(payloadPlusEnd, 24);
|
|
gps.Vy = BitConverter.ToSingle(payloadPlusEnd, 28);
|
|
gps.Vz = BitConverter.ToSingle(payloadPlusEnd, 32);
|
|
|
|
gps.Roll = BitConverter.ToSingle(payloadPlusEnd, 36);
|
|
gps.Pitch = BitConverter.ToSingle(payloadPlusEnd, 40);
|
|
gps.Yaw = BitConverter.ToSingle(payloadPlusEnd, 44);
|
|
|
|
gps.Status = BitConverter.ToInt32(payloadPlusEnd, 48);
|
|
gps.Counter = BitConverter.ToSingle(payloadPlusEnd, 52);
|
|
|
|
return gps;
|
|
}
|
|
|
|
static void PrintINSGPS(INSGPSFrame gps)
|
|
{
|
|
Console.WriteLine("=== INSGPS Data ===");
|
|
Console.WriteLine($"Lat={gps.Lat:F6}, Lon={gps.Lon:F6}, Alt={gps.Alt:F2}");
|
|
Console.WriteLine($"Vel: {gps.Vx:F3}, {gps.Vy:F3}, {gps.Vz:F3}");
|
|
Console.WriteLine($"Euler: R={gps.Roll:F3}, P={gps.Pitch:F3}, Y={gps.Yaw:F3}");
|
|
Console.WriteLine($"Status={gps.Status}, Counter={gps.Counter:F0}");
|
|
Console.WriteLine("-------------------------------");
|
|
}
|
|
|
|
// ===== CRC tables & funcs (port từ C++ của bạn) =====
|
|
static readonly byte[] CRC8Table = new byte[] {
|
|
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};
|
|
|
|
static readonly ushort[] CRC16Table = new ushort[] {
|
|
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
|
|
};
|
|
|
|
static byte CRC8_Table(byte[] p, int count)
|
|
{
|
|
byte crc8 = 0;
|
|
for (int i = 0; i < count; i++)
|
|
{
|
|
crc8 = CRC8Table[crc8 ^ p[i]];
|
|
}
|
|
return crc8;
|
|
}
|
|
|
|
static ushort CRC16_Table(byte[] p, int count)
|
|
{
|
|
ushort crc16 = 0;
|
|
for (int i = 0; i < count; i++)
|
|
{
|
|
crc16 = (ushort)(CRC16Table[((crc16 >> 8) ^ p[i]) & 0xff] ^ (crc16 << 8));
|
|
}
|
|
return crc16;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
//using ConsoleApp1;
|
|
//using System.IO.Ports;
|
|
//using System.Text;
|
|
|
|
//namespace ImuReader
|
|
//{
|
|
// internal class Program
|
|
// {
|
|
// const byte FRAME_HEAD = 0xFC;
|
|
// const byte FRAME_END = 0xFD;
|
|
|
|
// const byte TYPE_IMU = 0x40;
|
|
// const byte TYPE_AHRS = 0x41;
|
|
// const byte TYPE_INSGPS = 0x42;
|
|
// const byte TYPE_GROUND = 0xF0;
|
|
|
|
// const byte IMU_LEN = 0x38; // 56
|
|
// const byte AHRS_LEN = 0x30; // 48
|
|
// const byte INSGPS_LEN = 0x54; // 84
|
|
|
|
// static bool if_debug_;
|
|
// static bool frist_sn_;
|
|
// static byte read_sn_;
|
|
// static int sn_lost_;
|
|
|
|
// static void Main(string[] args)
|
|
// {
|
|
// Console.OutputEncoding = Encoding.UTF8;
|
|
|
|
// SerialPort serial = new SerialPort()
|
|
// {
|
|
// PortName = "COM5",
|
|
// BaudRate = 921600,
|
|
// ReadTimeout = 2000,
|
|
// Parity = Parity.None,
|
|
// StopBits = StopBits.One,
|
|
// DataBits = 8,
|
|
// };
|
|
|
|
// serial.Open();
|
|
// Console.WriteLine("[INFO] Serial port opened.");
|
|
|
|
// while (serial.IsOpen)
|
|
// {
|
|
// byte head = (byte)serial.ReadByte();
|
|
// if (head != FRAME_HEAD) continue;
|
|
|
|
// byte datatype = (byte)serial.ReadByte();
|
|
// byte length = (byte)serial.ReadByte();
|
|
|
|
// if (datatype != TYPE_IMU && datatype != TYPE_AHRS && datatype != TYPE_INSGPS && datatype != TYPE_GROUND)
|
|
// continue;
|
|
|
|
// // Bỏ qua ground packet
|
|
// if (datatype == TYPE_GROUND || datatype == 0x50)
|
|
// {
|
|
// byte[] ground_ignore = new byte[length + 4];
|
|
// serial.Read(ground_ignore, 0, ground_ignore.Length);
|
|
// continue;
|
|
// }
|
|
|
|
// byte sn_s = (byte)serial.ReadByte();
|
|
// byte crc8_s = (byte)serial.ReadByte();
|
|
// byte crc16_H_s = (byte)serial.ReadByte();
|
|
// byte crc16_L_s = (byte)serial.ReadByte();
|
|
|
|
// // Đọc payload + FRAME_END
|
|
// byte[] frame_data = new byte[length + 1];
|
|
// int read_count = serial.Read(frame_data, 0, frame_data.Length);
|
|
|
|
// if (frame_data[length] != FRAME_END)
|
|
// {
|
|
// Console.WriteLine($"[WARN] frame_end error, type=0x{datatype:X2}");
|
|
// continue;
|
|
// }
|
|
|
|
// // In dữ liệu thô ra hex
|
|
// byte[] full_frame = new byte[7 + frame_data.Length];
|
|
// full_frame[0] = head;
|
|
// full_frame[1] = datatype;
|
|
// full_frame[2] = length;
|
|
// full_frame[3] = sn_s;
|
|
// full_frame[4] = crc8_s;
|
|
// full_frame[5] = crc16_H_s;
|
|
// full_frame[6] = crc16_L_s;
|
|
// Buffer.BlockCopy(frame_data, 0, full_frame, 7, frame_data.Length);
|
|
|
|
// Console.WriteLine($"[RAW] {BitConverter.ToString(full_frame)}");
|
|
// }
|
|
// }
|
|
// }
|
|
//}
|