Exemplo n.º 1
0
        // Функция для сохранения данных в файлы .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("Сохранение завершено");
        }
Exemplo n.º 2
0
        // Функция для чтения данных напрямую с датчика
        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;
        }