0,0 → 1,159 |
///============================================================================ |
/// 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; |
} |
} |
} |