public short FL, FR, BR, BL; // front-left, front-right, back-right, back-left motor outputs
public void ReadFrom( Packet p )
{
FL = p.GetShort();
FR = p.GetShort();
BR = p.GetShort();
BL = p.GetShort();
}
public void ReadFrom( Packet p )
{
Thro = p.GetShort();
Aile = p.GetShort();
Elev = p.GetShort();
Rudd = p.GetShort();
Gear = p.GetShort();
Aux1 = p.GetShort();
Aux2 = p.GetShort();
Aux3 = p.GetShort();
BatteryVolts = p.GetShort();
}
public void ReadFrom( Packet p )
{
Temp = p.GetShort();
GyroX = p.GetShort();
GyroY = p.GetShort();
GyroZ = p.GetShort();
AccelX = p.GetShort();
AccelY = p.GetShort();
AccelZ = p.GetShort();
MagX = p.GetShort();
MagY = p.GetShort();
MagZ = p.GetShort();
}
private void ProcessByte( byte b )
{
if(sigByteIndex < 2)
{
if(sigByteIndex == 0)
{
if(b == 0x55)
{
sigByteIndex++;
packetByteIndex = 0;
}
return;
}
if(sigByteIndex == 1)
{
if(b == 0xAA)
{
sigByteIndex++;
packetByteIndex = 0;
}
else
{
sigByteIndex = 0;
}
return;
}
}
switch(packetByteIndex)
{
case 0:
currentPacket = new Packet();
currentPacket.mode = b;
if(b > 0x20)
{ // No such mode - bad data
sigByteIndex = 0;
packetByteIndex = 0;
return;
}
packetByteIndex++;
return;
case 1:
if(b != 0)
{ // No such mode - bad data
sigByteIndex = 0;
packetByteIndex = 0;
return;
}
packetByteIndex++;
currentChecksum = Checksum( 0, (UInt16)0xaa55 );
currentChecksum = Checksum( currentChecksum, (UInt16)currentPacket.mode );
return;
case 2:
currentPacket.len = (short)b;
packetByteIndex++;
return;
case 3:
currentPacket.len |= (short)(b << 8);
currentChecksum = Checksum( currentChecksum, (UInt16)currentPacket.len );
if(currentPacket.len > 1024)
{ // unreasonable packet size (> 1kb)
sigByteIndex = 0;
packetByteIndex = 0;
return;
}
currentPacket.len -= 6; // Subtract off signature and header size
if(currentPacket.len < 1)
{ // Can't have a zero length packet - bad data
sigByteIndex = 0;
packetByteIndex = 0;
return;
}
currentPacket.data = new byte[currentPacket.len];
packetByteIndex++;
return;
default:
currentPacket.data[packetByteIndex - 4] = b;
break;
}
packetByteIndex++;
if(packetByteIndex == (currentPacket.data.Length + 4)) // length + header bytes
{
sigByteIndex = 0;
packetByteIndex = 0;
// Validate the checksum
// only keep the packet if they match
int len = currentPacket.data.Length - 2;
UInt16 check = Checksum( currentChecksum, currentPacket.data, len );
UInt16 sourceCheck = (UInt16)(currentPacket.data[len] | (currentPacket.data[len + 1] << 8));
if(check == sourceCheck)
{
packetsArray[head] = currentPacket;
lock(packetsArray)
{
head = (head + 1) % packetsArray.Length;
if(tail == head) {
tail = (tail + 1) % packetsArray.Length; // Throw away oldest data if we fill the buffer
}
}
}
}
}
public int Alt, AltTemp, AltiEst; // Altimeter = 12 bytes
public void ReadFrom( Packet p )
{
Pitch = p.GetInt();
Roll = p.GetInt();
Yaw = p.GetInt();
Alt = p.GetInt();
AltTemp = p.GetInt();
AltiEst = p.GetInt();
}
public void ReadFrom( Packet p )
{
Version = p.GetShort();
MinCycles = p.GetShort();
MaxCycles = p.GetShort();
AvgCycles = p.GetShort();
Counter = p.GetInt(); // basically a sequence value
}
private void ProcessByte( byte b )
{
if(sigByteIndex < 2)
{
if(b == 0x77)
{
sigByteIndex++;
packetByteIndex = 0;
return;
}
else
{
sigByteIndex = 0;
packetByteIndex = 0;
return;
}
}
switch( packetByteIndex )
{
case 0:
currentPacket = new Packet();
currentPacket.mode = b;
packetByteIndex++;
return;
case 1:
currentPacket.len = b;
currentPacket.data = new byte[currentPacket.len];
packetByteIndex++;
return;
default:
currentPacket.data[packetByteIndex-2] = b;
break;
}
packetByteIndex++;
if(packetByteIndex == (currentPacket.data.Length + 2))
{
sigByteIndex = 0;
packetByteIndex = 0;
packetsArray[head] = currentPacket;
lock(packetsArray)
{
head = (head+1) % packetsArray.Length;
if(tail == head) {
tail = (tail + 1) % packetsArray.Length; // Throw away oldest data if we fill the buffer
}
}
}
}
