C# MAVLink类代码示例

        private void BUT_connect_Click(object sender, EventArgs e)
        {
            Comms.CommsSerialScan.Scan(false);

            DateTime deadline = DateTime.Now.AddSeconds(50);

            while (Comms.CommsSerialScan.foundport == false)
            {
                System.Threading.Thread.Sleep(100);

                if (DateTime.Now > deadline)
                {
                    CustomMessageBox.Show("Timeout waiting for autoscan/no mavlink device connected");
                    return;
                }
            }

            MAVLink com2 = new MAVLink();

            com2.BaseStream.PortName = Comms.CommsSerialScan.portinterface.PortName;
            com2.BaseStream.BaudRate = Comms.CommsSerialScan.portinterface.BaudRate;

            com2.Open(true);

            MainV2.Comports.Add(com2);

            bindingSource1.ResetBindings(false);
        }

        public void UpdateIcon(MAVLink mav,float x, float y, float z, bool movable)
        {
            foreach (var icon in icons)
            {
                if (icon.interf == mav)
                {
                    icon.Movable = movable;
                    if (!movable)
                    {
                        icon.x = 0;
                        icon.y = 0;
                        icon.z = 0;
                        icon.Color = Color.Blue;
                    }
                    else
                    {
                        icon.x = x;
                        icon.y = y;
                        icon.z = z;
                        icon.Color = Color.Red;
                    }
                    this.Invalidate();
                    return;
                }
            }

            icons.Add(new icon() { interf = mav, y=y, z = z,x = x, Movable = movable, Name = mav.ToString() });
        }

        bool ReceviedPacket(MAVLink.MAVLinkMessage rawpacket)
        {
            if (rawpacket.msgid == (byte) MAVLink.MAVLINK_MSG_ID.TERRAIN_REQUEST)
            {
                MAVLink.mavlink_terrain_request_t packet =
                    rawpacket.ToStructure<MAVLink.mavlink_terrain_request_t>();

                if (issending)
                    return false;

                lastmessage = rawpacket;
                lastrequest = packet;

                log.Info("received TERRAIN_REQUEST " + packet.lat/1e7 + " " + packet.lon/1e7 + " space " +
                         packet.grid_spacing + " " + Convert.ToString((long) packet.mask, 2));

                System.Threading.ThreadPool.QueueUserWorkItem(QueueSendGrid);
            }
            else if (rawpacket.msgid == (byte) MAVLink.MAVLINK_MSG_ID.TERRAIN_REPORT)
            {
                MAVLink.mavlink_terrain_report_t packet =
                    rawpacket.ToStructure<MAVLink.mavlink_terrain_report_t>();
                log.Info("received TERRAIN_REPORT " + packet.lat/1e7 + " " + packet.lon/1e7 + " " + packet.loaded + " " +
                         packet.pending);
            }
            return false;
        }

        private bool receivedPacket(MAVLink.MAVLinkMessage arg)
        {
            if (arg.msgid == (uint)MAVLink.MAVLINK_MSG_ID.STATUSTEXT)
            {
                var message = ASCIIEncoding.ASCII.GetString(arg.ToStructure<MAVLink.mavlink_statustext_t>().text);

                UpdateUserMessage(message);

                if (message.ToLower().Contains("calibration successful") ||
                 message.ToLower().Contains("calibration failed"))
                {
                    try
                    {
                        Invoke((MethodInvoker)delegate
                        {
                            BUT_calib_accell.Text = Strings.Done;
                            BUT_calib_accell.Enabled = false;
                        });

                        _incalibrate = false;
                        MainV2.comPort.UnSubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.STATUSTEXT, receivedPacket);
                    }
                    catch
                    {
                    }
                }
            }

            return true;
        }

        public MAVLinkSerialPort(MAVLinkInterface mavint, MAVLink.SERIAL_CONTROL_DEV port)
        {
            this.mavint = mavint;
            this.port = port;

            if (!mavint.BaseStream.IsOpen)
            {
                mavint.BaseStream.Open();
            }

            if (mavint.getHeartBeat().Length == 0)
            {
                throw new Exception("No valid heartbeats read from port");
            }

            if (subscription.Value != null)
                mavint.UnSubscribeToPacketType(subscription);

            subscription = mavint.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.SERIAL_CONTROL, ReceviedPacket, true);

            bgdata = new Thread(mainloop);
            bgdata.Name = "MAVLinkSerialPort";
            bgdata.IsBackground = true;
            bgdata.Start();
        }

        public HIL.Vector3 getOffsetFromLeader(MAVLink leader, MAVLink mav)
        {
            //convert Wgs84ConversionInfo to utm
            CoordinateTransformationFactory ctfac = new CoordinateTransformationFactory();

            GeographicCoordinateSystem wgs84 = GeographicCoordinateSystem.WGS84;

            int utmzone = (int)((leader.MAV.cs.lng - -183.0) / 6.0);

            IProjectedCoordinateSystem utm = ProjectedCoordinateSystem.WGS84_UTM(utmzone, leader.MAV.cs.lat < 0 ? false : true);

            ICoordinateTransformation trans = ctfac.CreateFromCoordinateSystems(wgs84, utm);

            double[] masterpll = { leader.MAV.cs.lng, leader.MAV.cs.lat };

            // get leader utm coords
            double[] masterutm = trans.MathTransform.Transform(masterpll);

            double[] mavpll = { mav.MAV.cs.lng, mav.MAV.cs.lat };

            //getLeader follower utm coords
            double[] mavutm = trans.MathTransform.Transform(mavpll);

            return new HIL.Vector3(masterutm[1] - mavutm[1], masterutm[0] - mavutm[0], 0);
        }

        public void setup(List<KeyValuePair<int, string>> source, string paramname, MAVLink.MAVLinkParamList paramlist)
            //, string paramname2 = "", Control enabledisable = null)
        {
            base.SelectedIndexChanged -= MavlinkComboBox_SelectedIndexChanged;

            _source2 = source;

            this.DisplayMember = "Value";
            this.ValueMember = "Key";
            this.DataSource = source;

            this.ParamName = paramname;


            if (paramlist.ContainsKey(paramname))
            {
                this.Enabled = true;
                this.Visible = true;

                enableControl(true);

                var item = paramlist[paramname];

                this.SelectedValue = (int) paramlist[paramname].Value;
            }

            base.SelectedIndexChanged += new EventHandler(MavlinkComboBox_SelectedIndexChanged);
        }

        public GMapMarkerPhoto(MAVLink.mavlink_camera_feedback_t mark, bool shotBellowMinInterval = false)
            : base(new PointLatLng(mark.lat/1e7, mark.lng/1e7))
        {
            local = mark;
            this.shotBellowMinInterval = shotBellowMinInterval;
            Offset = new Point(-localcache1.Width/2, -localcache1.Height/2);
            Size = localcache1.Size;
            Alt = mark.alt_msl;
            ToolTipMode = MarkerTooltipMode.OnMouseOver;

            Roll = local.roll - rolltrim;
            Pitch = local.pitch - pitchtrim;
            Yaw = local.yaw - yawtrim;

            ToolTipText = "Photo" + "\nAlt: " + mark.alt_msl + "\nNo: "+ mark.img_idx + "\nRoll: "+Roll.ToString("0.00");

            Tag = mark.time_usec;

            var footprint = ImageProjection.calc(new PointLatLngAlt(Position.Lat, Position.Lng, Alt), Roll,
                Pitch, Yaw, hfov, vfov);

            footprintpoly = new GMapPolygon(footprint.ConvertAll(x => x.Point()), "FP"+mark.time_usec);

            footprintpoly.Fill = Brushes.Transparent;
            footprintpoly.Stroke = Pens.Crimson;
        }

 public HIL.Vector3 getOffsets(MAVLink mav)
 {
     if (offsets.ContainsKey(mav))
     {
         return offsets[mav];
     }
     return new HIL.Vector3(offsets.Count, 0, 0);
 }

        public void setup(string paramname, MAVLink.MAVLinkParamList paramlist)
        {
            this.ParamName = paramname;

            if (paramlist.ContainsKey(paramname))
            {
                this.Enabled = true;

                list = ParameterMetaDataRepository.GetParameterBitMaskInt(ParamName,
                    MainV2.comPort.MAV.cs.firmware.ToString());
                chkcount = list.Count;

                int leftside = 9;
                int top = 9;

                uint value = (uint) paramlist[paramname].Value;

                for (int a = 0; a < chkcount; a++)
                {
                    CheckBox chk = new CheckBox();
                    chk.AutoSize = true;
                    chk.Text = list[a].Value.ToString();
                    chk.Location = new System.Drawing.Point(leftside, top);

                    chk.CheckedChanged -= MavlinkCheckBoxBitMask_CheckedChanged;

                    if ((value & (1 << list[a].Key)) > 0)
                    {
                        chk.Checked = true;
                    }

                    chklist.Add(new KeyValuePair<int, CheckBox>(list[a].Key, chk));
                    panel1.Controls.Add(chk);

                    chk.CheckedChanged += MavlinkCheckBoxBitMask_CheckedChanged;

                    //this.Controls.Add(new Label() { Location = chk.Location, Text = "test" });

                    leftside += chk.Width + 5;
                    if (leftside > 500)
                    {
                        top += chk.Height + 5;
                        leftside = 9;
                    }
                }


                this.panel1.Height = top + 25;

                //this.Height = top + 25;
            }
            else
            {
                this.Enabled = false;
            }
        }

 public GMapMarkerRallyPt(MAVLink.mavlink_rally_point_t mark)
     : base(new PointLatLng(mark.lat / 1e7, mark.lng / 1e7))
 {
     Size = SizeSt;
     Offset = new Point(-10, -40);
     Alt = mark.alt;
     Alt = (int)mark.alt; 
     ToolTipMode = MarkerTooltipMode.OnMouseOver;
     ToolTipText = "Rally Point" + "\nAlt: " + mark.alt;
 }

 public void setup(double OnValue, double OffValue, string[] paramname, MAVLink.MAVLinkParamList paramlist,
     Control enabledisable = null)
 {
     foreach (var s in paramname)
     {
         if (paramlist.ContainsKey(s))
         {
             setup(OnValue, OffValue, s, paramlist, enabledisable);
             return;
         }
     }
 }

        private bool ReceviedPacket(MAVLink.MAVLinkMessage arg)
        {
            if (arg.msgid == (byte) MAVLink.MAVLINK_MSG_ID.DATA_TRANSMISSION_HANDSHAKE)
            {
                var packet = arg.ToStructure<MAVLink.mavlink_data_transmission_handshake_t>();
                msgDataTransmissionHandshake = packet;
                imageBuffer.Close();
                imageBuffer = new MemoryStream((int)packet.size);
            }
            else if (arg.msgid == (byte)MAVLink.MAVLINK_MSG_ID.ENCAPSULATED_DATA)
            {
                var packet = arg.ToStructure<MAVLink.mavlink_encapsulated_data_t>();
                msgEncapsulatedData = packet;
                int start = msgDataTransmissionHandshake.payload * msgEncapsulatedData.seqnr;
                int left = (int)msgDataTransmissionHandshake.size - start;
                var writeamount = Math.Min(msgEncapsulatedData.data.Length, left);

                imageBuffer.Seek(start, SeekOrigin.Begin);
                imageBuffer.Write(msgEncapsulatedData.data, 0, writeamount);

                // we have a complete image
                if ((msgEncapsulatedData.seqnr+1) == msgDataTransmissionHandshake.packets)
                {
                    using (
                        Bitmap bmp = new Bitmap(msgDataTransmissionHandshake.width, msgDataTransmissionHandshake.height,
                            PixelFormat.Format8bppIndexed))
                    {
                        SetGrayscalePalette(bmp);

                        var bitmapData = bmp.LockBits(new Rectangle(Point.Empty, bmp.Size), ImageLockMode.ReadWrite,
                            bmp.PixelFormat);

                        if (imageBuffer.Length > msgDataTransmissionHandshake.size)
                            return true;

                        var buffer = imageBuffer.ToArray();

                        Marshal.Copy(buffer, 0, bitmapData.Scan0, buffer.Length);
                        bmp.UnlockBits(bitmapData);

                        if (newImage != null)
                            newImage(this, new ImageEventHandle(bmp));

                        //bmp.Save("test.bmp", ImageFormat.Bmp);
                    }
                }
            }

            return true;
        }

        public void setup(Type source, string paramname, MAVLink.MAVLinkParamList paramlist)//, string paramname2 = "", Control enabledisable = null)
        {
            base.SelectedIndexChanged -= MavlinkComboBox_SelectedIndexChanged;

            _source = source;

            this.DataSource = Enum.GetNames(source);

            this.ParamName = paramname;

            if (paramlist.ContainsKey(paramname))
            {
                this.Enabled = true;
                this.Visible = true;

                enableControl(true);

                this.Text = Enum.GetName(source, (Int32)paramlist[paramname].Value);
            }

            base.SelectedIndexChanged += new EventHandler(MavlinkComboBox_SelectedIndexChanged);
        }

        public void setup(double OnValue, double OffValue, string paramname, MAVLink.MAVLinkParamList paramlist,
            Control enabledisable = null)
        {
            base.CheckedChanged -= MavlinkCheckBox_CheckedChanged;

            this.OnValue = OnValue;
            this.OffValue = OffValue;
            this.ParamName = paramname;
            this._control = enabledisable;

            if (paramlist.ContainsKey(paramname))
            {
                this.Enabled = true;
                this.Visible = true;

                if (paramlist[paramname].Value == OnValue)
                {
                    this.Checked = true;
                    enableControl(true);
                }
                else if (paramlist[paramname].Value == OffValue)
                {
                    this.Checked = false;
                    enableControl(false);
                }
                else
                {
                    this.CheckState = System.Windows.Forms.CheckState.Indeterminate;
                    enableControl(false);
                }
            }
            else
            {
                this.Enabled = false;
            }

            base.CheckedChanged += new EventHandler(MavlinkCheckBox_CheckedChanged);
        }