// Функция для сохранения данных в файлы .imu/.gps
private void write_data(file file2write)
{
byte[] full_file = new byte[file2write.get_size()];
for (int i = 0; i < full_file.Length / 4096 - 1; i++)
{
for (int j = 0; j < 4096; j++)
{
full_file[i * 4096 + j] = file2write.get_packet(i).get_data(j);
}
}
uint[] ticks = new uint[full_file.Length / 35 + 1]; // примерное количество IMU пакетов
uint[] ticks2 = new uint[ticks.Length]; // примерное количество GPS пакетов неизвестно,
byte[] type = new byte[ticks.Length]; // но оно не может превышать число IMU пакетов
byte[] pack = new byte[32];
byte[] pack2 = new byte[26];
int crc;
int k = 0;
int k2 = 0;
int tt = 0;
int[] counter = new int[ticks.Length];
double[,] a = new double[ticks.Length, 3];
double[,] w = new double[ticks.Length, 3];
double[,] m = new double[ticks.Length, 3];
double[,] q = new double[ticks.Length, 4];
double[] anglex = new double[ticks.Length];
double[] angley = new double[ticks.Length];
double[] anglez = new double[ticks.Length];
double[] lat = new double[ticks2.Length];
double[] lon = new double[ticks2.Length];
double[] speed = new double[ticks2.Length];
double[] course = new double[ticks2.Length];
double[] time = new double[ticks2.Length];
double[] stat = new double[ticks2.Length];
double[] date = new double[ticks2.Length];
byte[] buffer = new byte[2];
byte[] buffer2 = new byte[4];
double[] corr_mag = { 2.4912, 2.4907, 2.4998, 2.5912, 3.4920, 2.5428, 2.4593, 2.5500, 2.5196, 2.5275, 1, 2.5017, 3.7391, 2.9516, 2.4844 };
double[] corr_accl = { 0.12, 0.17, 0.08, 0.09, 0.1, 0.12, 0.13, 0.1, 0.1431, 0.1, 0.145, 0.05, 0.1, 0.1, 0.32};
double[] corr_gyr = { 1.1111, 0.9333, 0.9333, 1.0185, 1.5122, 0.8355, 0.9148, 1.1154, 1.1453, 1.1129, 1.1028, 1.1160, 1.5063, 0.9, 0.9111 };
double[,] zero_mag = { {0.1780, 0.1092, -0.0961}, {0.1259, 0.0621, -0.2346}, { 0.4120, 0.0386, 0.2578}, {0.2565, -0.0926, 0.0760}, { 0.1221, -0.0659, 0.2637},
{0.4300, 0.0348, 0.2616}, {0.3156, -0.1437, -0.2155}, {-0.0928, 0.0603, 0.1485}, {0.0405, 0.3184, 0.1702}, {-0.0189, 0.0232, 0.3462},
{0, 0, 0 }, {0.4010, 0.0459, -0.9670}, { 0.2514, 0.0564, 0.2268}, {0.2202, 0.0130, 0.3759}, { 0.2044, 0.0045, 0.2669} };
infoLabel.Text = "Идет сохранение файла.";
this.Update();
for (int i = 0; i < full_file.Length - 30; i++)
{
Application.DoEvents();
if ((i < full_file.Length - 35)&&(full_file[i + 34] == 3) && (full_file[i + 33] == 16) &&
(full_file[i] == 16) && (full_file[i + 1] == 49)) // условие начала IMU пакета
{
crc = 0;
for (int j = 0; j < 32; j++)
{
pack[j] = full_file[i + j + 1];
if (j < 31)
crc = crc ^ pack[j];
}
if (crc == pack[pack.Length - 1])
{
//ticks[k] = pack[4] + pack[3] * (int)Math.Pow(2, 8) + pack[2] * (int)Math.Pow(2, 16) + pack[1] * (int)Math.Pow(2, 24);
ticks[k] = BitConverter.ToUInt32(pack, 1);
type[k] = pack[0];
if (type[k] == 49)
{
buffer[0] = pack[7]; buffer[1] = pack[8];
a[k, 0] = ((double)BitConverter.ToInt16(buffer, 0)) * 0.001766834114354;
buffer[0] = pack[5]; buffer[1] = pack[6];
a[k, 1] = (double)BitConverter.ToInt16(buffer, 0) * 0.001766834114354;
buffer[0] = pack[9]; buffer[1] = pack[10];
a[k, 2] = -(double)BitConverter.ToInt16(buffer, 0) * 0.001766834114354;
buffer[0] = pack[13]; buffer[1] = pack[14];
w[k, 0] = (double)BitConverter.ToInt16(buffer, 0) * 0.00053264 * corr_gyr[block_index - 1];
buffer[0] = pack[11]; buffer[1] = pack[12];
w[k, 1] = (double)BitConverter.ToInt16(buffer, 0) * 0.00053264 * corr_gyr[block_index - 1];
buffer[0] = pack[15]; buffer[1] = pack[16];
w[k, 2] = (double)BitConverter.ToInt16(buffer, 0) * 0.00053264 * corr_gyr[block_index - 1];
buffer[0] = pack[17]; buffer[1] = pack[18];
m[k, 0] = -((double)BitConverter.ToInt16(buffer, 0) * 0.00030518 - zero_mag[block_index - 1, 0]) * corr_mag[block_index - 1];
buffer[0] = pack[19]; buffer[1] = pack[20];
m[k, 1] = ((double)BitConverter.ToInt16(buffer, 0) * 0.00030518 - zero_mag[block_index - 1, 1]) * corr_mag[block_index - 1];
buffer[0] = pack[21]; buffer[1] = pack[22];
m[k, 2] = -((double)BitConverter.ToInt16(buffer, 0) * 0.00030518 - zero_mag[block_index - 1, 2]) * corr_mag[block_index - 1];
buffer[0] = pack[23]; buffer[1] = pack[24];
q[k, 0] = (double)BitConverter.ToInt16(buffer, 0);
buffer[0] = pack[25]; buffer[1] = pack[26];
q[k, 1] = (double)BitConverter.ToInt16(buffer, 0) * 0.00003125;
buffer[0] = pack[27]; buffer[1] = pack[28];
q[k, 2] = (double)BitConverter.ToInt16(buffer, 0) * 0.00003125;
buffer[0] = pack[29]; buffer[1] = pack[30];
q[k, 3] = (double)BitConverter.ToInt16(buffer, 0) * 0.00003125;
}
counter[k] = k2;
k++;
}
else
tt++;
}
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])
{
//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);
ticks2[k2] = BitConverter.ToUInt32(pack2, 0);
buffer2[0] = pack2[4]; buffer2[1] = pack2[5]; buffer2[2] = pack2[6]; buffer2[3] = pack2[7];
lat[k2] = ((double)BitConverter.ToInt32(buffer2, 0))/600000;
buffer2[0] = pack2[8]; buffer2[1] = pack2[9]; buffer2[2] = pack2[10]; buffer2[3] = pack2[11];
lon[k2] = ((double)BitConverter.ToInt32(buffer2, 0)) / 600000;
buffer[0] = pack2[12]; buffer[1] = pack2[13];
speed[k2] = (double)BitConverter.ToInt16(buffer, 0)/100;
buffer[0] = pack2[14]; buffer[1] = pack2[15];
course[k2] = (double)BitConverter.ToInt16(buffer, 0)/160;
buffer2[0] = pack2[16]; buffer2[1] = pack2[17]; buffer2[2] = pack2[18]; buffer2[3] = pack2[19];
time[k2] = ((double)BitConverter.ToInt32(buffer2, 0)) / 10;
stat[k2] = pack2[20];
buffer2[0] = pack2[21]; buffer2[1] = pack2[22]; buffer2[2] = pack2[23]; buffer2[3] = pack2[24];
date[k2] = ((double)BitConverter.ToInt32(buffer2, 0));
k2++;
}
}
}
// -------------- Сохранение в IMU/GPS
infoLabel.Text = "Сохранение файла.";
infoLabel.Update();
//fileLabel.Text += " - " + block_index;
//fileLabel.Update();
DenseVector Magn_coefs = new DenseVector(get_magn_coefs(block_index));
DenseVector Accl_coefs = new DenseVector(get_accl_coefs(block_index));
DenseVector Gyro_coefs = new DenseVector(12);
Kalman_class.Parameters Parameters = new Kalman_class.Parameters(Accl_coefs, Magn_coefs, Gyro_coefs);
Kalman_class.Sensors Sensors = new Kalman_class.Sensors(new DenseMatrix(1, 3, 0), new DenseMatrix(1, 3, 0), new DenseMatrix(1, 3, 0));
Matrix Initia_quat = new DenseMatrix(1, 4, 0);
Initia_quat.At(0, 0, 1);
Kalman_class.State State = new Kalman_class.State(Kalman_class.ACCLERATION_NOISE, Kalman_class.MAGNETIC_FIELD_NOISE, Kalman_class.ANGULAR_VELOCITY_NOISE,
Math.Pow(10, -6), Math.Pow(10, -15), Math.Pow(10, -15), Initia_quat);
double[] angles = new double[3];
double[] mw, ma, mm;
ma = new double[3];
mw = new double[3];
mm = new double[3];
Tuple<Vector, Kalman_class.Sensors, Kalman_class.State> AHRS_result;
string[] name_mass = log_name.Split('.');
string file2save = "";
for (int i = 0; i < name_mass.Length - 1; i++)
file2save += name_mass[i] + ".";
if (name_mass.Length == 1)
file2save = log_name + ".";
FileStream fs_imu = File.Create(file2save + "imu", 2048, FileOptions.None);
BinaryWriter str_imu = new BinaryWriter(fs_imu);
FileStream fs_gps = File.Create(file2save + "gps", 2048, FileOptions.None);
BinaryWriter str_gps = new BinaryWriter(fs_gps);
Int16 buf16; Byte buf8; Int32 buf32;
Double bufD; Single bufS; UInt32 bufU32;
progressBar.Invoke(new Action(() => progressBar.Maximum = k + k2));
progressBar.Invoke(new Action(() => progressBar.Value = 0));
double[] magn_c = get_magn_coefs(block_index);
double[] accl_c = get_accl_coefs(block_index);
double[] gyro_c = new double[12];
double[] w_helper = new double [ticks.Length];
for (int i=0; i<w_helper.Length; i++) w_helper[i] = w[i,0];
anglex = Signal_processing.Zero_average_corr(w_helper, w_helper.Length);
for (int i=0; i<w_helper.Length; i++) w_helper[i] = w[i,1];
angley = Signal_processing.Zero_average_corr(w_helper, w_helper.Length);
for (int i=0; i<w_helper.Length; i++) w_helper[i] = w[i,2];
anglez = Signal_processing.Zero_average_corr(w_helper, w_helper.Length);
for (int i = 0; i < k; i++)
{
Application.DoEvents();
progressBar.Invoke(new Action(() => progressBar.Value++));
/* Sensors.a.At(0, 0, a[i, 0]);
Sensors.a.At(0, 1, a[i, 1]);
Sensors.a.At(0, 2, a[i, 2]);
Sensors.w.At(0, 0, w[i, 0]);
Sensors.w.At(0, 1, w[i, 1]);
Sensors.w.At(0, 2, w[i, 2]);
Sensors.m.At(0, 0, m[i, 0]);
Sensors.m.At(0, 1, m[i, 1]);
Sensors.m.At(0, 2, m[i, 2]);
AHRS_result = Kalman_class.AHRS_LKF_EULER(Sensors, State, Parameters);
State = AHRS_result.Item3;*/
//------------------------------------------------------------------------
mm = single_correction(magn_c, m[i, 0], m[i, 1], m[i, 2]);
ma = single_correction(accl_c, a[i, 0], a[i, 1], a[i, 2]);
mw = single_correction(gyro_c, w[i, 0], w[i, 1], w[i, 2]);
//----------------------------------------------------------------------
//mw[0] = w[i, 0];
//mw[1] = w[i, 1];
//mw[2] = w[i, 2];
//ma[0] = a[i, 0];
//ma[1] = a[i, 1];
//ma[2] = a[i, 2];
//mm[0] = m[i, 0];
//mm[1] = m[i, 1];
//mm[2] = m[i, 2];
//----------------------------------------------------------------------
angles[0] = (anglez[i]);
angles[1] = (angley[i]);
angles[2] = (anglex[i]);
// IMU
buf16 = (Int16)(angles[0] * 10000);
str_imu.Write(buf16);
buf16 = (Int16)(angles[1] * 10000);
str_imu.Write(buf16);
buf16 = (Int16)(angles[2] * 10000);
str_imu.Write(buf16);
buf16 = (Int16)(mw[0] * 3000);
str_imu.Write(buf16);
buf16 = (Int16)(mw[1] * 3000);
str_imu.Write(buf16);
buf16 = (Int16)(mw[2] * 3000);
str_imu.Write(buf16);
buf16 = (Int16)((a[i,0]+corr_accl[block_index-1]) * 3000);
str_imu.Write(buf16);
buf16 = (Int16)(ma[1] * 3000);
str_imu.Write(buf16);
buf16 = (Int16)(ma[2] * 3000);
str_imu.Write(buf16);
buf16 = (Int16)(mm[0] * 3000);
str_imu.Write(buf16);
buf16 = (Int16)(mm[1] * 3000);
str_imu.Write(buf16);
buf16 = (Int16)(mm[2] * 3000);
str_imu.Write(buf16);
buf16 = (Int16)(q[i, 0]);
str_imu.Write(buf16);
//buf32 = (Int32)(counter[i]);
buf32 = (Int32)(ticks[i]);
str_imu.Write(buf32);
buf8 = (Byte)(0);
str_imu.Write(buf8);
}
for (int i = 0; i < k2; i++)
{
Application.DoEvents();
progressBar.Invoke(new Action(() => progressBar.Value++));
// GPS
bufD = (Double)(lat[i]) / ((180 / Math.PI) * 16.66);
str_gps.Write(bufD);
bufD = (Double)(lon[i]) / ((180 / Math.PI) * 16.66);
str_gps.Write(bufD);
bufD = (Double)(0);
str_gps.Write(bufD);
bufS = (Single)(time[i]);
str_gps.Write(bufS);
bufS = (Single)(speed[i]);
str_gps.Write(bufS);
bufS = (Single)(0);
str_gps.Write(bufS);
str_gps.Write(bufS);
//bufU32 = (UInt32)(i);
bufU32 = (UInt32)(ticks2[i]);
str_gps.Write(bufU32);
buf8 = (Byte)(0);
str_gps.Write(buf8);
str_gps.Write(buf8);
str_gps.Write(buf8);
}
// Запись даты в конец gps файла
int day = (int)date[k2 - 1] / 10000;
int month = (int)(date[k2 - 1] - day * 10000) / 100;
int year = (int)(2000 + date[k2 - 1] - day * 10000 - month * 100);
string datarec = String.Format("{0:d2}.{1:d2}.{2:d4}", day, month, year);
str_gps.Write(datarec);
str_imu.Flush();
str_imu.Close();
str_gps.Flush();
str_gps.Close();
MessageBox.Show("Сохранение завершено");
}
// Функция для чтения данных напрямую с датчика
private void readButton_Click(object sender, EventArgs e)
{
readButton.Enabled = false;
saveButton.Enabled = false;
try
{
fileBox.Items.Clear();
fileBox.Update();
byte[] buffer = new byte[8192]; // максимальный пакет 4096 байт, взято с запасом
int numb = 0;
char[] tempS = new char[200];
long numfiles = 0;
long fcur = 0;
char[] log_name_c = new char[2048];
System.Diagnostics.Stopwatch timer = new System.Diagnostics.Stopwatch();
//System.Diagnostics.Stopwatch control = new System.Diagnostics.Stopwatch();
if ((validCOM == 0) && (selected_com != "COM0"))
{
Connect();
//FileStream fs = File.Create(log_name, 2048, FileOptions.None);
//StreamWriter str_wr = new StreamWriter(fs);
//control.Start();
infoLabel.Text = "Идет считывание";
infoLabel.Update();
active_com.DiscardInBuffer();
timer.Start();
active_com.Write("v"); // запрос версии и номера датчика
while (active_com.BytesToRead < 4)
{
if (timer.ElapsedMilliseconds > 1000)
{
MessageBox.Show("Не получен ответ от датчика. Проверьте подключение.");
Disconnect();
readButton.Enabled = true;
return;
}
}
numb = active_com.Read(buffer, 0, 4);
if (numb < 4)
{
MessageBox.Show("Не получен ответ от датчика. Этап 1.");
Disconnect();
readButton.Enabled = true;
return;
}
block_index = buffer[0] * (int)Math.Pow(2, 24) + buffer[1] * (int)Math.Pow(2, 16) + buffer[2] * (int)Math.Pow(2, 8) + buffer[3];
active_com.Write("?"); // запрос на количество записей в памяти
Thread.Sleep(50);
numb = active_com.Read(buffer, 0, 4);
numfiles = buffer[0] * (int)Math.Pow(2, 24) + buffer[1] * (int)Math.Pow(2, 16) + buffer[2] * (int)Math.Pow(2, 8) + buffer[3];
active_com.Write("c");
Thread.Sleep(50); // Временная задержка, рабочий вариант
numb = active_com.Read(buffer, 0, 4);
fcur = buffer[0] * (int)Math.Pow(2, 24) + buffer[1] * (int)Math.Pow(2, 16) + buffer[2] * (int)Math.Pow(2, 8) + buffer[3];
if (numb < 4)
{
MessageBox.Show("Не получен ответ от датчика. Этап 2.");
Disconnect();
readButton.Enabled = true;
return;
}
while (fcur > 1)
{
active_com.Write("-");
active_com.Read(buffer, 0, 4);
Thread.Sleep(20);
active_com.Write("c");
Thread.Sleep(20);
numb = active_com.Read(buffer, 0, 4);
fcur = buffer[0] * (int)Math.Pow(2, 24) + buffer[1] * (int)Math.Pow(2, 16) + buffer[2] * (int)Math.Pow(2, 8) + buffer[3];
}
int fsum = 0;
progressBar.Maximum = (int)numfiles;
progressBar.Value = 0;
file file2add;
packet packet2add;
int file_count = 1;
int packet_count = 0;
packetBox.Text = "" + packet_count;
packetBox.Update();
active_com.ReadTimeout = 500; // maximum amount of time allowed
// for perfoming one reading operation
for (int q = 0; q < numfiles; q++)
{
progressBar.Value++;
fsum = 0;
active_com.Write("r");
Thread.Sleep(40);
numb = active_com.Read(buffer, 0, 4);
file2add = new file(file_count); // для каждой записи создается новый объект file
//str_wr.WriteLine("file " + q + " read bytes: " + numb);
while (fsum != 4096 * 255) // выход из цикла если все значения равны FF
{
fsum = 0;
Application.DoEvents();
active_com.Write("n");
Thread.Sleep(40);
numb = active_com.Read(buffer, 0, 4096);
for (int w = 0; w < numb; w++)
fsum += buffer[w];
packet2add = new packet(buffer, numb); // для каждого пакета - новый объект packet
file2add.add(packet2add);
packet_count++;
if (packet_count % 20 == 0)
{
packetBox.Text = "" + packet_count;
packetBox.Update();
}
//str_wr.Write(fsum + " ");
}
if (file2add.get_size() > 4096)
{
files_q.Enqueue(file2add); // объект file добавляется в очередь
file_count++; // только если в нем больше 1 пакета
}
//str_wr.WriteLine();
}
packetBox.Text = "" + packet_count;
packetBox.Update();
//str_wr.Flush();
//str_wr.Close();
//control.Stop();
}
if (validCOM == 1)
{
Disconnect();
//MessageBox.Show("Чтение завершено. " + control.ElapsedMilliseconds + " ms elapsed.");
saveButton.Enabled = true;
flags[1] = true; // this flag is used to keep readButton locked (unavailble)
MessageBox.Show("Чтение завершено.");
files_st = new file[files_q.Count];
for (int q = 0; q < files_st.Length; q++)
{
// вывод спика ненулевых записей в textBox
files_st[q] = (file)files_q.Dequeue();
fileBox.Items.Add("Запись " + files_st[q].get_id() + " - " + (files_st[q].get_size())/194700 + " мин");
}
check_save_available(sender, e);
}
}
catch (Exception crit_error)
{
MessageBox.Show("Произошла критическая ошибка. Возможные причины ошибки:\n" +
"• выбранный СОМ порт не связан с датчиком СКВП\n" +
"• датчик выключен\n" +
"\n Код ошибки: " + crit_error.Message);
if (active_com.IsOpen)
active_com.Close();
readButton.Enabled = true;
return;
}
return;
}
