Exemple #1
0
        private void OnPacketRecieved(object sender, MavlinkPacket e)
        {
            uint           x = Mv.PacketsReceived;
            MavlinkMessage m = e.Message;

            if (m.GetType() == Hb.GetType())
            {
                Hb = (Msg_heartbeat)e.Message;
            }

            if (m.GetType() == Ss.GetType())
            {
                Ss = (Msg_sys_status)e.Message;
            }

            if (m.GetType() == Ps.GetType())
            {
                Ps = (Msg_power_status)e.Message;
            }

            if (m.GetType() == At.GetType())
            {
                At = (Msg_attitude)e.Message;
            }

            if (m.GetType() == Gps.GetType())
            {
                Gps = (Msg_gps_raw_int)e.Message;
            }

            if (m.GetType() == Vfr.GetType())
            {
                Vfr = (Msg_vfr_hud)e.Message;
            }

            if (m.GetType() == Rp.GetType())
            {
                Rp = (Msg_raw_pressure)e.Message;
            }

            if (m.GetType() == Sp.GetType())
            {
                Sp = (Msg_scaled_pressure)e.Message;
                PressureAbsolute   = (int)(Sp.press_abs * 1000f);
                Temperature        = Sp.temperature;
                PressureDifference = (int)(Sp.press_diff * 1000f);
                Total++;

                if (OnAtmosphericData != null)
                {
                    OnAtmosphericData.Invoke(PressureAbsolute, PressureDifference, Temperature, Total);
                }
            }

            if (x > 0 && OnPacketCountGreaterThanZero != null)
            {
                OnPacketCountGreaterThanZero.Invoke();
            }
        }
Exemple #2
0
        void Mv_PacketReceived(object sender, MavLink.MavlinkPacket e)
        {
            uint           x = Mv.PacketsReceived;
            MavlinkMessage m = e.Message;

            if (m.GetType() == Hb.GetType())
            {
                Hb = (Msg_heartbeat)e.Message;
            }
            if (m.GetType() == Ss.GetType())
            {
                Ss = (Msg_sys_status)e.Message;
            }
            if (m.GetType() == Ps.GetType())
            {
                Ps = (Msg_power_status)e.Message;
            }
            if (m.GetType() == At.GetType())
            {
                At = (Msg_attitude)e.Message;
            }
            if (m.GetType() == Gps.GetType())
            {
                Gps = (Msg_gps_raw_int)e.Message;
            }
            if (m.GetType() == Vfr.GetType())
            {
                Vfr = (Msg_vfr_hud)e.Message;
            }
            if (m.GetType() == Rp.GetType())
            {
                Rp = (Msg_raw_pressure)e.Message;
            }
            if (m.GetType() == Sp.GetType())
            {
                Sp  = (Msg_scaled_pressure)e.Message;
                Prs = (int)(Sp.press_abs * 1000f);
                Tep = Sp.temperature;
                Dif = (int)(Sp.press_diff * 1000f);

                /*
                 * if (Sw == null)
                 * {
                 * Sw = new StreamWriter("c:\\Baro.csv");
                 * Sw.Write("\"Item\", \"Pressure\", \"Temp\", \"Difference\"\r\n");
                 * }
                 * String S = String.Format("{0}, {1}, {2}, {3}\r\n", Total++, Prs, Tep, Dif);
                 * Sw.Write(S);*/
            }
            if (x > 0)
            {
                if (MavLinkReader.ActiveForm != null)
                {
                    MavLinkReader.ActiveForm.Invalidate();
                }
            }
        }
        public void SendTest()
        {
            Mavlink      mavlink      = new Mavlink();
            Msg_attitude msg_Attitude = new Msg_attitude();

            msg_Attitude.roll = 1;
            MavlinkPacket packet = new MavlinkPacket(msg_Attitude);
            var           res    = mavlink.Send(packet);

            mavlink.PacketReceived += Mavlink_PacketReceived;
            mavlink.ParseBytes(res);
            Assert.AreNotEqual(mavlink.PacketsReceived, 0);
        }
