private void save_thread(object none)
{
int selected = selected_record;
byte[] full_file = File.ReadAllBytes("C:\\temp\\log_" + selected + ".txt");
Dispatcher.Invoke(new Action(() => set_stage("stage 1")));
uint[] ticks = new uint[full_file.Length / 35 + 1]; // примерное количество IMU пакетов
uint[] ticks2 = new uint[ticks.Length/15]; // примерное количество GPS пакетов неизвестно,
byte[] type = new byte[ticks.Length]; // но оно примерно в 20 раз меньше (взято с запасом)
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 = { 1, 2.2391, 1, 2.1586, 2.3980,
1, 1, 1.9201, 1.9963, 2.1538,
1, 1, 2.7105, 1.9782, 1.9013 };
double[] corr_accl = { 1, 0.9778, 1, 0.8928, 1.3899,
1, 0.9722, 0.9766, 0.9599, 0.9379,
0.9183, 0.9924, 1.2657, 0.9892, 0.9598 };
double[,] corr_gyr = { {1, 1, 1, 1, 1, 1, 1, 1, 1}, // 1
{1, 1, 1, 1, 1, 1, 1, 1, 1}, // 2
{1, 1, 1, 1, 1, 1, 1, 1, 1}, // 3
{1, 1, 1, 1, 1, 1, 1, 1, 1}, // 4
{-0.00373042367688305, 1.4558979622744, -0.0258818845609887, 0.00460253725775466, 1.4522057450117,
-0.035372440971534, -0.00301630637615598, 1.46124337866467, 0.0112681875599293}, // 5
{1, 1, 1, 1, 1, 1, 1, 1, 1}, // 6
{1, 1, 1, 1, 1, 1, 1, 1, 1}, // 7
{1, 1, 1, 1, 1, 1, 1, 1, 1}, // 8
{1, 1, 1, 1, 1, 1, 1, 1, 1}, // 9
{1, 1, 1, 1, 1, 1, 1, 1, 1}, // 10
{1, 1, 1, 1, 1, 1, 1, 1, 1}, // 11
{1, 1, 1, 1, 1, 1, 1, 1, 1}, // 12
{1, 1, 1, 1, 1, 1, 1, 1, 1}, // 13
{1, 1, 1, 1, 1, 1, 1, 1, 1}, // 14
{1, 1, 1, 1, 1, 1, 1, 1, 1}, // 15
}; // Gyro correction coeffs are listed in XYZ order
double accl_scale = 0;
if (block_index < 16)
accl_scale = corr_accl[block_index - 1];
double mag_scale = 0;
if (block_index < 16)
mag_scale = corr_mag[block_index - 1];
double[] gyr_scale = new double[9];
if (block_index < 16)
for (int t = 0; t < 9; t++)
gyr_scale[t] = corr_gyr[block_index - 1, t];
addText("\n" + get_time() + " Начало сохранения файла...");
for (int i = 0; i < full_file.Length - 30; i++)
{
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] = 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 *accl_scale;
buffer[0] = pack[5]; buffer[1] = pack[6];
a[k, 1] = (double)BitConverter.ToInt16(buffer, 0) * 0.001766834114354 *accl_scale;
buffer[0] = pack[9]; buffer[1] = pack[10];
a[k, 2] = -(double)BitConverter.ToInt16(buffer, 0) * 0.001766834114354 *accl_scale;
buffer[0] = pack[13]; buffer[1] = pack[14];
w[k, 0] = (double)BitConverter.ToInt16(buffer, 0) * 0.00053264;
buffer[0] = pack[11]; buffer[1] = pack[12];
w[k, 1] = (double)BitConverter.ToInt16(buffer, 0) * 0.00053264;
buffer[0] = pack[15]; buffer[1] = pack[16];
w[k, 2] = -(double)BitConverter.ToInt16(buffer, 0) * 0.00053264;
w[k, 0] = gyr_scale[0] * Math.Pow(w[k, 0], 2) + gyr_scale[1] * w[k, 0] + gyr_scale[2];
w[k, 1] = gyr_scale[3] * Math.Pow(w[k, 1], 2) + gyr_scale[4] * w[k, 1] + gyr_scale[5];
w[k, 2] = gyr_scale[6] * Math.Pow(w[k, 2], 2) + gyr_scale[7] * w[k, 2] + gyr_scale[8];
buffer[0] = pack[17]; buffer[1] = pack[18];
m[k, 0] = ((double)BitConverter.ToInt16(buffer, 0) * 0.00030518) * mag_scale;
buffer[0] = pack[19]; buffer[1] = pack[20];
m[k, 1] = -((double)BitConverter.ToInt16(buffer, 0) * 0.00030518) * mag_scale;
buffer[0] = pack[21]; buffer[1] = pack[22];
m[k, 2] = ((double)BitConverter.ToInt16(buffer, 0) * 0.00030518) * mag_scale;
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] = 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
double[] angles = new double[3];
double[] mw, ma, mm;
ma = new double[3];
mw = new double[3];
mm = new double[3];
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;
addText("\n" + get_time() + " Сохранение файлов:\n" + file2save + ".imu\n" + file2save + ".gps");
int pbar_adj = 50;
Dispatcher.Invoke(new Action(() => progressBar1.Maximum = (k+k2)/pbar_adj + 1));
Dispatcher.Invoke(new Action(() => progressBar1.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);
//-----------------------------------------------------------------------------------------------
// Получение углов путем использования фильтра Калмана (сложный вариант)
MathNet.Numerics.LinearAlgebra.Double.DenseVector Magn_coefs =
new MathNet.Numerics.LinearAlgebra.Double.DenseVector(get_magn_coefs(block_index));
MathNet.Numerics.LinearAlgebra.Double.DenseVector Accl_coefs =
new MathNet.Numerics.LinearAlgebra.Double.DenseVector(get_accl_coefs(block_index));
MathNet.Numerics.LinearAlgebra.Double.DenseVector Gyro_coefs = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(12);
Kalman_class.Parameters Parameters = new Kalman_class.Parameters(Accl_coefs, Magn_coefs, Gyro_coefs);
System.Func<int, int, double> filler = (x, y) => 0;
//Kalman_class.Sensors Sensors = new Kalman_class.Sensors(new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(1, 3, 0),
// new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(1, 3, 0), new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(1, 3, 0));
//MathNet.Numerics.LinearAlgebra.Double.Matrix Initia_quat = new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(1, 4, 0);
Kalman_class.Sensors Sensors = new Kalman_class.Sensors(MathNet.Numerics.LinearAlgebra.Double.DenseMatrix.Create(1,3, filler),
MathNet.Numerics.LinearAlgebra.Double.DenseMatrix.Create(1, 3, filler), MathNet.Numerics.LinearAlgebra.Double.DenseMatrix.Create(1, 3, filler));
MathNet.Numerics.LinearAlgebra.Double.Matrix Initia_quat = MathNet.Numerics.LinearAlgebra.Double.DenseMatrix.Create(1, 4, filler);
Initia_quat[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);
Tuple<MathNet.Numerics.LinearAlgebra.Double.Vector, Kalman_class.Sensors, Kalman_class.State> AHRS_result;
//----------------------------------------------------------------------------------------------
System.Diagnostics.Stopwatch timer = new System.Diagnostics.Stopwatch();
timer.Start();
for (int i = 0; i < k; i++)
{
if (i % pbar_adj == 0)
Dispatcher.Invoke(new Action(() => progressBar1.Value++));
if (i == ((int)(k + k2) / 20))
{
long min = (timer.ElapsedMilliseconds * 16) / 1000 / 60;
long sec = (timer.ElapsedMilliseconds * 16 - min * 1000 * 60) / 1000;
addText("\n" + get_time() + " Сохранение файлов займет около\n" +
min + " минут(ы) и " + sec + " секунд(ы).");
}
//------------------------------------------------------------------
// Легкий варинат - интегрирование угловых скоростей
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]);
//angles[0] = (anglez[i]);
//angles[1] = (angley[i]);
//angles[2] = (anglex[i]);
//----------------------------------------------------------------------
// Сложный вариант - фильтр Калмана
for (int j = 0; j < 3; j++)
{
Sensors.a.At(0, j, a[i, j]);
Sensors.w.At(0, j, w[i, j]);
Sensors.m.At(0, j, m[i, j]);
}
//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;
//ma[0] = AHRS_result.Item2.a[0, 0];
//ma[1] = AHRS_result.Item2.a[0, 1];
//ma[2] = AHRS_result.Item2.a[0, 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] = (AHRS_result.Item1.At(0));
angles[1] = (AHRS_result.Item1.At(1));
angles[2] = (AHRS_result.Item1.At(2));
//------------------------------------------------------------------------
// 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)(ma[0] * 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)(ticks[i]);
str_imu.Write(buf32);
buf8 = (Byte)(0);
str_imu.Write(buf8);
}
for (int i = 0; i < k2; i++)
{
if (i % pbar_adj == 0)
Dispatcher.Invoke(new Action(() => progressBar1.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)(ticks2[i]);
str_gps.Write(bufU32);
buf8 = (Byte)(0);
str_gps.Write(buf8);
str_gps.Write(buf8);
str_gps.Write(buf8);
}
// Запись даты в конец gps файла
bool inertial = false, sattelite = false;
if (k > 0)
inertial = true;
if (k2 > 0)
{
sattelite = true;
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);
}
if (!inertial || !sattelite)
addText("\n" + get_time() + "Неполный комплект данных:\nИнерциальные данные - " + inertial + "\nСпутниковые данные - " + sattelite);
str_imu.Flush();
str_imu.Close();
str_gps.Flush();
str_gps.Close();
Dispatcher.Invoke(new Action(() => progressBar1.Value = (k+k2)/pbar_adj + 1));
//addText("\n" + get_time() + " Сохранение завершено! Всего " + timer.ElapsedMilliseconds / 1000 + " секунд!");
addText("\n" + get_time() + " Сохранение завершено!");
System.Media.SystemSounds.Asterisk.Play();
timer.Stop();
Dispatcher.Invoke(new Action(() => set_stage("stage 2")));
}
private void read_file()
{
double[,] q;
double[,] a;
double[,] w;
double[,] m;
int k = 0, k2 = 0;
uint[] ticks;
uint[] ticks2;
byte[] type;
double[] lat;
double[] lon;
double[] speed;
double[] course;
double[] time;
double[] stat;
double[] date;
byte[] full_file = File.ReadAllBytes(file2read);
ticks = new uint[full_file.Length / 35 + 1];
ticks2 = new uint[ticks.Length];
type = new byte[ticks.Length];
byte[] pack = new byte[32];
byte[] pack2 = new byte[26];
int crc;
int tt = 0;
a = new double[ticks.Length, 3];
w = new double[ticks.Length, 3];
m = new double[ticks.Length, 3];
q = new double[ticks.Length, 4];
int[] counter = new int[ticks.Length];
lat = new double[ticks2.Length];
lon = new double[ticks2.Length];
speed = new double[ticks2.Length];
course = new double[ticks2.Length];
time = new double[ticks2.Length];
stat = new double[ticks2.Length];
date = new double[ticks2.Length];
byte[] buffer = new byte[2];
byte[] buffer2 = new byte[4];
this.Update();
for (int i = 0; i < full_file.Length - 30; i++)
{
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))
{
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] * (uint)Math.Pow(2, 8) + pack[2] * (uint)Math.Pow(2, 16) + pack[1] * (uint)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.0018;
buffer[0] = pack[5]; buffer[1] = pack[6];
a[k, 1] = (double)BitConverter.ToInt16(buffer, 0) * 0.0018;
buffer[0] = pack[9]; buffer[1] = pack[10];
a[k, 2] = -(double)BitConverter.ToInt16(buffer, 0) * 0.0018;
buffer[0] = pack[13]; buffer[1] = pack[14];
w[k, 0] = (double)BitConverter.ToInt16(buffer, 0) * 0.00053375;
buffer[0] = pack[11]; buffer[1] = pack[12];
w[k, 1] = (double)BitConverter.ToInt16(buffer, 0) * 0.00053375;
buffer[0] = pack[15]; buffer[1] = pack[16];
w[k, 2] = (double)BitConverter.ToInt16(buffer, 0) * 0.00053375;
buffer[0] = pack[17]; buffer[1] = pack[18];
m[k, 0] = -(double)BitConverter.ToInt16(buffer, 0) * 0.00030518;
buffer[0] = pack[19]; buffer[1] = pack[20];
m[k, 1] = (double)BitConverter.ToInt16(buffer, 0) * 0.00030518;
buffer[0] = pack[21]; buffer[1] = pack[22];
m[k, 2] = -(double)BitConverter.ToInt16(buffer, 0) * 0.00030518;
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))
{
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] * (uint)Math.Pow(2, 8) +
// pack2[1] * (uint)Math.Pow(2, 16) + pack2[0] * (uint)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++;
}
}
}
readButton.Enabled = false;
fileButton.Enabled = false;
textBox1.Enabled = false;
readLabel.Text = "Идет чтение:";
readLabel.Update();
int block_index = Convert.ToInt32(textBox1.Text.ToString());
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((float)Math.Pow(10, 2), (float)Math.Pow(10, 2), (float)Math.Pow(10, -3),
(float)Math.Pow(10, -6), (float)Math.Pow(10, -15), (float)Math.Pow(10, -15), Initia_quat);
float[] angles = new float[3];
float[] mw, ma, mm;
ma = new float[3];
mw = new float[3];
mm = new float[3];
Tuple<Vector, Kalman_class.Sensors, Kalman_class.State> AHRS_result;
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));
float[] magn_c = get_magn_coefs(block_index);
float[] accl_c = get_accl_coefs(block_index);
float[] gyro_c = new float[12];
for (int i = 0; i < k; i++)
{
progressBar.Invoke(new Action(() => progressBar.Value++));
Sensors.a.At(0, 0, (float)a[i, 0]);
Sensors.a.At(0, 1, (float)a[i, 1]);
Sensors.a.At(0, 2, (float)a[i, 2]);
Sensors.w.At(0, 0, (float)w[i, 0]);
Sensors.w.At(0, 1, (float)w[i, 1]);
Sensors.w.At(0, 2, (float)w[i, 2]);
Sensors.m.At(0, 0, (float)m[i, 0]);
Sensors.m.At(0, 1, (float)m[i, 1]);
Sensors.m.At(0, 2, (float)m[i, 2]);
AHRS_result = Kalman_class.AHRS_LKF_EULER(Sensors, State, Parameters);
State = AHRS_result.Item3;
mm = single_correction(magn_c, (float)m[i, 0], (float)m[i, 1], (float)m[i, 2]);
ma = single_correction(accl_c, (float)a[i, 0], (float)a[i, 1], (float)a[i, 2]);
mw = single_correction(gyro_c, (float)w[i, 0], (float)w[i, 1], (float)w[i, 2]);
angles[0] = (float)(AHRS_result.Item1.At(0) / 1.735);
angles[1] = (float)(AHRS_result.Item1.At(1) / 1.735);
angles[2] = (float)(AHRS_result.Item1.At(2) / 1.735);
// IMU
buf16 = (Int16)(angles[0] * 10000 * 100 / (180 / Math.PI));
str_imu.Write(buf16);
buf16 = (Int16)(angles[1] * 10000 * 100 / (180 / Math.PI));
str_imu.Write(buf16);
buf16 = (Int16)(angles[2] * 10000 * 100 / (180 / Math.PI));
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)(ma[0] * 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++)
{
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);
}
str_imu.Flush();
str_imu.Close();
str_gps.Flush();
str_gps.Close();
//MessageBox.Show("Сохранение завершено");
readLabel.Text = "Чтение завершено";
readButton.Enabled = true;
fileButton.Enabled = true;
textBox1.Enabled = true;
}
private void write_data(packet[] source, int index)
{
int length = source.Length;
saveBox.Text = "Сохранение файла " + index;
saveBox.Update();
progressBar.Value = 0;
progressBar.Maximum = length;
string additional = "";
double[] magn_c = new double[12];
double[] accl_c = new double[12];
double[] gyro_c = new double[12];
double Mk = 1;
double Wk = 1;
double Ak = 1;
switch (index)
{
case 1:
additional = "left_oar";
double[] temp1 = { 0.021012275928952, 0.067860169975568, 0.000127689343889, 0.045296151352541,
-0.009324017077391, 0.026722923577867, 0.008307857594568, 0.005284287915262,
-0.035947192687517, -0.012542925115207, 0.061512131892943, 0.004409782721854 };
accl_c = temp1;
Ak = 0.996259867299029;
double[] temp11 = { 0.002873025761360, -0.000277783847693, -0.008949237789463, 0.024839518300883,
0.018134114269133, 0.004957678597933, 0.059903966032509, -0.020001702990621,
-0.048455202844697, 0.040007724599001, -0.002723436449578, 0.028657182612509 };
magn_c = temp11;
Mk = 1.989928230657113;
Wk = 1.024951581925949;
break;
case 2:
additional = "right_oar";
double[] temp2 = { 0.006489103530672, 0.007711972047671, 0.008910927008256, 0.004526628966860,
-0.030613701020120, 0.009733926042102, 0.000520434141698, -0.020438431372878,
-0.000690207812167, 0.003527827615877, 0.003268096379523, -0.009084146831565 };
accl_c = temp2;
Ak = 0.993917643789002;
double[] temp22 = {-0.004338472524034, -0.007279143920897, -0.016596561254121, -0.040100105586272,
0.026447190185438, 0.062723828297715, 0.014235202706375, -0.032778454891706,
-0.016686216543873, 0.049183804843751, -0.170661765769371, 0.174784618340082 };
magn_c = temp22;
Mk = 1.981050375613622;
Wk = 0.999473669119779;
break;
case 3:
additional = "first_hand";
double[] temp3 = { 0.012248789806271, 0.010963952889031, 0.010082940049261, 0.017732310046796,
-0.010807018986018, -0.017866987099261, -0.038976713533177, 0.011937965171044,
0.017418474194167, 0.010157002539499, 0.006639710879831, -0.008997250633174 };
accl_c = temp3;
Ak = 0.995116429313122;
double[] temp33 = {-0.024902579646778, -0.020478721707093, -0.026353375666180, -0.001744353714878,
0.001038721251414, -0.008552827405651, 0.000013148017514, 0.005334631470956,
-0.004628038571913, -0.041660570389140, 0.203013174006507, 0.110944181992475 };
magn_c = temp33;
Mk = 2.000804739331122;
Wk = 1.005957834380545;
break;
case 4:
additional = "second_hand";
double[] temp4 = { 0.009773445955991, 0.010515969102165, 0.046332523300049, -0.009734857220358,
0.088901575635824, 0.011068909952491, -0.012572850762404, 0.170713670954205,
-0.008974449587517, 0.001063872406408, -0.005104328004269, -0.003338723771119 };
accl_c = temp4;
Ak = 0.995096553223738;
double[] temp44 = { 0.014780841871083, 0.018746310904878, 0.009620181136691, -0.006116082059700,
-0.000324924134046, -0.004326570176434, -0.007611544691470, 0.009652319917883,
0.002813293660957, 0.093081170636918, 0.035125384985654, -0.014316950547061 };
magn_c = temp44;
Mk = 2.063752665877927;
Wk = 1.013532577606638;
break;
case 5:
additional = "seat";
double[] temp5 = { 0.018860976044349, 0.012994456079329, -0.017726498806178, 0.014155888603851,
0.135660705289298, -0.005517590513633, -0.006515740524433, 0.089222505526992,
0.007556195473340, -0.034224056011697, -0.032625814493799, -0.091725250270515 };
accl_c = temp5;
Ak = 0.990392687940513;
break;
}
FileStream fs_imu = File.Create(saveFileDialog.FileName + "_" + additional + ".imu", 2048, FileOptions.None);
BinaryWriter str_imu = new BinaryWriter(fs_imu);
Int16 buf16; Byte buf8; Int32 buf32;
Double bufD; Single bufS; UInt32 bufU32;
DenseVector Magn_coefs = new DenseVector(12);
DenseVector Accl_coefs = new DenseVector(12);
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;
double[] w_helper = new double[source.Length];
double[] anglex = new double[source.Length];
double[] angley = new double[source.Length];
double[] anglez = new double[source.Length];
//for (int i = 0; i < w_helper.Length; i++) w_helper[i] = source[i].w[0];
//anglex = Signal_processing.Zero_average_corr(w_helper, w_helper.Length);
//for (int i = 0; i < w_helper.Length; i++) w_helper[i] = source[i].w[1];
//angley = Signal_processing.Zero_average_corr(w_helper, w_helper.Length);
//for (int i = 0; i < w_helper.Length; i++) w_helper[i] = source[i].w[2];
//anglez = Signal_processing.Zero_average_corr(w_helper, w_helper.Length);
double[] read_coefs = new double[0];
double additional_mult = 1;
switch (source[0].type)
{
case 0x41:
double[] tempc1 = { 0.000833, 0.04 * Math.PI / 180F, 0.00014286, 0.00002, 31, 0.07386, 0, 0 };
read_coefs = tempc1;
break;
case 0x51:
double[] tempc2 = { 0.0039, 0, 0, 0, 0, 0, 0, 0 };
read_coefs = tempc2;
break;
case 0x61:
double[] tempc3 = { 0.0008, 0.02 * Math.PI / 180F, 0.0001, 0.00004, 25, 0.00565, 0.0055, 0.000030518 };
read_coefs = tempc3;
additional_mult = -1;
break;
}
double[] quats = new double[4];
DenseMatrix quat_m = new DenseMatrix(1, 4);
Matrix DCM = new DenseMatrix(3, 3);
Matrix sens = new DenseMatrix(1, 3);
Matrix res = new DenseMatrix(1, 3);
double tempr = new double();
double sqr = new double();
for (int i = 0; i < length; i++)
{
progressBar.Value++;
// умножены на -1 для того чтобы оси соответствовали правой тройке
// и осям на датчиках
mw[0] = source[i].w[0] * read_coefs[1]*Wk;// *-1;
mw[1] = source[i].w[1] * read_coefs[1]*Wk;// * -1;
mw[2] = source[i].w[2] * read_coefs[1]*Wk;// * -1;
ma[0] = source[i].a[0] * read_coefs[0]*Ak;// * -1;
ma[1] = source[i].a[1] * read_coefs[0]*Ak;// * -1;
ma[2] = source[i].a[2] * read_coefs[0]*Ak;// * -1;
mm[0] = source[i].m[0] * read_coefs[2]*Mk;// * -1;
mm[1] = source[i].m[1] * read_coefs[2]*Mk;// * -1;
mm[2] = source[i].m[2] * read_coefs[2]*Mk;// * -1;
//mw = single_correction(gyro_c, w[i, 0], w[i, 1], w[i, 2]);
Sensors.a.At(0, 0, ma[0]);
Sensors.a.At(0, 1, ma[1]);
Sensors.a.At(0, 2, ma[2]);
Sensors.w.At(0, 0, mw[0]);
Sensors.w.At(0, 1, mw[1]);
Sensors.w.At(0, 2, mw[2]);
Sensors.m.At(0, 0, mm[0]);
Sensors.m.At(0, 1, mm[1]);
Sensors.m.At(0, 2, mm[2]);
AHRS_result = Kalman_class.AHRS_LKF_EULER(Sensors, State, Parameters);
State = AHRS_result.Item3;
mm = single_correction(magn_c, mm[0], mm[1], mm[2]);
ma = single_correction(accl_c, ma[0], ma[1], ma[2]);
//------------------------------------------------------------------------
//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]);
//----------------------------------------------------------------------
//----------------------------------------------------------------------
angles[0] = (AHRS_result.Item1.At(0));
angles[1] = (AHRS_result.Item1.At(1));
angles[2] = (AHRS_result.Item1.At(2));
//angles[0] = (anglez[i]);
//angles[1] = (angley[i]);
//angles[2] = (anglex[i]);
//sqr = Math.Sqrt(mm[0]*mm[0] + mm[1]*mm[1] + mm[2]*mm[2]);
//mm[0] = mm[0] / sqr;
//mm[1] = mm[1] / sqr;
//mm[2] = mm[2] / sqr;
//sqr = sqr;
//if (source[i].quat[1] != 0)
//{
// quats[0] = source[i].quat[0] * read_coefs[6];
// quats[1] = source[i].quat[1] * read_coefs[7];
// quats[2] = source[i].quat[2] * read_coefs[7];
// quats[3] = source[i].quat[3] * read_coefs[7];
// quat_m.At(0, 0, quats[0]);
// quat_m.At(0, 1, quats[1]);
// quat_m.At(0, 2, quats[2]);
// quat_m.At(0, 3, quats[3]);
// DCM = Kalman_class.quat_to_DCM(quat_m);
// res = Kalman_class.dcm2angle(DCM);
// //angles = quat2angle(quats);
// //DCM = Kalman_class.Matrix_Transpose(quat2dcm(quats));
// angles[0] = res.At(0, 0);
// angles[1] = res.At(0, 1);
// angles[2] = res.At(0, 2);
// //DMC = Kalman_class.Matrix_Transpose(DMC);
// /*sens.At(0, 0, ma[0]);
// sens.At(0, 1, ma[1]);
// sens.At(0, 2, ma[2]);
// res = Kalman_class.Matrix_Mult(sens, DCM);
// ma[0] = res.At(0, 0);
// ma[1] = res.At(0, 1);
// ma[2] = res.At(0, 2);
// sens.At(0, 0, mw[0]);
// sens.At(0, 1, mw[1]);
// sens.At(0, 2, mw[2]);
// res = Kalman_class.Matrix_Mult(sens, DCM);
// mw[0] = res.At(0, 0);
// mw[1] = res.At(0, 1);
// mw[2] = res.At(0, 2);
// sens.At(0, 0, mm[0]);
// sens.At(0, 1, mm[1]);
// sens.At(0, 2, mm[2]);
// res = Kalman_class.Matrix_Mult(sens, DCM);
// mm[0] = res.At(0, 0);
// mm[1] = res.At(0, 1);
// mm[2] = res.At(0, 2);*/
//}
tempr = read_coefs[4] + source[i].temper * read_coefs[5];
// IMU
// QQ
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);
// w
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);
// a
buf16 = (Int16)(ma[0] * 3000);
str_imu.Write(buf16);
buf16 = (Int16)(ma[1] * 3000);
str_imu.Write(buf16);
buf16 = (Int16)(ma[2] * 3000);
str_imu.Write(buf16);
// m
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)(tempr); // t
str_imu.Write(buf16);
//buf32 = (Int32)(counter[i]);
buf32 = (Int32)(source[i].ticks); // pps_imu
str_imu.Write(buf32);
buf8 = (Byte)(0); // check_sum
str_imu.Write(buf8);
}
str_imu.Flush();
str_imu.Close();
}
// Функция для сохранения данных в файлы .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("Сохранение завершено");
}
