void ih_PacketReceived(object sender, PacketReceivedEventArgs e)
{
switch (e.Packet.Type)
{
case PacketType.Ping:
break;
case PacketType.Pong:
break;
case PacketType.SSOutput:
break;
case PacketType.SSInput:
break;
case PacketType.Scalar16:
{
Scalar16Packet packet = (Scalar16Packet)e.Packet;
SetRCGain(packet.Value);
break;
}
case PacketType.Scalar32:
{
Scalar32Packet packet = (Scalar32Packet)e.Packet;
switch (packet.ScalarType)
{
case Scalar32Type.Clock:
SetClock(packet.Value.UInt32Value);
break;
case Scalar32Type.Heading:
SetHeading(packet.Value.SingleValue);
break;
default:
break;
}
break;
}
case PacketType.Vector2_16:
break;
case PacketType.Vector2_32:
break;
case PacketType.Vector3_16:
{
Vector3_16Packet packet = (Vector3_16Packet)e.Packet;
switch (packet.VectorType)
{
case Vector3_16Type.RotPid:
SetRotationRatePid(packet.Value.Int16X, packet.Value.Int16Y, packet.Value.Int16Z);
break;
default:
break;
}
break;
}
case PacketType.Vector3_32:
{
Vector3_32Packet packet = (Vector3_32Packet)e.Packet;
switch (packet.VectorType)
{
case Vector3_32Type.Gryo:
SetGyro(packet.Value.SingleX, packet.Value.SingleY, packet.Value.SingleZ);
break;
case Vector3_32Type.Attitude:
SetAttitude(packet.Value.SingleX, packet.Value.SingleY, packet.Value.SingleZ);
break;
case Vector3_32Type.Acceleration:
break;
case Vector3_32Type.Velocity:
break;
case Vector3_32Type.Position:
SetPosition(packet.Value.SingleX, packet.Value.SingleY, packet.Value.SingleZ);
break;
case Vector3_32Type.Gps:
break;
case Vector3_32Type.AttitudePid:
SetAttitudePid(packet.Value.SingleX, packet.Value.SingleY, packet.Value.SingleZ);
break;
case Vector3_32Type.VelocityPid:
break;
case Vector3_32Type.AltRatePid:
break;
case Vector3_32Type.AltPid:
break;
default:
break;
}
break;
}
case PacketType.Vector4_16:
{
Vector4Packet packet = (Vector4Packet)e.Packet;
switch (packet.VectorType)
{
case Vector4Type.RCInput:
SetRCRaw(packet.Value.W, packet.Value.X, packet.Value.Y, packet.Value.Z);
break;
case Vector4Type.ControlInput:
break;
case Vector4Type.MotorOutput:
SetMotorOutput(packet.Value.W, packet.Value.X, packet.Value.Y, packet.Value.Z);
break;
default:
break;
}
break;
}
default:
break;
}
}
private void ParsePackets()
{
if (buffer == null || buffer.Length == 0)
{
return;
}
while (buffer.Length > 0)
{
PacketType pt = (PacketType)(buffer[0] & 0x0F);
int len = buffer[0] >> 4;
if (buffer.Length < len + 3)
{
return;
}
if (!ValidateChecksum(buffer, len + 3))
{
//throw new InvalidOperationException("Invalid cheksum");
byte[] newBuffer = new byte[buffer.Length - 1];
Buffer.BlockCopy(buffer, 1, newBuffer, 0, newBuffer.Length);
buffer = newBuffer;
}
else
{
Packet p = null;
switch (pt)
{
case PacketType.Scalar16:
{
ushort value = BitConverter.ToUInt16(buffer, 2);
p = new Scalar16Packet(value);
break;
}
case PacketType.Scalar32:
{
Scalar32Type scalarType = (Scalar32Type)buffer[1];
Scalar32 value = new Scalar32(BitConverter.ToUInt32(buffer, 2));
p = new Scalar32Packet(scalarType, value);
break;
}
case PacketType.Vector3_16:
{
Vector3_16Type type = (Vector3_16Type)buffer[1];
p = new Vector3_16Packet(type, new Vector3_16(buffer, 2));
break;
}
case PacketType.Vector3_32:
{
Vector3_32Type type = (Vector3_32Type)buffer[1];
p = new Vector3_32Packet(new Vector3_32(buffer, 2), type);
break;
}
case PacketType.Vector4_16:
{
Vector4Type type = (Vector4Type)buffer[1];
ushort w = BitConverter.ToUInt16(buffer, 2);
ushort x = BitConverter.ToUInt16(buffer, 4);
ushort y = BitConverter.ToUInt16(buffer, 6);
ushort z = BitConverter.ToUInt16(buffer, 8);
p = new Vector4Packet(type, new Vector4(w, x, y, z));
break;
}
default:
throw new InvalidOperationException(String.Format("Unknown type: {0}", pt));
break;
}
if (p != null)
{
EventHandler <PacketReceivedEventArgs> handler = PacketReceived;
if (handler != null)
{
handler(this, new PacketReceivedEventArgs(p));
}
}
byte[] newBuffer = new byte[buffer.Length - len - 3];
Buffer.BlockCopy(buffer, len + 3, newBuffer, 0, newBuffer.Length);
buffer = newBuffer;
}
}
}
