//Create joystick packet from floating point axis.
//Center = 0.0.
//Up/Right =1.0.
//Down/Left=-1.0.
private static byte[] CreateJoyPacket(float fRx, float fRy, float fLx, float fLy, float speed)
{
//template joy packet.
var packet = new byte[] { 0xcc, 0xb0, 0x00, 0x7f, 0x60, 0x50, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x12, 0x16, 0x01, 0x0e, 0x00, 0x25, 0x54 };
var axis1 = (short)(660.0F * fRx + 1024.0F); //RightX center=1024 left =364 right =-364
var axis2 = (short)(660.0F * fRy + 1024.0F); //RightY down =364 up =-364
var axis3 = (short)(660.0F * fLy + 1024.0F); //LeftY down =364 up =-364
var axis4 = (short)(660.0F * fLx + 1024.0F); //LeftX left =364 right =-364
var axis5 = (short)(660.0F * speed + 1024.0F); //Speed.
if (speed > 0.1f)
{
axis5 = 0x7fff;
}
var packedAxis = ((long)axis1 & 0x7FF) | (((long)axis2 & 0x7FF) << 11) | ((0x7FF & (long)axis3) << 22) | ((0x7FF & (long)axis4) << 33) | ((long)axis5 << 44);
packet[9] = ((byte)(int)(0xFF & packedAxis));
packet[10] = ((byte)(int)(packedAxis >> 8 & 0xFF));
packet[11] = ((byte)(int)(packedAxis >> 16 & 0xFF));
packet[12] = ((byte)(int)(packedAxis >> 24 & 0xFF));
packet[13] = ((byte)(int)(packedAxis >> 32 & 0xFF));
packet[14] = ((byte)(int)(packedAxis >> 40 & 0xFF));
//Add time info.
var now = DateTime.Now;
packet[15] = (byte)now.Hour;
packet[16] = (byte)now.Minute;
packet[17] = (byte)now.Second;
packet[18] = (byte)(now.Millisecond & 0xff);
packet[19] = (byte)(now.Millisecond >> 8);
Crc.CalcUCrc(packet, 4);//Not really needed.
//calc crc for packet.
Crc.CalcCrc(packet, packet.Length);
return(packet);
}
private static void SetPacketCrCs(byte[] packet)
{
Crc.CalcUCrc(packet, 4);
Crc.CalcCrc(packet, packet.Length);
}
