Plane.Sdk3/PlaneGcsSdk_Shared/Copters/PlaneCopter.cs

// #define LOG_SEQUENCE_NUMBER
//#define LOG_PACKETS

using Plane.Protocols;
using Plane.Communication;
using TaskTools.Utilities;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using System.Diagnostics;
using System.IO;
using System.Threading.Tasks;

namespace Plane.Copters
{
#if PRIVATE
    public
#else
    internal
#endif
        partial class PlaneCopter
    {
        uint iErrorData;
        ulong iDataCount;
        private int _recnumber = 0;
        private int _sendnumber = 0;

        public bool IsConnected { get { return Connection.IsOpen; } }

        public string Id { get; set; }

        public byte mavlinkversion { get; set; }
        public MAVLink.MAV_TYPE aptype { get; set; }
        public MAVLink.MAV_AUTOPILOT apname { get; set; }
        private bool CommOK = true;
        public byte sysid { get; set; }
        public byte compid { get; set; }
        public byte recvpacketcount { get; set; }
        public bool armed { get; set; }
        public bool failsafe { get; set; }

        public uint mode { get; set; }
        public uint commModuleMode { get; set; }
        public byte commModuleVersion { get; set; }

        public bool isUnlocked { get; set; }

        public float battery_voltage { get; set; }
        public byte battery_remaining { get; set; }
        public float current_battery { get; set; }
        public float freemem { get; set; }
        public float brklevel { get; set; }

        public bool useLocation { get; set; }
        public float gpsalt { get; set; }
        public byte gpsstatus { get; set; }
        public float gpshdop { get; set; }
        public byte satcount { get; set; }
        public int retain { get; set; }
        public float groundspeed { get; set; }
        public float groundcourse { get; set; }
        public double lat { get; set; }
        public double lng { get; set; }
        public float nav_roll { get; set; }
        public float nav_pitch { get; set; }
        public short nav_bearing { get; set; }
        public short target_bearing { get; set; }
        public float wp_dist { get; set; }
        public float alt_error { get; set; }
        public float aspd_error { get; set; }
        public float xtrack_error { get; set; }
        public float roll { get; set; }
        public float pitch { get; set; }
        public float yaw { get; set; }
        public short heading { get; set; }
        public float airspeed { get; set; }
        public float alt { get { return altorigin - altoffset; } }
        public float altoffset = 0;
        public float altorigin { get; private set; } = 0;
        public float ch3percent { get; set; }

        public ushort ch1in { get; set; }
        public ushort ch2in { get; set; }
        public ushort ch3in { get; set; }
        public ushort ch4in { get; set; }
        public ushort ch5in { get; set; }
        public ushort ch6in { get; set; }
        public ushort ch7in { get; set; }
        public ushort ch8in { get; set; }
        public float rxrssi { get; set; }

        public byte remrssi { get; set; }

        public uint timebootms { get; set; }

        private int DataTimeOut = 0;

        public Dictionary<string, float?> param = new Dictionary<string, float?>();
        public Dictionary<string, MAVLink.MAV_PARAM_TYPE> param_types = new Dictionary<string, MAVLink.MAV_PARAM_TYPE>();

        public IConnection Connection { get; set; }

        public ushort FlightDistance2D { get; private set; }

        private SynchronizationContext _uiSyncContext;
        private bool _autoSendHeartbeat = true;
        private bool _autoRequestData = true;

        public PlaneCopter(IConnection connection, SynchronizationContext uiSyncContext, bool autoSendHeartbeat = true, bool autoRequestData = true)
        {
            this.Connection = connection;
            _uiSyncContext = uiSyncContext;
            _autoSendHeartbeat = autoSendHeartbeat;
            _autoRequestData = autoRequestData;
        }

        public async Task ConnectAsync()
        {
            //sysid = 2;
            SendReq = false;
            CommOK = true;
            await Connection.OpenAsync().ConfigureAwait(false);
            StartCommunication();
        }

        public async Task DisconnectAsync()
        {
#if DEBUG && !NETFX_CORE && LOG_SEQUENCE_NUMBER
            File.AppendAllText($"SequenceNumbers_{Id}.log", _sequenceLog.ToString());
#endif
            await StopDatastreamAsync(MAVLink.MAV_DATA_STREAM.ALL, 0);
            CommOK = false;
            Connection.Close();
        }

        public async Task GetCopterDataAsync()
        {
            //停止所有数据流
            await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.ALL, 0).ConfigureAwait(false);
            //需要电池电压，电流，GPS数据
            /*
             *  目前接收的数据是
             *  gps 38 字节
                hud 28
                SYS_STATUS  39
                MISSION_CURRENT  10
                HEARTBEAT  17
                共132字节
               5hz 592/s 太高
             */
            await RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM.EXTENDED_STATUS, 1).ConfigureAwait(false); //10 1电池电压、电流、电量,152-内存、62-导航控制输出、24-GPS数据、42-航点数据
                //不需要姿态信息
            //         await RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM.EXTRA1, 1).ConfigureAwait(false); //5 请求 飞行姿态 roll、pitch、yaw、rollspeed、pitchspeed、yawspeed--- 30
            //需要高度和机头朝向数据
            await RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM.EXTRA2, 1).ConfigureAwait(false); //10 请求vfr airspeed(空速)、groundspeed(地速)、alt(高度)、climb(爬升率)、heading(头朝向)、throttle(实际油门) ---74
            //不需要通道数据
            //await RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM.RC_CHANNELS, 1).ConfigureAwait(false); //1 请求 通道数据
            //不需要原始数据
            //  await RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, ratesensors).ConfigureAwait(false); // 请求 传感器原始值

         /*  //已经停止所有数据了，不用单独停止
            // Disable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT. |
            await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.RAW_CONTROLLER, 0).ConfigureAwait(false);
            // Disable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages. |
            await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.POSITION, 0).ConfigureAwait(false);
            // Disable EXTRA3 (Dependent on the autopilot |)
            await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.EXTRA3, 0).ConfigureAwait(false);
            // Disable Plane Stream
            await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.GHOST_STREAM, 0).ConfigureAwait(false);
            // Disable RAW_SENSORS
            await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, 0).ConfigureAwait(false); // 
            // 5 Disable MSG_ATTITUDE  roll、pitch、yaw、rollspeed、pitchspeed、yawspeed-- - 30
            await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.EXTRA1, 0).ConfigureAwait(false); //
            */

        }

        private async void StartCommunication()
        {
            try
            {
                var writingTask = _autoSendHeartbeat ? StartSendingHeartbeatsAsync() : TaskUtils.CompletedTask;
                var readingTask = StartReadingPacketsAsync();
                await Task.WhenAll(writingTask, readingTask).ConfigureAwait(false);
            }
            catch (Exception ex)
            {
                RaiseMessageCreated(ex.ToString());
            }
        }

#if DEBUG && LOG_PACKETS

        private Stopwatch _sendHeartbeatStopwatch = new Stopwatch();
        private TimeSpan _sendHeartbeatTimeSpan;

