Subversion Repositories Projects

Rev

Details | Last modification | View Log | RSS feed

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