ControlGUI/GUI/ComAgent.cs

using GUI.DataProvider;
using System;
using System.Collections.Concurrent;
using System.Collections.Generic;
using System.IO.Ports;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;

namespace GUI
{
    public interface IComAgentDelegate
    {
        void ReceiveError(string msg);
        void ReceiveSuccess();
    }

    public class ComAgent : SerialPort, IGUIDataSource
    {
        public float Yaw { get; set; } = 0f;
        public float Roll { get; set; } = 0f;
        public float Pitch { get; set; } = 0f;

        public bool Q, W, E, A, S, D, L, R;
        public sbyte dx, dy;
        readonly Thread sendThread;
        readonly ConcurrentQueue<Action> ioTasks;

        public void Move(int dx, int dy)
        {
            this.dx = (sbyte)(this.dx + dx);
            this.dy = (sbyte)(this.dy + dy);
        }

        public void ResetMove()
        {
            dx = dy = 0;
        }

        public IComAgentDelegate agentDelegate;

        public ComAgent() : base()
        {
            DataReceived += receive;
            ioTasks = new();
            (sendThread = new(() =>
            {
                while (true)
                {
                    try
                    {
                        if (ioTasks.TryDequeue(out Action act)) act();
                    }
                    catch (Exception e)
                    {
                        Console.WriteLine($"{DateTime.Now} {e.Message}");
                    }
                    Thread.Sleep(10);
                }
            }) { IsBackground = true }).Start();
        }

        void EnqueueDiscardableIOTask(Action task)
        {
            int discarded = 0;
            while (ioTasks.Count > 10)
            {
                ioTasks.TryDequeue(out Action _);
                discarded++;
            }
            if (discarded > 0) Console.WriteLine($"{DateTime.Now} Discard {discarded} old io job due too much jobs blocking.");
            ioTasks.Enqueue(task);
        }

        public void ThrowError(string reason, bool shutdown = false)
        {
            Console.WriteLine($"{DateTime.Now} Connection {(shutdown ? "breaked" : "warned")} due {reason}");
            if (shutdown)
            {
                agentDelegate?.ReceiveError(reason);
                Close();
            }
        }

        public new void Close()
        {
            try { base.Close(); }
            catch { }
        }

        public void SendFrame()
        {
            byte[] data = new byte[4];
            data[0] = 0x0A;
            data[1] = (byte)(Q.t() + W.t() * 2 + E.t() * 4 + A.t() * 8 + S.t() * 16 + D.t() * 32 + L.t() * 64 + R.t() * 128);
            data[2] = (byte)dx;
            data[3] = (byte)dy;
            data = data.PackData();
            dx = dy = 0;
            EnqueueDiscardableIOTask(() => Write(data, 0, data.Length));
        }

        void receive(object sender, SerialDataReceivedEventArgs e)
        {
            byte[] buff = new byte[10];
            int cursor = 0;
            while (ReadByte() != 0x0A) ;
            buff[cursor++] = 0x0A;
            while ((cursor += Read(buff, cursor, buff.Length - cursor)) < buff.Length) ;
            //Read(buff, 0, buff.Length);
            if (buff[0] != 0x0A)
            {
                ThrowError($"Expect meta 0x0A, but got {buff[0]}", true);
                return;
            }
            if (!buff.CheckData())
            {
                ThrowError($"Check fail, source data is {buff}");
                return;
            }
            ushort yaw100 = BitConverter.ToUInt16(buff, 1),
                roll100 = BitConverter.ToUInt16(buff, 3),
                pitch100 = BitConverter.ToUInt16(buff, 5);
            Yaw = yaw100 * 0.01f;
            Roll = roll100 * 0.01f;
            Pitch = pitch100 * 0.01f;
            agentDelegate?.ReceiveSuccess();
        }
    }
}