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; } }