private void write_data_raw(int index)
{
byte[] full_file = new byte[byte2read[index].Count];
//byte[] full_file = File.ReadAllBytes("D:\\Projects\\КСК синхронизация\\3 этап\\rec2_3.log");
string[] name = saveFileDialog.FileName.Split('\\');
string path = "";
for (int i = 0; i < name.Length - 1; i++)
path += name[i] + "\\";
FileStream fs_log = File.Create(path + "raw_data\\" + name[name.Length - 1] + "_" + index + ".log", 2048, FileOptions.None);
BinaryWriter str_log = new BinaryWriter(fs_log);
packet pack = new packet();
GPS_packet gps_pack = new GPS_packet();
byte[] pack2 = new byte[26];
byte[] buffer = new byte[2];
byte[] buffer2 = new byte[4];
byte[] temp = new byte[2];
int crc;
progressBar.Value = 0;
byte type_flag = 0;
bool first_init = true;
double[] read_coefs = new double[10];
if (full_file.Length != 0)
{
for (int i = 0; i < full_file.Length; i++)
{
full_file[i] = (byte)byte2read[index].Dequeue();
}
str_log.Write(full_file);
str_log.Flush();
str_log.Close();
progressBar.Maximum = full_file.Length - 39;
for (int i = 0; i < full_file.Length - 39; i++)
{
if (i % 50 == 0)
progressBar.Value += 49;
if ((full_file[i] == 0x10) && (full_file[i + 38] == 0x10) && (full_file[i + 39] == 0x03) &&
((full_file[i + 1] == 0x41) || (full_file[i + 1] == 0x51) || (full_file[i + 1] == 0x61)))
{
type_flag = full_file[i + 1];
crc = 0;
for (int j = i + 1; j < i + 37; j++)
{
crc = crc ^ full_file[j];
}
//crc = full_file[i + 37];
if (crc == full_file[i + 37])
{
pack = new packet();
pack.frame1 = full_file[i];
pack.type = full_file[i + 1];
pack.ticks = BitConverter.ToUInt32(full_file, i + 2);
//0.833F/1000, 0.04, 142.9F
pack.a = new short[3];
pack.a[0] = BitConverter.ToInt16(full_file, i + 6);
pack.a[1] = BitConverter.ToInt16(full_file, i + 8);
pack.a[2] = BitConverter.ToInt16(full_file, i + 10);
pack.w = new short[3];
pack.w[0] = BitConverter.ToInt16(full_file, i + 12);
pack.w[1] = BitConverter.ToInt16(full_file, i + 14);
pack.w[2] = BitConverter.ToInt16(full_file, i + 16);
//temp[0] = full_file[i + 13];
//temp[1] = full_file[i + 12];
//pack.w[0] = BitConverter.ToInt16(temp, 0);
//temp[0] = full_file[i + 15];
//temp[1] = full_file[i + 14];
//pack.w[1] = BitConverter.ToInt16(temp,0);
//temp[0] = full_file[i + 17];
//temp[1] = full_file[i + 16];
//pack.w[2] = BitConverter.ToInt16(temp, 0);
pack.m = new short[3];
pack.m[0] = BitConverter.ToInt16(full_file, i + 18);
pack.m[1] = BitConverter.ToInt16(full_file, i + 20);
pack.m[2] = BitConverter.ToInt16(full_file, i + 22);
pack.quat = new short[4];
pack.quat[0] = BitConverter.ToInt16(full_file, i + 24);
pack.quat[1] = BitConverter.ToInt16(full_file, i + 26);
pack.quat[2] = BitConverter.ToInt16(full_file, i + 28);
pack.quat[3] = BitConverter.ToInt16(full_file, i + 30);
pack.bar = BitConverter.ToInt16(full_file, i + 32);
pack.temper = BitConverter.ToInt16(full_file, i + 34);
pack.snum = full_file[i + 36];
pack.crc = full_file[i + 37];
pack.frame2 = full_file[i + 38];
pack.frame3 = full_file[i + 39];
switch (type_flag)
{
case 0x41:
if (pack.snum == (0xF0 ^ 1))
read_queue[0].Enqueue(pack); // left oar
else if (pack.snum == (0xE0 ^ 1))
read_queue[1].Enqueue(pack); // right oar
break;
case 0x51:
if (pack.snum == 1)
read_queue[4].Enqueue(pack); // seat
break;
case 0x61:
if (pack.snum == (0xF0 ^ 1))
read_queue[2].Enqueue(pack); // first hand
else if (pack.snum == (0xE0 ^ 1))
read_queue[3].Enqueue(pack); // second hand
break;
}
}
}
if ((full_file[i + 29] == 3) && (full_file[i + 28] == 16) && (full_file[i] == 16) &&
(full_file[i + 1] == 50)) // условие начала GPS пакета
{
crc = 50;
for (int j = 0; j < 26; j++)
{
pack2[j] = full_file[i + j + 2];
if (j < 25)
crc = crc ^ pack2[j];
}
if (crc == pack2[pack2.Length - 1])
{
gps_pack = new GPS_packet();
//ticks2[k2] = pack2[3] + pack2[2] * (int)Math.Pow(2, 8) +
// pack2[1] * (int)Math.Pow(2, 16) + pack2[0] * (int)Math.Pow(2, 24);
gps_pack.ticks_gps = BitConverter.ToUInt32(pack2, 0);
buffer2[0] = pack2[4]; buffer2[1] = pack2[5]; buffer2[2] = pack2[6]; buffer2[3] = pack2[7];
gps_pack.lat = ((double)BitConverter.ToInt32(buffer2, 0)) / 600000;
buffer2[0] = pack2[8]; buffer2[1] = pack2[9]; buffer2[2] = pack2[10]; buffer2[3] = pack2[11];
gps_pack.lon = ((double)BitConverter.ToInt32(buffer2, 0)) / 600000;
buffer[0] = pack2[12]; buffer[1] = pack2[13];
gps_pack.speed = (double)BitConverter.ToInt16(buffer, 0) / 100;
buffer[0] = pack2[14]; buffer[1] = pack2[15];
gps_pack.course = (double)BitConverter.ToInt16(buffer, 0) / 160;
buffer2[0] = pack2[16]; buffer2[1] = pack2[17]; buffer2[2] = pack2[18]; buffer2[3] = pack2[19];
gps_pack.time = ((double)BitConverter.ToInt32(buffer2, 0)) / 10;
gps_pack.stat = pack2[20];
buffer2[0] = pack2[21]; buffer2[1] = pack2[22]; buffer2[2] = pack2[23]; buffer2[3] = pack2[24];
gps_pack.date = ((double)BitConverter.ToInt32(buffer2, 0));
GPS_queue.Enqueue(gps_pack);
}
}
}
}
}
private void write_gps(GPS_packet[] source, int index)
{
int length = source.Length;
saveBox.Text = "Сохранение файла " + index;
saveBox.Update();
progressBar.Value = 0;
progressBar.Maximum = length;
string additional = "";
switch (index)
{
case 1:
additional = "left_oar";
break;
case 2:
additional = "right_oar";
break;
case 3:
additional = "first_hand";
break;
case 4:
additional = "second_hand";
break;
case 5:
additional = "seat";
break;
}
FileStream fs_gps = File.Create(saveFileDialog.FileName + "_" + additional + ".gps", 2048, FileOptions.None);
BinaryWriter str_gps = new BinaryWriter(fs_gps);
Int16 buf16; Byte buf8; Int32 buf32;
Double bufD; Single bufS; UInt32 bufU32;
double[] read_coefs = new double[0];
double additional_mult = 1;
for (int i = 0; i < length; i++)
{
progressBar.Invoke(new Action(() => progressBar.Value++));
// GPS
bufD = (Double)(source[i].lat) / ((180 / Math.PI) * 16.66);
str_gps.Write(bufD);
bufD = (Double)(source[i].lon) / ((180 / Math.PI) * 16.66);
str_gps.Write(bufD);
bufD = (Double)(0);
str_gps.Write(bufD);
bufS = (Single)(source[i].time);
str_gps.Write(bufS);
bufS = (Single)(source[i].speed);
str_gps.Write(bufS);
bufS = (Single)(0);
str_gps.Write(bufS);
str_gps.Write(bufS);
//bufU32 = (UInt32)(i);
bufU32 = (UInt32)(source[i].ticks_gps);
str_gps.Write(bufU32);
buf8 = (Byte)(0);
str_gps.Write(buf8);
str_gps.Write(buf8);
str_gps.Write(buf8);
}
// Запись даты в конец gps файла
int day = (int)source[length - 1].date / 10000;
int month = (int)(source[length - 1].date - day * 10000) / 100;
int year = (int)(2000 + source[length - 1].date - day * 10000 - month * 100);
string datarec = String.Format("{0:d2}.{1:d2}.{2:d4}", day, month, year);
str_gps.Write(datarec);
str_gps.Flush();
str_gps.Close();
}
private void readButton_Click(object sender, EventArgs e)
{
Button source = (Button)sender;
switch (source.Text)
{
case "Начать считывание":
Connect();
source.Text = "Остановить чтение";
stop_reading = false;
Thread[] reading = new Thread[ports_total];
for (int i = 0; i < 5; i++)
{
read_queue[i] = new Queue();
}
for (int i = 0; i < ports_total; i++)
{
//read_queue[i] = new Queue();
//reading[i] = new Thread(thread_read_flexible);
reading[i] = new Thread(thread_read_raw);
reading[i].Start();
}
control.Restart();
break;
case "Остановить чтение":
stop_reading = true;
control.Stop();
//read_queue[4] = new Queue();
//MessageBox.Show("Чтение завершено.");
MessageBox.Show("Количество байт принятых с СОМ портов:\n" + byte2read[0].Count + " " + byte2read[1].Count + " " + byte2read[2].Count +
" " + byte2read[3].Count);
for (int i = 0; i < ports_total; i++)
write_data_raw(i);
source.Text = "Начать считывание";
progressBar.Value = 0;
thread_counter = 0;
int[] length = {read_queue[0].Count, read_queue[1].Count,
read_queue[2].Count, read_queue[3].Count, read_queue[4].Count};
packet[] sensor_1 = new packet[length[0]];
packet[] sensor_2 = new packet[length[1]];
packet[] sensor_3 = new packet[length[2]];
packet[] sensor_4 = new packet[length[3]];
packet[] sensor_5 = new packet[length[4]];
int counter = 0;
bool[] finished = new bool[4 + 1];
while (true)
{
if ((!finished[0])&&(counter < length[0]))
sensor_1[counter] = (packet)read_queue[0].Dequeue();
else
finished[0] = true;
if ((!finished[1]) && (counter < length[1]))
sensor_2[counter] = (packet)read_queue[1].Dequeue();
else
finished[1] = true;
if ((!finished[2]) && (counter < length[2]))
sensor_3[counter] = (packet)read_queue[2].Dequeue();
else
finished[2] = true;
if ((!finished[3]) && (counter < length[3]))
sensor_4[counter] = (packet)read_queue[3].Dequeue();
else
finished[3] = true;
if ((!finished[4]) && (counter < length[4]))
sensor_5[counter] = (packet)read_queue[4].Dequeue();
else
finished[4] = true;
counter++;
if (finished[0] && finished[1] && finished[2] && finished[3] && finished[4])
break;
}
MessageBox.Show("Количество пакетов принятых с датчиков:\n" + sensor_1.Length + " " + sensor_2.Length + " " + sensor_3.Length
+ " " + sensor_4.Length + " " + sensor_5.Length);
if (sensor_1.Length != 0)
write_data(sensor_1, 1);
if (sensor_2.Length != 0)
write_data(sensor_2, 2);
if (sensor_3.Length != 0)
write_data(sensor_3, 3);
if (sensor_4.Length != 0)
write_data(sensor_4, 4);
if (sensor_5.Length != 0)
write_data(sensor_5, 5);
int glen = GPS_queue.Count;
GPS_packet[] pack1 = new GPS_packet[glen];
for (int i = 0; i < glen; i++)
pack1[i] = (GPS_packet)GPS_queue.Dequeue();
if (pack1.Length != 0)
write_gps(pack1, 1);
//MessageBox.Show("Сохранение завершено.");
saveBox.Text = "Сохранение завершено";
break;
}
}
