131 lines
4.0 KiB
C#
131 lines
4.0 KiB
C#
namespace UbloxSC20.UBXDecoder;
|
|
public class UbxDecoder
|
|
{
|
|
const byte SYNC_CHAR_1 = 0xB5;
|
|
const byte SYNC_CHAR_2 = 0x62;
|
|
const byte CLASS_NAV = 0x02;
|
|
const byte MSG_SFRBX = 0x13; // ID for UBX-RXM-SFRBX
|
|
|
|
[System.Runtime.InteropServices.StructLayout(System.Runtime.InteropServices.LayoutKind.Sequential, Pack = 1)]
|
|
struct UbxHeader
|
|
{
|
|
public byte Sync1;
|
|
public byte Sync2;
|
|
public byte ClassId;
|
|
public byte MsgId;
|
|
public ushort Length;
|
|
}
|
|
|
|
[System.Runtime.InteropServices.StructLayout(System.Runtime.InteropServices.LayoutKind.Sequential, Pack = 1)]
|
|
struct UbxSfrbxMessage
|
|
{
|
|
public UbxHeader Header;
|
|
public byte GnssId;
|
|
public byte SvId;
|
|
public byte SigId;
|
|
public byte FreqId;
|
|
public uint[] LastPayload;
|
|
public byte CkA;
|
|
public byte CkB;
|
|
}
|
|
|
|
public class UbxSfrbxResult
|
|
{
|
|
public byte GnssId { get; set; }
|
|
public byte SvId { get; set; }
|
|
public byte SigId { get; set; }
|
|
public byte FreqId { get; set; }
|
|
public uint[] LastPayload { get; set; }
|
|
|
|
}
|
|
|
|
private bool VerifyChecksum(byte[] data, ushort length, byte ckA, byte ckB)
|
|
{
|
|
byte a = 0, b = 0;
|
|
for (ushort i = 0; i < length; i++)
|
|
{
|
|
a += data[i];
|
|
b += a;
|
|
}
|
|
return (a == ckA) && (b == ckB);
|
|
}
|
|
|
|
public UbxSfrbxResult DecodeUbxSfrbx(byte[] buffer)
|
|
{
|
|
if (buffer.Length < System.Runtime.InteropServices.Marshal.SizeOf(typeof(UbxHeader)) + 4)
|
|
{
|
|
Console.WriteLine("Buffer too small!");
|
|
return null;
|
|
}
|
|
|
|
UbxHeader header;
|
|
IntPtr headerPtr = System.Runtime.InteropServices.Marshal.AllocHGlobal(System.Runtime.InteropServices.Marshal.SizeOf(typeof(UbxHeader)));
|
|
try
|
|
{
|
|
System.Runtime.InteropServices.Marshal.Copy(buffer, 0, headerPtr, System.Runtime.InteropServices.Marshal.SizeOf(typeof(UbxHeader)));
|
|
header = (UbxHeader)System.Runtime.InteropServices.Marshal.PtrToStructure(headerPtr, typeof(UbxHeader));
|
|
}
|
|
finally
|
|
{
|
|
System.Runtime.InteropServices.Marshal.FreeHGlobal(headerPtr);
|
|
}
|
|
|
|
// Kiểm tra độ dài thực tế của buffer
|
|
if (buffer.Length < header.Length + System.Runtime.InteropServices.Marshal.SizeOf(typeof(UbxHeader)) + 2)
|
|
{
|
|
Console.WriteLine("Buffer length mismatch!");
|
|
return null;
|
|
}
|
|
|
|
// Kiểm tra checksum
|
|
if (!VerifyChecksum(buffer[2..], (ushort)(header.Length + 4), buffer[^2], buffer[^1]))
|
|
{
|
|
Console.WriteLine("Checksum verification failed!");
|
|
return null;
|
|
}
|
|
|
|
// Giải mã các trường chính
|
|
UbxSfrbxMessage msg = new UbxSfrbxMessage
|
|
{
|
|
Header = header,
|
|
GnssId = buffer[6],
|
|
SvId = buffer[7],
|
|
SigId = buffer[8],
|
|
FreqId = buffer[9],
|
|
CkA = buffer[^2],
|
|
CkB = buffer[^1]
|
|
};
|
|
|
|
// Lấy payload (các byte còn lại sau các trường chính)
|
|
int payloadStartIndex = 14;
|
|
int payloadLength = header.Length - (payloadStartIndex - 6); // Trừ đi các byte đã ánh xạ
|
|
uint[] lastPayload = new uint[payloadLength / 4];
|
|
|
|
for (int i = 0; i < lastPayload.Length; i++)
|
|
{
|
|
lastPayload[i] = BitConverter.ToUInt32(buffer, payloadStartIndex + i * 4);
|
|
}
|
|
|
|
// Trả về kết quả
|
|
return new UbxSfrbxResult
|
|
{
|
|
GnssId = msg.GnssId,
|
|
SvId = msg.SvId,
|
|
SigId = msg.SigId,
|
|
FreqId = msg.FreqId,
|
|
LastPayload = lastPayload
|
|
};
|
|
}
|
|
|
|
public static byte[] HexStringToByteArray(string hex)
|
|
{
|
|
int numberChars = hex.Length;
|
|
byte[] bytes = new byte[numberChars / 2];
|
|
for (int i = 0; i < numberChars; i += 2)
|
|
{
|
|
bytes[i / 2] = Convert.ToByte(hex.Substring(i, 2), 16);
|
|
}
|
|
return bytes;
|
|
}
|
|
|
|
} |