Пример #1
0
 private void AllConfig(AllConfig ac)
 {
     _model = new ConfigurationModel();
     _model.SetFromAllConfig(ac);
     ReadModel();
 }
        /*!
         *    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;
        }
Пример #3
0
 private void ReceiveAllConfig(AllConfig ac)
 {
     Console.WriteLine("GUI received");
     this.BeginInvoke(new D_ReceiveAllConfig(AllConfig), new object[] { ac });
 }
        /*!
         *    Sends the complete configuration set AllConfig to the gluonpilot.
         */
        public override void Send(AllConfig ac)
        {
            SendTelemetry(
                ac.telemetry_basicgps,
                ac.telemetry_gyroaccraw,
                ac.telemetry_gyroaccproc,
                ac.telemetry_ppm,
                ac.telemetry_pressuretemp,
                ac.telemetry_attitude,
                ac.telemetry_control);

            Thread.Sleep(200);

            // neutral acc
            WriteChecksumLine("SA;" +
                ac.acc_x_neutral.ToString() + ";" +
                ac.acc_y_neutral.ToString() + ";" +
                ac.acc_z_neutral.ToString() + "");
            /*Console.WriteLine("\nSA;" +
                ac.acc_x_neutral.ToString() + ";" +
                ac.acc_y_neutral.ToString() + ";" +
                ac.acc_z_neutral.ToString() + "\n");*/
            Thread.Sleep(200);

            // neutral gyro
            WriteChecksumLine("SY;" +
                ac.gyro_x_neutral.ToString() + ";" +
                ac.gyro_y_neutral.ToString() + ";" +
                ac.gyro_z_neutral.ToString() + "");
            /*Console.WriteLine("\nSY;" +
                ac.gyro_x_neutral.ToString() + ";" +
                ac.gyro_y_neutral.ToString() + ";" +
                ac.gyro_z_neutral.ToString() + "\n");*/
            Thread.Sleep(200);

            // channel config
            SendConfigChannels(ac.rc_ppm, ac.channel_ap, ac.channel_motor, ac.channel_pitch, ac.channel_roll, ac.channel_yaw);

            Thread.Sleep(200);
            // gps config
            WriteChecksumLine("SG;" + (ac.gps_initial_baudrate / 10).ToString() + ";" + ac.gps_enable_waas);
            //Console.WriteLine("\nSG;" + (ac.gps_initial_baudrate / 10).ToString() + "\n");

            Thread.Sleep(200);

            SendPidPitch2Elevator(ac.pid_pitch2elevator_p, ac.pid_pitch2elevator_i, ac.pid_pitch2elevator_d,
                                ac.pid_pitch2elevator_imin, ac.pid_pitch2elevator_imax, ac.pid_pitch2elevator_dmin);

            Thread.Sleep(200);

            SendPidRoll2Aileron(ac.pid_roll2aileron_p, ac.pid_roll2aileron_i, ac.pid_roll2aileron_d,
                                ac.pid_roll2aileron_imin, ac.pid_roll2aileron_imax, ac.pid_roll2aileron_dmin);

            Thread.Sleep(200);

            WriteChecksumLine("PH;" +
                ac.pid_heading2roll_p.ToString(System.Globalization.CultureInfo.InvariantCulture) + ";" +
                ac.pid_heading2roll_i.ToString(System.Globalization.CultureInfo.InvariantCulture) + ";" +
                ac.pid_heading2roll_d.ToString(System.Globalization.CultureInfo.InvariantCulture) + ";" +
                ac.pid_heading2roll_imin.ToString(System.Globalization.CultureInfo.InvariantCulture) + ";" +
                ac.pid_heading2roll_imax.ToString(System.Globalization.CultureInfo.InvariantCulture) + ";" +
                ac.pid_heading2roll_dmin.ToString(System.Globalization.CultureInfo.InvariantCulture) + "");
            /*Console.WriteLine("\nPH;" +
                ac.pid_heading2roll_p.ToString(System.Globalization.CultureInfo.InvariantCulture) + ";" +
                ac.pid_heading2roll_i.ToString(System.Globalization.CultureInfo.InvariantCulture) + ";" +
                ac.pid_heading2roll_d.ToString(System.Globalization.CultureInfo.InvariantCulture) + ";" +
                ac.pid_heading2roll_imin.ToString(System.Globalization.CultureInfo.InvariantCulture) + ";" +
                ac.pid_heading2roll_imax.ToString(System.Globalization.CultureInfo.InvariantCulture) + ";" +
                ac.pid_heading2roll_dmin.ToString(System.Globalization.CultureInfo.InvariantCulture) + "\n");
            */
            Thread.Sleep(200);

            WriteChecksumLine("PA;" +
                ac.pid_altitude2pitch_p.ToString(System.Globalization.CultureInfo.InvariantCulture) + ";" +
                ac.pid_altitude2pitch_i.ToString(System.Globalization.CultureInfo.InvariantCulture) + ";" +
                ac.pid_altitude2pitch_d.ToString(System.Globalization.CultureInfo.InvariantCulture) + ";" +
                ac.pid_altitude2pitch_imin.ToString(System.Globalization.CultureInfo.InvariantCulture) + ";" +
                ac.pid_altitude2pitch_imax.ToString(System.Globalization.CultureInfo.InvariantCulture) + ";" +
                ac.pid_altitude2pitch_dmin.ToString(System.Globalization.CultureInfo.InvariantCulture) + "");
            /*Console.WriteLine("\nPA;" +
                ac.pid_altitude2pitch_p.ToString(System.Globalization.CultureInfo.InvariantCulture) + ";" +
                ac.pid_altitude2pitch_i.ToString(System.Globalization.CultureInfo.InvariantCulture) + ";" +
                ac.pid_altitude2pitch_d.ToString(System.Globalization.CultureInfo.InvariantCulture) + ";" +
                ac.pid_altitude2pitch_imin.ToString(System.Globalization.CultureInfo.InvariantCulture) + ";" +
                ac.pid_altitude2pitch_imax.ToString(System.Globalization.CultureInfo.InvariantCulture) + ";" +
                ac.pid_altitude2pitch_dmin.ToString(System.Globalization.CultureInfo.InvariantCulture) + "\n");
            */
            Thread.Sleep(200);

            SendServoReverse(ac.servo_reverse[0], ac.servo_reverse[1], ac.servo_reverse[2], ac.servo_reverse[3], ac.servo_reverse[4], ac.servo_reverse[5], ac.manual_trim);
            Thread.Sleep(200);

            if (ac.manual_trim)
            {
                for (int i = 0; i < 6; i++)
                {
                    SendServoMinNeutralMax(i, ac.servo_min[i], ac.servo_neutral[i], ac.servo_max[i]);
                    Thread.Sleep(200);
                }
            }

            SendControlSettings(ac.control_mixing,
                                ac.control_max_pitch,
                                ac.control_min_pitch,
                                ac.control_max_roll,
                                ac.control_aileron_differential,
                                ac.control_waypoint_radius,
                                ac.control_cruising_speed,
                                ac.control_stabilization_with_altitude_hold,
                                ac.control_altitude_mode);

            Thread.Sleep(200);

            SendAutoThrottleConfig(ac.auto_throttle_min_pct, ac.auto_throttle_max_pct, ac.auto_throttle_cruise_pct, ac.auto_throttle_p_gain_10, ac.auto_throttle_enabled);
        }
 /*!
  *    Sends the complete configuration set AllConfig to the gluonpilot.
  */
 public override void Send(AllConfig ac)
 {
 }