#endif

        public async Task StartSendingHeartbeatsAsync()
        {
            await Task.Run(async () =>
            {
                while (CommOK)
                {
                    if (!IsConnected)
                    {
                        break;
                    }
                    // if (heatbeatSend.Second != DateTime.Now.Second)
                    if (heatbeatSend.AddMilliseconds(1000) <= DateTime.Now)
                    {
                        var htb = new MAVLink.mavlink_heartbeat_t()
                        {
                            type = (byte)MAVLink.MAV_TYPE.GCS,
                            autopilot = (byte)MAVLink.MAV_AUTOPILOT.GENERIC,
                            mavlink_version = 3,
                        };
    //                    await SendPacketAsync(htb).ConfigureAwait(false);  //发心跳包
#if DEBUG && LOG_PACKETS
                        if (!_sendHeartbeatStopwatch.IsRunning)
                        {
                            _sendHeartbeatStopwatch.Start();
                        }
                        else
                        {
                            var elapsed = _sendHeartbeatStopwatch.Elapsed;
                            Debug.WriteLine(elapsed - _sendHeartbeatTimeSpan, "--------------------------发送心跳间隔");
                            _sendHeartbeatTimeSpan = elapsed;
                        }
#endif
                        heatbeatSend = DateTime.Now;
                        DataTimeOut += 1;
                        if (DataTimeOut > 3)
                        {
                            bInitChannel = false;
                            SendReq = false;
                        }
                    }
                    else
                    {
                        await Task.Delay(100).ConfigureAwait(false);
                    }
                }
            }).ConfigureAwait(false);
        }

        private async Task StartReadingPacketsAsync()
        {
            await Task.Run(async () =>
            {
                while (CommOK)
                {
                    if (!IsConnected)
                    {
                        RaiseEventOnUIThread(ConnectionBroken);
                        break;
                    }

                    var packet = await ReadPacketAsync().ConfigureAwait(false);
                    if (packet != null)
                    {
                        PacketReceived?.Invoke(this, new PacketReceivedEventArgs(packet));
                        var handled = true;
                      //  Debug.WriteLine(packet[5],"收到数据类型");
                        switch (packet[5])
                        {
                            case MAVLink.MAVLINK_MSG_ID_HEARTBEAT: //0
                                AnalyzeHeartbeatPacket(packet);
                                break;
                            case MAVLink.MAVLINK_MSG_ID_SYS_STATUS: //1
                                AnalyzeSysStatusPacket(packet);
                                break;
                            case MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT: //24
                                AnalyzeGpsRawIntPacket(packet);
                                break;
                            case MAVLink.MAVLINK_MSG_ID_VFR_HUD: //74
                                AnalyzeVfrHudPacket(packet);
                                break;
                            case MAVLink.MAVLINK_MSG_ID_STATUSTEXT:   //253
                                AnalyzeStatusTextPacket(packet);
                                break;

                                ////以上为基本数据，

                            case MAVLink.MAVLINK_MSG_ID_RADIO:  //109
                                AnalyzeRadioPacket(packet);
                                break;
                            case MAVLink.MAVLINK_MSG_ID_PARAM_VALUE: //22
                                AnalyzeParamValuePacket(packet);
                                break;
                            case MAVLink.MAVLINK_MSG_ID_MEMINFO: //152
                                AnalyzeMemInfoPacket(packet);
                                break;
                            case MAVLink.MAVLINK_MSG_ID_MISSION_CURRENT: //42
                                AnalyzeMissionCurrentPacket(packet);
                                break;

                            case MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT: //33
                                AnalyzeGlobalPositionIntPacket(packet);
                                break;
                            case MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: //62
                                AnalyzeNavControllerOutputPacket(packet);
                                break;
                            case MAVLink.MAVLINK_MSG_ID_ATTITUDE: //30
                                AnalyzeAttitudePacket(packet);
                                break;

                            case MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW: //35
                                AnalyzeRCChannelsRawPacket(packet);
                                break;
                            case MAVLink.MAVLINK_MSG_ID_RAW_IMU:   //没有用27
                                AnalyzeRawImuPacket(packet);
                                break;
                            case MAVLink.MAVLINK_MSG_ID_SCALED_IMU2: //没有用 116
                                AnalyzeScaledImu2Packet(packet);
                                break;
                            case MAVLink.MAVLINK_MSG_ID_SENSOR_OFFSETS: //150
                                AnalyzeSensorOffsetsPacket(packet);
                                break;

                            default:
                                handled = false;
                                UnhandledPacketReceived?.Invoke(this, new PacketReceivedEventArgs(packet));
                                break;
                        }
                        if (handled)
                        {
                            PacketHandled?.Invoke(this, new PacketReceivedEventArgs(packet));
                        }
                    }
                }
            }).ConfigureAwait(false);
        }

        public async Task GetParamsListAsync()
        {
            MAVLink.mavlink_param_request_list_t req = new MAVLink.mavlink_param_request_list_t();
            req.target_system = sysid;
            req.target_component = compid;
            await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_PARAM_REQUEST_LIST, req);
        }

        public static List<KeyValuePair<int, string>> getModesList()
        {
            var flightModes = EnumTranslator.Translate<ac2modes_cn>();
            return flightModes.ToList();
        }

        // 林俊清, 20151029, 由于只在 myComm 循环中读取数据包，因此去掉了锁。
        //volatile object readlock = new object();

#if DEBUG && LOG_PACKETS
        private StringBuilder _log = new StringBuilder();
        private Stopwatch _positionStopwatch = new Stopwatch();
        private TimeSpan _lastPositionTimeSpan;
#endif

#if DEBUG && LOG_SEQUENCE_NUMBER
        private StringBuilder _sequenceLog = new StringBuilder("-----------------------\n");
