From 04066eda960e6ea037568ffa4fac2b1addd1b986 Mon Sep 17 00:00:00 2001 From: ttminh03 Date: Fri, 23 Jan 2026 02:28:54 +0700 Subject: [PATCH] add imu --- .vs/ConsoleApp1/v18/.suo | Bin 0 -> 3584 bytes .vs/VSWorkspaceState.json | 7 + .vs/imu.slnx/v18/.wsuo | Bin 0 -> 13312 bytes .vs/imu.slnx/v18/DocumentLayout.json | 12 + ConsoleApp1.csproj | 10 + ConsoleApp1.sln | 25 + Program.cs | 517 ++++++++++++++++++ obj/ConsoleApp1.csproj.nuget.dgspec.json | 91 +++ obj/ConsoleApp1.csproj.nuget.g.props | 16 + obj/ConsoleApp1.csproj.nuget.g.targets | 2 + ...CoreApp,Version=v8.0.AssemblyAttributes.cs | 4 + obj/Debug/net8.0/ConsoleApp1.AssemblyInfo.cs | 23 + .../ConsoleApp1.AssemblyInfoInputs.cache | 1 + ....GeneratedMSBuildEditorConfig.editorconfig | 17 + .../net8.0/ConsoleApp1.GlobalUsings.g.cs | 8 + obj/Debug/net8.0/ConsoleApp1.assets.cache | Bin 0 -> 228 bytes ...ConsoleApp1.csproj.CoreCompileInputs.cache | 1 + .../ConsoleApp1.csproj.FileListAbsolute.txt | 8 + obj/project.assets.json | 97 ++++ obj/project.nuget.cache | 13 + 20 files changed, 852 insertions(+) create mode 100644 .vs/ConsoleApp1/v18/.suo create mode 100644 .vs/VSWorkspaceState.json create mode 100644 .vs/imu.slnx/v18/.wsuo create mode 100644 .vs/imu.slnx/v18/DocumentLayout.json create mode 100644 ConsoleApp1.csproj create mode 100644 ConsoleApp1.sln create mode 100644 Program.cs create mode 100644 obj/ConsoleApp1.csproj.nuget.dgspec.json create mode 100644 obj/ConsoleApp1.csproj.nuget.g.props create mode 100644 obj/ConsoleApp1.csproj.nuget.g.targets create mode 100644 obj/Debug/net8.0/.NETCoreApp,Version=v8.0.AssemblyAttributes.cs create mode 100644 obj/Debug/net8.0/ConsoleApp1.AssemblyInfo.cs create mode 100644 obj/Debug/net8.0/ConsoleApp1.AssemblyInfoInputs.cache create mode 100644 obj/Debug/net8.0/ConsoleApp1.GeneratedMSBuildEditorConfig.editorconfig create mode 100644 obj/Debug/net8.0/ConsoleApp1.GlobalUsings.g.cs create mode 100644 obj/Debug/net8.0/ConsoleApp1.assets.cache create mode 100644 obj/Debug/net8.0/ConsoleApp1.csproj.CoreCompileInputs.cache create mode 100644 obj/Debug/net8.0/ConsoleApp1.csproj.FileListAbsolute.txt create mode 100644 obj/project.assets.json create mode 100644 obj/project.nuget.cache diff --git a/.vs/ConsoleApp1/v18/.suo b/.vs/ConsoleApp1/v18/.suo new file mode 100644 index 0000000000000000000000000000000000000000..8104657a75c8a5fae07e79a85b8b7a8fe993e44e GIT binary patch literal 3584 zcmca`Uhu)fjZzO8(10BSGsD0CoD6J8;*3Bx6O1pwz`z1zgT(&*|NkE(%nZap<|r6K zA@CRIR#5znf{_^lK@9moSi+#d;L4B(q>C6TL0Oq-AjZIolIMxh2rDm$QA>;(J>Ln13p#X~j$eqCI3k-Z2G8u}2FcWBhI)gLN1-T3bKr)XZ6`UW6;VyGxNCdhS z9T)*?M3B8OfG&@VFN8~i3~`vjPM*41XFF*Q4=0{Novy4?*P+ ip>i8vnW9?wB9O5)#y7E6Cvvvb-6VW!N96`z2mk$X+a5eLqiqJ9e| zYfhRuT}$RBqKsX3nN1a$KNT@5`_a#@|N8OonJ3im_@i#;cZFCqm(4pupVr+;b0cDw z;OkvxLj@nU2jA9z%BorF>y-1tofU4*%LU;&0Ko`wjHf&dKYi)$cJ?=zLNYSwEx7JLq{t_d8U%e_Yp{s?U1Q z^l6m`lLM={|Nq`v-T!6{_y0cWWo!S72`W}AtsSs?nJXgNtjd1gq*}ZG;UWEO!B?>N zt?T;W0P?F>vFEkz=z+ft`Qron+mha|4=(+8S84qn{ET_W6Dr9MDw$DvKRMMY$v3RD zrSQM-9X|6W9WhshGHYh!hfy#3kO$RL3Io?W;b)Kzy2&-7yYm{q#l|%!Z{%XQd0~gx=@WGO3lyk53{0y&CtB>Pi*p zwq>Q}Ofrwf=9*;OO7(39|AMscoFt62C-nD*B##E53zLz%-x%Vbr1V_#E<9zr4}FSGNmn4bYa1~t+t}pIA3Q)-J%~KB&!;iet3m1 zONoOY#)pF`^dDdQjDx-$pL3XfHy^6?-e6r6|0mWzXt(yimVWTv9gx>5@=iV`Y&|_G z=;y3zgn!E0|C~QpCHE*BME-;Ge79;``Geg^|El;mWs|Uov+CWrq~U+q{u>t83qmCi z=eLE8bnvwl^2hf%=aDohO&pS?#n;M-)}rjdh_nwIi2S1p_MiQ1W51~Wyhm2n{&U*Q zs*-I34=>}Yu(3d{S2C@Y(c_HXa7qNrVbM=)`hsd)`I9fhXQNnsF9{R+kw0i<5B;+4 zSzGPd;Ep!t%@q9i%fHsoa&e&fqND#S|JUpvlCtCT}w*VcYpYsHtJ}y4`(#Za~Hit+A8#HD5wSsy1 zM|ehfVzl}T_8)!1r;m%zezp8({}Xw!-$4QStlF^q_+syA7a$3IqjDums;;0{_{j6c z1o(db`)%y7R)Q69J)sU+0DMu`4xZ8)T>BH0Lz?ps+KE>wVfYsu$`@p-b2^D^gvf}z zICII-PQ_oDHq8U?DbA!3f7mZp{;b2=!iZsYUpG4*-?bTh=wN(xj``zyj9r>te{7(T zj>PY^rJ1(cx&F=PjF0naaI76jGnO&P{eTS9yh-E5!sN0*6Nw>O`b%;t%wHpr7vc~h? zh|`@W_-n5`hu;N9b!xg~Wz@87x!?clr$3#({o8l*?$6?wXpOZlB2Ka_xtEJ15$6J-P1o+@XoSu8F=QU59$!aM;}} z$AfEm|DKH`gY9vj+N8a~3}o44l?#BcUCO%gH>@;E4dV}(xAfm0+#PjvX;?dARwvAi zBtaIKY$VaQ%@xy|GvsL4!dB@Wxnz8=wJu-W>HqQ8&P77eM5U0Mnfqk+>z=gZ4}5>+ zRx{)Ah3eJS`u+oUtE?Rd?4E9Y|M`D@{|Spf#+)qhTxdQ>`PWM9ffmOxe)zWdER-<( z0i8N-BwFz0fNdVgjl+&9{@Jg2_Nm6j=cJ1hfBEt4Hxf=oe;!)-p9~(rfdu&KTES29 z{muO^H8MnG$QGDJd7R|@Cs$_kuSxLvPO;M8|ArOqpEp@~8ss~w-y5^OOq@e#gFk8V zujq=Ee}icgHT$i1qJb;axfy)&d;ae~HXq72xOI~Mcm5A?4fFrb_s^Fk%eeN(pMR?5 z|LW)egYs{E|6Bh3F_`fSZrTUn`(NVk@BZ(9t@tk}&$Lfz + + + Exe + net8.0 + enable + enable + + + diff --git a/ConsoleApp1.sln b/ConsoleApp1.sln new file mode 100644 index 0000000..e073322 --- /dev/null +++ b/ConsoleApp1.sln @@ -0,0 +1,25 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Version 17 +VisualStudioVersion = 17.14.36414.22 d17.14 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "ConsoleApp1", "ConsoleApp1.csproj", "{450FBA11-1FB5-4439-8CE3-32F0E61FFA45}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Release|Any CPU = Release|Any CPU + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {450FBA11-1FB5-4439-8CE3-32F0E61FFA45}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {450FBA11-1FB5-4439-8CE3-32F0E61FFA45}.Debug|Any CPU.Build.0 = Debug|Any CPU + {450FBA11-1FB5-4439-8CE3-32F0E61FFA45}.Release|Any CPU.ActiveCfg = Release|Any CPU + {450FBA11-1FB5-4439-8CE3-32F0E61FFA45}.Release|Any CPU.Build.0 = Release|Any CPU + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection + GlobalSection(ExtensibilityGlobals) = postSolution + SolutionGuid = {59CD9CC3-7ADD-44A3-9DC4-74F1674CD71F} + EndGlobalSection +EndGlobal diff --git a/Program.cs b/Program.cs new file mode 100644 index 0000000..3ddab48 --- /dev/null +++ b/Program.cs @@ -0,0 +1,517 @@ +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)}"); +// } +// } +// } +//} diff --git a/obj/ConsoleApp1.csproj.nuget.dgspec.json b/obj/ConsoleApp1.csproj.nuget.dgspec.json new file mode 100644 index 0000000..816fc50 --- /dev/null +++ b/obj/ConsoleApp1.csproj.nuget.dgspec.json @@ -0,0 +1,91 @@ +{ + "format": 1, + "restore": { + "C:\\Users\\Admin\\source\\repos\\imu\\ConsoleApp1.csproj": {} + }, + "projects": { + "C:\\Users\\Admin\\source\\repos\\imu\\ConsoleApp1.csproj": { + "version": "1.0.0", + "restore": { + "projectUniqueName": "C:\\Users\\Admin\\source\\repos\\imu\\ConsoleApp1.csproj", + "projectName": "ConsoleApp1", + "projectPath": "C:\\Users\\Admin\\source\\repos\\imu\\ConsoleApp1.csproj", + "packagesPath": "C:\\Users\\Admin\\.nuget\\packages\\", + "outputPath": "C:\\Users\\Admin\\source\\repos\\imu\\obj\\", + "projectStyle": "PackageReference", + "fallbackFolders": [ + "C:\\Program Files (x86)\\Microsoft Visual Studio\\Shared\\NuGetPackages" + ], + "configFilePaths": [ + "C:\\Users\\Admin\\AppData\\Roaming\\NuGet\\NuGet.Config", + "C:\\Program Files (x86)\\NuGet\\Config\\Microsoft.VisualStudio.FallbackLocation.config", + "C:\\Program Files (x86)\\NuGet\\Config\\Microsoft.VisualStudio.Offline.config" + ], + "originalTargetFrameworks": [ + "net8.0" + ], + "sources": { + "C:\\Program Files (x86)\\Microsoft SDKs\\NuGetPackages\\": {}, + "https://api.nuget.org/v3/index.json": {} + }, + "frameworks": { + "net8.0": { + "targetAlias": "net8.0", + "projectReferences": {} + } + }, + "warningProperties": { + "warnAsError": [ + "NU1605" + ] + }, + "restoreAuditProperties": { + "enableAudit": "true", + "auditLevel": "low", + "auditMode": "direct" + }, + "SdkAnalysisLevel": "10.0.100" + }, + "frameworks": { + "net8.0": { + "targetAlias": "net8.0", + "imports": [ + "net461", + "net462", + "net47", + "net471", + "net472", + "net48", + "net481" + ], + "assetTargetFallback": true, + "warn": true, + "downloadDependencies": [ + { + "name": "Microsoft.AspNetCore.App.Ref", + "version": "[8.0.22, 8.0.22]" + }, + { + "name": "Microsoft.NETCore.App.Host.win-x64", + "version": "[8.0.22, 8.0.22]" + }, + { + "name": "Microsoft.NETCore.App.Ref", + "version": "[8.0.22, 8.0.22]" + }, + { + "name": "Microsoft.WindowsDesktop.App.Ref", + "version": "[8.0.22, 8.0.22]" + } + ], + "frameworkReferences": { + "Microsoft.NETCore.App": { + "privateAssets": "all" + } + }, + "runtimeIdentifierGraphPath": "C:\\Program Files\\dotnet\\sdk\\10.0.100/PortableRuntimeIdentifierGraph.json" + } + } + } + } +} \ No newline at end of file diff --git a/obj/ConsoleApp1.csproj.nuget.g.props b/obj/ConsoleApp1.csproj.nuget.g.props new file mode 100644 index 0000000..1875eb4 --- /dev/null +++ b/obj/ConsoleApp1.csproj.nuget.g.props @@ -0,0 +1,16 @@ + + + + True + NuGet + $(MSBuildThisFileDirectory)project.assets.json + $(UserProfile)\.nuget\packages\ + C:\Users\Admin\.nuget\packages\;C:\Program Files (x86)\Microsoft Visual Studio\Shared\NuGetPackages + PackageReference + 7.0.0 + + + + + + \ No newline at end of file diff --git a/obj/ConsoleApp1.csproj.nuget.g.targets b/obj/ConsoleApp1.csproj.nuget.g.targets new file mode 100644 index 0000000..3dc06ef --- /dev/null +++ b/obj/ConsoleApp1.csproj.nuget.g.targets @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/obj/Debug/net8.0/.NETCoreApp,Version=v8.0.AssemblyAttributes.cs b/obj/Debug/net8.0/.NETCoreApp,Version=v8.0.AssemblyAttributes.cs new file mode 100644 index 0000000..2217181 --- /dev/null +++ b/obj/Debug/net8.0/.NETCoreApp,Version=v8.0.AssemblyAttributes.cs @@ -0,0 +1,4 @@ +// +using System; +using System.Reflection; +[assembly: global::System.Runtime.Versioning.TargetFrameworkAttribute(".NETCoreApp,Version=v8.0", FrameworkDisplayName = ".NET 8.0")] diff --git a/obj/Debug/net8.0/ConsoleApp1.AssemblyInfo.cs b/obj/Debug/net8.0/ConsoleApp1.AssemblyInfo.cs new file mode 100644 index 0000000..59389a0 --- /dev/null +++ b/obj/Debug/net8.0/ConsoleApp1.AssemblyInfo.cs @@ -0,0 +1,23 @@ +//------------------------------------------------------------------------------ +// +// This code was generated by a tool. +// Runtime Version:4.0.30319.42000 +// +// Changes to this file may cause incorrect behavior and will be lost if +// the code is regenerated. +// +//------------------------------------------------------------------------------ + +using System; +using System.Reflection; + +[assembly: System.Reflection.AssemblyCompanyAttribute("ConsoleApp1")] +[assembly: System.Reflection.AssemblyConfigurationAttribute("Debug")] +[assembly: System.Reflection.AssemblyFileVersionAttribute("1.0.0.0")] +[assembly: System.Reflection.AssemblyInformationalVersionAttribute("1.0.0")] +[assembly: System.Reflection.AssemblyProductAttribute("ConsoleApp1")] +[assembly: System.Reflection.AssemblyTitleAttribute("ConsoleApp1")] +[assembly: System.Reflection.AssemblyVersionAttribute("1.0.0.0")] + +// Generated by the MSBuild WriteCodeFragment class. + diff --git a/obj/Debug/net8.0/ConsoleApp1.AssemblyInfoInputs.cache b/obj/Debug/net8.0/ConsoleApp1.AssemblyInfoInputs.cache new file mode 100644 index 0000000..81a6c67 --- /dev/null +++ b/obj/Debug/net8.0/ConsoleApp1.AssemblyInfoInputs.cache @@ -0,0 +1 @@ +9b4e1ef5f091261e36ca58379f5aa497ab4a942f19c985f94bfc1a6e372da8c3 diff --git a/obj/Debug/net8.0/ConsoleApp1.GeneratedMSBuildEditorConfig.editorconfig b/obj/Debug/net8.0/ConsoleApp1.GeneratedMSBuildEditorConfig.editorconfig new file mode 100644 index 0000000..7329026 --- /dev/null +++ b/obj/Debug/net8.0/ConsoleApp1.GeneratedMSBuildEditorConfig.editorconfig @@ -0,0 +1,17 @@ +is_global = true +build_property.TargetFramework = net8.0 +build_property.TargetFrameworkIdentifier = .NETCoreApp +build_property.TargetFrameworkVersion = v8.0 +build_property.TargetPlatformMinVersion = +build_property.UsingMicrosoftNETSdkWeb = +build_property.ProjectTypeGuids = +build_property.InvariantGlobalization = +build_property.PlatformNeutralAssembly = +build_property.EnforceExtendedAnalyzerRules = +build_property._SupportedPlatformList = Linux,macOS,Windows +build_property.RootNamespace = ConsoleApp1 +build_property.ProjectDir = C:\Users\Admin\source\repos\imu\ +build_property.EnableComHosting = +build_property.EnableGeneratedComInterfaceComImportInterop = +build_property.EffectiveAnalysisLevelStyle = 8.0 +build_property.EnableCodeStyleSeverity = diff --git a/obj/Debug/net8.0/ConsoleApp1.GlobalUsings.g.cs b/obj/Debug/net8.0/ConsoleApp1.GlobalUsings.g.cs new file mode 100644 index 0000000..d12bcbc --- /dev/null +++ b/obj/Debug/net8.0/ConsoleApp1.GlobalUsings.g.cs @@ -0,0 +1,8 @@ +// +global using System; +global using System.Collections.Generic; +global using System.IO; +global using System.Linq; +global using System.Net.Http; +global using System.Threading; +global using System.Threading.Tasks; diff --git a/obj/Debug/net8.0/ConsoleApp1.assets.cache b/obj/Debug/net8.0/ConsoleApp1.assets.cache new file mode 100644 index 0000000000000000000000000000000000000000..b22557139f8885486ea2645f2d1935ce4f6c728e GIT binary patch literal 228 zcmWIWc6a1qU|^V`;==Rs!p3ha{oPIYDr8%Vsdt3dTMbD$SilDnt-DG^rFOE1-Hzc)M5oy zBZ~?PGgTwY7~jm~qWt3gv=W7|%;M6-9EISL(v-~nnBa`WqSTZazf$+qk^rb-gxm!H D1r#*~ literal 0 HcmV?d00001 diff --git a/obj/Debug/net8.0/ConsoleApp1.csproj.CoreCompileInputs.cache b/obj/Debug/net8.0/ConsoleApp1.csproj.CoreCompileInputs.cache new file mode 100644 index 0000000..df563de --- /dev/null +++ b/obj/Debug/net8.0/ConsoleApp1.csproj.CoreCompileInputs.cache @@ -0,0 +1 @@ +081d6e8eef10765976f2bfcaf63b43f78ad9830fa7649b95ef08105205357345 diff --git a/obj/Debug/net8.0/ConsoleApp1.csproj.FileListAbsolute.txt b/obj/Debug/net8.0/ConsoleApp1.csproj.FileListAbsolute.txt new file mode 100644 index 0000000..275d648 --- /dev/null +++ b/obj/Debug/net8.0/ConsoleApp1.csproj.FileListAbsolute.txt @@ -0,0 +1,8 @@ +C:\Users\Admin\Desktop\imu\imu_c_sharp\ConsoleApp1\obj\Debug\net8.0\ConsoleApp1.GeneratedMSBuildEditorConfig.editorconfig +C:\Users\Admin\Desktop\imu\imu_c_sharp\ConsoleApp1\obj\Debug\net8.0\ConsoleApp1.AssemblyInfoInputs.cache +C:\Users\Admin\Desktop\imu\imu_c_sharp\ConsoleApp1\obj\Debug\net8.0\ConsoleApp1.AssemblyInfo.cs +C:\Users\Admin\Desktop\imu\imu_c_sharp\ConsoleApp1\obj\Debug\net8.0\ConsoleApp1.csproj.CoreCompileInputs.cache +C:\Users\Admin\imu\obj\Debug\net8.0\ConsoleApp1.GeneratedMSBuildEditorConfig.editorconfig +C:\Users\Admin\imu\obj\Debug\net8.0\ConsoleApp1.AssemblyInfoInputs.cache +C:\Users\Admin\imu\obj\Debug\net8.0\ConsoleApp1.AssemblyInfo.cs +C:\Users\Admin\imu\obj\Debug\net8.0\ConsoleApp1.csproj.CoreCompileInputs.cache diff --git a/obj/project.assets.json b/obj/project.assets.json new file mode 100644 index 0000000..a5c2a59 --- /dev/null +++ b/obj/project.assets.json @@ -0,0 +1,97 @@ +{ + "version": 3, + "targets": { + "net8.0": {} + }, + "libraries": {}, + "projectFileDependencyGroups": { + "net8.0": [] + }, + "packageFolders": { + "C:\\Users\\Admin\\.nuget\\packages\\": {}, + "C:\\Program Files (x86)\\Microsoft Visual Studio\\Shared\\NuGetPackages": {} + }, + "project": { + "version": "1.0.0", + "restore": { + "projectUniqueName": "C:\\Users\\Admin\\source\\repos\\imu\\ConsoleApp1.csproj", + "projectName": "ConsoleApp1", + "projectPath": "C:\\Users\\Admin\\source\\repos\\imu\\ConsoleApp1.csproj", + "packagesPath": "C:\\Users\\Admin\\.nuget\\packages\\", + "outputPath": "C:\\Users\\Admin\\source\\repos\\imu\\obj\\", + "projectStyle": "PackageReference", + "fallbackFolders": [ + "C:\\Program Files (x86)\\Microsoft Visual Studio\\Shared\\NuGetPackages" + ], + "configFilePaths": [ + "C:\\Users\\Admin\\AppData\\Roaming\\NuGet\\NuGet.Config", + "C:\\Program Files (x86)\\NuGet\\Config\\Microsoft.VisualStudio.FallbackLocation.config", + "C:\\Program Files (x86)\\NuGet\\Config\\Microsoft.VisualStudio.Offline.config" + ], + "originalTargetFrameworks": [ + "net8.0" + ], + "sources": { + "C:\\Program Files (x86)\\Microsoft SDKs\\NuGetPackages\\": {}, + "https://api.nuget.org/v3/index.json": {} + }, + "frameworks": { + "net8.0": { + "targetAlias": "net8.0", + "projectReferences": {} + } + }, + "warningProperties": { + "warnAsError": [ + "NU1605" + ] + }, + "restoreAuditProperties": { + "enableAudit": "true", + "auditLevel": "low", + "auditMode": "direct" + }, + "SdkAnalysisLevel": "10.0.100" + }, + "frameworks": { + "net8.0": { + "targetAlias": "net8.0", + "imports": [ + "net461", + "net462", + "net47", + "net471", + "net472", + "net48", + "net481" + ], + "assetTargetFallback": true, + "warn": true, + "downloadDependencies": [ + { + "name": "Microsoft.AspNetCore.App.Ref", + "version": "[8.0.22, 8.0.22]" + }, + { + "name": "Microsoft.NETCore.App.Host.win-x64", + "version": "[8.0.22, 8.0.22]" + }, + { + "name": "Microsoft.NETCore.App.Ref", + "version": "[8.0.22, 8.0.22]" + }, + { + "name": "Microsoft.WindowsDesktop.App.Ref", + "version": "[8.0.22, 8.0.22]" + } + ], + "frameworkReferences": { + "Microsoft.NETCore.App": { + "privateAssets": "all" + } + }, + "runtimeIdentifierGraphPath": "C:\\Program Files\\dotnet\\sdk\\10.0.100/PortableRuntimeIdentifierGraph.json" + } + } + } +} \ No newline at end of file diff --git a/obj/project.nuget.cache b/obj/project.nuget.cache new file mode 100644 index 0000000..664a78d --- /dev/null +++ b/obj/project.nuget.cache @@ -0,0 +1,13 @@ +{ + "version": 2, + "dgSpecHash": "cQgPDFTPAeA=", + "success": true, + "projectFilePath": "C:\\Users\\Admin\\source\\repos\\imu\\ConsoleApp1.csproj", + "expectedPackageFiles": [ + "C:\\Users\\Admin\\.nuget\\packages\\microsoft.windowsdesktop.app.ref\\8.0.22\\microsoft.windowsdesktop.app.ref.8.0.22.nupkg.sha512", + "C:\\Users\\Admin\\.nuget\\packages\\microsoft.netcore.app.ref\\8.0.22\\microsoft.netcore.app.ref.8.0.22.nupkg.sha512", + "C:\\Users\\Admin\\.nuget\\packages\\microsoft.aspnetcore.app.ref\\8.0.22\\microsoft.aspnetcore.app.ref.8.0.22.nupkg.sha512", + "C:\\Users\\Admin\\.nuget\\packages\\microsoft.netcore.app.host.win-x64\\8.0.22\\microsoft.netcore.app.host.win-x64.8.0.22.nupkg.sha512" + ], + "logs": [] +} \ No newline at end of file