Exemple #4
0
    // Use this for initialization
    void Start()
    {
        SystemId    = (byte)MAV_MODE.MAV_MODE_MANUAL_ARMED;
        ComponentId = (byte)MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1;
        Application.runInBackground = true;
        mlpn            = new Msg_local_position_ned();
        mgpi            = new Msg_global_position_int();
        Mheratbeat      = new Msg_heartbeat();
        Msts            = new Msg_sys_status();
        Matt            = new Msg_attitude();
        Mdatastream     = new Msg_request_data_stream();
        Mgpsr           = new Msg_gps_raw_int();
        Mattq           = new Msg_attitude_quaternion();
        Mvhud           = new Msg_vfr_hud();
        MattTar         = new Msg_attitude_target();
        MHome           = new Msg_home_position();
        setMHome        = new Msg_set_home_position();
        setMode         = new Msg_set_mode();
        EstimatorStatus = new Msg_estimator_status();
        Mvirbration     = new Msg_vibration();
        ExtendState     = new Msg_extended_sys_state();

        latlonalt    = new LatLonAlt();
        Malt         = new Msg_altitude();
        positionL    = new Msg_position_target_local_ned();
        setPositionL = new Msg_set_position_target_local_ned();
        setPositionG = new Msg_set_position_target_global_int();
        PositionG    = new Msg_position_target_global_int();

        MhiIMU        = new Msg_highres_imu();
        BatteryStatus = new Msg_battery_status();


        player = GameObject.FindGameObjectWithTag("Player");
        DM     = player.GetComponent <DroneMove> ();
        InitSocket();

        setHomePosition();
        setPositionTargetLocal();
        setPositionTargetGlobal();
    }
        public void recvMavMsg(object sender, MavlinkPacket e)
        {
            Console.WriteLine("recvMavMsg: " + Thread.CurrentThread.ManagedThreadId.ToString());

            string str = e.Message.ToString();

            if (str == "MavLink.Msg_heartbeat")
            {
                /* convert */
                Msg_heartbeat ht = (Msg_heartbeat)e.Message;

                /* save the incoming msg */
                mode = ht.custom_mode;

                /* publish */
                commDataEvent(mode);
            }
            else if (e.Message.ToString() == "Mavlink.Msg_attitude")
            {
                Msg_attitude at = (Msg_attitude)e.Message;
            }
        }
Exemple #6
0
        void Mv_PacketReceived(object sender, MavLink.MavlinkPacket e)
        {
            uint x = Mv.PacketsReceived;

            Systemid    = e.SystemId;
            Componentid = e.ComponentId;
            Sequence    = e.SequenceNumber;
            MavlinkMessage m = e.Message;

            if (m.GetType() == Hb.GetType())
            {
                Hb = (Msg_heartbeat)e.Message;
            }
            if (m.GetType() == Ss.GetType())
            {
                Ss = (Msg_sys_status)e.Message;
            }
            if (m.GetType() == Ps.GetType())
            {
                Ps = (Msg_power_status)e.Message;
            }
            if (m.GetType() == At.GetType())
            {
                At = (Msg_attitude)e.Message;
            }
            if (m.GetType() == Gps.GetType())
            {
                Gps = (Msg_gps_raw_int)e.Message;
            }
            if (m.GetType() == Vfr.GetType())
            {
                Vfr = (Msg_vfr_hud)e.Message;
            }
            if (m.GetType() == Rp.GetType())
            {
                Rp = (Msg_raw_pressure)e.Message;
            }
            if (m.GetType() == Sp.GetType())
            {
                Sp  = (Msg_scaled_pressure)e.Message;
                Prs = (int)(Sp.press_abs * 1000f);
                Tep = Sp.temperature;
                Dif = (int)(Sp.press_diff * 1000f);

                /*
                 * Special log of Barometric data to the D drive
                 * Can be disable if not needed
                 */
                ut.LogPressure(Sp);
            }
            if (m.GetType() == Ca.GetType())
            {
                Ca = (Msg_command_ack)e.Message;
            }
            if (m.GetType() == St.GetType())
            {
                St = (Msg_statustext)e.Message;
            }
            if (m.GetType() == Mc.GetType())
            {
                Mc = (Msg_mission_count)e.Message;
                MI = 0;
                GetMissionValues.RunWorkerAsync();
            }
            if (m.GetType() == Mi[0].GetType())
            {
                Mi[MI++] = (Msg_mission_item)e.Message;
                if (MI >= Mc.count)
                {
                    MI = -1;
                }
            }
            if (x > 0)
            {
                if (MavLinkReader.ActiveForm != null)
                {
                    MavLinkReader.ActiveForm.Invalidate();
                }
            }
        }
 private void Process(Msg_attitude msg)
 {
     Roll    = msg.roll * 180.0 / Math.PI;
     Pitch   = msg.pitch * 180.0 / Math.PI;
     Heading = msg.yaw * 180.0 / Math.PI;
 }
