That looks like it should work.
Here is some more info/code if you can use it.
When processing dashboard data in .net Jon Skeet’s EndianBinaryReader (http://www.yoda.arachsys.com/csharp/miscutil/) will be useful. It data fully supports both big and little endian.
A complete version of the code I show below is available at http://frc1103dashboard.codeplex.com/releases/view/41737.
The following code (VB 2008) is used to decode the non-user data. DsIinputs/Outputs simply break the bits into a boolean array, and RobotStatus/Error were never implemented.
Public Function Parse(ByVal data As Byte()) As Byte()
Dim reader As EndianBinaryReader = Nothing
Try
'reader = New BinaryReader(New MemoryStream(data))
reader = New MiscUtil.IO.EndianBinaryReader(New MiscUtil.Conversion.BigEndianBitConverter(), New MemoryStream(data))
dataGraph.PacketNumber = reader.ReadUInt16()
dataGraph.DigitalIns = New DsInputs(reader.ReadByte())
dataGraph.DigitalOuts = New DsOutputs(reader.ReadByte())
dataGraph.Battery = reader.EReadBattery()
dataGraph.Status = New RobotStatus(reader.ReadByte())
dataGraph.Errors = New RobotError(reader.ReadByte())
dataGraph.TeamNumber = reader.ReadTeamNumber()
Dim versionBytes As Byte() = reader.ReadBytes(8)
dataGraph.DSVersion = Text.Encoding.ASCII.GetString(versionBytes)
dataGraph.UnusedBuffer1 = reader.ReadUInt32()
dataGraph.UnusedBuffer2 = reader.ReadUInt16()
'I believe this is only important for robot-to-DS communication, but haven't confirmed.
dataGraph.ReplyPacketNumber = reader.ReadUInt16()
Dim userData = reader.ReadBytes(984)
dataGraph.UserData = userData
Return userData
reader.Close()
Return data
Catch ex As Exception
'TODO: Add exception handling logic.
Throw
Finally
If reader IsNot Nothing Then
reader.Close()
End If
End Try
End Function
(VB) The extension methods used above to decode battery, and team number.
Public Module BinaryReaderExtensions
<Extension()> _
Public Function ReadTeamNumber(ByVal reader As EndianBinaryReader) As Integer
Dim temp1 = reader.ReadByte()
Dim temp2 = reader.ReadByte()
Return temp1 * 100 + temp2
End Function
<Extension()> _
Public Function EReadBattery(ByVal reader As EndianBinaryReader) As Double
' battery voltage is transmitted follows: 0x12 0x17 is 12.17 volts.
Return Convert.ToDouble(reader.ReadByte().ToString("x")) + (Convert.ToDouble(reader.ReadByte().ToString("x")) / 100.0)
End Function
End Module
This code (C#) verifies the integrity of the packet, then forwards it, indirectly, to the above code.
public void ParseBytes(byte] data)
{
if (data.Length == DSPacketLength && FrcPacketUtils.VerifyFrcCrc(data))
{
UserData = data;
SendNewData(data);
InvalidPacketCount = 0;
}
else
{
InvalidPacketCount += 1;
Debug.Print("Invalid DS data received, ignoring");
}
}
The VerifyFRCCrc function (VB)
Public Function VerifyFrcCrc(ByVal data As Byte()) As Boolean
Dim calculatedCrc As UInteger = 0
Dim dataCrc As UInteger = 0
Dim crc As New Crc32()
dataCrc = BitConverter.ToUInt32(data.Skip(data.Length - 4).Take(4).ToArray(), 0)
'remove CRC bytes from data before calculating CRC.
Dim crcData(data.Length - 1) As Byte
data.Take(data.Length - 4).ToArray.CopyTo(crcData, 0)
calculatedCrc = BitConverter.ToUInt32(crc.ComputeHash(crcData), 0)
Return dataCrc = calculatedCrc
End Function
The CRC32 class I found on a blog somewhere, but don’t have documentation of where.
using System;
using System.Security.Cryptography;
public class Crc32 : HashAlgorithm
{
public const UInt32 DefaultPolynomial = 0xedb88320;
public const UInt32 DefaultSeed = 0xffffffff;
private UInt32 hash;
private UInt32 seed;
private UInt32] table;
private static UInt32] defaultTable;
public Crc32()
{
table = InitializeTable(DefaultPolynomial);
seed = DefaultSeed;
Initialize();
}
public Crc32(UInt32 polynomial, UInt32 seed)
{
table = InitializeTable(polynomial);
this.seed = seed;
Initialize();
}
public override void Initialize()
{
hash = seed;
}
protected override void HashCore(byte] buffer, int start, int length)
{
hash = CalculateHash(table, hash, buffer, start, length);
}
protected override byte] HashFinal()
{
byte] hashBuffer = UInt32ToBigEndianBytes(~hash);
this.HashValue = hashBuffer;
return hashBuffer;
}
public override int HashSize
{
get { return 32; }
}
public static UInt32 Compute(byte] buffer)
{
return ~CalculateHash(InitializeTable(DefaultPolynomial), DefaultSeed, buffer, 0, buffer.Length);
}
public static UInt32 Compute(UInt32 seed, byte] buffer)
{
return ~CalculateHash(InitializeTable(DefaultPolynomial), seed, buffer, 0, buffer.Length);
}
public static UInt32 Compute(UInt32 polynomial, UInt32 seed, byte] buffer)
{
return ~CalculateHash(InitializeTable(polynomial), seed, buffer, 0, buffer.Length);
}
private static UInt32] InitializeTable(UInt32 polynomial)
{
if (polynomial == DefaultPolynomial && defaultTable != null)
return defaultTable;
UInt32] createTable = new UInt32[256];
for (int i = 0; i < 256; i++)
{
UInt32 entry = (UInt32)i;
for (int j = 0; j < 8; j++)
if ((entry & 1) == 1)
entry = (entry >> 1) ^ polynomial;
else
entry = entry >> 1;
createTable* = entry;
}
if (polynomial == DefaultPolynomial)
defaultTable = createTable;
return createTable;
}
private static UInt32 CalculateHash(UInt32] table, UInt32 seed, byte] buffer, int start, int size)
{
UInt32 crc = seed;
for (int i = start; i < size; i++)
unchecked
{
crc = (crc >> 8) ^ table[buffer[i] ^ crc & 0xff];
}
return crc;
}
private byte] UInt32ToBigEndianBytes(UInt32 x)
{
return new byte] {
(byte)((x >> 24) & 0xff),
(byte)((x >> 16) & 0xff),
(byte)((x >> 8) & 0xff),
(byte)(x & 0xff)
};
}
}
Code (C#) to extract the three sections of the user data, and process them.
using (var stream = new MemoryStream(userData))
using (var reader = new MiscUtil.IO.EndianBinaryReader(new MiscUtil.Conversion.BigEndianBitConverter(), stream))
{
var packetNumber = reader.ReadByte();
var highDataLength = reader.ReadInt32();
var highData = reader.ReadBytes(highDataLength);
if (highDataLength > 0)
StatusData.Update(highData);
var errorString = reader.ReadInt32();
var lowDataLength = reader.ReadInt32();
var lowData = reader.ReadBytes(lowDataLength);
if (lowDataLength > 0)
ioData.Update(lowData);
}
If you have any questions/comments please contact me at eric [at] ehaskins [dot] net, or through the Skype name listed in my profile.*