#endif

        private byte[] _readBuffer = new byte[263];

        /// <summary>
        /// 异步读取数据包。// 林俊清, 20151029, 去掉了锁，不要在除读数据包循环之外的地方调用此方法。
        /// </summary>
        /// <returns>表示此异步操作的 <see cref="Task{TResult}"/> 实例，其结果为读取到的数据包。</returns>
        private async Task<byte[]> ReadPacketAsync()
        {
            if (!Connection.IsOpen)
            {
                return null;
            }

            int length = 0;
            int readnumber = 0;  //_recnumber;
            try
            {
                readnumber = await Connection.ReadAsync(_readBuffer, 0, 1);
                _recnumber += readnumber;
                if (readnumber == 0)
                {
                    return null;
                }
                if (_readBuffer[0] == MAVLink.MAVLINK_STX)
                {
                    readnumber = await Connection.ReadAsync(_readBuffer, 1, 5);
                    _recnumber += readnumber;
                    if (readnumber == 0)
                    {
                        return null;
                    }
                    
#if DEBUG && LOG_SEQUENCE_NUMBER
                    // _sequenceLog.Append(DateTime.Now.ToString("HHmmss.fff")).Append('\t').Append(string.Format("{0,3}", _readBuffer[2].ToString())).Append('\t').Append(string.Format("{0,3}", _readBuffer[5].ToString()));
                    _sequenceLog.Append(DateTime.Now.ToString()).Append('\t').Append(string.Format("{0,3}", _readBuffer[2].ToString())).Append('\t').Append(string.Format("{0,3}", _readBuffer[5].ToString()));
#endif
                    // packet length
                    length = 6 + _readBuffer[1] + 2; // header + data + checksum
                    readnumber = await Connection.ReadAsync(_readBuffer, 6, length - 6);
                    _recnumber += readnumber;
                    if (readnumber == 0)
                    {
                        return null;
                    }
                }
                else
                {
#if DEBUG && LOG_PACKETS
                    _log.AppendLine("---------------");
                    _log.AppendLine("unexpected head: " + _readBuffer[0]);
#endif

                    return null;
                }
            }
            catch
            {
                return null;
            }

            uint crcvalue = BitConverter.ToUInt16(_readBuffer, length - 2);

#if DEBUG && LOG_PACKETS
            _log.AppendLine("---------------");
            for (int i = 0; i < 6; i++)
            {
                _log.Append(_readBuffer[i]).Append(" ");
            }
            _log.AppendLine();
            for (int i = 6; i < length - 2; i++)
            {
                _log.Append(_readBuffer[i]).Append(" ");
            }
            _log.AppendLine();
            for (int i = length - 2; i < length; i++)
            {
                _log.Append(_readBuffer[i]).Append(" ");
            }
            _log.AppendLine();
#endif
            byte messageType = _readBuffer[5];

#if DEBUG && LOG_PACKETS
            if (messageType == 33)
            {
                if (_positionStopwatch.IsRunning)
                {
                    var elapsed = _positionStopwatch.Elapsed;
                    Debug.WriteLine((elapsed - _lastPositionTimeSpan).TotalMilliseconds, "-----------------------------------------------position packet");
                    _lastPositionTimeSpan = elapsed;
                }
                else
                {
                    _positionStopwatch.Start();
                }
            }
#endif

            ushort checksum = MavlinkCRC.crc_calculate(_readBuffer, length - 2);
            checksum = MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[messageType], checksum);
            iDataCount += 1;

#if DEBUG && LOG_SEQUENCE_NUMBER
            _sequenceLog.Append('\t').Append(crcvalue == checksum).AppendLine();
#endif

            if (crcvalue == checksum)
            {
                return _readBuffer;
            }
            else
            {
#if DEBUG && LOG_PACKETS
                _log.AppendLine("crc not passed");
#endif

                iErrorData += 1;
                var handler = ErrorDataEvent;
                if (handler != null)
                {
                    RunOnUIThread(() => handler(iErrorData, iDataCount, _readBuffer[5]));
                }
                return null;
            }
        }

        public MAVState MAV = new MAVState();

        /// <summary>
        /// Used to extract mission from log file - both sent or received
        /// </summary>
        /// <param name="buffer">packet</param>
        void getWPsfromstream(ref byte[] buffer)
        {
            if (buffer[5] == (byte)MAVLink.MAVLINK_MSG_ID_MISSION_COUNT)
            {
                // clear old
                MAV.wps.Clear();
            }

            if (buffer[5] == (byte)MAVLink.MAVLINK_MSG_ID_MISSION_ITEM)
            {
                MAVLink.mavlink_mission_item_t wp = buffer.ByteArrayToStructure<MAVLink.mavlink_mission_item_t>(6);

                if (wp.current == 2)
                {
                    // guide mode wp
                    MAV.GuidedMode = wp;
                }
                else
                {
                    MAV.wps[wp.seq] = wp;
                }

                //Console.WriteLine("WP # {7} cmd {8} p1 {0} p2 {1} p3 {2} p4 {3} x {4} y {5} z {6}", wp.param1, wp.param2, wp.param3, wp.param4, wp.x, wp.y, wp.z, wp.seq, wp.command);
            }

            if (buffer[5] == (byte)MAVLink.MAVLINK_MSG_ID_RALLY_POINT)
            {
                MAVLink.mavlink_rally_point_t rallypt = buffer.ByteArrayToStructure<MAVLink.mavlink_rally_point_t>(6);

                MAV.rallypoints[rallypt.idx] = rallypt;

                //Console.WriteLine("RP # {0} {1} {2} {3} {4}", rallypt.idx, rallypt.lat, rallypt.lng, rallypt.alt, rallypt.break_alt);
            }

            if (buffer[5] == (byte)MAVLink.MAVLINK_MSG_ID_FENCE_POINT)
            {
                MAVLink.mavlink_fence_point_t fencept = buffer.ByteArrayToStructure<MAVLink.mavlink_fence_point_t>(6);

                MAV.fencepoints[fencept.idx] = fencept;
            }
        }


        /// <summary>
        /// used for outbound packet sending
        /// </summary>
        public int packetcount = 0;
        private bool SendReq;
#pragma warning disable CS0414 // The field is assigned but its value is never used
        private bool giveComport;
