[Window Form] 繪製地面站姿態儀(九) 使用SerialPort連接飛控

 我們使用Mission Planner提供的Example來進行Serial Port連線，

並取得飛控中的姿態資訊後與我們之前完成的UI結合在一起。

首先先下載Mission Planner的Source Code。

打開Mission Planner的資料夾中的SimpleExample。

路徑 => MissionPlanner-master\ExtLibs\SimpleExample

在我們的UI中增加兩個ComboBox，一個選擇Serial Port，一個選擇BaudRate，

最後再加一個Button1當作Connect。

設定兩個ComboBox，

ComboBox1取得系統上的Serial Port

    private void comboBox1_MouseClick(object sender, MouseEventArgs e)
    {
        //取得系統上的Comport
        comboBox1.DataSource = SerialPort.GetPortNames();
    }

在建構式中設定ComboBox2，手動增加BaudRate

    var list = new List <int> { 19200,115200 };
    comboBox2.DataSource = list;

BaudRate可以參考Mission Planner的常用BaudRate，

但實際值還是要看飛控上的設定為主。

載入MAVLink:

    在SimpleExample執行後會產生一個exe檔與相關的dll，

    我們只需要將生成的MAVLink.dll引入到專案中就可以使用MAVLink了。

    參考 => (右鍵)加入參考 => 瀏覽 =>
    (下載的SourceCode => MissionPlanner-master\ExtLibs\SimpleExample\bin\Debug)
    選取 MAVLink.dll => 確定

設定Button

    private void Button1_Click(object sender, EventArgs e)
    {
        if (serialPort1.IsOpen)
            serialPort1.Close();
        serialPort1.PortName = comboBox1.Text;
        serialPort1.BaudRate = int.Parse(comboBox2.Text);
        serialPort1.Open();
        BackgroundWorker worker = new BackgroundWorker();
        worker.DoWork += Worker_DoWork;
        worker.RunWorkerAsync();
    }

    private MAVLink.MavlinkParse mavlink = new MAVLink.MavlinkParse();
    private object readLock = new object();
    private byte sysid;
    private byte compid;

    private void Worker_DoWork(object sender, DoWorkEventArgs e)
    {
        while (serialPort1.IsOpen)
        {
            try
            {
                MAVLink.MAVLinkMessage packet;
                lock (readLock)
                {
                    packet = mavlink.ReadPacket(serialPort1.BaseStream);
                    if (packet == null || packet.data == null) continue;
                }

                if (packet.data.GetType() == typeof(MAVLink.mavlink_heartbeat_t))
                {
                    var hb = (MAVLink.mavlink_heartbeat_t)packet.data;

                    // save the sysid and compid of the seen MAV
                    sysid = packet.sysid;
                    compid = packet.compid;

                    // request streams at 2 hz
                    var buffer = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.REQUEST_DATA_STREAM,
                        new MAVLink.mavlink_request_data_stream_t()
                    {
                        req_message_rate = 2,
                        req_stream_id = (byte)MAVLink.MAV_DATA_STREAM.ALL,
                        start_stop = 1,
                        target_component = compid,
                        target_system = sysid
                    });

                    serialPort1.Write(buffer, 0, buffer.Length);

                    buffer = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb);

                    serialPort1.Write(buffer, 0, buffer.Length);
                }

                if (sysid != packet.sysid || compid != packet.compid)
                    continue;

                //Console.WriteLine(packet.msgtypename);

                if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.ATTITUDE)
                //or
                //if (packet.data.GetType() == typeof(MAVLink.mavlink_attitude_t))
                {
                    var att = (MAVLink.mavlink_attitude_t)packet.data;
                    var roll = att.roll * 57.2958;
                    var yaw = att.yaw * 57.2958;
                    var pitch = att.pitch * 57.2958;
                    userControl1.Roll = (float)roll;
                    userControl1.Yaw = (float)yaw;
                    userControl1.Pitch = (float)pitch;
                    //Console.WriteLine($@"roll => {roll}  yaw => {yaw}  pitch => {pitch}");
                }
            }
            catch (Exception)
            {
                throw;
            }
            Thread.Sleep(1);
        }
    }