Пример #1
0
        //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);
        }
Пример #2
0
 private static void SetPacketCrCs(byte[] packet)
 {
     Crc.CalcUCrc(packet, 4);
     Crc.CalcCrc(packet, packet.Length);
 }