Пример #6
0
 private void AllConfig(AllConfig ac)
 {
     _model.SetFromAllConfig(ac);
     ReadModel();
 }
Пример #7
0
 private void ReceiveAllConfig(AllConfig ac)
 {
     this.BeginInvoke(new D_ReceiveAllConfig(AllConfig), new object[] { ac });
 }
Пример #8
0
        /*!
         *    Converts to _model to AllConfig communication frame.
         */
        public AllConfig ToAllConfig()
        {
            Console.WriteLine("To all config");
            AllConfig ac = new AllConfig();
            ConfigurationModel _model = this;
            ac.acc_x_neutral = _model.NeutralAccX;
            ac.acc_y_neutral = _model.NeutralAccY;
            ac.acc_z_neutral = _model.NeutralAccZ;
            ac.gyro_x_neutral = _model.NeutralGyroX;
            ac.gyro_y_neutral = _model.NeutralGyroY;
            ac.gyro_z_neutral = _model.NeutralGyroZ;

            ac.imu_rotated = _model.ImuRotated;
            ac.neutral_pitch = _model.NeutralPitch;

            ac.channel_pitch = _model.ChannelPitch;
            ac.channel_roll = _model.ChannelRoll;
            ac.channel_motor = _model.ChannelMotor;
            ac.channel_yaw = _model.ChannelYaw;
            ac.channel_ap = _model.ChannelAp;
            ac.rc_ppm = _model.RcTransmitterFromPpm;

            ac.control_max_pitch = _model.ControlMaxPitch;
            ac.control_min_pitch = _model.ControlMinPitch;
            ac.control_max_roll = _model.ControlMaxRoll;
            ac.control_mixing = _model.ControlMixing;
            ac.control_waypoint_radius = _model.WaypointRadius;
            ac.control_stabilization_with_altitude_hold = _model.StabilizationWithAltitudeHold;
            ac.control_cruising_speed = _model.CruisingSpeed;
            ac.control_aileron_differential = _model.ControlAileronDiff;
            ac.control_altitude_mode = _model.AltitudeMode;

            ac.telemetry_basicgps = _model.TelemetryGpsBasic;
            ac.telemetry_gyroaccraw = _model.TelemetryGyroAccRaw;
            ac.telemetry_gyroaccproc = _model.TelemetryGyroAccProc;
            ac.telemetry_ppm = _model.TelemetryPpm;
            ac.telemetry_pressuretemp = _model.TelemetryPressureTemp;
            ac.telemetry_attitude = _model.TelemetryAttitude;
            ac.telemetry_control = _model.TelemetryControl;

            ac.gps_initial_baudrate = _model.GpsInitialBaudrate;
            ac.gps_enable_waas = _model.GpsEnableWaas;

            ac.pid_pitch2elevator_p = _model.Pitch2ElevatorPidModel.P;
            ac.pid_pitch2elevator_i = _model.Pitch2ElevatorPidModel.I;
            ac.pid_pitch2elevator_d = _model.Pitch2ElevatorPidModel.D;
            ac.pid_pitch2elevator_imin = _model.Pitch2ElevatorPidModel.IMin;
            ac.pid_pitch2elevator_imax = _model.Pitch2ElevatorPidModel.IMax;
            ac.pid_pitch2elevator_dmin = _model.Pitch2ElevatorPidModel.DMin;

            ac.pid_roll2aileron_p = _model.Roll2AileronPidModel.P;
            ac.pid_roll2aileron_i = _model.Roll2AileronPidModel.I;
            ac.pid_roll2aileron_d = _model.Roll2AileronPidModel.D;
            ac.pid_roll2aileron_imin = _model.Roll2AileronPidModel.IMin;
            ac.pid_roll2aileron_imax = _model.Roll2AileronPidModel.IMax;
            ac.pid_roll2aileron_dmin = _model.Roll2AileronPidModel.DMin;

            ac.pid_heading2roll_p = _model.Heading2RollPidModel.P;
            ac.pid_heading2roll_i = _model.Heading2RollPidModel.I;
            ac.pid_heading2roll_d = _model.Heading2RollPidModel.D;
            ac.pid_heading2roll_imin = _model.Heading2RollPidModel.IMin;
            ac.pid_heading2roll_imax = _model.Heading2RollPidModel.IMax;
            ac.pid_heading2roll_dmin = _model.Heading2RollPidModel.DMin;

            ac.pid_altitude2pitch_p = _model.Altitude2PitchPidModel.P;
            ac.pid_altitude2pitch_i = _model.Altitude2PitchPidModel.I;
            ac.pid_altitude2pitch_d = _model.Altitude2PitchPidModel.D;
            ac.pid_altitude2pitch_imin = _model.Altitude2PitchPidModel.IMin;
            ac.pid_altitude2pitch_imax = _model.Altitude2PitchPidModel.IMax;
            ac.pid_altitude2pitch_dmin = _model.Altitude2PitchPidModel.DMin;


            ac.servo_reverse[0] = _model.ReverseServo1;
            ac.servo_reverse[1] = _model.ReverseServo2;
            ac.servo_reverse[2] = _model.ReverseServo3;
            ac.servo_reverse[3] = _model.ReverseServo4;
            ac.servo_reverse[4] = _model.ReverseServo5;
            ac.servo_reverse[5] = _model.ReverseServo6;
            ac.manual_trim = _model.ManualTrim;
            ac.servo_min[0] = _model.ServoMin[0];
            ac.servo_min[1] = _model.ServoMin[1];
            ac.servo_min[2] = _model.ServoMin[2];
            ac.servo_min[3] = _model.ServoMin[3];
            ac.servo_min[4] = _model.ServoMin[4];
            ac.servo_min[5] = _model.ServoMin[5];
            ac.servo_max[0] = _model.ServoMax[0];
            ac.servo_max[1] = _model.ServoMax[1];
            ac.servo_max[2] = _model.ServoMax[2];
            ac.servo_max[3] = _model.ServoMax[3];
            ac.servo_max[4] = _model.ServoMax[4];
            ac.servo_max[5] = _model.ServoMax[5];
            ac.servo_neutral[0] = _model.ServoNeutral[0];
            ac.servo_neutral[1] = _model.ServoNeutral[1];
            ac.servo_neutral[2] = _model.ServoNeutral[2];
            ac.servo_neutral[3] = _model.ServoNeutral[3];
            ac.servo_neutral[4] = _model.ServoNeutral[4];
            ac.servo_neutral[5] = _model.ServoNeutral[5];
            ac.auto_throttle_enabled = _model.AutoThrottleEnabled;
            ac.auto_throttle_min_pct = _model.AutoThrottleMinPct;
            ac.auto_throttle_max_pct = _model.AutoThrottleMaxPct;
            ac.auto_throttle_cruise_pct = _model.AutoThrottleCruisePct;
            ac.auto_throttle_p_gain_10 = (int)(_model.AutoThrottlePGain * 10);

            ac.osd_bitmask = _model.OsdBitmask;

            return ac;
        }
