Subversion Repositories Projects

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
2233 - 1
///============================================================================
2
/// This file is part of MIKROKOPTER SERIAL CONTROL TUTORIAL.
3
/// by JOHN C. MACDONALD at Ira A. Fulton College of Engineering and Technology
4
/// (http://hdl.lib.byu.edu/1877/2747)
5
/// (http://hdl.lib.byu.edu/1877/2748)
6
///============================================================================
7
 
8
using System;
9
using System.Collections.Generic;
10
using System.Linq;
11
using System.Text;
12
using System.Diagnostics;
13
 
14
namespace MKLiveView
15
{
16
    static class FlightControllerMessage
17
    {
18
        public static byte[] CreateMessage(char commandId, byte address)
19
        {
20
            return CreateMessage(commandId, address, new byte[0]);
21
        }
22
 
23
        public static byte[] CreateMessage(char commandId, byte address, byte[] data)
24
        {
25
            List<byte> result = new List<byte>();
26
            //Encoding.ASCII.GetBytes(command);
27
 
28
            // add header
29
            result.Add(Convert.ToByte('#'));
30
            result.Add((byte)(Convert.ToByte('a') + address));
31
            result.Add(Convert.ToByte(commandId));
32
 
33
            // add data
34
            result.AddRange(Encode64(data));
35
 
36
            // add footer
37
            result.AddRange(getCRCBytes(result.ToArray()));
38
            result.Add(Convert.ToByte('\r'));
39
 
40
            return result.ToArray();
41
        }
42
 
43
        public static void ParseMessage(byte[] message, out char commandId, out byte address, out byte[] data)
44
        {
45
            // Debug.Assert(message.Length >= 6);
46
            if (message.Length >= 6)
47
            {
48
                // header
49
                // '#' == message[0]
50
                byte[] Data = new byte[message.Length - 3];
51
                Array.Copy(message, 0, Data, 0, message.Length - 3);
52
                byte[] crcData = getCRCBytes(Data);
53
 
54
                if (crcData.Length == 2 && crcData[0] == message[message.Length - 3] && crcData[1] == message[message.Length - 2])
55
                {
56
 
57
                    commandId = Convert.ToChar(message[2]);
58
                    address = (byte)(message[1] - Convert.ToByte('a'));
59
 
60
                    // data
61
                    data = Decode64(message, 3, message.Length - 6);
62
                }
63
                else
64
                {
65
                    commandId = '\0';
66
                    address = 255;
67
                    data = null;
68
                }
69
            }
70
            else
71
            {
72
                commandId = '\0';
73
                address = 0;
74
                data = null;
75
            }
76
            // footer
77
            // CRC1 == message[message.Length - 3]
78
            // CRC2 == message[message.Length - 2]
79
            // '\r' == message[message.Length - 1]
80
        }
81
 
82
        public static byte[] Encode64(byte[] data)
83
        {
84
            List<byte> encodedData = new List<byte>();
85
 
86
            byte a,b,c;
87
            int index = 0;
88
            byte k = Convert.ToByte('=');
89
 
90
            while (index < data.Length)
91
            {
92
                if (index < data.Length) a = data[index++]; else a = 0;
93
                if (index < data.Length) b = data[index++]; else b = 0;
94
                if (index < data.Length) c = data[index++]; else c = 0;
95
 
96
                encodedData.Add((byte)(k + (a >> 2)));
97
                encodedData.Add((byte)(k + (((a & 0x03) << 4) | ((b & 0xf0) >> 4))));
98
                encodedData.Add((byte)(k + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6))));
99
                encodedData.Add((byte)(k + ( c & 0x3f)));
100
            }
101
 
102
            return encodedData.ToArray();
103
        }
104
 
105
 
106
        public static byte[] Decode64(byte[] data, int startIndex, int count)
107
        {
108
            // data should be in chunks of 4 right?
109
           // Debug.Assert(count % 4 == 0);
110
 
111
            List<byte> decodedData = new List<byte>();
112
 
113
            byte k = Convert.ToByte('=');
114
            byte a,b,c,d;
115
            byte x,y,z;
116
            int index = startIndex;
117
 
118
            while (index <= count + startIndex - 4)
119
            {
120
                a = (byte)(data[index++] - k);
121
                b = (byte)(data[index++] - k);
122
                c = (byte)(data[index++] - k);
123
                d = (byte)(data[index++] - k);
124
 
125
                x = (byte)((a << 2) | (b >> 4));
126
                y = (byte)(((b & 0x0f) << 4) | (c >> 2));
127
                z = (byte)(((c & 0x03) << 6) | d);
128
 
129
                decodedData.Add(x);
130
                decodedData.Add(y);
131
                decodedData.Add(z);
132
            }
133
 
134
            return decodedData.ToArray();
135
        }
136
 
137
 
138
        // cyclic redundancy check (CRC) or polynomial code checksum used to verify message
139
        // it is an insecure hash function designed to detect accidental changes to raw computer data (wikipedia)
140
        private static byte[] getCRCBytes(byte[] data)
141
        {
142
            byte[] crcBytes = new byte[2];
143
 
144
            uint tmpCRC = 0;
145
            for(int i = 0; i < data.Length ;i++)
146
            {
147
                tmpCRC += data[i];
148
            }
149
            tmpCRC %= 4096;
150
            crcBytes[0] = (byte)(Convert.ToByte('=') + tmpCRC / 64);
151
            crcBytes[1] = (byte)(Convert.ToByte('=') + tmpCRC % 64);
152
 
153
            return crcBytes;
154
        }
155
    }
156
}