示例#1
0
        private void UpdateGpsBasic(GpsBasic gb)
        {
            _stb_speed.SpeedMS       = gb.Speed_ms;
            _tb_gps_sattellites.Text = gb.NumberOfSatellites.ToString();

            double time = (DateTime.Now - _beginDateTime).TotalSeconds;

            _speedLine.AddPoint(new PointPair(time, gb.Speed_ms * 3.6));
            Scale xScale = _zgc_speed.GraphPane.XAxis.Scale;

            if (time > xScale.Max - xScale.MajorStep)
            {
                xScale.Max = time + xScale.MajorStep;
                xScale.Min = xScale.Max - _timewindow;
            }

            _zgc_speed.AxisChange();
            _zgc_speed.Invalidate(true);
        }
        private void UpdateGpsBasic(GpsBasic gb)
        {
            if (gb.Status == 1)
            {
                _rb_gps_status_active.Checked = true;
            }
            else if (gb.Status == 0)
            {
                _rb_gps_status_void.Checked = true;
            }
            else
            {
                _rb_gps_notfound.Checked = true;
            }

            _tb_gps_numsat.Text    = gb.NumberOfSatellites.ToString();
            _tb_gps_latitude.Text  = gb.Latitude.ToString();
            _tb_gps_longitude.Text = gb.Longitude.ToString();
            _tb_gps_height.Text    = gb.Height_m.ToString();
            _tb_gps_speed.Text     = gb.Speed_ms.ToString();
            _tb_gps_heading.Text   = gb.Heading_deg.ToString();
        }
        private void ReceiveGpsBasic(GpsBasic gb)
        {
            D_UpdateGpsBasic d = new D_UpdateGpsBasic(UpdateGpsBasic);

            this.BeginInvoke(d, new object[] { gb });
        }
