private void DataTask()
{
while (true)
{
evtDataReady.WaitOne();
String str = Encoding.UTF8.GetString(RxTcpBuf, 0, bytesRead);
Debug.WriteLine(str);
String s1 = "\"laser_points\":\"";
String s2 = null;
int i = 0;
bool r = str.Contains(s1);
if (true == r)
{
i = str.IndexOf(s1) + s1.Length;
s2 = str.Substring(i);
}
string tok = "\"";
i = s2.IndexOf(tok);
string sf = s2.Remove(i);
Debug.WriteLine(sf);
byte[] data = Convert.FromBase64String(sf);
int acc = 0;
int dacc = 0;
byte[] dData = new byte[1024 * 18];
while (acc != data.Length)
{
if (acc < 9)
{
dData[dacc++] = data[acc++];
continue;
}
switch (data[acc])
{
case 0x81:
case 0x7F:
byte sentinel = data[acc];
byte rcount = data[acc + 1];
byte rval = data[acc + 2];
if (0 == rcount)
{
if (0x7f == sentinel)
{
if (0x81 == rval)
{
acc += 3;
}
}
else
{
if (0x81 == sentinel)
{
if (0x7f == rval)
{
acc += 3;
}
}
}
dData[dacc++] = data[acc++];
}
else
{
if (rcount < 0x0f)
{
if (0 != rval)
{
dData[dacc++] = data[acc++];
Debug.Write("Something wrong\r\n");
continue;
}
while (0 != rcount)
{
dData[dacc++] = rval;
rcount--;
}
acc += 3;
}
else
{
dData[dacc++] = data[acc++];
}
}
break;
default:
dData[dacc++] = data[acc++];
break;
}
}
#if false
//Read the contents of the file into a stream
FileStream fs = File.Create("Data.txt");
using (BinaryWriter writer = new BinaryWriter(fs))
{
writer.Write(data, 0, data.Length);
}
//Read the contents of the file into a stream
fs = File.Create("DData.txt");
using (BinaryWriter writer = new BinaryWriter(fs))
{
writer.Write(dData, 0, dacc);
}
#endif
Protocol pkt = new Protocol();
int pos = 0x09;
byte[] cabin = new byte[12];
byte[] size = new byte[4];
Array.ConstrainedCopy(dData, 5, size, 0, 4);
int total = MyConvert.ByteToInt32(size, 0);
acc = 0;
#if false
fs = File.Create("cabin.txt");
byte[] terminate = new byte[2];
terminate[0] = 0x0a;
terminate[1] = 0x0d;
#endif
while (pos < total)
{
//Buffer.BlockCopy(dData, 0, cabin, 0, 12);
Array.ConstrainedCopy(dData, pos, cabin, 0, 12);
pkt.SetData(cabin);
float rad = pkt.Radian;
float dist = pkt.Distance;
bool valid = pkt.Valid;
#if false
fs.Write(cabin, 0, 12);
fs.Write(terminate, 0, 2);
#endif
pos += 12;
Invoke(new DELEGATE_DATA_UPDATE(DataUpdate), rad, dist, valid);
}
#if false
fs.Close();
#endif
}
}
private void ScanUpdate(string str)
{
String s1 = "\"laser_points\":\"";
String s2 = null;
int i = 0;
bool r = str.Contains(s1);
if (true == r)
{
i = str.IndexOf(s1) + s1.Length;
s2 = str.Substring(i);
}
string tok = "\"";
i = s2.IndexOf(tok);
string sf = s2.Remove(i);
Debug.WriteLine(sf);
byte[] data = Convert.FromBase64String(sf);
int acc = 0;
int dacc = 0;
byte[] dData = new byte[1024 * 18];
while (acc != data.Length)
{
if (acc < 9)
{
dData[dacc++] = data[acc++];
continue;
}
switch (data[acc])
{
case 0x81:
case 0x7F:
byte sentinel = data[acc];
byte rcount = data[acc + 1];
byte rval = data[acc + 2];
if (0 == rcount)
{
if (0x7f == sentinel)
{
if (0x81 == rval)
{
acc += 3;
}
}
else
{
if (0x81 == sentinel)
{
if (0x7f == rval)
{
acc += 3;
}
}
}
dData[dacc++] = data[acc++];
}
else
{
if (rcount < 0x0f)
{
while (0 != rcount)
{
dData[dacc++] = rval;
rcount--;
}
acc += 3;
}
else
{
dData[dacc++] = data[acc++];
}
}
break;
default:
dData[dacc++] = data[acc++];
break;
}
}
//Read the contents of the file into a stream
FileStream fs = File.Create("Data.txt");
using (BinaryWriter writer = new BinaryWriter(fs))
{
writer.Write(data, 0, data.Length);
}
//Read the contents of the file into a stream
fs = File.Create("DData.txt");
using (BinaryWriter writer = new BinaryWriter(fs))
{
writer.Write(dData, 0, dacc);
}
Protocol pkt = new Protocol();
int pos = 0x09;
byte[] cabin = new byte[12];
byte[] size = new byte[4];
Array.ConstrainedCopy(dData, 5, size, 0, 4);
int total = MyConvert.ByteToInt32(size, 0);
acc = 0;
while (pos < total)
{
//Buffer.BlockCopy(dData, 0, cabin, 0, 12);
Array.ConstrainedCopy(dData, pos, cabin, 0, 12);
pkt.SetData(cabin);
float rad = pkt.Radian;
float dist = pkt.Distance;
bool valid = pkt.Valid;
pos += 12;
tboxOutput.Text += rad.ToString() + " " + dist.ToString() + " " + valid.ToString() + "\r\n";
}
}
