Subversion Repositories Projects

///============================================================================
/// This file is part of MIKROKOPTER SERIAL CONTROL TUTORIAL.
/// by JOHN C. MACDONALD at Ira A. Fulton College of Engineering and Technology
/// (http://hdl.lib.byu.edu/1877/2747)
/// (http://hdl.lib.byu.edu/1877/2748)
///============================================================================
/// made minor changes here...
///============================================================================


using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Diagnostics;

namespace MKLiveView
{
    static class FlightControllerMessage
    {
        public static byte[] CreateMessage(char commandId, byte address)
        {
            return CreateMessage(commandId, address, new byte[0]);
        }

        public static byte[] CreateMessage(char commandId, byte address, byte[] data)
        {
            List<byte> result = new List<byte>();
            //Encoding.ASCII.GetBytes(command);
           
            // add header
            result.Add(Convert.ToByte('#'));
            result.Add((byte)(Convert.ToByte('a') + address));
            result.Add(Convert.ToByte(commandId));

            // add data
            result.AddRange(Encode64(data));
           
            // add footer
            result.AddRange(getCRCBytes(result.ToArray()));
            result.Add(Convert.ToByte('\r'));

            return result.ToArray();
        }

        public static void ParseMessage(byte[] message, out char commandId, out byte address, out byte[] data)
        {
            // Debug.Assert(message.Length >= 6);
            if (message.Length >= 6)
            {
                // header
                // '#' == message[0]
                byte[] Data = new byte[message.Length - 3];
                Array.Copy(message, 0, Data, 0, message.Length - 3);
                byte[] crcData = getCRCBytes(Data);

                if (crcData.Length == 2 && crcData[0] == message[message.Length - 3] && crcData[1] == message[message.Length - 2])
                {

                    commandId = Convert.ToChar(message[2]);
                    address = (byte)(message[1] - Convert.ToByte('a'));

                    // data
                    data = Decode64(message, 3, message.Length - 6);
                }
                else
                {
                    commandId = '\0';
                    address = 255;
                    data = null;
                }
            }
            else
            {
                commandId = '\0';
                address = 0;
                data = null;
            }
            // footer
            // CRC1 == message[message.Length - 3]
            // CRC2 == message[message.Length - 2]
            // '\r' == message[message.Length - 1]
        }

        public static byte[] Encode64(byte[] data)
        {
            List<byte> encodedData = new List<byte>();

            byte a,b,c;
            int index = 0;
            byte k = Convert.ToByte('=');

            while (index < data.Length)
            {
                if (index < data.Length) a = data[index++]; else a = 0;
                if (index < data.Length) b = data[index++]; else b = 0;
                if (index < data.Length) c = data[index++]; else c = 0;

                encodedData.Add((byte)(k + (a >> 2)));
                encodedData.Add((byte)(k + (((a & 0x03) << 4) | ((b & 0xf0) >> 4))));
                encodedData.Add((byte)(k + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6))));
                encodedData.Add((byte)(k + ( c & 0x3f)));
            }

            return encodedData.ToArray();
        }

       
        public static byte[] Decode64(byte[] data, int startIndex, int count)
        {
            // data should be in chunks of 4 right?
           // Debug.Assert(count % 4 == 0);

            List<byte> decodedData = new List<byte>();

            byte k = Convert.ToByte('=');
            byte a,b,c,d;
            byte x,y,z;
            int index = startIndex;

            while (index <= count + startIndex - 4)
            {
                a = (byte)(data[index++] - k);
                b = (byte)(data[index++] - k);
                c = (byte)(data[index++] - k);
                d = (byte)(data[index++] - k);

                x = (byte)((a << 2) | (b >> 4));
                y = (byte)(((b & 0x0f) << 4) | (c >> 2));
                z = (byte)(((c & 0x03) << 6) | d);

                decodedData.Add(x);
                decodedData.Add(y);
                decodedData.Add(z);
            }

            return decodedData.ToArray();
        }


        // cyclic redundancy check (CRC) or polynomial code checksum used to verify message
        // it is an insecure hash function designed to detect accidental changes to raw computer data (wikipedia)
        private static byte[] getCRCBytes(byte[] data)
        {
            byte[] crcBytes = new byte[2];

            uint tmpCRC = 0;
            for(int i = 0; i < data.Length ;i++)
            {
                tmpCRC += data[i];
            }
            tmpCRC %= 4096;
            crcBytes[0] = (byte)(Convert.ToByte('=') + tmpCRC / 64);
            crcBytes[1] = (byte)(Convert.ToByte('=') + tmpCRC % 64);

            return crcBytes;
        }
    }
}