Exemple #8
0
        private void ProcessMessage(MavlinkMessage msg)
        {
            if (msg == null)
            {
                return;
            }

            Type t = msg.GetType();

            if (currentStep != StateMachineStep.Connected && t != typeof(Msg_configuration_version) && t != typeof(Msg_configuration_version2))
            {
                return;
            }

            if (t == typeof(Msg_configuration_control))
            {
                Msg_configuration_control msg_control = (msg as Msg_configuration_control);
                switch ((CONFIG_COMMAND)msg_control.command)
                {
                case CONFIG_COMMAND.CONFIG_COMMAND_ACK:

                    if (msg_control.param1 == (byte)CONFIG_PORT.CONFIG_PORT_1)
                    {
                        lblPort1.Text = ((ProtocolType)msg_control.param2).GetDescription();
                    }
                    else if (msg_control.param1 == (byte)CONFIG_PORT.CONFIG_PORT_2)
                    {
                        lblPort2.Text = ((ProtocolType)msg_control.param2).GetDescription();
                    }

                    tsLoading.Visible = false;

                    btnPort1Configure.Enabled = true;
                    btnPort2Configure.Enabled = true;


                    break;

                case CONFIG_COMMAND.CONFIG_COMMAND_NACK:
                    tsLoading.Visible = false;

                    btnPort1Configure.Enabled = true;
                    btnPort2Configure.Enabled = true;

                    MessageBox.Show(this, string.Format("Error while saving Configuration for Port {0}", msg_control.param1), "Error", MessageBoxButtons.OK, MessageBoxIcon.Error);

                    break;

                case CONFIG_COMMAND.CONFIG_COMMAND_BOOTLOADER:
                    serialPort.Close();
                    StateMachineUpdate(StateMachineStep.None);
                    break;

                default:
                    break;
                }
            }
            else if (t == typeof(Msg_configuration_port))
            {
                Msg_configuration_port msg_config = (msg as Msg_configuration_port);
                try
                {
                    PortConfiguration config = new PortConfiguration();
                    config.Port     = msg_config.port;
                    config.Protocol = (ProtocolType)msg_config.protocol;
                    if (config.Configuration != null)
                    {
                        config.Configuration.DeSerialize(msg_config.data);
                    }

                    tsLoading.Visible = false;

                    if (PortConfigurationForm.ShowDialog(this, config) == DialogResult.OK)
                    {
                        try
                        {
                            Msg_configuration_port msg_send = new Msg_configuration_port();
                            msg_send.port     = msg_config.port;
                            msg_send.protocol = (byte)config.Protocol;
                            if (config.Configuration != null)
                            {
                                msg_send.data = config.Configuration.Serialize();
                            }
                            else
                            {
                                msg_send.data = new byte[128];
                            }

                            mavlink_packet.Message = msg_send;

                            byte[] bytes = mavlink.Send(mavlink_packet);
                            serialPort.Write(bytes, 0, bytes.Length);

                            tsLoading.Visible = true;

                            btnPort1Configure.Enabled = false;
                            btnPort2Configure.Enabled = false;

                            Task.Factory.StartNew(() =>
                            {
                                Extensions.TaskExtensions.Delay(5000).Wait();

                                if (!Disposing && !IsDisposed)
                                {
                                    BeginInvoke(((Action)(() =>
                                    {
                                        if (!Disposing && !IsDisposed)
                                        {
                                            if (tsLoading.Visible)
                                            {
                                                tsLoading.Visible = false;

                                                btnPort1Configure.Enabled = true;
                                                btnPort2Configure.Enabled = true;

                                                MessageBox.Show(this, "Could not load Configuration. A connection timeout occurred.", "Timeout", MessageBoxButtons.OK, MessageBoxIcon.Error);
                                            }
                                        }
                                    })));
                                }
                            });
                        }
                        catch (Exception ex)
                        {
                            StateMachineUpdate(StateMachineStep.None);
                            MessageBox.Show(this, "Error while communicating:\n" + ex.Message, "Error", MessageBoxButtons.OK, MessageBoxIcon.Error);
                        }
                    }
                    else
                    {
                        btnPort1Configure.Enabled = true;
                        btnPort2Configure.Enabled = true;
                    }
                }
                catch (Exception ex)
                {
                    tsLoading.Visible = false;

                    btnPort1Configure.Enabled = true;
                    btnPort2Configure.Enabled = true;

                    MessageBox.Show(this, string.Format("Error loading Configuration for Port {0}\n{1}", msg_config.port, ex.Message), "Error",
                                    MessageBoxButtons.OK, MessageBoxIcon.Error);
                }
            }
            else if (t == typeof(Msg_configuration_version) || t == typeof(Msg_configuration_version2))
            {
                StateMachineUpdate(StateMachineStep.Connected);

                uint version = 0;

                if (t == typeof(Msg_configuration_version))
                {
                    //0x0009
                    Msg_configuration_version msg_version = (msg as Msg_configuration_version);
                    lblPort1.Text = ((ProtocolType)msg_version.port1).GetDescription();
                    lblPort2.Text = ((ProtocolType)msg_version.port2).GetDescription();

                    version = msg_version.fw_version;
                    version = version << 8;
                }
                else
                {
                    //0x00000901
                    Msg_configuration_version2 msg_version = (msg as Msg_configuration_version2);
                    version = msg_version.fw_version;

                    lblPort1.Text = ((ProtocolType)msg_version.port1).GetDescription();
                    lblPort2.Text = ((ProtocolType)msg_version.port2).GetDescription();
                }

                tsFWVersion.Text = string.Format("{0}.{1}.{2}", (byte)(version >> 16), (byte)(version >> 8), (byte)version);

                btnUpdate.Visible     = FIRMWARE_VERSION > version;
                lblUpdateHint.Visible = FIRMWARE_VERSION != version;

                lblUpdateHint.Text = (FIRMWARE_VERSION > version ? "Your Firmware ist outdated.\n Please click Update to flash your AnySense with latest Firmware Version." :
                                      "Your Konfiguration Manager is outdated.\nPlease visit our support page for more information.");

                btnPort1Configure.Enabled = FIRMWARE_VERSION == version;
                btnPort2Configure.Enabled = FIRMWARE_VERSION == version;
            }
            else if (t == typeof(Msg_heartbeat))
            {
                Msg_heartbeat heartbeat = (msg as Msg_heartbeat);
                lblFlightMode.Text = ((FlightMode)heartbeat.custom_mode).GetDescription();
                tsUniAdapterHeartbeat.BackgroundImage = KonfigurationTool.Properties.Resources.led_g_an;
                if (UAHeartbeat == null || UAHeartbeat.Status == TaskStatus.RanToCompletion)
                {
                    UAHeartbeat = Task.Factory.StartNew(() =>
                    {
                        Extensions.TaskExtensions.Delay(300).Wait();
                        if (!Disposing && !IsDisposed)
                        {
                            BeginInvoke(((Action)(() =>
                            {
                                if (!Disposing && !IsDisposed)
                                {
                                    tsUniAdapterHeartbeat.BackgroundImage = KonfigurationTool.Properties.Resources.led_aus;
                                }
                            })));
                        }
                    });
                }

                retry = 0;
            }
            else if (t == typeof(Msg_configuration_naza_heartbeat))
            {
                tsFCHearbeat.BackgroundImage = KonfigurationTool.Properties.Resources.led_b_an;
                if (FCHeartbeat == null || FCHeartbeat.Status == TaskStatus.RanToCompletion)
                {
                    FCHeartbeat = Task.Factory.StartNew(() =>
                    {
                        Extensions.TaskExtensions.Delay(300).Wait();

                        if (!Disposing && !IsDisposed)
                        {
                            BeginInvoke(((Action)(() =>
                            {
                                if (!Disposing && !IsDisposed)
                                {
                                    tsFCHearbeat.BackgroundImage = KonfigurationTool.Properties.Resources.led_aus;
                                }
                            })));
                        }
                    });
                }
            }
            else if (t == typeof(Msg_sys_status))
            {
                Msg_sys_status sys = (msg as Msg_sys_status);
                lblBattery.Text = (sys.voltage_battery / 1000f).ToString("0.00 V");
                lblCurrent.Text = (sys.current_battery / 100f).ToString("0.00 A");
            }
            else if (t == typeof(Msg_gps_raw_int))
            {
                Msg_gps_raw_int gps = (msg as Msg_gps_raw_int);
                lblCOG.Text       = (gps.cog / 100f).ToString("0.00 °");
                lblAltitude.Text  = (gps.alt / 1000f).ToString("0.00 m");
                lblGPSFix.Text    = ((FixType)gps.fix_type).GetDescription();
                lblHDOP.Text      = (gps.eph / 100f).ToString("0.00");
                lblVDOP.Text      = (gps.epv / 100f).ToString("0.00");
                lblLatitude.Text  = (gps.lat / 10000000.0f).ToString("0.000000");
                lblLongitude.Text = (gps.lon / 10000000.0f).ToString("0.000000");
                lblSpeed.Text     = (gps.vel / 100f).ToString("0.00 m/s");
                lblNumSat.Text    = gps.satellites_visible.ToString();
            }
            else if (t == typeof(Msg_vfr_hud))
            {
                Msg_vfr_hud vfr = (msg as Msg_vfr_hud);
                lblClimb.Text    = vfr.climb.ToString("0.00 m/s");
                lblSpeed.Text    = vfr.groundspeed.ToString("0.00 m/s");
                lblHeading.Text  = vfr.heading.ToString("0 °");
                lblThrottle.Text = vfr.throttle.ToString() + " %";
            }
            else if (t == typeof(Msg_attitude))
            {
                Msg_attitude att = (msg as Msg_attitude);
                lblRoll.Text  = (att.roll * 180.0 / Math.PI).ToString("0.00 °");
                lblPitch.Text = (att.pitch * 180.0 / Math.PI).ToString("0.00 °");
            }
            else if (t == typeof(Msg_rc_channels))
            {
                Msg_rc_channels rc = (msg as Msg_rc_channels);
                lblRC1.Text = rc.chan2_raw.ToString();
                lblRC2.Text = rc.chan3_raw.ToString();
                lblRC3.Text = rc.chan6_raw.ToString();
                lblRC4.Text = rc.chan4_raw.ToString();
                lblRC5.Text = rc.chan8_raw.ToString();
                lblRC6.Text = rc.chan9_raw.ToString();
                lblRC7.Text = rc.chan5_raw.ToString();
                lblRC8.Text = rc.chan10_raw.ToString();
            }
            else if (t == typeof(Msg_rc_channels_raw))
            {
                Msg_rc_channels_raw rc = (msg as Msg_rc_channels_raw);
                lblRC1.Text = rc.chan1_raw.ToString();
                lblRC2.Text = rc.chan2_raw.ToString();
                lblRC3.Text = rc.chan3_raw.ToString();
                lblRC4.Text = rc.chan4_raw.ToString();
                lblRC5.Text = rc.chan5_raw.ToString();
                lblRC6.Text = rc.chan6_raw.ToString();
                lblRC7.Text = rc.chan7_raw.ToString();
                lblRC8.Text = rc.chan8_raw.ToString();
            }
            else if (t == typeof(Msg_battery_status))
            {
                Msg_battery_status batt = (msg as Msg_battery_status);

                lbCell1.Text  = string.Format("{0:00}. {1:0.00} V", 1, ((float)batt.voltages[0] / 1000f));
                lbCell2.Text  = string.Format("{0:00}. {1:0.00} V", 2, ((float)batt.voltages[1] / 1000f));
                lbCell3.Text  = string.Format("{0:00}. {1:0.00} V", 3, ((float)batt.voltages[2] / 1000f));
                lbCell4.Text  = string.Format("{0:00}. {1:0.00} V", 4, ((float)batt.voltages[3] / 1000f));
                lbCell5.Text  = string.Format("{0:00}. {1:0.00} V", 5, ((float)batt.voltages[4] / 1000f));
                lbCell6.Text  = string.Format("{0:00}. {1:0.00} V", 6, ((float)batt.voltages[5] / 1000f));
                lbCell7.Text  = string.Format("{0:00}. {1:0.00} V", 7, ((float)batt.voltages[6] / 1000f));
                lbCell8.Text  = string.Format("{0:00}. {1:0.00} V", 8, ((float)batt.voltages[7] / 1000f));
                lbCell9.Text  = string.Format("{0:00}. {1:0.00} V", 9, ((float)batt.voltages[8] / 1000f));
                lbCell10.Text = string.Format("{0:00}. {1:0.00} V", 10, ((float)batt.voltages[9] / 1000f));
            }
        }