#pragma warning restore CS0414 // The field is assigned but its value is never used

        public async Task GeneratePacketAsync<TMavlinkPacket>(byte messageType, TMavlinkPacket indata)
        {
            byte[] data;

            if (mavlinkversion == 3)
            {
                data = MavlinkUtil.StructureToByteArray(indata);
            }
            else
            {
                data = MavlinkUtil.StructureToByteArrayBigEndian(indata);
            }
           // data = MavlinkUtil.StructureToByteArrayBigEndian(indata);
            //Console.WriteLine(DateTime.Now + " PC Doing req "+ messageType + " " + this.BytesToRead);
            byte[] packet = new byte[data.Length + 6 + 2];

            if (mavlinkversion == 3 || mavlinkversion == 0)
            {
                packet[0] = MAVLink.MAVLINK_STX;
            }
            else if (mavlinkversion == 2)
            {
                packet[0] = (byte)'U';
            }
            packet[1] = (byte)(data.Length);
            packet[2] = (byte)packetcount;
            //packet[2] = 35;
            packetcount++;

            packet[3] = 255; // this is always 255 - MYGCS

            packet[4] = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER;

            packet[5] = messageType;

            int i = 6;
            foreach (byte b in data)
            {
                packet[i] = b;
                i++;
            }

            ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6);

            if (mavlinkversion == 3 || mavlinkversion == 0)
            {
                checksum = MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[messageType], checksum);
            }

            byte ck_a = (byte)(checksum & 0xFF); ///< High byte
            byte ck_b = (byte)(checksum >> 8); ///< Low byte

            packet[i] = ck_a;
            i += 1;
            packet[i] = ck_b;
            i += 1;

            if (IsConnected)
            {
                try
                {
                    await Connection.WriteAsync(packet, 0, i);
                   // Console.WriteLine("sendout:" + i);
                    _sendnumber += i; //统计发送数量
                }
                catch
                {
                    // 林俊清, 20151031, 异常已通过 IConnection.ExceptionThrown 事件传到外部，此处吞掉就行了。
                }
            }
        }


        public async Task GenerateRTKPacketAsync<TMavlinkPacket>(byte messageType, TMavlinkPacket indata,int rtklength)
        {
            byte[] data;
            int rtkdataleng= rtklength + 3;
            if (mavlinkversion == 3)
            {
                data = MavlinkUtil.StructureToByteArray(indata);
            }
            else
            {
                data = MavlinkUtil.StructureToByteArrayBigEndian(indata);
            }
            // data = MavlinkUtil.StructureToByteArrayBigEndian(indata);
            //Console.WriteLine(DateTime.Now + " PC Doing req "+ messageType + " " + this.BytesToRead);
 
            byte[] packet = new byte[rtkdataleng + 6 + 2];

            if (mavlinkversion == 3 || mavlinkversion == 0)
            {
                packet[0] = MAVLink.MAVLINK_STX;
            }
            else if (mavlinkversion == 2)
            {
                packet[0] = (byte)'U';
            }
            packet[1] = (byte)(rtkdataleng);
            packet[2] = (byte)packetcount;
            //packet[2] = 35;
            packetcount++;

            packet[3] = 255; // this is always 255 - MYGCS

            packet[4] = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER;

            packet[5] = messageType;

            int i = 6;
            foreach (byte b in data)
            {
                if (i < rtkdataleng + 6 )
                    packet[i] = b;
                else break;
                i++;
            }

            ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6);

            if (mavlinkversion == 3 || mavlinkversion == 0)
            {
                checksum = MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[messageType], checksum);
            }

            byte ck_a = (byte)(checksum & 0xFF); ///< High byte
            byte ck_b = (byte)(checksum >> 8); ///< Low byte

            packet[i] = ck_a;
            i += 1;
            packet[i] = ck_b;
            i += 1;

            if (IsConnected)
            {
                try
                {
                    await Connection.WriteAsync(packet, 0, i);
                    //Console.WriteLine("sendout:" + i);
                    _sendnumber += i; //统计发送数量
                }
                catch
                {
                    // 林俊清, 20151031, 异常已通过 IConnection.ExceptionThrown 事件传到外部，此处吞掉就行了。
                }
            }
        }

        public async Task<bool> DoLEDAsync()
        {
            return await DoLEDCommandAsync();
        }

        public async Task<bool> DoLEDCommandAsync()
        {
            giveComport = true;
            MAVLink.mavlink_msg_id_led_control led = new MAVLink.mavlink_msg_id_led_control();
            led.target_system = sysid;
            led.target_component = compid;//(byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL;;
            led.instance = 0;
            led.pattern = 0;
            led.custom_len = 4;
            led.custom_bytes = new byte[24];
            led.custom_bytes[0] = 255;
            led.custom_bytes[1] = 255;
            led.custom_bytes[2] = 255;
            led.custom_bytes[3] = 2;

            await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_LED_CONTROL, led);
            return true;
        }

        public async Task<bool> DoARMAsync(bool armit)
        {
            return await DoCommandAsync(MAVLink.MAV_CMD.COMPONENT_ARM_DISARM, armit ? 1 : 0, 21196, 0, 0, 0, 0, 0);
        }

        public async Task<bool> DoMotorTestAsync(int motor, MAVLink.MOTOR_TEST_THROTTLE_TYPE thr_type, int throttle, int timeout)
        {
            return await DoCommandAsync(MAVLink.MAV_CMD.DO_MOTOR_TEST, (float)motor, (float)(byte)thr_type, (float)throttle, (float)timeout, 0, 0, 0);
        }

        public async Task<bool> DoTakeoffAsync()
        {
            ///<summary> Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude|  </summary>
            return await DoCommandAsync(MAVLink.MAV_CMD.TAKEOFF, 0, 0, 0, -1, 0, 0, 15);
        }
        public async Task<bool> DoMissionStartAsync(int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat)
        {
            ///<summary> Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude|  </summary>
            return await DoCommandAsync(MAVLink.MAV_CMD.MISSION_START, hour_utc, minute_utc, second_utc, (float)Missionlng, (float)Missionlat, 0, 0);
        }
        public async Task<bool> DoCommandAsync(MAVLink.MAV_CMD actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7)
        {
            giveComport = true;
            //byte[] buffer;

            MAVLink.mavlink_command_long_t req = new MAVLink.mavlink_command_long_t();

            req.target_system = sysid;
            req.target_component = compid;

            if (actionid == MAVLink.MAV_CMD.COMPONENT_ARM_DISARM)
            {
                req.target_component = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL;
            }

            req.command = (ushort)actionid;

           
            Console.WriteLine("P1 = "  + p1);
            req.param1 = p1;
            req.param2 = p2;
            req.param3 = p3;
            req.param4 = p4;
            req.param5 = p5;
            req.param6 = p6;
            req.param7 = p7;

            await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_COMMAND_LONG, req);
            return true;

            //DateTime start = DateTime.Now;
            //int retrys = 3;

            //int timeout = 2000;

            ////// imu calib take a little while
            //if (actionid == MAVLink.MAV_CMD.PREFLIGHT_CALIBRATION && p5 == 1)
            //{
            //    // this is for advanced accel offsets, and blocks execution
            //    return true;
            //}
            //else if (actionid == MAVLink.MAV_CMD.PREFLIGHT_CALIBRATION)
            //{
            //    retrys = 1;
            //    timeout = 25000;
            //}
            //else if (actionid == MAVLink.MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN)
            //{
            //    await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_COMMAND_LONG, req);
            //    giveComport = false;
            //    return true;
            //}
            //else if (actionid == MAVLink.MAV_CMD.COMPONENT_ARM_DISARM)
            //{
            //    // 10 seconds as may need an imu calib
            //    timeout = 10000;
            //}

            //while (true)
            //{
            //    if (!(start.AddMilliseconds(timeout) > DateTime.Now))
            //    {
            //        if (retrys > 0)
            //        {
            //            //log.Info("doAction Retry " + retrys);
            //            await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_COMMAND_LONG, req);
            //            start = DateTime.Now;
            //            retrys--;
            //            continue;
            //        }
            //        giveComport = false;
            //        throw new Exception("Timeout on read - doAction");
            //    }

            //    buffer = readPacket();
            //    if (buffer.Length > 5)
            //    {
            //        if (buffer[5] == MAVLink.MAVLINK_MSG_ID_COMMAND_ACK)
            //        {
            //            var ack = buffer.ByteArrayToStructure<MAVLink.mavlink_command_ack_t>(6);
            //            if (ack.result == (byte)MAVLink.MAV_RESULT.ACCEPTED)
            //            {
            //                giveComport = false;
            //                return true;
            //            }
            //            else
            //            {
            //                giveComport = false;
            //                return false;
            //            }
            //        }
            //    }
            //}
        }


        public double GetBearing(float lat1,float lng1,float lat2,float lng2)
        {
            var latitude1 = Math.PI / 180*lat1;
            var latitude2 = Math.PI / 180 * lat2;
            var longitudeDifference = Math.PI / 180*(lng2 - lng1);

            var y = Math.Sin(longitudeDifference) * Math.Cos(latitude2);
            var x = Math.Cos(latitude1) * Math.Sin(latitude2) - Math.Sin(latitude1) * Math.Cos(latitude2) * Math.Cos(longitudeDifference);

            return (180 / Math.PI*(Math.Atan2(y, x)) + 360) % 360;
        }


        public async Task SetGuidedModeWPAsync(Locationwp gotohere)
        {
            //if (gotohere.alt == 0 || gotohere.lat == 0 || gotohere.lng == 0)
            //    return;

            //giveComport = true;

            //try
            //{
            //gotohere.id = (byte)MAVLink.MAV_CMD.WAYPOINT;
            //老方法无法设置速度
            //  await SetGuidedWPAsync(gotohere, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte)2);
            //}
            //catch  {  }

            //giveComport = false;


            //老方法无法设置速度
            await SetWPAsync(gotohere, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte)2);
            
           /* 
           TaskTools.HIL.Vector3 targetVelocity = new TaskTools.HIL.Vector3();
           float targspeed = (float)2.0; //目标速度
            double targetbearing= GetBearing((float)lat, (float)lng, (float)gotohere.lat, (float)gotohere.lng);  //目标机头方向
            targetVelocity.x = Math.Cos(targetbearing * 180 / Math.PI) * targspeed;   //分解速度
            targetVelocity.y = Math.Sin(targetbearing * 180 / Math.PI) *  targspeed;  //分解速度
           // await setPositionTargetGlobalInt(true, false, false, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT,
            //    gotohere.lat, gotohere.lng, gotohere.alt, targetVelocity.x, targetVelocity.y, 2.0);
           
             await setPositionTargetGlobalInt(true, false, false, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT,
                gotohere.lat, gotohere.lng, gotohere.alt, 0, 0, 0);
                */
           

        }




        public async Task setPositionTargetGlobalInt( bool pos, bool vel, bool acc, MAVLink.MAV_FRAME frame, double lat, double lng, double alt, double vx, double vy, double vz)
        {
           MAVLink.mavlink_set_position_target_global_int_t target = new MAVLink.mavlink_set_position_target_global_int_t()
            {
                target_system = sysid,
                target_component = compid,
                alt = (float)alt,
                lat_int = (int)(lat * 1e7),
                lon_int = (int)(lng * 1e7),
                coordinate_frame = (byte)frame,
                vx = (float)vx,
                vy = (float)vy,
                vz = (float)vz
            };

            target.type_mask = 7 + 56 + 448;

            if (pos)
                target.type_mask -= 7;
            if (vel)
                target.type_mask -= 56;
            if (acc)
                target.type_mask -= 448;

            await GeneratePacketAsync(MAVLink.SET_POSITION_TARGET_GLOBAL_INT, target);

        }


        public async Task SetNewWPAltAsync(Locationwp gotohere)
        {
            //givecomport = true;

            //try
            //{
            gotohere.id = (byte)MAVLink.MAV_CMD.WAYPOINT;

            //MAVLink.MAV_MISSION_RESULT ans = 
            await SetWPAsync(gotohere, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte)3);

            //    if (ans != mavlink.mav_mission_result.mav_mission_accepted)
            //        throw new exception("alt change failed");
            //}
            //catch { givecomport = false; /*log.error(ex);*/ throw; }

            //givecomport = false;
        }

        public async Task SetWPAsync(Locationwp loc, ushort index, MAVLink.MAV_FRAME frame, byte current = 0)
        {
            giveComport = true;
            MAVLink.mavlink_mission_item_t req = new MAVLink.mavlink_mission_item_t();

            req.target_system = sysid;
            req.target_component = compid; // MAVLINK_MSG_ID_MISSION_ITEM

            req.command = loc.id;


            req.current = current;

            req.frame = (byte)frame;

            req.x = (float)(loc.lat);
            req.y = (float)(loc.lng);
            req.z = (float)(loc.alt);

            req.param1 = loc.p1;
            req.param2 = loc.p2;
            req.param3 = loc.p3;
            req.param4 = loc.p4;

            req.seq = index;

            //log.InfoFormat("setWP {6} frame {0} cmd {1} p1 {2} x {3} y {4} z {5}", req.frame, req.command, req.param1, req.x, req.y, req.z, index);

            // request
            await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_MISSION_ITEM, req);
        }

        public async Task SetChannelsAsync(ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null, ushort? ch5 = null, ushort? ch6 = null, ushort? ch7 = null, ushort? ch8 = null)
        {
            //某些情况下ch4设置成0会导致自旋
            ushort checkyaw = ch4 ?? ch4in;
            if ((checkyaw < 1200)|| (checkyaw > 1800))
                checkyaw = 1500;
            ////
            MAVLink.mavlink_rc_channels_override_t rc = new MAVLink.mavlink_rc_channels_override_t();
            rc.target_component = compid;
            rc.target_system = sysid;
            rc.chan1_raw = ch1 ?? ch1in;
            rc.chan2_raw = ch2 ?? ch2in;
            rc.chan3_raw = ch3 ?? ch3in;
            rc.chan4_raw = checkyaw;
            rc.chan5_raw = ch5 ?? ch5in;
            rc.chan6_raw = ch6 ?? ch6in;
            rc.chan7_raw = ch7 ?? ch7in;
            rc.chan8_raw = ch8 ?? ch8in;
            await SendPacketAsync(rc);
        }

        public async Task SetMobileControlAsync(ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null, ushort? ch5 = null, ushort? ch6 = null, ushort? ch7 = null, ushort? ch8 = null, float? yaw = null, float? alt = null)
        {
            /*       MAVLink.mavlink_rc_mobile_control_t rc = new MAVLink.mavlink_rc_mobile_control_t();
                   rc.target_component = compid;
                   rc.target_system = sysid;
                   rc.chan1_raw = ch1 ?? ch1in;
                   rc.chan2_raw = ch2 ?? ch2in;
                   rc.chan3_raw = ch3 ?? ch3in;
                   rc.chan4_raw = ch4 ?? ch4in;
                   rc.chan5_raw = ch5 ?? ch5in;
                   rc.chan6_raw = ch6 ?? ch6in;
                   rc.chan7_raw = ch7 ?? ch7in;
                   rc.chan8_raw = ch8 ?? ch8in;
                   rc.Yaw = yaw ?? this.yaw;
                   rc.alt = alt ?? this.alt;
                   await SendPacketAsync(rc);
                   */
            float ch4yaw;
            ch4yaw = yaw ?? this.yaw;
            if (ch4yaw != this.yaw)
            {
                ch4yaw = ch4yaw % 360;
                await DoCommandAsync(MAVLink.MAV_CMD.CONDITION_YAW, ch4yaw, 0, 1, 0, 0, 0, 0);
            }

         /*
            float ch4yaw;
            MAVLink.mavlink_rc_channels_override_t rc = new MAVLink.mavlink_rc_channels_override_t();
            rc.target_component = compid;
            rc.target_system = sysid;
            rc.chan1_raw = ch1 ?? ch1in;
            rc.chan2_raw = ch2 ?? ch2in;
            rc.chan3_raw = ch3 ?? ch3in;
            rc.chan4_raw = 1500;
            rc.chan5_raw = ch5 ?? ch5in;
            rc.chan6_raw = ch6 ?? ch6in;
            rc.chan7_raw = ch7 ?? ch7in;
            rc.chan8_raw = ch8 ?? ch8in;
            ch4yaw= yaw ?? this.yaw;
            rc.chan4_raw =(ushort)ch4yaw;
            await SendPacketAsync(rc);
            */

        }

        public async Task<float> GetParamAsync(string paramName, int millisecondsTimeout = Timeout.Infinite)
        {
            if (param.ContainsKey(paramName)) param[paramName] = null;
            var paramId = Encoding.ASCII.GetBytes(paramName);
            Array.Resize(ref paramId, 16);
            var req = new MAVLink.mavlink_param_request_read_t
            {
                param_id = paramId,
                param_index = -1,
                target_component = compid,
                target_system = sysid
            };
            var stopwatch = Stopwatch.StartNew();
            while (true)
            {
                for (int ii = 0; ii < 5; ii++)
                {
                    await SendPacketAsync(req);
                    await Task.Delay(10).ConfigureAwait(false);
                }

                int i = 0;
                for (; i < 5 && (!param.ContainsKey(paramName) || param[paramName] == null); i++)
                {
                    await Task.Delay(50).ConfigureAwait(false);
                }
                if (i < 5)
                {
                    stopwatch.Stop();
                    break;
                }
                else if (millisecondsTimeout != Timeout.Infinite && stopwatch.ElapsedMilliseconds >= millisecondsTimeout)
                {
                    stopwatch.Stop();
                    throw new TimeoutException("The GetParamAsync operation has timed out.");
                }
            }
            return param[paramName].Value;
        }

        public async Task SendPacketAsync<TMavlinkPacket>(TMavlinkPacket indata)
        {
            bool validPacket = false;
            byte a = 0;
            var packetType = indata.GetType();
            foreach (Type ty in MAVLink.MAVLINK_MESSAGE_INFO)
            {
                if (ty == packetType)
                {
                    validPacket = true;
                    await GeneratePacketAsync(a, indata);
                    return;
                }
                a++;
            }
            if (!validPacket)
            {
                RaiseMessageCreated("The packet type is not valid.");
                //log.Info("Mavlink : NOT VALID PACKET await SendPacketAsync() " + indata.GetType().ToString());
            }
        }

        public double[] packetspersecond { get; set; }
        DateTime[] packetspersecondbuild = new DateTime[256];

        byte ratesensors = 1;
        public byte RateSensors { get { return ratesensors; } set { ratesensors = value; } }

        private DateTime heatbeatSend;
        private bool bInitChannel = false;
        private bool bGPSBadHealth;
        private bool bGyroBadHealth;
        private bool bAccel;
        private bool bCompass;
        private bool bBarometer;

        public short gx;
        public short gy;
        public short gz;
        public short ax;
        public short ay;
        public short az;
        public short mx;
        public short my;
        public short mz;

        public short gx2;
        public short gy2;
        public short gz2;
        public short ax2;
        public short ay2;
        public short az2;
        public short mx2;
        public short my2;
        public short mz2;

        public float mag_ofs_x;
        public float mag_ofs_y;
        public float mag_ofs_z;
        public float mag_declination;
        public int raw_press;
        public int raw_temp;
        public float gyro_cal_x;
        public float gyro_cal_y;
        public float gyro_cal_z;
        public float accel_cal_x;
        public float accel_cal_y;
        public float accel_cal_z;

        public ushort param_total;
        public ushort WpCount;

        public async Task RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM id, byte hzrate)
        {

            double pps = 0;

            switch (id)
            {
                case MAVLink.MAV_DATA_STREAM.ALL:

                    break;
                case MAVLink.MAV_DATA_STREAM.EXTENDED_STATUS:
                    if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_SYS_STATUS] < DateTime.Now.AddSeconds(-2))
                        break;
                    pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_SYS_STATUS];
                    if (hzratecheck(pps, hzrate))
                    {
                        return;
                    }
                    break;
                case MAVLink.MAV_DATA_STREAM.EXTRA1:
                    if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_ATTITUDE] < DateTime.Now.AddSeconds(-2))
                        break;
                    pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_ATTITUDE];
                    if (hzratecheck(pps, hzrate))
                    {
                        return;
                    }
                    break;
                case MAVLink.MAV_DATA_STREAM.EXTRA2:
                    if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_VFR_HUD] < DateTime.Now.AddSeconds(-2))
                        break;
                    pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_VFR_HUD];
                    if (hzratecheck(pps, hzrate))
                    {
                        return;
                    }
                    break;
                case MAVLink.MAV_DATA_STREAM.EXTRA3:
                    if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_AHRS] < DateTime.Now.AddSeconds(-2))
                        break;
                    pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_AHRS];
                    if (hzratecheck(pps, hzrate))
                    {
                        return;
                    }
                    break;
                case MAVLink.MAV_DATA_STREAM.POSITION:
                    if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT] < DateTime.Now.AddSeconds(-2))
                        break;
                    pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT];
                    if (hzratecheck(pps, hzrate))
                    {
                        return;
                    }
                    break;
                case MAVLink.MAV_DATA_STREAM.RAW_CONTROLLER:
                    if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED] < DateTime.Now.AddSeconds(-2))
                        break;
                    pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED];
                    if (hzratecheck(pps, hzrate))
                    {
                        return;
                    }
                    break;
                case MAVLink.MAV_DATA_STREAM.RAW_SENSORS:
                    if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_RAW_IMU] < DateTime.Now.AddSeconds(-2))
                        break;
                    pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_RAW_IMU];
                    if (hzratecheck(pps, hzrate))
                    {
                        return;
                    }
                    break;
                case MAVLink.MAV_DATA_STREAM.RC_CHANNELS:
                    if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW] < DateTime.Now.AddSeconds(-2))
                        break;
                    pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW];
                    if (hzratecheck(pps, hzrate))
                    {
                        return;
                    }
                    break;
            }

            //packetspersecond[temp[5]];

            if (pps == 0 && hzrate == 0)
            {
                return;
            }

           // log.InfoFormat("Request stream {0} at {1} hz", Enum.Parse(typeof(MAV_DATA_STREAM), id.ToString()), hzrate);
            await GetDatastreamAsync(id, hzrate);
        }

        public async Task StopDatastreamAsync(MAVLink.MAV_DATA_STREAM id, byte hzrate)
        {

            double pps = 0;

            switch (id)
            {
                case MAVLink.MAV_DATA_STREAM.ALL:

                    break;
                case MAVLink.MAV_DATA_STREAM.EXTENDED_STATUS:
                    if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_SYS_STATUS] < DateTime.Now.AddSeconds(-2))
                        break;
                    pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_SYS_STATUS];
                    if (hzratecheck(pps, hzrate))
                    {
                        return;
                    }
                    break;
                case MAVLink.MAV_DATA_STREAM.EXTRA1:
                    if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_ATTITUDE] < DateTime.Now.AddSeconds(-2))
                        break;
                    pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_ATTITUDE];
                    if (hzratecheck(pps, hzrate))
                    {
                        return;
                    }
                    break;
                case MAVLink.MAV_DATA_STREAM.EXTRA2:
                    if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_VFR_HUD] < DateTime.Now.AddSeconds(-2))
                        break;
                    pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_VFR_HUD];
                    if (hzratecheck(pps, hzrate))
                    {
                        return;
                    }
                    break;
                case MAVLink.MAV_DATA_STREAM.EXTRA3:
                    if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_AHRS] < DateTime.Now.AddSeconds(-2))
                        break;
                    pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_AHRS];
                    if (hzratecheck(pps, hzrate))
                    {
                        return;
                    }
                    break;
                case MAVLink.MAV_DATA_STREAM.POSITION:
                    if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT] < DateTime.Now.AddSeconds(-2))
                        break;
                    pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT];
                    if (hzratecheck(pps, hzrate))
                    {
                        return;
                    }
                    break;
                case MAVLink.MAV_DATA_STREAM.RAW_CONTROLLER:
                    if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED] < DateTime.Now.AddSeconds(-2))
                        break;
                    pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED];
                    if (hzratecheck(pps, hzrate))
                    {
                        return;
                    }
                    break;
                case MAVLink.MAV_DATA_STREAM.RAW_SENSORS:
                    if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_RAW_IMU] < DateTime.Now.AddSeconds(-2))
                        break;
                    pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_RAW_IMU];
                    if (hzratecheck(pps, hzrate))
                    {
                        return;
                    }
                    break;
                //case MAVLink.MAV_DATA_STREAM.SCALED_IMU2:

                //    break;
                case MAVLink.MAV_DATA_STREAM.RC_CHANNELS:
                    if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW] < DateTime.Now.AddSeconds(-2))
                        break;
                    pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW];
                    if (hzratecheck(pps, hzrate))
                    {
                        return;
                    }
                    break;
            }
            //log.InfoFormat("Request stream {0} at {1} hz", Enum.Parse(typeof(MAV_DATA_STREAM), id.ToString()), hzrate);
            await StopGetDatastreamAsync(id, hzrate);
        }

        bool hzratecheck(double pps, int hzrate)
        {

            if (hzrate == 0 && pps == 0)
            {
                return true;
            }
            else if (hzrate == 1 && pps >= 0.5 && pps <= 2)
            {
                return true;
            }
            else if (hzrate == 3 && pps >= 2 && hzrate < 5)
            {
                return true;
            }
            else if (hzrate == 10 && pps > 5 && hzrate < 15)
            {
                return true;
            }
            else if (hzrate > 15 && pps > 15)
            {
                return true;
            }

            return false;

        }

        async Task GetDatastreamAsync(MAVLink.MAV_DATA_STREAM id, byte hzrate)
        {
            MAVLink.mavlink_request_data_stream_t req = new MAVLink.mavlink_request_data_stream_t();
            req.target_system = sysid;
            req.target_component = compid;

            req.req_message_rate = hzrate;
            req.start_stop = 1; // start
            req.req_stream_id = (byte)id; // id


            // send each one twice.
            await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req);
            await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req);
        }
        public void GetCommunicationNumber(out int recnumber, out int sendnumber)
        {
            recnumber=_recnumber;
            sendnumber = _sendnumber;
        }


        //重设数据量
        public void ResetCommunicationNumber()
        {
            _recnumber = 0;
            _sendnumber = 0;
        }
        async Task StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM id, byte hzrate)
        {
            MAVLink.mavlink_request_data_stream_t req = new MAVLink.mavlink_request_data_stream_t();
            req.target_system = sysid;
            req.target_component = compid;

            req.req_message_rate = 0;
            req.start_stop = 0; // start
            req.req_stream_id = (byte)id; // id

            // send each one twice.
            await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req);
            await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req);
        }

        public async Task SetParamAsync(string paramname, float value)
        {
            // param type is set here, however it is always sent over the air as a float 100int = 100f.
            var req = new MAVLink.mavlink_param_set_t { target_system = sysid, target_component = compid, param_type = (byte)param_types[paramname] };

            byte[] temp = Encoding.GetEncoding("ASCII").GetBytes(paramname);

            Array.Resize(ref temp, 16);
            req.param_id = temp;
            req.param_value = (value);

            await GeneratePacketAsync((byte)MAVLink.MAVLINK_MSG_ID_PARAM_SET, req);
        }

        public async Task SetParam2Async(string paramname, float value)
        {
            // param type is set here, however it is always sent over the air as a float 100int = 100f.
            var req = new MAVLink.mavlink_param_set_t { target_system = sysid, target_component = compid, param_type = 4 };

            byte[] temp = Encoding.GetEncoding("ASCII").GetBytes(paramname);

            Array.Resize(ref temp, 16);
            req.param_id = temp;
            req.param_value = (value);

            await GeneratePacketAsync((byte)MAVLink.MAVLINK_MSG_ID_PARAM_SET, req);
        }

        public async Task SetModeAsync(ac2modes modein)
        {
            try
            {
                MAVLink.mavlink_set_mode_t mode = new MAVLink.mavlink_set_mode_t();

                if (TranslateMode(modein, ref mode))
                {
                    await SetModeAsync(mode);
                }
            }
            catch { RaiseMessageCreated("Failed to change Modes"); }
        }

        public async Task SetModeAsync(string modein)
        {
            try
            {
                MAVLink.mavlink_set_mode_t mode = new MAVLink.mavlink_set_mode_t();

                if (TranslateMode(modein, ref mode))
                {
                    await SetModeAsync(mode);
                }
            }
            catch { RaiseMessageCreated("Failed to change Modes"); }
        }

        public async Task SetModeAsync(MAVLink.mavlink_set_mode_t mode, MAVLink.MAV_MODE_FLAG base_mode = 0)
        {
            mode.base_mode |= (byte)base_mode;

            await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_SET_MODE, mode);
            await Task.Delay(10);
            await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_SET_MODE, mode);
        }




        public async Task InjectGpsDataAsync(byte[] data, ushort length)
        {
            MAVLink.mavlink_gps_inject_data_t gps = new MAVLink.mavlink_gps_inject_data_t();
            var msglen = 110;
            int datalen = 0;
            var len = (length % msglen) == 0 ? length / msglen : (length / msglen) + 1;
            for (int a = 0; a < len; a++)
            {
                datalen = Math.Min(length - a * msglen, msglen);
                gps.data = new byte[msglen];
                Array.Copy(data, a * msglen, gps.data, 0, datalen);
                gps.len = (byte)datalen;
                gps.target_component = compid;
                gps.target_system = sysid;
                await GenerateRTKPacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_INJECT_DATA, gps, gps.len);
               // Console.WriteLine("send:" + (ushort)gps.len);
            }
        }

        public bool TranslateMode(ac2modes modein, ref MAVLink.mavlink_set_mode_t mode)
        {
            //MAVLink09.mavlink_set_mode_t mode = new MAVLink09.mavlink_set_mode_t();            
            mode.target_system = sysid;

            switch (modein)
            {
                case ac2modes.STABILIZE:
                case ac2modes.AUTO:
                case ac2modes.RTL:
                case ac2modes.LOITER:
                case ac2modes.ACRO:
                case ac2modes.ALT_HOLD:
                case ac2modes.CIRCLE:
                case ac2modes.POSITION:
                case ac2modes.LAND:
                case ac2modes.OF_LOITER:
                case ac2modes.GUIDED:       
                    mode.base_mode = (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED;
                    mode.custom_mode = (uint)modein;
                    break;
                default:
                    RaiseMessageCreated("错误模式: " + modein);
                    return false;
            }
            return true;
        }

        public bool TranslateMode(string modein, ref MAVLink.mavlink_set_mode_t mode)
        {
            //MAVLink09.mavlink_set_mode_t mode = new MAVLink09.mavlink_set_mode_t();            
            mode.target_system = sysid;

            var modeValue = EnumTranslator.GetValue<ac2modes>(modein.ToUpper());
            switch (modeValue)
            {
                case (int)ac2modes.STABILIZE:
                case (int)ac2modes.AUTO:
                case (int)ac2modes.RTL:
                case (int)ac2modes.LOITER:
                case (int)ac2modes.ACRO:
                case (int)ac2modes.ALT_HOLD:
                case (int)ac2modes.CIRCLE:
                case (int)ac2modes.POSITION:
                case (int)ac2modes.LAND:
                case (int)ac2modes.OF_LOITER:
                case (int)ac2modes.GUIDED:
                    mode.base_mode = (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED;
                    mode.custom_mode = (uint)modeValue;
                    break;
                default:
                    RaiseMessageCreated("错误模式: " + modein);
                    return false;
            }
            return true;
        }

        public async Task GetWPsAsync()
        {
            MAVLink.mavlink_mission_request_list_t req = new MAVLink.mavlink_mission_request_list_t();

            req.target_system = sysid;
            req.target_component = compid;

            await GeneratePacketAsync((byte)MAVLink.MAVLINK_MSG_ID_MISSION_REQUEST_LIST, req);
        }

        /// <summary>
        /// Gets specfied WP
        /// </summary>
        /// <param name="index"></param>
        /// <returns>WP</returns>
        public async Task GetWPAsync(ushort index)
        {
            giveComport = true;
            MAVLink.mavlink_mission_request_t req = new MAVLink.mavlink_mission_request_t();
            req.target_system = sysid;
            req.target_component = compid;
            req.seq = index;
            //Console.WriteLine("getwp req "+ DateTime.Now.Millisecond);
            // request
            await GeneratePacketAsync((byte)MAVLink.MAVLINK_MSG_ID_MISSION_REQUEST, req);
        }

        public async Task SetWPTotalAsync(ushort wp_total)
        {
            giveComport = true;
            MAVLink.mavlink_mission_count_t req = new MAVLink.mavlink_mission_count_t();

            req.target_system = sysid;
            req.target_component = compid; // MSG_NAMES.MISSION_COUNT

            req.count = wp_total;

            await GeneratePacketAsync((byte)MAVLink.MAVLINK_MSG_ID_MISSION_COUNT, req);
        }

        /// <param name="loc">任务信息。</param>
        /// <param name="index">任务编号。</param>
        /// <param name="frame">坐标系。</param>
        /// <param name="current">0：在 Auto 模式下飞往指定位置；2：切到 GUIDED 模式，立即飞往指定位置；3：仅改变高度，立即执行。</param>
        /// <param name="autocontinue">指示是否自动继续执行下一个任务。</param>
        public async Task WriteWPAsync(Locationwp loc, ushort index, MAVLink.MAV_FRAME frame, byte current = 0, byte autocontinue = 1)
        {
            MAVLink.mavlink_mission_item_t req = new MAVLink.mavlink_mission_item_t();

            req.target_system = sysid;
            req.target_component = compid; // MSG_NAMES.MISSION_ITEM

            req.command = loc.id;

            req.current = current;
            req.autocontinue = autocontinue;

            req.frame = (byte)frame;
            req.y = (float)(loc.lng);
            req.x = (float)(loc.lat);
            req.z = (float)(loc.alt);

            req.param1 = loc.p1;
            req.param2 = loc.p2;
            req.param3 = loc.p3;
            req.param4 = loc.p4;

            req.seq = index;

            //log.InfoFormat("setWP {6} frame {0} cmd {1} p1 {2} x {3} y {4} z {5}", req.frame, req.command, req.param1, req.x, req.y, req.z, index);

            // request
            await GeneratePacketAsync((byte)MAVLink.MAVLINK_MSG_ID_MISSION_ITEM, req);
        }

        const float rad2deg = (float)(180 / Math.PI);
        const float deg2rad = (float)(1.0 / rad2deg);

        public async Task<bool> SetSensorOffsetsAsync(sensoroffsetsenum sensor, float x, float y, float z)
        {
            return await DoCommandAsync(Plane.Protocols.MAVLink.MAV_CMD.PREFLIGHT_SET_SENSOR_OFFSETS, (int)sensor, x, y, z, 0, 0, 0);
        }

        public byte rssi;


        private void RaiseMessageCreated(string message)
        {
            var handler = MessageCreated;
            if (handler != null)
            {
                var e = new MessageCreatedEventArgs { Message = message };
                RunOnUIThread(() =>
                {
                    handler(this, e);
                });
            }
        }

        private void RunOnUIThread(Action action)
        {
            if (SynchronizationContext.Current == _uiSyncContext)
            {
                action();
            }
            else
            {
                _uiSyncContext.Post(delegate { action(); }, null);
            }
        }

        private void RaiseEventOnUIThread(EventHandler ev)
        {
            if (ev != null)
            {
                RunOnUIThread(() => ev(this, EventArgs.Empty));
            }
        }

        private void RaiseEventOnUIThread<TEventArgs>(EventHandler<TEventArgs> ev, TEventArgs e)
        {
            if (ev != null)
            {
                RunOnUIThread(() => ev(this, e));
            }
        }
    }
}