コード例 #1
0
        public void sendWPS(List <PointLatLngAlt> Points)
        {
            MAVLink.MAV_FRAME frame = MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT;
            Locationwp        home  = new Locationwp();

            try
            {
                home.id  = (byte)MAVLink.MAV_CMD.WAYPOINT;
                home.lat = 23.1647061918178;
                home.lng = 113.455638885498;
                home.alt = 20; // use saved home
            }
            catch { throw new Exception("Your home location is invalid"); }

            ushort count = (ushort)(Points.Count + 1);//含家地址

            UpLoadForm upform = new UpLoadForm();

            upform.StartPosition = FormStartPosition.Manual;
            upform.Location      = new System.Drawing.Point((this.form.Width - upform.Width) / 2, (this.form.Height - upform.Height) / 2);

            //   this.upform.Visible = true;

            // Application.DoEvents();

            upform.Show();



            //    Application.DoEvents();
            upform.SetMessage(0, count);
            // this.upform.ShowDialog();
            //  Thread.Sleep(100);

            setWPTotal(count);
            this.ListMessage("已发送航点数量...");

            upform.SetMessage(1, 0);

            bool mission_uploading = true;

            Locationwp[] wp_location = new Locationwp[count - 1];
            int          i           = 0;

            foreach (PointLatLngAlt point in Points)
            {
                wp_location[i].id  = (byte)MAVLink.MAV_CMD.WAYPOINT;
                wp_location[i].lat = point.Lat;
                wp_location[i].lng = point.Lng;
                wp_location[i].alt = point.Alt;
                i++;
            }
            int seq = 0;

            bool isSendTotalSuccess = false;
            int  cnt = 0;

            while (mission_uploading)
            {
                Program.mav_msg_handler.mission_request = false;
                Program.mav_msg_handler.mission_ack     = false;
                Program.mav_msg_handler.SetTimeout(5);
                while (true)
                {
                    if (Program.mav_msg_handler.Wait())
                    {
                        if (isSendTotalSuccess)
                        {
                            ListMessage("发送航点超时出错");
                            mission_uploading = false;
                            break;
                        }
                        else
                        {
                            cnt++;
                            if (cnt >= 3)
                            {
                                ListMessage("发送航点数量超时出错");
                                mission_uploading = false;
                                break;
                            }
                            else
                            {
                                ListMessage("发送航点数量超时出错,准备重发...");
                                setWPTotal(count);
                                break;
                            }
                        }
                    }

                    if (upform.isCancel)
                    {
                        this.ListMessage("您已经取消航点发送...");
                        upform.Close();
                        return;
                    }

                    if (Program.mav_msg_handler.mission_ack)
                    {
                        //  this.ListMessage("收到ACK...");
                        if (Program.mav_msg_handler.mavlink_mission_ack.type != 0)
                        {
                            ListMessage("发送航点出错");
                        }
                        else
                        {
                            ListMessage("航点上传完毕");
                        }
                        mission_uploading = false;
                        break;
                    }

                    if (Program.mav_msg_handler.mission_request == true)
                    {
                        isSendTotalSuccess = true;
                        //   this.ListMessage("收到REQUEST...");
                        seq = Program.mav_msg_handler.mavlink_mission_request.seq;
                        if (seq == 0)
                        {
                            setWP(home, 0, MAVLink.MAV_FRAME.GLOBAL, 0);
                            ListMessage("正在发送HOME...");
                        }
                        else
                        {
                            setWP(wp_location[seq - 1], (ushort)(seq), frame, 0);
                            ListMessage("正在发送航点" + seq.ToString() + "...");
                            upform.SetMessage(2, seq);
                            upform.SetCount(seq, count);
                        }
                        break;
                    }
                    Application.DoEvents();
                }
            }


            Thread.Sleep(500);
            upform.SetMessage(3, 0);
            Thread.Sleep(500);
            setWPACK();
            upform.Close();
        }
コード例 #2
0
        public List <PointLatLngAlt> drawPoints = new List <PointLatLngAlt>();  //点集
        public void getWPs()
        {
            bool succeed           = false;
            List <Locationwp> cmds = new List <Locationwp>();



            UpLoadForm up = new UpLoadForm();

            up.SetMessage(4, 0);
            up.Show();
            int cmdcount = getWPCount();

            up.SetMessage(5, cmdcount);



            Program.mav_msg_handler.mission_items.Clear();
            ushort a = 0;

            for (a = 0; a < cmdcount; a++)
            {
                up.SetCount1(a, cmdcount);
                Locationwp wp = getWP(a, ref succeed);

                if (succeed)
                {
                    cmds.Add(wp);
                }
                else
                {
                    break;
                }

                if (up.isCancel)
                {
                    ListMessage("已取消下载航点...");
                    up.Close();
                    return;
                }


                Application.DoEvents();
            }

            if (succeed)
            {
                up.SetMessage(7, 0);
                drawPoints.Clear();
                foreach (Locationwp loc in cmds)
                {
                    PointLatLngAlt PL = new PointLatLngAlt();
                    PL.Lat = loc.lat;
                    PL.Lng = loc.lng;
                    PL.Alt = loc.alt;
                    drawPoints.Add(PL);
                }
            }
            else
            {
                up.SetMessage(8, 0);
            }
            Thread.Sleep(500);
            up.Close();
        }