Пример #9
0
        /*!
         *    Updates it's state according to allconfig
         */
        public void SetFromAllConfig(AllConfig ac)
        {
            ConfigurationModel _model = this;
            _model.NeutralAccX = ac.acc_x_neutral;
            _model.NeutralAccY = ac.acc_y_neutral;
            _model.NeutralAccZ = ac.acc_z_neutral;
            _model.NeutralGyroX = ac.gyro_x_neutral;
            _model.NeutralGyroY = ac.gyro_y_neutral;
            _model.NeutralGyroZ = ac.gyro_z_neutral;

            _model.NeutralPitch = ac.neutral_pitch;
            _model.ImuRotated = ac.imu_rotated;

            _model.TelemetryGpsBasic = ac.telemetry_basicgps;
            _model.TelemetryGyroAccRaw = ac.telemetry_gyroaccraw;
            _model.TelemetryGyroAccProc = ac.telemetry_gyroaccproc;
            _model.TelemetryPpm = ac.telemetry_ppm;
            _model.TelemetryPressureTemp = ac.telemetry_pressuretemp;
            _model.TelemetryAttitude = ac.telemetry_attitude;
            _model.TelemetryControl = ac.telemetry_control;

            _model.ChannelAp = ac.channel_ap;
            _model.ChannelPitch = ac.channel_pitch;
            _model.ChannelRoll = ac.channel_roll;
            _model.ChannelYaw = ac.channel_yaw;
            _model.ChannelMotor = ac.channel_motor;
            _model.RcTransmitterFromPpm = ac.rc_ppm;

            _model.ControlMixing = ac.control_mixing;
            _model.ControlMaxPitch = ac.control_max_pitch;
            _model.ControlMinPitch = ac.control_min_pitch;
            _model.ControlMaxRoll = ac.control_max_roll;
            _model.ControlAileronDiff = ac.control_aileron_differential;
            _model.AltitudeMode = ac.control_altitude_mode;

            _model.CruisingSpeed = ac.control_cruising_speed;
            _model.StabilizationWithAltitudeHold = ac.control_stabilization_with_altitude_hold;
            _model.WaypointRadius = ac.control_waypoint_radius;

            _model.GpsInitialBaudrate = ac.gps_initial_baudrate;
            _model.GpsOperationalBaudrate = ac.gps_operational_baudrate;
            _model.GpsEnableWaas = ac.gps_enable_waas;

            _model.Pitch2ElevatorPidModel = new PidModel(ac.pid_pitch2elevator_p,
                                                         ac.pid_pitch2elevator_i,
                                                         ac.pid_pitch2elevator_d,
                                                         ac.pid_pitch2elevator_imin,
                                                         ac.pid_pitch2elevator_imax,
                                                         ac.pid_pitch2elevator_dmin);
            _model.Roll2AileronPidModel = new PidModel(ac.pid_roll2aileron_p,
                                                         ac.pid_roll2aileron_i,
                                                         ac.pid_roll2aileron_d,
                                                         ac.pid_roll2aileron_imin,
                                                         ac.pid_roll2aileron_imax,
                                                         ac.pid_roll2aileron_dmin);
            _model.Heading2RollPidModel = new PidModel(ac.pid_heading2roll_p,
                                                         ac.pid_heading2roll_i,
                                                         ac.pid_heading2roll_d,
                                                         ac.pid_heading2roll_imin,
                                                         ac.pid_heading2roll_imax,
                                                         ac.pid_heading2roll_dmin);
            _model.Altitude2PitchPidModel = new PidModel(ac.pid_altitude2pitch_p,
                                                         ac.pid_altitude2pitch_i,
                                                         ac.pid_altitude2pitch_d,
                                                         ac.pid_altitude2pitch_imin,
                                                         ac.pid_altitude2pitch_imax,
                                                         ac.pid_altitude2pitch_dmin);
            _model.ReverseServo1 = ac.servo_reverse[0];
            _model.ReverseServo2 = ac.servo_reverse[1];
            _model.ReverseServo3 = ac.servo_reverse[2];
            _model.ReverseServo4 = ac.servo_reverse[3];
            _model.ReverseServo5 = ac.servo_reverse[4];
            _model.ReverseServo6 = ac.servo_reverse[5];

            // Due to an annoying bug
            _model.ServoMin = new int[6];
            _model.ServoMax = new int[6];
            _model.ServoNeutral = new int[6];

            for (int i = 0; i < 6; i++)
            {
                _model.ServoMin[i] = ac.servo_min[i];
                _model.ServoMax[i] = ac.servo_max[i];
                _model.ServoNeutral[i] = ac.servo_neutral[i];
            }
            _model.ManualTrim = ac.manual_trim;

            _model.AutoThrottleEnabled = ac.auto_throttle_enabled;
            _model.AutoThrottleMinPct = ac.auto_throttle_min_pct;
            _model.AutoThrottleMaxPct = ac.auto_throttle_max_pct;
            _model.AutoThrottleCruisePct = ac.auto_throttle_cruise_pct;
            _model.AutoThrottlePGain = (double)ac.auto_throttle_p_gain_10 / 10;

            _model.OsdBitmask = ac.osd_bitmask;
        }
Пример #10
0
 public abstract void Send(AllConfig ac);