示例#4
0
        private void ReceiveGpsBasic(GpsBasic gb)
        {
            string s = "";

            this.BeginInvoke(new D_UpdateGpsBasic(UpdateGpsBasic), new object[] { gb });
        }
        /*!
         *    This function (that executes in a separate thread) is an
         *    infinite loop that receives all lines from the serial port
         *    and parses and executes them.
         */
        private object ReceiveThreadedData(object state)
        {
            //_serialPort.ReadTimeout = 1000;
            bool   recognised_frame = true;
            string line             = string.Empty;

            while (file_to_replay != null && file_to_replay.BaseStream != null && !file_to_replay.EndOfStream)
            {
                try
                {
                    while (!Play)
                    {
                        Thread.Sleep(100);
                    }

                    KeyValuePair <TimeSpan, string> kvp = ReadReplayLine();
                    if (DoubleSpeed)
                    {
                        Thread.Sleep((int)(kvp.Key.TotalMilliseconds / 2.0));
                    }
                    else if (QuadSpeed)
                    {
                        Thread.Sleep((int)(kvp.Key.TotalMilliseconds / 4.0));
                    }
                    else
                    {
                        Thread.Sleep(kvp.Key);
                    }
                    //Console.WriteLine(kvp.Key.ToString() + " - " + kvp.Value);
                    line = kvp.Value;

                    //line = _serialPort.ReadLine();
                    if (line.StartsWith("$")) // line with checksum
                    {
                        string[] frame = line.Substring(1, line.Length - 1).Split('*');
                        //line = frame[0];
                        if (calculateChecksum(frame[0]) == Int32.Parse(frame[1], System.Globalization.NumberStyles.HexNumber))
                        {
                            line = frame[0];
                        }
                        else
                        {
                            throw new Exception("Checksum error");
                        }
                    }

                    if (logfile != null)
                    {
                        logfile.WriteLine("[" + DateTime.Now.ToString("MM/dd/yyyy HH:mm:ss") + "] " + line);
                    }

                    lock (this)
                    {
                        bytes_read += line.Length + 1;
                    }

                    line = line.Replace("\r", "");

                    string[] lines = line.Split(';');
                    //Console.WriteLine(line + "\n\r");
                    // TR: Gyro & Acc raw
                    recognised_frame = true;
                    if (lines[0].EndsWith("TR") && lines.Length >= 6)
                    {
                        double     acc_x_raw = double.Parse(lines[1]);
                        double     acc_y_raw = double.Parse(lines[2]);
                        double     acc_z_raw = double.Parse(lines[3]);
                        double     gyro_x    = double.Parse(lines[4]);
                        double     gyro_y    = double.Parse(lines[5]);
                        double     gyro_z    = double.Parse(lines[6]);
                        GyroAccRaw ga        = new GyroAccRaw(acc_x_raw, acc_y_raw, acc_z_raw, gyro_x, gyro_y, gyro_z);
                        if (GyroAccRawCommunicationReceived != null)
                        {
                            GyroAccRawCommunicationReceived(ga);
                        }
                    }

                    // TP: Processed gyro & acc
                    else if (lines[0].EndsWith("TP") && lines.Length >= 6)
                    {
                        double           acc_x  = double.Parse(lines[1]) / 1000.0;
                        double           acc_y  = double.Parse(lines[2]) / 1000.0;
                        double           acc_z  = double.Parse(lines[3]) / 1000.0;
                        double           gyro_x = double.Parse(lines[4]) / 1000.0 * 180.0 / 3.14;
                        double           gyro_y = double.Parse(lines[5]) / 1000.0 * 180.0 / 3.14;
                        double           gyro_z = double.Parse(lines[6]) / 1000.0 * 180.0 / 3.14;
                        GyroAccProcessed ga     = new GyroAccProcessed(acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z);
                        if (GyroAccProcCommunicationReceived != null)
                        {
                            GyroAccProcCommunicationReceived(ga);
                        }
                    }

                    // TH: Pressure & Temp
                    else if (lines[0].EndsWith("TH") && lines.Length >= 2)
                    {
                        float        pressure = float.Parse(lines[1]);
                        float        temp     = float.Parse(lines[2]);
                        PressureTemp pt       = new PressureTemp(temp, pressure);
                        if (PressureTempCommunicationReceived != null)
                        {
                            PressureTempCommunicationReceived(pt);
                        }
                    }

                    // CA: All configuration
                    else if (lines[0].EndsWith("CA") && lines.Length >= 2)
                    {
                        AllConfig ac = new AllConfig();
                        ac.acc_x_neutral  = int.Parse(lines[1]);
                        ac.acc_y_neutral  = int.Parse(lines[2]);
                        ac.acc_z_neutral  = int.Parse(lines[3]);
                        ac.gyro_x_neutral = int.Parse(lines[4]);
                        ac.gyro_y_neutral = int.Parse(lines[5]);
                        ac.gyro_z_neutral = int.Parse(lines[6]);

                        ac.telemetry_basicgps     = int.Parse(lines[7]);
                        ac.telemetry_ppm          = int.Parse(lines[8]);
                        ac.telemetry_gyroaccraw   = int.Parse(lines[9]);
                        ac.telemetry_gyroaccproc  = int.Parse(lines[10]);
                        ac.telemetry_pressuretemp = int.Parse(lines[11]);
                        ac.telemetry_attitude     = int.Parse(lines[12]);

                        ac.gps_initial_baudrate     = int.Parse(lines[13]) * 10;
                        ac.gps_operational_baudrate = int.Parse(lines[14]) * 10;

                        ac.channel_ap    = int.Parse(lines[15]) + 1;
                        ac.channel_motor = int.Parse(lines[16]) + 1;
                        ac.channel_pitch = int.Parse(lines[17]) + 1;
                        ac.channel_roll  = int.Parse(lines[18]) + 1;
                        ac.channel_yaw   = int.Parse(lines[19]) + 1;

                        ac.pid_pitch2elevator_p    = double.Parse(lines[20], System.Globalization.CultureInfo.InvariantCulture);
                        ac.pid_pitch2elevator_d    = double.Parse(lines[21], System.Globalization.CultureInfo.InvariantCulture);
                        ac.pid_pitch2elevator_i    = double.Parse(lines[22], System.Globalization.CultureInfo.InvariantCulture);
                        ac.pid_pitch2elevator_imin = double.Parse(lines[23], System.Globalization.CultureInfo.InvariantCulture);
                        ac.pid_pitch2elevator_imax = double.Parse(lines[24], System.Globalization.CultureInfo.InvariantCulture);
                        ac.pid_pitch2elevator_dmin = double.Parse(lines[25], System.Globalization.CultureInfo.InvariantCulture);

                        ac.pid_roll2aileron_p    = double.Parse(lines[26], System.Globalization.CultureInfo.InvariantCulture);
                        ac.pid_roll2aileron_d    = double.Parse(lines[27], System.Globalization.CultureInfo.InvariantCulture);
                        ac.pid_roll2aileron_i    = double.Parse(lines[28], System.Globalization.CultureInfo.InvariantCulture);
                        ac.pid_roll2aileron_imin = double.Parse(lines[29], System.Globalization.CultureInfo.InvariantCulture);
                        ac.pid_roll2aileron_imax = double.Parse(lines[30], System.Globalization.CultureInfo.InvariantCulture);
                        ac.pid_roll2aileron_dmin = double.Parse(lines[31], System.Globalization.CultureInfo.InvariantCulture);

                        ac.pid_heading2roll_p    = double.Parse(lines[32], System.Globalization.CultureInfo.InvariantCulture);
                        ac.pid_heading2roll_d    = double.Parse(lines[33], System.Globalization.CultureInfo.InvariantCulture);
                        ac.pid_heading2roll_i    = double.Parse(lines[34], System.Globalization.CultureInfo.InvariantCulture);
                        ac.pid_heading2roll_imin = double.Parse(lines[35], System.Globalization.CultureInfo.InvariantCulture);
                        ac.pid_heading2roll_imax = double.Parse(lines[36], System.Globalization.CultureInfo.InvariantCulture);
                        ac.pid_heading2roll_dmin = double.Parse(lines[37], System.Globalization.CultureInfo.InvariantCulture);

                        ac.pid_altitude2pitch_p    = double.Parse(lines[38], System.Globalization.CultureInfo.InvariantCulture);
                        ac.pid_altitude2pitch_d    = double.Parse(lines[39], System.Globalization.CultureInfo.InvariantCulture);
                        ac.pid_altitude2pitch_i    = double.Parse(lines[40], System.Globalization.CultureInfo.InvariantCulture);
                        ac.pid_altitude2pitch_imin = double.Parse(lines[41], System.Globalization.CultureInfo.InvariantCulture);
                        ac.pid_altitude2pitch_imax = double.Parse(lines[42], System.Globalization.CultureInfo.InvariantCulture);
                        ac.pid_altitude2pitch_dmin = double.Parse(lines[43], System.Globalization.CultureInfo.InvariantCulture);

                        int  r  = int.Parse(lines[44]);
                        byte r2 = (byte)r;
                        ac.servo_reverse[0] = (r & 1) != 0;
                        ac.servo_reverse[1] = (r & 2) != 0;
                        ac.servo_reverse[2] = (r & 4) != 0;
                        ac.servo_reverse[3] = (r & 8) != 0;
                        ac.servo_reverse[4] = (r & 16) != 0;
                        ac.servo_reverse[5] = (r & 32) != 0;

                        for (int i = 0; i < 6; i++)
                        {
                            ac.servo_min[i]     = int.Parse(lines[45 + i * 3], System.Globalization.CultureInfo.InvariantCulture);
                            ac.servo_max[i]     = int.Parse(lines[46 + i * 3], System.Globalization.CultureInfo.InvariantCulture);
                            ac.servo_neutral[i] = int.Parse(lines[47 + i * 3], System.Globalization.CultureInfo.InvariantCulture);
                        }

                        ac.rc_ppm = 1 - int.Parse(lines[63]);

                        ac.control_mixing    = int.Parse(lines[64]);
                        ac.control_max_pitch = int.Parse(lines[65]);
                        ac.control_max_roll  = int.Parse(lines[66]);

                        // for backwards compatibility
                        if (lines.Length > 67)
                        {
                            ac.control_waypoint_radius = int.Parse(lines[67]);
                        }
                        if (lines.Length > 68)
                        {
                            ac.control_cruising_speed = int.Parse(lines[68]);
                        }
                        if (lines.Length > 69)
                        {
                            ac.control_stabilization_with_altitude_hold = int.Parse(lines[69]) == 0 ? false : true;
                        }
                        if (lines.Length > 70)
                        {
                            ac.control_aileron_differential = int.Parse(lines[70]);
                        }
                        if (lines.Length > 71)
                        {
                            ac.telemetry_control = int.Parse(lines[71]);
                        }
                        if (lines.Length > 72)
                        {
                            ac.auto_throttle_enabled    = int.Parse(lines[72]) == 1;
                            ac.auto_throttle_min_pct    = int.Parse(lines[73]);
                            ac.auto_throttle_max_pct    = int.Parse(lines[74]);
                            ac.auto_throttle_cruise_pct = int.Parse(lines[75]);
                            ac.auto_throttle_p_gain_10  = int.Parse(lines[76]);
                        }
                        if (lines.Length > 77)
                        {
                            ac.control_min_pitch = int.Parse(lines[77]);
                        }

                        if (lines.Length > 78)
                        {
                            ac.manual_trim = int.Parse(lines[78]) == 0 ? false : true;
                            Console.WriteLine("receive: " + lines[78]);
                        }
                        if (lines.Length > 79)
                        {
                            ac.control_altitude_mode = int.Parse(lines[79]);
                        }
                        else
                        {
                            Console.WriteLine("FOUT");
                        }

                        if (lines.Length > 80)
                        {
                            ac.gps_enable_waas = int.Parse(lines[80]);
                        }
                        else
                        {
                            Console.WriteLine("FOUT");
                        }

                        if (lines.Length > 81)
                        {
                            ac.osd_bitmask      = int.Parse(lines[81]);
                            ac.osd_RssiMode     = int.Parse(lines[82]);
                            ac.osd_voltage_low  = ((double)int.Parse(lines[83])) / 50.0;
                            ac.osd_voltage_high = ((double)int.Parse(lines[84])) / 50.0;
                        }

                        if (lines.Length > 84)
                        {
                            ac.imu_rotated   = int.Parse(lines[85]);
                            ac.neutral_pitch = int.Parse(lines[86]);
                        }

                        if (AllConfigCommunicationReceived != null)
                        {
                            AllConfigCommunicationReceived(ac);
                        }
                    }

                    // TT: RC transmitter
                    else if (lines[0].EndsWith("TT") && lines.Length >= 7)
                    {
                        RcInput rc = new RcInput(
                            new int[] {
                            int.Parse(lines[1]),
                            int.Parse(lines[2]),
                            int.Parse(lines[3]),
                            int.Parse(lines[4]),
                            int.Parse(lines[5]),
                            int.Parse(lines[6]),
                            int.Parse(lines[7])
                        });
                        if (RcInputCommunicationReceived != null)
                        {
                            RcInputCommunicationReceived(rc);
                        }
                    }
                    // TG: GPS basic
                    else if (lines[0].EndsWith("TG") && lines.Length >= 7)
                    {
                        //Console.WriteLine(line);
                        GpsBasic gb = new GpsBasic(
                            double.Parse(lines[2], System.Globalization.CultureInfo.InvariantCulture),
                            double.Parse(lines[3], System.Globalization.CultureInfo.InvariantCulture),
                            int.Parse(lines[7]),
                            double.Parse(lines[5]) / 100,
                            double.Parse(lines[4]) / 10,
                            int.Parse(lines[6]),
                            int.Parse(lines[1])
                            );
                        if (GpsBasicCommunicationReceived != null)
                        {
                            GpsBasicCommunicationReceived(gb);
                        }
                    }
                    // TA: Attitude
                    else if (lines[0].EndsWith("TA") && lines.Length >= 3)
                    {
                        Attitude att = new Attitude(
                            double.Parse(lines[1], CultureInfo.InvariantCulture) / 1000.0 / 3.14 * 180.0,
                            double.Parse(lines[2], CultureInfo.InvariantCulture) / 1000.0 / 3.14 * 180.0,

                            /*double.Parse(lines[3], CultureInfo.InvariantCulture) / 1000.0 / 3.14 * 180.0,
                            *  double.Parse(lines[4], CultureInfo.InvariantCulture) / 1000.0 / 3.14 * 180.0,*/0, 0,
                            double.Parse(lines[3], CultureInfo.InvariantCulture) / 1000.0 / 3.14 * 180.0
                            );
                        if (AttitudeCommunicationReceived != null)
                        {
                            AttitudeCommunicationReceived(att);
                        }
                    }
                    // DT: Datalog table
                    else if (lines[0].EndsWith("DT") && lines.Length >= 4)
                    {
                        DatalogTable dt = new DatalogTable(
                            int.Parse(lines[1]),
                            int.Parse(lines[3]),
                            int.Parse(lines[4]),
                            int.Parse(lines[2]),
                            0);
                        if (DatalogTableCommunicationReceived != null)
                        {
                            DatalogTableCommunicationReceived(dt);
                        }
                    }
                    // DH: Datalog header
                    else if (lines[0].EndsWith("DH") && lines.Length >= 4)
                    {
                        DatalogHeader = new string[lines.Length - 1];

                        for (int i = 1; i < lines.Length; i++)
                        {
                            DatalogHeader[i - 1] = lines[i];
                        }
                    }
                    // DD: Datalog data
                    else if (lines[0].EndsWith("DD") && lines.Length >= 4)
                    {
                        string[] logline = new string[DatalogHeader.Length];
                        for (int i = 0; i < logline.Length; i++)
                        {
                            logline[i] = lines[i + 1];
                        }
                        DatalogLine dl = new DatalogLine(
                            logline, DatalogHeader);
                        if (DatalogLineCommunicationReceived != null)
                        {
                            DatalogLineCommunicationReceived(dl);
                        }
                    }
                    // ND: Navigation data (Navigation instruction)
                    else if (lines[0].EndsWith("ND") && lines.Length >= 6)
                    {
                        Console.WriteLine(line);
                        lines[1] = lines[1].Replace("nan", "0");
                        lines[2] = lines[2].Replace("nan", "0");
                        lines[3] = lines[3].Replace("nan", "0");
                        lines[4] = lines[4].Replace("nan", "0");
                        lines[5] = lines[5].Replace("nan", "0");
                        lines[6] = lines[6].Replace("nan", "0");

                        NavigationInstruction ni =
                            new NavigationInstruction(
                                int.Parse(lines[1]),
                                (NavigationInstruction.navigation_command) int.Parse(lines[2]),
                                double.Parse(lines[3], CultureInfo.InvariantCulture),
                                double.Parse(lines[4], CultureInfo.InvariantCulture),
                                int.Parse(lines[5]),
                                int.Parse(lines[6]));
                        if (NavigationInstructionCommunicationReceived != null)
                        {
                            NavigationInstructionCommunicationReceived(ni);
                        }
                    }
                    // TS: Servos (simulation)
                    else if (lines[0].EndsWith("TS") && lines.Length >= 3)
                    {
                        Console.WriteLine(line);

                        Servos s =
                            new Servos(
                                int.Parse(lines[1]),
                                int.Parse(lines[2]),
                                int.Parse(lines[3]));
                        if (ServosCommunicationReceived != null)
                        {
                            ServosCommunicationReceived(s);
                        }
                    }
                    // Control
                    else if (lines[0].EndsWith("TC") && lines.Length >= 3)
                    {
                        ControlInfo ci =
                            new ControlInfo();
                        ci.FlightMode            = (ControlInfo.FlightModes) int.Parse(lines[1]);
                        ci.CurrentNavigationLine = int.Parse(lines[2]);
                        ci.Altitude = int.Parse(lines[3]);
                        if (lines.Length >= 5)
                        {
                            ci.Batt1Voltage = double.Parse(lines[4]) / 10.0;
                            if (lines.Length >= 6)
                            {
                                ci.FlightTime = int.Parse(lines[5]);
                                ci.BlockTime  = int.Parse(lines[6]);
                                ci.RcLink     = int.Parse(lines[7]);
                                ci.Throttle   = int.Parse(lines[8]);
                            }
                            if (lines.Length >= 10)
                            {
                                ci.TargetAltitude = int.Parse(lines[9]);
                            }
                            if (lines.Length >= 11)
                            {
                                ci.Batt2Voltage = double.Parse(lines[10]) / 10.0;
                                ci.Batt_mAh     = double.Parse(lines[11]) * 10.0;
                            }
                        }
                        if (ControlInfoCommunicationReceived != null)
                        {
                            ControlInfoCommunicationReceived(ci);
                        }
                    }
                    else
                    {
                        recognised_frame = false;
                        Console.WriteLine(line);
                        if (NonParsedCommunicationReceived != null)
                        {
                            NonParsedCommunicationReceived(line);
                        }
                    }
                }
                catch (TimeoutException toe)
                {
                    if (CommunicationAlive)
                    {
                        if (CommunicationLost != null && SecondsConnectionLost() >= 5.0)
                        {
                            CommunicationLost();
                            CommunicationAlive = false;
                        }
                    }
                }
                catch (IOException ioe)
                {
                    // happens when thread is shut down
                }
                catch (Exception e)
                {
                    ;
                }

                try
                {
                    if (recognised_frame)
                    {
                        //Console.WriteLine(line);
                        LastValidFrame = DateTime.Now;
                        FramesReceived++;
                        if (!CommunicationAlive)
                        {
                            CommunicationAlive = true;
                            if (CommunicationEstablished != null)
                            {
                                CommunicationEstablished();
                            }
                        }
                    }
                    if (CommunicationReceived != null)
                    {
                        CommunicationReceived(line);
                    }
                }
                catch (Exception e)
                {
                    ;
                }
            }
            CommunicationAlive = false;
            CommunicationLost();

            return(null);
        }
示例#6
0
 void serial_GpsBasicCommunicationReceived(GpsBasic gpsbasic)
 {
     this.BeginInvoke(new D_UpdateGpsBasic(UpdateGpsBasic), new object[] { gpsbasic });
 }