Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 2288 → Rev 2289

/MKLiveView/v1.0/Source/FlightControllerMessage.cs
0,0 → 1,156
///============================================================================
/// 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)
///============================================================================
 
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;
}
}
}