[fix] 退出异常调整测试灯光

详细描述

# 类型 包含:
# feat：新功能（feature）
# fix：修补bug
# docs：文档（documentation）
# style： 格式（不影响代码运行的变动）
# refactor：重构（即不是新增功能，也不是修改bug的代码变动）
# test：增加测试
# chore：构建过程或辅助工具的变动

PlaneGcsSdk.Contract_Private/Copters/ICopterActions.cs

@@ -11,7 +11,7 @@ namespace Plane.Copters
         Task CircleAsync();
 
         /// <summary>
-        /// // 林俊清, 20160122, 不明确。目测是发送重启命令，在刷固件之前用到。
+        /// // 王海, 20160122, 不明确。目测是发送重启命令，在刷固件之前用到。
         /// </summary>
         /// <param name="bootloaderMode"></param>
         /// <returns>表示此命令异步发送操作的 <see cref="Task"/> 实例。</returns>

PlaneGcsSdk.Contract_Private/PlaneGcsSdk.Contract_Private.csproj

@@ -34,15 +34,17 @@
     <WarningLevel>4</WarningLevel>
     <DocumentationFile>bin\Release\PlaneGcsSdk.Contract_Private.xml</DocumentationFile>
   </PropertyGroup>
-  <ItemGroup>
-    <!-- A reference to the entire .NET Framework is automatically included -->
-  </ItemGroup>
   <ItemGroup>
     <Compile Include="Copters\DataStreamReceivedEventArgs.cs" />
     <Compile Include="Copters\DataStreamType.cs" />
     <Compile Include="Copters\ICopterActions.cs" />
     <Compile Include="Properties\AssemblyInfo.cs" />
   </ItemGroup>
+  <ItemGroup>
+    <Reference Include="System.Drawing">
+      <HintPath>C:\Program Files (x86)\Reference Assemblies\Microsoft\Framework\.NETFramework\v4.8\System.Drawing.dll</HintPath>
+    </Reference>
+  </ItemGroup>
   <Import Project="..\PlaneGcsSdk.Contract_Shared\PlaneGcsSdk.Contract_Shared.projitems" Label="Shared" />
   <Import Project="$(MSBuildExtensionsPath32)\Microsoft\Portable\$(TargetFrameworkVersion)\Microsoft.Portable.CSharp.targets" />
   <!-- To modify your build process, add your task inside one of the targets below and uncomment it. 

PlaneGcsSdk.Contract_Shared/Communication/ConnectionEstablishedEventArgs.cs

@@ -13,5 +13,7 @@ namespace Plane.Communication
         public IConnection Connection { get; private set; }
 
         public string RemoteAddress { get; private set; }
+
+
     }
 }

PlaneGcsSdk.Contract_Shared/Copters/Constants.cs

@@ -23,5 +23,10 @@
         /// 最高速度，单位为 m/s。
         /// </summary>
         public const float MAX_VEL = 5f;
+
+        public const float MAX_VEL_XY = 5.0f;  //米/秒  水平速度
+        public const float MAX_VEL_UP = 2.5f;  //上升速度
+        public const float MAX_VEL_DOWN = 1.5f;  //下降速度
+
     }
 }

PlaneGcsSdk.Contract_Shared/Copters/CopterLocationType.cs

@@ -0,0 +1,8 @@
+namespace Plane.Copters
+{
+    public enum CopterLocationType
+    {
+        GPS,
+        RTK
+    }
+}

PlaneGcsSdk.Contract_Shared/Copters/FlightCommand.cs

@@ -33,6 +33,16 @@
         /// <summary>
         /// 命令飞行器起飞。
         /// </summary>
-        TakeOff = 22
+        TakeOff = 22,
+
+        /// <summary>
+        /// 命令飞行器改变速度
+        /// </summary>
+        ChangeSpeed = 178,
+
+        /// <summary>
+        /// 命令LED颜色改变
+        /// </summary>
+        LEDControl = 203
     }
 }

PlaneGcsSdk.Contract_Shared/Copters/ICopter.cs

@@ -2,7 +2,8 @@
 
 namespace Plane.Copters
 {
-    public partial interface ICopter : ICopterStatus, ICopterEvents, ICopterActions, ICopterCommunication, INotifyPropertyChanged
+    public partial interface ICopter : ICopterStatus, ICopterEvents, ICopterVirtualId, ICopterActions, ICopterCommunication, INotifyPropertyChanged, ICopterAttribute
     {
+       
     }
 }

PlaneGcsSdk.Contract_Shared/Copters/ICopterActions.cs

@@ -110,6 +110,10 @@ namespace Plane.Copters
         Task SetParamAsync(string paramName, float value, int millisecondsTimeout = Timeout.Infinite);
         bool GetShowLEDAsync();
 
+        void GetCommunicationNumber(out int recnumber, out int sendnumber);
+       //重设数据量
+        void ResetCommunicationNumber();
+
         Task SetShowLEDAsync(bool Ledon);
         Task SetShowLEDFlashAsync(int Flashtype, int millisecondsTime);
 
@@ -126,5 +130,32 @@ namespace Plane.Copters
         /// </summary>
         /// <returns>表示此命令异步发送操作的 <see cref="Task"/> 实例。</returns>
         Task StopPairingAsync();
+
+        /// <summary>
+        /// 发送命令
+        /// </summary>
+        /// <param name="actionid"></param>
+        /// <param name="p1"></param>
+        /// <param name="p2"></param>
+        /// <param name="p3"></param>
+        /// <param name="p4"></param>
+        /// <param name="p5"></param>
+        /// <param name="p6"></param>
+        /// <param name="p7"></param>
+        /// <returns></returns>
+        Task DoCommandAsync(int actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7);
+        /// <summary>
+        /// 发送命令数据包已生成
+        /// </summary>
+        /// <param name="actionid"></param>
+        /// <param name="data"></param>
+        /// <returns></returns>
+        Task DoCommandAckAsync(ushort command, byte result);
+        
+
+
+        //强制刷新位置
+        void RefreashLoc();
+
     }
 }

PlaneGcsSdk.Contract_Shared/Copters/ICopterActionsSharedByCopterManager.cs

@@ -31,7 +31,9 @@ namespace Plane.Copters
         /// <param name="lng">经度。</param>
         /// <param name="alt">相对于解锁点的高度。</param>
         /// <returns>表示此命令异步发送操作的 <see cref="Task"/> 实例。</returns>
-        Task FlyToAsync(double lat, double lng, float alt);
+        /// flytime =飞行时间 秒
+        ///  flytype   0;匀速  1;加减速  2;单加速  3;单减速
+        Task FlyToAsync(double lat, double lng, float alt,float flytime=0,int flytype = 0);
 
         /// <summary>
         /// 切换到 <see cref="CopterState.HoverMode"/> 并悬停。此操作需要使用 GPS 定位，卫星数不足导致定位不准时非常危险。
@@ -129,7 +131,11 @@ namespace Plane.Copters
 
         Task InjectGpsDataAsync(byte[] data, ushort length);
 
+        Task MissionStartAsync(int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat);
 
+        Task LEDAsync();
 
+        //电机转动
+        Task MotorTestAsync(int motor);
     }
 }

PlaneGcsSdk.Contract_Shared/Copters/ICopterAttribute.cs

@@ -0,0 +1,26 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace Plane.Copters
+{
+    /// <summary>
+    /// 定义用于设置飞行器部分可编辑的属性
+    /// </summary>
+    public partial interface ICopterAttribute
+    {
+        /// <summary>
+        /// 飞行器摆放时地面的相对高度
+        /// </summary>
+        float GroundAlt { get; set; }
+        float RetainInt{ get; }
+        //模拟飞行更新间隔
+        int sim_update_int { get; set; }
+        float maxspeed_xy { get; set; }
+        float maxspeed_up { get; set; }
+        float maxspeed_down { get; set; }
+        float acc_z { get; set; }
+        float acc_xy { get; set; }
+
+    }
+}

PlaneGcsSdk.Contract_Shared/Copters/ICopterStatus.cs

@@ -1,5 +1,6 @@
 using Plane.Geography;
 using System;
+using System.Drawing;
 
 namespace Plane.Copters
 {
@@ -173,14 +174,33 @@ namespace Plane.Copters
         /// </summary>
         float Voltage { get; }
 
-        //定位类型RTK,GPS
-        CopterLocationType LocationType { get; }
-
+        /// <summary>
+        /// 预留字节用于通信模块条件下的返回值
+        /// </summary>
+        byte[] Retain { get; }
 
         /// <summary>
-        /// MissionStatus=1表示正在飞向目标中，0标识达到目标
+        /// 飞机上通信模块版本
         /// </summary>
-        int MissionStatus { get; set; }
+        byte CommModuleVersion { get; }
+
+        /// <summary>
+        /// LED颜色
+        /// </summary>
+        string LEDColor { get; set; }
+        /// <summary>
+        /// LED灯光模式 0常亮 1闪烁 2随机闪烁(RGB无意义)
+        /// </summary>
+        int LEDMode { get; set; }
+        /// <summary>
+        /// LED变化间隔
+        /// </summary>
+        float LEDInterval { get; set; }
+
+        /// <summary>
+        /// LED显示颜色---内部计算后的真实颜色，用于显示
+        /// </summary>
+        Color LEDShowColor { get; set; }
 
     }
 }

PlaneGcsSdk.Contract_Shared/Copters/ICopterVirtualId.cs

@@ -0,0 +1,29 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace Plane.Copters
+{
+    /// <summary>
+    /// 定义用于获取飞行器虚拟ID(主要用于无序摆放飞机，自动按位置计算当前飞机ID)
+    /// </summary>
+    public interface ICopterVirtualId
+    {
+        /// <summary>
+        /// 获取自动排序生成的虚拟ID
+        /// </summary>
+        int VirtualId { get; set; }
+
+
+        /// <summary>
+        /// 是否展示虚拟ID: true:显示虚拟ID；false：显示Name
+        /// </summary>
+        bool DisplayVirtualId { get; set; }
+
+        /// <summary>
+        /// 是否展示ID: true:显示ID；false：显示Name
+        /// </summary>
+        bool DisplayID { get; set; }
+    }
+
+}

PlaneGcsSdk.Contract_Shared/Copters/IFakeCopter.cs

@@ -12,7 +12,7 @@ namespace Plane.Copters
             double? latitude = null,   //23.14973333,
             double? longitude = null,   //113.40974166,
             float? altitude = null,   //0,
-            string name = null,   //"林俊清的飞行器",
+            string name = null,   //"王海的飞行器",
             byte? batteryPer = null,   //10,
             short? heading = null,   //33,
             bool? isConnected = null,   //true,

PlaneGcsSdk.Contract_Shared/Copters/IMission.cs

@@ -49,5 +49,20 @@
         /// 获取或设置序号。
         /// </summary>
         ushort Sequence { get; set; }
-    }
+
+        /// <summary>
+        /// 获取或设置LED控制RGB的Red
+        /// </summary>
+        float R { get; set; }
+
+        /// <summary>
+        /// 获取或设置LED控制RGB的Green
+        /// </summary>
+        float G { get; set; }
+
+        /// <summary>
+        /// 获取或设置LED控制RGB的Blue
+        /// </summary>
+        float B { get; set; }
+}
 }

PlaneGcsSdk.Contract_Shared/Copters/IVisibleStatus.cs

@@ -10,6 +10,6 @@ namespace Plane.Copters
         /// <summary>
         /// 获取名称。
         /// </summary>
-        string Name { get; }
+        string Name { get; set; }
     }
 }

PlaneGcsSdk.Contract_Shared/Geography/GeographyUtils.cs

@@ -55,6 +55,45 @@ namespace Plane.Geography
         {
             return CalcDistance(l1.Latitude, l1.Longitude, l1.Altitude, l2.Latitude, l2.Longitude, l2.Altitude);
         }
+        /// <summary>
+        /// 计算空间中两点间的距离，单位为米。
+        /// </summary>
+        /// <param name="lat1">纬度 1。</param>
+        /// <param name="lng1">经度 1。</param>
+        /// <param name="lat2">纬度 2。</param>
+        /// <param name="lng2">经度 2。</param>
+        /// <returns>空间中两点间的距离，单位为米。</returns>
+        public static double CalcDistance_simple(double lat1, double lng1,  double lat2, double lng2)
+        {
+            double dx = lng2 - lng1;
+            double dy = lat2 - lat1;
+            double b = (lat1 + lat2) * 0.5;
+            double Lx = (0.05 * b * b * b - 19.16 * b * b + 47.13 * b + 110966) * dx;
+            double Ly = (17 * b + 110352) * dy;
+            return Math.Sqrt(Lx * Lx + Ly * Ly);
+        }
+
+
+        /// <summary>
+        /// 计算空间中两点间的距离，单位为米。
+        /// </summary>
+        /// <param name="lat1">纬度 1。</param>
+        /// <param name="lng1">经度 1。</param>
+        /// <param name="alt1">高度 1。</param>
+        /// <param name="lat2">纬度 2。</param>
+        /// <param name="lng2">经度 2。</param>
+        /// <param name="alt2">高度 2。</param>
+        /// <returns>空间中两点间的距离，单位为米。</returns>
+        public static double CalcDistance_simple(double lat1, double lng1, double alt1, double lat2, double lng2,double alt2)
+        {
+            double dx = lng2 - lng1;
+            double dy = lat2 - lat1;
+            double b = (lat1 + lat2) * 0.5;
+            double Lx = (0.05 * b * b * b - 19.16 * b * b + 47.13 * b + 110966) * dx;
+            double Ly = (17 * b + 110352) * dy;
+            double d= Math.Sqrt(Lx * Lx + Ly * Ly);
+            return Math.Sqrt(Math.Pow((alt2 - alt1), 2) + Math.Pow(d, 2));
+        }
 
         /// <summary>
         /// 计算空间中两点之间的距离，单位为米。

PlaneGcsSdk.Contract_Shared/PlaneGcsSdk.Contract_Shared.projitems

@@ -20,8 +20,9 @@
     <Compile Include="$(MSBuildThisFileDirectory)CopterManagement\ICopterManager.cs" />
     <Compile Include="$(MSBuildThisFileDirectory)Copters\Constants.cs" />
     <Compile Include="$(MSBuildThisFileDirectory)Copters\CopterCommand.cs" />
-    <Compile Include="$(MSBuildThisFileDirectory)Copters\CopterLocationType.cs" />
     <Compile Include="$(MSBuildThisFileDirectory)Copters\CopterState.cs" />
+    <Compile Include="$(MSBuildThisFileDirectory)Copters\ICopterAttribute.cs" />
+    <Compile Include="$(MSBuildThisFileDirectory)Copters\ICopterVirtualId.cs" />
     <Compile Include="$(MSBuildThisFileDirectory)Copters\PLObservableObject.cs" />
     <Compile Include="$(MSBuildThisFileDirectory)Copters\FlightCommand.cs" />
     <Compile Include="$(MSBuildThisFileDirectory)Copters\GpsFixType.cs" />

PlaneGcsSdk_Private_NET45/Copters/FakeCopter.Private.cs

@@ -6,7 +6,7 @@ namespace Plane.Copters
     public partial class FakeCopter
     {
         /// <summary>
-        /// // 林俊清, 20160122, 不明确。目测是发送重启命令，在刷固件之前用到。
+        /// // 王海, 20160122, 不明确。目测是发送重启命令，在刷固件之前用到。
         /// </summary>
         /// <param name="bootloaderMode"></param>
         /// <returns>表示此命令异步发送操作的 <see cref="Task"/> 实例。</returns>

PlaneGcsSdk_Private_NET45/Copters/PLCopter.Private.cs

@@ -10,7 +10,7 @@ namespace Plane.Copters
         public PlaneCopter InternalCopter { get { return _internalCopter; } }
 
         /// <summary>
-        /// // 林俊清, 20160122, 不明确。目测是发送重启命令，在刷固件之前用到。
+        /// // 王海, 20160122, 不明确。目测是发送重启命令，在刷固件之前用到。
         /// </summary>
         /// <param name="bootloaderMode"></param>
         /// <returns>表示此命令异步发送操作的 <see cref="Task"/> 实例。</returns>

PlaneGcsSdk_Private_NET45/Copters/PlaneCopter.Private.cs

@@ -6,7 +6,7 @@ namespace Plane.Copters
     partial class PlaneCopter
     {
         /// <summary>
-        /// // 林俊清, 20160122, 不明确。目测是发送重启命令，在刷固件之前用到。
+        /// // 王海, 20160122, 不明确。目测是发送重启命令，在刷固件之前用到。
         /// </summary>
         /// <param name="bootloaderMode"></param>
         /// <returns>表示此命令异步发送操作的 <see cref="Task"/> 实例。</returns>

PlaneGcsSdk_Private_NET46/PlaneGcsSdk_Private_NET46.csproj

@@ -9,8 +9,9 @@
     <AppDesignerFolder>Properties</AppDesignerFolder>
     <RootNamespace>Plane</RootNamespace>
     <AssemblyName>PlaneGcsSdk_Private_NET46</AssemblyName>
-    <TargetFrameworkVersion>v4.6</TargetFrameworkVersion>
+    <TargetFrameworkVersion>v4.8</TargetFrameworkVersion>
     <FileAlignment>512</FileAlignment>
+    <TargetFrameworkProfile />
   </PropertyGroup>
   <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
     <DebugSymbols>true</DebugSymbols>
@@ -31,8 +32,10 @@
     <DocumentationFile>bin\Release\PlaneGcsSdk_Private_NET46.xml</DocumentationFile>
   </PropertyGroup>
   <ItemGroup>
+    <Reference Include="PresentationCore" />
     <Reference Include="System" />
     <Reference Include="System.Core" />
+    <Reference Include="System.Drawing" />
     <Reference Include="System.Xml.Linq" />
     <Reference Include="System.Data.DataSetExtensions" />
     <Reference Include="Microsoft.CSharp" />
@@ -57,6 +60,10 @@
     <Compile Include="Properties\AssemblyInfo.cs" />
   </ItemGroup>
   <ItemGroup>
+    <ProjectReference Include="..\..\Plane.Libraries\Plane.Windows.Messages\Plane.Windows.Messages.csproj">
+      <Project>{413C18E2-235F-4E15-B5C1-633657DE5D7A}</Project>
+      <Name>Plane.Windows.Messages</Name>
+    </ProjectReference>
     <ProjectReference Include="..\PlaneGcsSdk.Contract_Private\PlaneGcsSdk.Contract_Private.csproj">
       <Project>{47141894-ece3-48ca-8dcf-ca751bda231e}</Project>
       <Name>PlaneGcsSdk.Contract_Private</Name>

PlaneGcsSdk_Shared/Communication/TcpClientWithTimeout.cs

@@ -0,0 +1,74 @@
+using System;
+using System.Net.Sockets;
+using System.Threading;
+
+/// <summary>
+/// TcpClientWithTimeout 用来设置一个带连接超时功能的类
+/// 使用者可以设置毫秒级的等待超时时间 (1000=1second)
+/// 例如:
+/// TcpClient connection = new TcpClientWithTimeout('127.0.0.1',80,1000).Connect();
+/// </summary>
+public class TcpClientWithTimeout
+{
+    protected string _hostname;
+    protected int _port;
+    protected int _timeout_milliseconds;
+    protected TcpClient connection;
+    protected bool connected;
+    protected Exception exception;
+
+    public TcpClientWithTimeout(string hostname, int port, int timeout_milliseconds)
+    {
+        _hostname = hostname;
+        _port = port;
+        _timeout_milliseconds = timeout_milliseconds;
+    }
+    public TcpClient Connect()
+    {
+        // kick off the thread that tries to connect
+        connected = false;
+        exception = null;
+        Thread thread = new Thread(new ThreadStart(BeginConnect));
+        thread.IsBackground = true; // 作为后台线程处理
+                                    // 不会占用机器太长的时间
+        thread.Start();
+
+        // 等待如下的时间
+        thread.Join(_timeout_milliseconds);
+
+        if (connected == true)
+        {
+            // 如果成功就返回TcpClient对象
+            thread.Abort();
+            return connection;
+        }
+        if (exception != null)
+        {
+            // 如果失败就抛出错误
+            thread.Abort();
+            throw exception;
+        }
+        else
+        {
+            // 同样地抛出错误
+            thread.Abort();
+            string message = string.Format("TcpClient connection to {0}:{1} timed out",
+              _hostname, _port);
+            throw new TimeoutException(message);
+        }
+    }
+    protected void BeginConnect()
+    {
+        try
+        {
+            connection = new TcpClient(_hostname, _port);
+            // 标记成功，返回调用者
+            connected = true;
+        }
+        catch (Exception ex)
+        {
+            // 标记失败
+            exception = ex;
+        }
+    }
+}

PlaneGcsSdk_Shared/Communication/TcpConnection.cs

@@ -1,6 +1,7 @@
 #if !NETFX_CORE
 
 using System;
+using System.Net;
 using System.Net.Sockets;
 using System.Threading.Tasks;
 
@@ -12,6 +13,8 @@ namespace Plane.Communication
     public class TcpConnection : TcpConnectionBase
     {
         private string _remoteHostname;
+        const int tcpReceiveBufferSize = 20 * 1024;  //40 * 1024,
+        const int tcpReceiveTimeout = 1200;
 
         private int _remotePort;
 
@@ -21,8 +24,8 @@ namespace Plane.Communication
             _remotePort = remotePort;
             _client = new TcpClient
             {
-                ReceiveBufferSize = 40 * 1024,
-                ReceiveTimeout = 1200
+                ReceiveBufferSize = tcpReceiveBufferSize,
+                ReceiveTimeout = tcpReceiveTimeout
             };
         }
 
@@ -44,36 +47,79 @@ namespace Plane.Communication
                 }
                 _isBroken = false;
             }
+            
             _stream = _client.GetStream();
         }
 
+        public async Task<bool> BindIP(string ip)
+        {
+            bool bind = false;
+            try
+            {
+                IPAddress IPLocal = IPAddress.Parse(ip);
+                _client.Client.Bind(new IPEndPoint(IPLocal, 0));
+                bind = true;
+            }
+            catch
+            {
+                bind = false;
+            }
+            await Task.Delay(10).ConfigureAwait(false);
+            return bind;
+        }
+
         public override async Task OpenAsync()
         {
+            string logstr;
             if (!IsOpen)
             {
+                if (_client == null)
+                    CreateClientAndConnect();
                 try
                 {
-                    await _client.ConnectAsync(_remoteHostname, _remotePort).ConfigureAwait(false);
-                }
-                catch (SocketException)
+                    var connectTask = _client.ConnectAsync(_remoteHostname, _remotePort);
+                    if (await Task.WhenAny(connectTask, Task.Delay(5000)) == connectTask)
                     {
-                    await CreateClientAndConnectAsync();
+                        if (_client.Client != null)  //需要测试
+                            await connectTask.ConfigureAwait(false);
+                    }
+                    else
+                    {
+                        // Connection timed out
+                        throw new TimeoutException("Connection timed out");
+                    }
+                }
+                catch (SocketException e)
+                {
+                    logstr = e.Message;
+                    CloseClient(); // 关闭并清理客户端
                 }
                 catch (ObjectDisposedException)
                 {
-                    await CreateClientAndConnectAsync();
+                    CloseClient(); // 关闭并清理客户端
+                }
+                catch (Exception)
+                {
+                    CloseClient(); // 处理其他可能的异常
+                   // throw; // 可选：重新抛出异常，以便调用者知道发生了错误
                 }
                 _isBroken = false;
             }
+            if (_client != null)
                 _stream = _client.GetStream();
         }
+        private void CloseClient()
+        {
+            _client?.Close(); // 如果 _client 不为 null，则关闭连接
+            _client = null; // 将 _client 设置为 null，以便垃圾回收器可以回收它
+        }
 
         private void CreateClientAndConnect()
         {
             _client = new TcpClient(_remoteHostname, _remotePort)
             {
-                ReceiveBufferSize = 40 * 1024,
-                ReceiveTimeout = 1200
+                ReceiveBufferSize = tcpReceiveBufferSize,
+                ReceiveTimeout = tcpReceiveTimeout
             };
         }
 
@@ -81,8 +127,8 @@ namespace Plane.Communication
         {
             _client = new TcpClient
             {
-                ReceiveBufferSize = 40 * 1024,
-                ReceiveTimeout = 1200
+                ReceiveBufferSize = tcpReceiveBufferSize,// 40 * 1024,
+                ReceiveTimeout = tcpReceiveTimeout// 1200
             };
             await _client.ConnectAsync(_remoteHostname, _remotePort).ConfigureAwait(false);
         }

PlaneGcsSdk_Shared/Communication/TcpConnectionBase.cs

@@ -24,7 +24,10 @@ namespace Plane.Communication
             {
                 try
                 {
+                    if (_client.Connected)
                         return _client.Available;
+                    else
+                        return 0;
                 }
                 catch (ObjectDisposedException ex)
                 {
@@ -34,6 +37,24 @@ namespace Plane.Communication
             }
         }
 
+        public  bool IsOnline
+        {
+            get
+            {
+                try
+                {
+              //      bool bret ;
+             //       bret = _client.Client != null;
+                  //  bret = bret && !((_client.Client.Poll(1000, SelectMode.SelectRead) && (_client.Client.Available == 0)) || !_client.Client.Connected);
+                    return _client != null && _client.Client != null && !((_client.Client.Poll(1000, SelectMode.SelectRead) && (_client.Client.Available == 0)) || !_client.Client.Connected);
+                }
+                catch (ObjectDisposedException)
+                {
+                    return false;
+                }
+            }
+        }
+
         public bool IsOpen
         {
             get
@@ -51,8 +72,8 @@ namespace Plane.Communication
 
         public void Close()
         {
-            _stream.Close();
-            _client.Close();
+            _stream?.Close();
+            _client?.Close();
         }
 
         public abstract Task OpenAsync();
@@ -89,6 +110,12 @@ namespace Plane.Communication
             {
                 while (Available < count)
                 {
+                    if (Available > 0)
+                        Console.WriteLine("Available = " + Available);
+                    //if (!IsOpen)
+                    //    return 0;
+                    if (!IsOnline)
+                        return 0;
                     await Task.Delay(5).ConfigureAwait(false);
                 }
                 return await _stream.ReadAsync(buffer, offset, count);

PlaneGcsSdk_Shared/Communication/TcpServerConnectionManager.cs

@@ -21,7 +21,7 @@ namespace Plane.Communication
 
         private bool _isListening;
 
-        // TODO: 林俊清, 20150909, 从配置文件中获取要监听的 IP 地址和端口号。
+        // TODO: 王海, 20150909, 从配置文件中获取要监听的 IP 地址和端口号。
         private TcpListener _listener;
 
         private bool _shouldListen;

PlaneGcsSdk_Shared/Communication/TimeOutSocket.cs

@@ -0,0 +1,66 @@
+using System;
+using System.Collections.Generic;
+using System.Net.Sockets;
+using System.Text;
+using System.Threading;
+
+namespace Plane.Communication
+{
+    class TimeOutSocket
+    {
+        private static bool IsConnectionSuccessful = false;
+        private static Exception socketexception;
+        private static ManualResetEvent TimeoutObject = new ManualResetEvent(false);
+
+        public static TcpClient Connect(string serverip, int serverport, int timeoutMSec)
+        {
+            TimeoutObject.Reset();
+            socketexception = null;
+
+            TcpClient tcpclient = new TcpClient();
+
+            tcpclient.BeginConnect(serverip, serverport,
+                new AsyncCallback(CallBackMethod), tcpclient);
+
+            if (TimeoutObject.WaitOne(timeoutMSec, false))
+            {
+                if (IsConnectionSuccessful)
+                {
+                    return tcpclient;
+                }
+                else
+                {
+                    throw socketexception;
+                }
+            }
+            else
+            {
+                tcpclient.Close();
+                throw new TimeoutException("TimeOut Exception");
+            }
+        }
+        private static void CallBackMethod(IAsyncResult asyncresult)
+        {
+            try
+            {
+                IsConnectionSuccessful = false;
+                TcpClient tcpclient = asyncresult.AsyncState as TcpClient;
+
+                if (tcpclient.Client != null)
+                {
+                    tcpclient.EndConnect(asyncresult);
+                    IsConnectionSuccessful = true;
+                }
+            }
+            catch (Exception ex)
+            {
+                IsConnectionSuccessful = false;
+                socketexception = ex;
+            }
+            finally
+            {
+                TimeoutObject.Set();
+            }
+        }
+    }
+}

PlaneGcsSdk_Shared/Communication/UdpServerConnectionManager.NET.cs

@@ -41,6 +41,7 @@ namespace Plane.Communication
                             {
                                 var connection = new UdpServerConnection(ep, _client.SendAsync);
                                 _connections.Add(remoteAddress, connection);
+                                //调用AddOrUpdateCopter添加飞机
                                 RaiseConnectionEstablished(connection, remoteAddress);
                             }
                             _connections[remoteAddress].EnqueueDatagram(data);

PlaneGcsSdk_Shared/Communication/UdpServerConnectionManager.cs

@@ -27,6 +27,11 @@ namespace Plane.Communication
             _connections.Clear();
         }
 
+        public void DeleteConnections(string remoteAddress)
+        {
+            _connections.Remove(remoteAddress);
+        }
+
         public void Dispose()
         {
             if (_disposed)

PlaneGcsSdk_Shared/Communication/UdpServerConnection_NET.cs

@@ -9,7 +9,6 @@ namespace Plane.Communication
     internal class UdpServerConnection : UdpConnectionBase
     {
         private IPEndPoint _remoteEP;
-        private IPEndPoint _boardaddEP;
 
         private Func<byte[], int, IPEndPoint, Task<int>> _sendFunc;
 
@@ -18,7 +17,6 @@ namespace Plane.Communication
         /// </summary>
         public UdpServerConnection(IPEndPoint remoteEP, Func<byte[], int, IPEndPoint, Task<int>> sendFunc)
         {
-            _boardaddEP = new IPEndPoint(IPAddress.Parse("192.168.62.255"), remoteEP.Port);
             _remoteEP = remoteEP;
             _sendFunc = sendFunc;
         }
@@ -39,13 +37,6 @@ namespace Plane.Communication
             try
             {
                 await _sendFunc(datagram, bytes, _remoteEP).ConfigureAwait(false);
-                /*
-                //如果是广播包,用于广播消息，比如RTK等
-                if (false)
-                  await _sendFunc(datagram, bytes, _boardaddEP).ConfigureAwait(false);
-                else
-                  await _sendFunc(datagram, bytes, _remoteEP).ConfigureAwait(false);
-                  */
             }
             catch (Exception ex)
             {

PlaneGcsSdk_Shared/CommunicationManagement/CommConnection.cs

@@ -0,0 +1,42 @@
+using Plane.Communication;
+using System;
+using System.Collections.Generic;
+using System.Text;
+using System.Threading.Tasks;
+
+namespace Plane.CommunicationManagement
+{
+    public class CommConnection : IConnection
+    {
+        public bool IsOpen { get; protected set; }
+
+        public event EventHandler<ExceptionThrownEventArgs> ExceptionThrown;
+
+        public int BytesToRead()
+        {
+            return 0;
+        }
+
+        public void Close()
+        {
+            IsOpen = false;
+        }
+
+        public Task OpenAsync()
+        {
+            IsOpen = true;
+            return TaskUtils.CompletedTask;
+        }
+
+        public async Task<int> ReadAsync(byte[] buffer, int offset, int count)
+        {
+            await TaskUtils.CompletedTask;
+            return 0;
+        }
+
+        public async Task WriteAsync(byte[] buffer, int offset, int count)
+        {
+            await TaskUtils.CompletedTask;
+        }
+    }
+}

PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs

@@ -0,0 +1,864 @@
+using Plane.Communication;
+using Plane.Protocols;
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.Linq;
+using System.Net;
+using System.Net.NetworkInformation;
+using System.Net.Sockets;
+using System.Text;
+using System.Threading.Tasks;
+using System.Windows.Media;
+
+using System.Runtime.InteropServices;
+using Plane.Windows.Messages;
+
+namespace Plane.CommunicationManagement
+{
+    //通信模块
+    public partial class CommModuleManager: ExceptionThrownEventSource, IDisposable
+    {
+        [DllImport("kernel32")] //返回取得字符串缓冲区的长度
+        private static extern int GetPrivateProfileString(string section, string key, string def, StringBuilder retVal, int size, string filePath);
+
+        private static CommModuleManager _Instance = new CommModuleManager();
+        public static CommModuleManager Instance { get { return _Instance; } }
+
+        public TcpConnection Connection = null;
+        public bool CommOK = false;
+        private const string MODULE_IP = "192.168.199.51";         //"192.168.199.51"; // "192.168.199.51"; //在用版本  ----//"192.168.1.65"; //新版本
+        private string ServerIP = MODULE_IP;
+        // private const string LOCAL_IP = "192.168.199.50";  //"192.168.199.50"; 
+        private const int PORT = 9551;
+        private bool _disposed;
+        public int CommModuleCopterCount = 0;
+        public int CommModuleCurMode = 0;
+        private long packetCountNum = 0;
+
+        
+        /// <summary>
+        /// 用于判断写入任务操作超时的秒表。
+        /// </summary>
+        private Stopwatch _writeMissionStopwatch;
+
+        private void LoadIPSet()
+        {
+            StringBuilder temp = new StringBuilder(255);
+
+            string path = Environment.CurrentDirectory + @"\Config.ini";
+
+            int i = GetPrivateProfileString("Default", "ServerIP", "", temp, 255, path);
+            ServerIP= temp.ToString();
+
+            if (ServerIP == "") ServerIP = MODULE_IP;
+        }
+
+
+        public async Task ConnectAsync()
+        {
+            LoadIPSet();
+            Connection = new TcpConnection(ServerIP, PORT);
+            await ConnectOpenAsync();
+        }
+
+        public void Connect()
+        {
+            LoadIPSet();
+            Task.Run(async () =>
+            {
+                Connection = new TcpConnection(ServerIP, PORT);
+                await ConnectOpenAsync();
+            }
+            );
+        }
+
+        public void CloseConnection()
+        {
+            CommOK = false;
+            Connection?.Close();
+        }
+
+
+
+
+
+
+
+
+/*
+
+
+        //获取内网IP
+        private IPAddress GetInternalIP()
+        {
+            NetworkInterface[] nics = NetworkInterface.GetAllNetworkInterfaces();
+
+            foreach (NetworkInterface adapter in nics)
+            {
+                foreach (var uni in adapter.GetIPProperties().UnicastAddresses)
+                {
+                    if (uni.Address.AddressFamily == AddressFamily.InterNetwork)
+                    {
+                        return uni.Address;
+                    }
+                }
+            }
+            return null;
+        }
+
+        /// <summary>
+        /// 获取本地连接IP地址、无线连接IP地址
+        /// </summary>
+        /// <param name="k">0:本地连接，1:本地连接1，2:无线网络连接</param>
+        /// <returns></returns>
+        public static IPAddress GetIP(int k)
+        {
+            NetworkInterface[] interfaces = System.Net.NetworkInformation.NetworkInterface.GetAllNetworkInterfaces();
+            int len = interfaces.Length;
+            IPAddress[] mip = { IPAddress.Parse("0.0.0.0"), IPAddress.Parse("0.0.0.0"), IPAddress.Parse("0.0.0.0") };
+
+            for (int i = 0; i < len; i++)
+            {
+                NetworkInterface ni = interfaces[i];
+
+                if (ni.Name == "本地连接")
+                {
+                    IPInterfaceProperties property = ni.GetIPProperties();
+                    foreach (UnicastIPAddressInformation ip in property.UnicastAddresses)
+                    {
+                        if (ip.Address.AddressFamily == System.Net.Sockets.AddressFamily.InterNetwork)
+                        {
+                            mip[0] = ip.Address;
+                        }
+                    }
+                }
+                else if (ni.Name == "本地连接2")
+                {
+                    IPInterfaceProperties property = ni.GetIPProperties();
+                    foreach (UnicastIPAddressInformation ip in property.UnicastAddresses)
+                    {
+                        if (ip.Address.AddressFamily == System.Net.Sockets.AddressFamily.InterNetwork)
+                        {
+                            mip[1] = ip.Address;
+                        }
+                    }
+                }
+                else if (ni.Name == "无线网络连接")
+                {
+                    IPInterfaceProperties property = ni.GetIPProperties();
+                    foreach (UnicastIPAddressInformation ip in property.UnicastAddresses)
+                    {
+                        if (ip.Address.AddressFamily == System.Net.Sockets.AddressFamily.InterNetwork)
+                        {
+                            mip[2] = ip.Address;
+                        }
+                    }
+                }
+            }
+            return mip[k];
+        }
+
+        */
+        private async Task ConnectOpenAsync()
+        {
+            CommOK = true;
+            //不绑定IP也可以通讯，有线网络需要设置192.168.199.X网段地址
+           // string locip = GetIP(0).ToString();
+           // bool bind = await Connection.BindIP(locip);
+           // if (bind)
+            {
+                try
+                {
+                    if (!Connection.IsOnline)
+                        await Connection.OpenAsync().ConfigureAwait(false);
+                }
+                catch
+                { }
+            }
+
+            if (Connection.IsOnline)
+            {
+                Message.Connect(true);
+                Message.Show($"通讯基站连接成功!");
+                times = 0;
+                SendQuery();
+                await StartReadingPacketsAsync();
+            }
+            else
+            {
+                Message.Connect(false);
+                Reconnect();
+            }
+        }
+
+        //循环发送查询，保持通信正常
+        private void SendQuery()
+        {
+            Task.Run(async () =>
+            {
+                long lastPacketCountNum = 0;
+                int faulttimes = 0; //等待没有数据次数
+                const int revtimeout = 2000; //等待超时ms  old=1200
+                while (CommOK)
+                {
+                    if (Connection != null)
+                    {
+                        //发送心跳包-等待回复消息
+                        await WriteCommPacketAsync(0, MavComm.COMM_QUERY_STATUS);
+                      //  await SendQueryStatusPacketsAsync();
+                        await Task.Delay(revtimeout).ConfigureAwait(false); 
+                        //等待是否接受到返回的数据包，如果没有收到认为连结断开，重新联接
+                        if (packetCountNum > lastPacketCountNum)
+                        {
+                            lastPacketCountNum = packetCountNum;
+                            if (faulttimes>0)
+                                Message.Show("收到新数据包，重置超时次数...");
+                            faulttimes = 0;
+                        }
+                        else
+                        {
+                            faulttimes++;
+                            Message.Show("设备回复超时("+ revtimeout + "ms)" + faulttimes+"次");
+                            //2次没数据包返回就重连
+                            if (faulttimes > 1)
+                            {
+                                Message.Show("长时间未收到设备回复数据包");
+                                break;
+                            }
+                        }
+                    }
+                }
+                Message.Show("主动断开连接，重连");
+                Reconnect();
+            }).ConfigureAwait(false);
+        }
+
+     /*  //直接调用不用封装函数了  
+       private async Task SendQueryStatusPacketsAsync()
+        {
+            await WriteCommPacketAsync(0, MavComm.COMM_QUERY_STATUS);
+            await Task.Delay(100);//100
+        }
+        */
+
+        int times = 0;
+        //这里容易报空引用异常 ，没有连接通讯设备时
+        private void Reconnect()
+        {
+           // Message.Show($"正在重新连接...");
+            Task.Run(async () =>
+            {
+                CloseConnection();
+                await Task.Delay(250).ConfigureAwait(false);
+               // Message.Show($"正在重连:次数{++times}");
+                await Task.Delay(250).ConfigureAwait(false);
+                await ConnectAsync();
+                
+            }).ConfigureAwait(false);
+        }
+
+        private async Task StartReadingPacketsAsync()
+        {
+            await Task.Run(async () =>
+            {
+                while (CommOK)
+                {
+                    if (Connection == null || !Connection.IsOnline)
+                    {
+                        if (Connection==null)
+                            Message.Show("空连接异常");
+                        else  if (!Connection.IsOnline)
+                        {
+                            Message.Show("检测到设备已断开连接");
+                        }
+                        break;
+                    }
+
+                    var packet = await ReadPacketAsync().ConfigureAwait(false);
+                    if (packet != null)
+                    {
+                        packetCountNum++;
+                        short serialNum = BitConverter.ToInt16(packet, 4);
+                        short copterNum = BitConverter.ToInt16(packet, 6);
+                        short commandType = BitConverter.ToInt16(packet, 8);
+                        byte[] dealData;
+                        switch (commandType)
+                        {
+                            case MavComm.COMM_DOWNLOAD_MODE:
+                                dealData = packet.Skip(10).ToArray();
+                                AnalyzeMissionStartPacket(copterNum, dealData);
+                                break;
+
+                            case MavComm.COMM_GET_COPTERS_COUNT:
+                                dealData = packet.Skip(16).ToArray();
+                                AnalyzeCopterInfosPacket(dealData);
+                                break;
+
+                            case MavComm.COMM_NOTIFICATION:
+                                short messageType = BitConverter.ToInt16(packet, 10);
+                                dealData = packet.Skip(12).ToArray();
+                                switch (messageType)
+                                {
+                                    case (short)MavComm.MessageType.STR:
+                                        AnalyzeStringPacket(copterNum, dealData);
+                                        break;
+
+                                    case (short)MavComm.MessageType.SCAN2:
+                                        AnalyzeComm2RetrunPacket(copterNum, dealData);
+                                        break;
+
+                                    case (short)MavComm.MessageType.SCAN3:
+                                        AnalyzeComm3RetrunPacket(copterNum, dealData);
+                                        break;
+                                    case 4:  //版本13通讯模块收到飞机返回的数据
+                                        AnalyzeComm4RetrunPacket(copterNum, dealData);
+                                        break;
+                                }
+                                break;
+                            case 0:
+                                int packetLength = packet.Length;
+                                short errorId = BitConverter.ToInt16(packet, packetLength - 6);
+                                short copterCount = BitConverter.ToInt16(packet, packetLength - 4);
+                                short curMode = BitConverter.ToInt16(packet, packetLength - 2);
+                                string msg = string.Format("错误Id={0}, 检测飞机总数={1}，工作模式={2} 流水号={3}", errorId, copterCount, curMode, serialNum);
+                                CommModuleCopterCount = copterCount;
+                                CommModuleCurMode = curMode;
+                                //Message.Show(msg);
+                                break;
+                        } 
+                    }
+                }
+            }).ConfigureAwait(false);
+            Message.Show("退出读设备数据........");
+            //Reconnect();
+        }
+        
+        private async Task<byte[]> ReadPacketAsync()
+        {
+            int readnumber = 0;
+            if (Connection == null || !Connection.IsOnline)
+            {
+                return null;
+            }
+            try
+            {
+                byte[] bufferHaed = new byte[2];
+                readnumber = await Connection.ReadAsync(bufferHaed, 0, 2);
+                if (bufferHaed[0] == 0x13 && bufferHaed[1] == 0x77)
+                {
+                    byte[] bufferLength = new byte[2];
+                    readnumber = await Connection.ReadAsync(bufferLength, 0, 2);
+                    short datalength = BitConverter.ToInt16(bufferLength, 0);
+                    //Console.WriteLine("datalength = "+ datalength);
+
+                    byte[] bufferData = new byte[datalength];
+                    readnumber = await Connection.ReadAsync(bufferData, 0, datalength);
+                   
+//                     foreach (byte b in bufferData)
+//                     {
+//                         Console.Write(b.ToString("X2"));
+//                         Console.Write(" ");
+//                     }
+//                     Console.WriteLine("");
+
+                    byte[] needCRCData = new byte[datalength + 4];
+                    Array.Copy(bufferHaed, 0, needCRCData, 0, 2);
+                    Array.Copy(bufferLength, 0, needCRCData, 2, 2);
+                    Array.Copy(bufferData, 0, needCRCData, 4, datalength);
+
+                    byte[] crc = checkCRC16(needCRCData);
+                    byte[] bufferCRC16 = new byte[2];
+                    readnumber = await Connection.ReadAsync(bufferCRC16, 0, 2);
+
+                    if (crc[0] == bufferCRC16[0] && crc[1] == bufferCRC16[1])
+                    {
+                        return needCRCData;
+                    }
+                }                
+            }
+            catch
+            {
+                //Console.WriteLine("错误");
+                return null;
+            }
+            return null;
+        }
+
+       
+
+        short serial_Number = 1;
+
+        public async Task GenerateDataAsync<TMavCommPacket>(short copterId, short messageType, TMavCommPacket indata)
+        {
+            byte[] data;
+            data = MavlinkUtil.StructureToByteArray(indata);
+            await WriteCommPacketAsync(copterId, messageType, data);
+        }
+
+        public async Task UpdateCommModule()
+        {
+            MavComm.comm_update_copter_module commUpdate = new MavComm.comm_update_copter_module();
+            commUpdate.mav_count = (short)CommModuleCopterCount;
+            commUpdate.update_code = 0x7713;
+            await GenerateDataAsync(0, MavComm.COMM_UPDATE_COPTER_MODULE, commUpdate);
+        }
+
+
+        public async Task TestModule(short ModuleNo, short TestLong=100)
+        {
+
+            
+            byte[] packet = new byte[2];
+            packet[0] = (byte)(ModuleNo);
+            packet[1] = (byte)(TestLong);
+            byte[] senddata = packet;
+         
+       //     Message.Show("长度 = " + senddata.Length);
+                await WriteCommPacketAsync(0, MavComm.COMM_TEST_MODULE, senddata);
+          //      await Task.Delay(1000).ConfigureAwait(false);
+     
+        }
+
+
+
+        public async Task SetModulePower(short ModuleNo, short ModulePower)
+        {
+            byte[] packet = { 0x01, 0x00, 0x03, 0x00, 0x01, 0x0A };
+            packet[2] = (byte)(ModuleNo & 0xFF);
+            packet[3] = (byte)((ModuleNo & 0xFF00) >> 8);
+            packet[5] = (byte)(ModulePower);
+           await WriteCommPacketAsync(0, MavComm.COMM_TO_MODULE, packet);
+        }
+
+
+
+
+
+        bool temp = false;
+        //测试用 灯光间隔1S闪烁
+        public async Task TestLED(short id, string colorString)
+        {
+            byte[] packet = DoLEDCommandAsync(51, 0, 51);
+            temp = !temp;
+            while (temp)
+            {
+                Message.Show("发送测试灯光");
+                await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, packet);
+                //如果是广播同时发送到广播端口
+                if ((id==0)&& Recomisopen)
+                {
+                   await BroadcastSendAsync(packet);
+                }
+                await Task.Delay(1000).ConfigureAwait(false);
+            } 
+        }
+
+        //拉烟测试
+        public async Task TestFire(short id, int channel)
+        {
+
+            MAVLink.mavlink_msg_id_led_control led = new MAVLink.mavlink_msg_id_led_control();
+            led.target_system = 1;
+            led.target_component = 1;//(byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL;;
+            led.instance = 0;
+            led.pattern = 0;
+            led.custom_len = 1;//命令类型——测试拉烟
+            led.custom_bytes = new byte[24];
+
+            led.custom_bytes[0] = (byte)channel;
+            
+            byte[] data;
+            data = MavlinkUtil.StructureToByteArray(led);
+
+            byte[] packet = new byte[data.Length + 6 + 2];
+            packet[0] = MAVLink.MAVLINK_STX;
+
+            packet[1] = (byte)(data.Length);
+            packet[2] = 1;
+            packet[3] = 255; // this is always 255 - MYGCS
+
+            packet[4] = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER;
+
+            packet[5] = MAVLink.MAVLINK_MSG_ID_LED_CONTROL;
+            int i = 6;
+            foreach (byte b in data)
+            {
+                packet[i] = b;
+                i++;
+            }
+
+            ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6);
+            checksum = MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[MAVLink.MAVLINK_MSG_ID_LED_CONTROL], 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;
+
+
+            //await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, packet);
+            byte[] senddata = packet;
+
+            for (int times = 0; times < 3; times++)
+            {
+                senddata = senddata.Concat(packet).ToArray();
+            }
+
+
+            temp = !temp;
+            while (temp)
+            {
+                await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, packet);
+                await Task.Delay(1000).ConfigureAwait(false);
+            }
+        }
+
+
+       public async Task TestBattery(short id, float minivol)
+        {
+
+            MAVLink.mavlink_msg_id_led_control led = new MAVLink.mavlink_msg_id_led_control();
+            led.target_system = 1;
+            led.target_component = 1;//(byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL;;
+            led.instance = 0;
+            led.pattern = 0;
+            led.custom_len = 5;//命令类型——测试电池电压
+            led.custom_bytes = new byte[24];
+
+            led.custom_bytes[0] = (byte)(int)(minivol); //整数部分
+            led.custom_bytes[1] = (byte)(int)((minivol-(int)minivol)*100);  //小数部分  --2位
+
+
+
+
+
+            byte[] data;
+            data = MavlinkUtil.StructureToByteArray(led);
+
+            byte[] packet = new byte[data.Length + 6 + 2];
+            packet[0] = MAVLink.MAVLINK_STX;
+
+            packet[1] = (byte)(data.Length);
+            packet[2] = 1;
+            packet[3] = 255; // this is always 255 - MYGCS
+
+            packet[4] = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER;
+
+            packet[5] = MAVLink.MAVLINK_MSG_ID_LED_CONTROL;
+            int i = 6;
+            foreach (byte b in data)
+            {
+                packet[i] = b;
+                i++;
+            }
+
+            ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6);
+            checksum = MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[MAVLink.MAVLINK_MSG_ID_LED_CONTROL], 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;
+
+
+            //await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, packet);
+            byte[] senddata = packet;
+
+            for (int times = 0; times < 3; times++)
+            {
+                senddata = senddata.Concat(packet).ToArray();
+            }
+
+
+            {  //发3次
+                await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, packet);
+                await Task.Delay(1000).ConfigureAwait(false);
+                await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, packet);
+                await Task.Delay(1000).ConfigureAwait(false);
+                await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, packet);
+
+            }
+        }
+       
+
+        /// <summary>
+        /// 生成通信模块packet并且发送
+        /// </summary>
+        /// <param name="copterId">飞机ID</param>
+        /// <param name="messageType">命令类型</param>
+        /// <param name="data">数据类型：空表示只切换模式</param>
+        /// <param name="batchPacket">小批量数据包</param>
+        /// <returns></returns>
+        public async Task WriteCommPacketAsync(short copterId, short messageType, byte[] data = null, byte[] batchPacket = null)
+        {
+            //新盒子的协议--暂时不用-需要问张伟
+            /*
+            if (messageType == MavComm.COMM_DOWNLOAD_COMM && copterId == 0)
+            {
+                short byteNum;
+                short length;
+                if (batchPacket == null)
+                {
+                    byteNum = 0;
+                    length = (short)((0x5 << 12) ^ byteNum);
+                    batchPacket = BitConverter.GetBytes(length);
+                }
+                else
+                {
+                    byteNum = (short)(batchPacket.Length / 2);
+                    length = (short)((0x5 << 12) ^ byteNum);
+                    batchPacket = BitConverter.GetBytes(length).Concat(batchPacket).ToArray();
+                }
+            }
+            */
+            if (data != null &&  batchPacket != null) data = batchPacket.Concat(data).ToArray();
+
+            int packetlength = data == null ? 0 : data.Length;
+
+            //数据长度+10
+            byte[] packet = new byte[packetlength + 10];
+
+            byte[] buffer_header = new byte[2];
+            buffer_header = BitConverter.GetBytes(MavComm.COMM_SEND_PACKET_HEADER);
+            Array.Copy(buffer_header, 0, packet, 0, 2);         //发送数据头 2字节
+
+            byte[] buffer_length; 
+            buffer_length = BitConverter.GetBytes((Int16)(packetlength + 6));
+            Array.Copy(buffer_length, 0, packet, 2, 2);         //数据长度 2字节
+
+            byte[] buffer_serial;
+            buffer_serial = BitConverter.GetBytes(serial_Number++); 
+            Array.Copy(buffer_serial, 0, packet, 4, 2);         //流水号  2字节
+
+            byte[] buffer_copterId = new byte[2];
+            buffer_copterId = BitConverter.GetBytes((Int16)copterId);
+            Array.Copy(buffer_copterId, 0, packet, 6, 2);       //飞机号   2字节
+
+            byte[] buffer_messageType = new byte[2];
+            buffer_messageType = BitConverter.GetBytes((Int16)messageType);
+            Array.Copy(buffer_messageType, 0, packet, 8, 2);    //命令类型  2字节
+
+            if (data != null)
+                Array.Copy(data, 0, packet, 10, data.Length);   //数据内容  10字节开始
+
+            byte[] buffer_CRC = checkCRC16(packet);               //计算所有数据的crc
+
+            byte[] buffer_packet = new byte[packet.Length + 2];    //分配新的带crc的数据包
+            Array.Copy(packet, buffer_packet, packet.Length);
+            Array.Copy(buffer_CRC, 0, buffer_packet, packet.Length, 2);   //带crc的数据包
+
+            if (Connection != null && Connection.IsOnline)
+            {
+                //if (messageType==83)
+                //   Console.WriteLine("RTCM:" + (ushort)rtcm.length + "[" + rtcm.packet[0] + "," + rtcm.packet[1] + "," + rtcm.packet[2] + "," + rtcm.packet[3] + "," + rtcm.packet[4] + "]");
+                // Console.WriteLine("[{0}]Send rtcm:{1}", buffer_serial[0], packetlength);
+                try
+                {
+                    await Connection.WriteAsync(buffer_packet, 0, buffer_packet.Length);
+                }
+                catch 
+                {
+
+                }
+            }
+
+        }
+
+        public async Task<bool> MissionPacket<TMavCommPacket>(short copterId, byte messageType, TMavCommPacket[] indata)
+        {
+            //数据长度
+            int dataLength = 6 + 2 + indata.Length * 32;
+            byte[] data = new byte[dataLength];
+
+            byte[] uses = new byte[] { 0, 0, 1, 0, 0, 0 };
+            //填充初始数据
+            Array.Copy(uses, 0, data, 0, 6);
+
+            Int16 countNum = (Int16)indata.Length;
+            Array.Copy(BitConverter.GetBytes(countNum), 0, data, 6, 2);
+
+
+            //新盒子的协议--暂时不用-需要问张伟
+            /*
+            Int16 countNum = (Int16)indata.Length;
+
+            byte[] uses = new byte[] { 0, 1, 0, 0, 0, 0 };
+
+            Array.Copy(uses, 0, data, 0, 6);
+
+            Array.Copy(BitConverter.GetBytes(countNum), 0, data, 2, 2);
+            Array.Copy(BitConverter.GetBytes(countNum), 0, data, 6, 2);
+            */
+
+
+
+
+            int offset = 8;
+
+            foreach (TMavCommPacket MavPacket in indata)
+            {
+                byte[] MavPacketData = MavlinkUtil.StructureToByteArray(MavPacket);
+                byte[] tempData = new byte[32];
+
+                //张伟只需要32个有用数据 0-27， 30-31， 34-35
+                Array.Copy(MavPacketData, 0, tempData, 0, 28);
+                Array.Copy(MavPacketData, 30, tempData, 28, 2);
+                Array.Copy(MavPacketData, 34, tempData, 30, 2);
+
+                Array.Copy(tempData, 0, data, offset, 32);
+                offset += 32;
+            }
+
+            await WriteCommPacketAsync(copterId, messageType, data);
+
+            if (_writeMissionStopwatch == null) _writeMissionStopwatch = Stopwatch.StartNew();
+            else _writeMissionStopwatch.Restart();
+
+            //先延时5秒判断通信模块是否返回错误ID，0为正确。 如果已经错误了就不需要等待下发了。
+            MissionStartCopterId = 0; MissionErrorId = -1;
+            ErrorCode = 0;
+
+            if (await AwaitCommMissionStartAsync(copterId, 5000))
+            {
+                _writeMissionStopwatch.Restart();
+                MissionSendCopterId = 0;
+                //等待通信模块地面站数据下发后，才能开始下个飞机的航点写入
+                //下发不代表写入成功
+                return await AwaitCommMissionRequestAsync(copterId, 10000).ConfigureAwait(false);
+            }
+            else
+                return false;
+        }
+
+        public async Task<bool> AwaitCommMissionStartAsync(short copterId, int millisecondsTimeout)
+        {
+            while (MissionStartCopterId != copterId)
+            {
+                if (_writeMissionStopwatch.ElapsedMilliseconds >= millisecondsTimeout)
+                {
+                    return false;
+                }
+                await Task.Delay(50).ConfigureAwait(false);
+            }
+            if (MissionErrorId == 0)
+                return true;
+            else
+            {
+                //Message.Show($"{copterId}:返回错误--{MissionErrorId}");
+                return false;
+            }
+        }
+
+        public async Task<bool> AwaitCommMissionRequestAsync(short copterId, int millisecondsTimeout)
+        {
+            while (MissionSendCopterId != copterId)
+            {
+                if (_writeMissionStopwatch.ElapsedMilliseconds >= millisecondsTimeout || ErrorCode != 0)
+                {
+                    return false;
+                }
+                await Task.Delay(50).ConfigureAwait(false);
+            }
+            return true;
+        }
+
+        private long GetCreatetime()
+        {
+            DateTime DateStart = new DateTime(2000, 1, 1, 0, 0, 0);
+            return Convert.ToInt64((DateTime.Now - DateStart).TotalMilliseconds);
+        }
+
+        public static byte[] CRC16(byte[] data)
+        {
+            int len = data.Length;
+            if (len > 0)
+            {
+                ushort crc = 0xFFFF;
+
+                for (int i = 0; i < len; i++)
+                {
+                    crc = (ushort)(crc ^ (data[i]));
+                    for (int j = 0; j < 8; j++)
+                    {
+                        crc = (crc & 1) != 0 ? (ushort)((crc >> 1) ^ 0xA001) : (ushort)(crc >> 1);
+                    }
+                }
+                byte hi = (byte)((crc & 0xFF00) >> 8);  //高位置
+                byte lo = (byte)(crc & 0x00FF);         //低位置
+                return new byte[] { hi, lo };
+            }
+            return new byte[] { 0, 0 };
+        }
+
+        public byte[] CRCCalc(byte[] crcbuf)
+        {
+            //计算并填写CRC校验码
+            int crc = 0xffff;
+            int len = crcbuf.Length;
+            for (int n = 0; n < len; n++)
+            {
+                byte i;
+                crc = crc ^ crcbuf[n];
+                for (i = 0; i < 8; i++)
+                {
+                    int TT;
+                    TT = crc & 1;
+                    crc = crc >> 1;
+                    crc = crc & 0x7fff;
+                    if (TT == 1)
+                    {
+                        crc = crc ^ 0xa001;
+                    }
+                    crc = crc & 0xffff;
+                }
+
+            }
+            byte[] redata = new byte[2];
+            redata[1] = (byte)((crc >> 8) & 0xff);
+            redata[0] = (byte)((crc & 0xff));
+            return redata;
+        }
+
+        public byte[] checkCRC16(byte[] puchMsg)
+        {
+
+            int usDataLen = puchMsg.Length;
+
+            ushort iTemp = 0x0;
+            ushort flag = 0x0;
+
+            for (int i = 0; i < usDataLen; i++)
+            {
+                iTemp ^= puchMsg[i];
+                for (int j = 0; j < 8; j++)
+                {
+                    flag = (ushort)(iTemp & 0x01);
+                    iTemp >>= 1;
+                    if (flag == 1)
+                    {
+                        iTemp ^= 0xa001;
+                    }
+                }
+            }
+            byte[] ret = BitConverter.GetBytes(iTemp);
+            return ret;
+        }
+
+        public void Dispose()
+        {
+            if (!CommOK)
+            {
+                return;
+            }
+            CommOK = false;
+            CloseConnection();
+        }
+    }
+}

PlaneGcsSdk_Shared/CommunicationManagement/CommModulePacketAnalysis.cs

@@ -0,0 +1,213 @@
+using Plane.Communication;
+using Plane.Protocols;
+using Plane.Windows.Messages;
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+
+namespace Plane.CommunicationManagement
+{
+    public partial class CommModuleManager
+    {
+        private int MissionStartCopterId = 0;
+        private int MissionSendCopterId = 0;
+        private int MissionErrorId = -1;
+        private int ErrorCode = 0;
+        public event EventHandler<CommunicationReceiveCopterInfoEventArgs> CommunicationReceived;
+        public event EventHandler<CommunicationCopterDisconnectEventArgs> CommunicationCopterDisconnect;
+        public event EventHandler<CommunicationConnectEventArgs> CommunicationConnected;
+        private Dictionary<int, CommWriteMissinState> missionWriteState = new Dictionary<int, CommWriteMissinState>();
+        private static readonly object MissinLocker = new object();
+
+        public Dictionary<int, CommWriteMissinState> MissionWriteState
+        {
+            get {return missionWriteState; }
+        }
+
+        public void ClearMissionWriteState()
+        {
+            missionWriteState.Clear();
+        }
+
+        private void AnalyzeMissionStartPacket(short copterId, byte[] buffer)
+        {
+            short errorId = BitConverter.ToInt16(buffer, 0);
+            MissionStartCopterId = copterId;
+            MissionErrorId = errorId;
+            if(errorId != 0)
+                Message.Show($"{copterId}:返回 = {errorId}");
+        }
+
+        private void AnalyzeStringPacket(short copterId, byte[] buffer)
+        {
+            int count = Array.IndexOf(buffer, (byte)0);
+            string msg = System.Text.Encoding.Default.GetString(buffer, 0, count).Replace("tcpserver", "flicube");
+            Message.Show(msg);
+        }
+
+        private void AnalyzeComm2RetrunPacket(short copterId, byte[] buffer)
+        {
+            ushort ret = BitConverter.ToUInt16(buffer, 0);
+            switch (ret)
+            {
+                case MavComm.SEARCH_FINDCOPTER:
+                    Message.Show(copterId.ToString() + "---飞机开始相应");
+                    break;
+
+                case MavComm.SEARCH_COMPLETED:
+                    Message.Show(copterId.ToString() + "---设置成功");
+                    break;
+
+                case MavComm.SEARCH_OUTTIME:
+                    Message.Show("超时无响应");
+                    break;
+
+                case MavComm.MISSION_SEND_COMPLETED:
+                    MissionSendCopterId = copterId;
+                    break;
+
+                case MavComm.P2P_SEND_FAILED:
+                    Message.Show("点对点通信发送失败");
+                    break;
+                default:
+                    if (ret > MavComm.ERROR_CODE_START && ret <= MavComm.ERROR_CODE_END)
+                    {
+                        ErrorCode = ret;
+                        Message.Show($"{copterId}--错误码：{ret}");
+                    }
+                        
+                    break;
+            }
+        }
+
+        private void AnalyzeComm3RetrunPacket(short copterId, byte[] buffer)
+        {
+            byte type = buffer[0];
+            byte connectRet;
+            switch (type)
+            {
+                case 0x01:
+                    //connectRet = buffer[1];
+                    //if (connectRet == 0x0)  //飞机连接上 不管飞控和通信模块的连接
+                        CommunicationConnected?.Invoke(this, new CommunicationConnectEventArgs(copterId));
+                    break;
+                case 0x02:
+                    connectRet = buffer[1];
+                    if (connectRet == 0x0)  //飞机断开
+                        CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId));
+                    break;
+
+                case 0x03:
+                    SaveMissionWriteStat(copterId, buffer[1]);
+                    break;
+
+                case 0x04:
+                    Message.Show($"飞机{copterId}:功率设置成功，新回传功率：{buffer[1]}");
+                    break;
+
+                case 0x0e:
+                    if (copterId == 0)
+                        Message.Show("----------全部更新完成----------");
+                    else
+                        Message.Show($"飞机{copterId}:更新进度{buffer[1]}%");
+                    break;
+
+                default:
+                      //  Message.Show($"Comm3返回：{type}");
+                    break;
+            }
+        }
+
+        private void SaveMissionWriteStat(short copterId, byte wirteMissRet)
+        {
+            Task.Run(async () =>
+            {
+                lock (MissinLocker)
+                {
+                   // var copter = _copterManager.Copters.FirstOrDefault(c => c.Id == arrs[0]);
+
+                    if (wirteMissRet == 0x20)
+                    {
+                        if (missionWriteState.ContainsKey(copterId))
+                            missionWriteState[copterId].WriteSucceed = true;
+                        Message.Show($"ID:{copterId}:航点写入成功");
+                    }
+                    if (wirteMissRet > 0x20 && wirteMissRet < 0x30)
+                    {
+                        if (missionWriteState.ContainsKey(copterId))
+                            missionWriteState[copterId].WriteSucceed = false;
+                        Message.Show($"ID:{copterId}:航点写入失败");
+                    }
+                }
+                await Task.Delay(10).ConfigureAwait(false);
+            }
+           );
+            
+        }
+
+        private void AnalyzeCopterInfosPacket(byte[] buffer)
+        {
+            ushort beginNum = BitConverter.ToUInt16(buffer, 0);
+            ushort endNum = BitConverter.ToUInt16(buffer, 2);
+            int count = endNum - beginNum + 1;
+            byte[] copter_packets = buffer.Skip(4).ToArray();
+            if (copter_packets.Length != count * 4)
+            {
+                return;
+            }
+
+            int offset = 0;
+            for (int i = beginNum; i <= endNum; i++)
+            {
+                byte[] onePacket = copter_packets.Skip(offset).Take(4).ToArray();
+                UInt16 state = BitConverter.ToUInt16(onePacket, 0);
+                short copterId = (short)i;
+                switch (state >> 13)
+                {
+                    //0B000
+                    case 0x0000 >> 13:
+                        CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId));
+                        break;
+                    //0B100
+                    case 0x8000 >> 13:
+                        break;
+                    //0B110
+                    case 0xC000 >> 13:
+                    //0B111
+                    case 0xE000 >> 13:
+                        CommunicationConnected?.Invoke(this, new CommunicationConnectEventArgs(copterId));
+                        break;
+                }
+                offset += 4;
+            }
+        }
+
+        private void AnalyzeComm4RetrunPacket(short copterId, byte[] buffer)
+        {
+            byte[] packet = buffer.Take(28).ToArray();
+            byte[] state_packet = buffer.Skip(28).ToArray();
+            UInt16 state = BitConverter.ToUInt16(state_packet,0);
+            byte version = state_packet[3];
+            switch (state >> 13)
+            {
+                //0B000
+                case 0x0000 >> 13:
+                    CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId));
+                    break;
+                //0B100
+                case 0x8000 >> 13:
+                    break;
+                //0B110
+                case 0xC000 >> 13:
+                //0B111
+                case 0xE000 >> 13: //最后正在用的版本
+                    CommunicationReceived?.Invoke(this, new CommunicationReceiveCopterInfoEventArgs(copterId, packet, version));
+                    break;
+            }
+
+        }
+    }
+}

PlaneGcsSdk_Shared/CommunicationManagement/CommWriteMissinState.cs

@@ -0,0 +1,20 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace Plane.CommunicationManagement
+{
+    public class CommWriteMissinState
+    {
+        public CommWriteMissinState(bool sendAchieved)
+        {
+            this.SendAchieved = sendAchieved;
+        }
+        public int StateReturn = 0;
+        public int ErrorCode = 0;
+        public bool SendAchieved = false;
+        public bool WriteSucceed = false;
+
+        public bool IsFailed { get { return StateReturn != 0 || ErrorCode != 0 || !SendAchieved || !WriteSucceed; } }
+    }
+}

PlaneGcsSdk_Shared/CommunicationManagement/CommunicationReceiveCopterInfoEventArgs.cs

@@ -0,0 +1,40 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace Plane.CommunicationManagement
+{
+    public class CommunicationReceiveCopterInfoEventArgs
+    {
+        public CommunicationReceiveCopterInfoEventArgs(int id, byte[] package, byte commModuleVersion)
+        {
+            this.Id = id;
+            this.Package = package;
+            this.CommModuleVersion = commModuleVersion;
+        }
+
+        public int Id { get; private set; }
+        public byte[] Package { get; private set; }
+        public byte CommModuleVersion { get; private set; }
+    }
+
+    public class CommunicationConnectEventArgs
+    {
+        public CommunicationConnectEventArgs(int id)
+        {
+            this.Id = id;
+        }
+
+        public int Id { get; private set; }
+    }
+
+    public class CommunicationCopterDisconnectEventArgs
+    { 
+        public CommunicationCopterDisconnectEventArgs(int id)
+        {
+            this.Id = id;
+        }
+
+        public int Id { get; private set; }
+    }
+}

PlaneGcsSdk_Shared/CopterControllers/AvatarController.cs

@@ -63,7 +63,7 @@ namespace Plane.CopterControllers
         {
             if (_copterManager.Copter != null && _acceptingMobileControlStates.Contains(_copterManager.Copter.State))
             {
-                // TODO: 林俊清, 20151109, 整理并使用 Plane.Copters.Constants 类中的常量。
+                // TODO: 王海, 20151109, 整理并使用 Plane.Copters.Constants 类中的常量。
 
                 ushort maxChannelOffset = (ushort)(Speed == SpeedType.SpeedFast ? 400 : 200);
 

PlaneGcsSdk_Shared/CopterControllers/GuidController.cs

@@ -0,0 +1,355 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace Plane.CopterControllers
+{
+    class GuidController
+    {
+
+        float fabsf(float vvalue)
+        {
+            return Math.Abs(vvalue);
+
+        }
+        float sqrt(float vvalue)
+        {
+            return (float)Math.Sqrt(vvalue);
+
+        }
+
+
+        /// <summary>
+        /// 计算小航点飞行
+        /// </summary>
+        /// <param name="Distance"></param>
+        /// <param name="fc_acc"></param>
+        /// <param name="fc_maxspeed"></param>
+        /// <returns></returns>
+
+        //单算减速，不算加速的最小飞行时间
+        float getMinfligthtime_v2(float Distance, float fc_acc, float fc_maxspeed)
+        {
+            float fDis = fabsf(Distance); // 总距离
+            float facc = fabsf(fc_acc);   // 减速度
+
+            // 物体开始时即以最大速度运动，不考虑加速过程
+            float vel = fc_maxspeed;
+
+            // 计算减速所需的时间和距离
+            // 减速时间 (从最大速度减速到0)
+            float dectime = vel / facc;
+            // 减速阶段覆盖的距离
+            float decdis = (vel * dectime) - (0.5f * facc * dectime * dectime);
+
+            // 判断是否有足够的距离进行减速
+            if (decdis >= fDis)
+            {
+                // 如果减速所需距离已经超过或等于总距离，调整最大速度
+                // 使用公式 v^2 = u^2 + 2as 解出 v
+                vel = (float)Math.Sqrt(2 * facc * fDis);
+                // 重新计算减速时间
+                dectime = vel / facc;
+            }
+
+            // 计算匀速阶段时间
+            float unftime = 0.0f;
+            if (decdis < fDis)
+            {
+                // 如果有剩余距离进行匀速运动
+                unftime = (fDis - decdis) / vel;
+            }
+
+            // 总飞行时间 = 匀速阶段时间 + 减速阶段时间
+            return unftime + dectime;
+        }
+
+        //单算减速，不算加速的最大飞行速度
+        public float CalculateMaximumVelocity(float distance, float time, float acceleration)
+        {
+            // 二次方程系数
+            float a_coeff = 1;
+            float b_coeff = -2 * time;
+            float c_coeff = (2 * distance) / acceleration;
+
+            // 计算判别式
+            float discriminant = b_coeff * b_coeff - 4 * a_coeff * c_coeff;
+
+            if (discriminant < 0)
+            {
+                return -1; // 无实数解
+            }
+
+            // 计算二次方程的根
+            float t1_root1 = (-b_coeff + (float)Math.Sqrt(discriminant)) / (2 * a_coeff);
+            float t1_root2 = (-b_coeff - (float)Math.Sqrt(discriminant)) / (2 * a_coeff);
+
+            // 选择合理的根作为 t1
+            float t1 = Math.Min(t1_root1, t1_root2);
+            if (t1 < 0 || t1 > time)
+            {
+                return -1; // 无合理解
+            }
+
+            // 计算最大速度 v
+            return acceleration * t1;
+        }
+
+        //单算加速，不算减速的距离速度计算
+         float getspeeddist_V2(float flight_time, float Distance, float curr_time, float fc_acc, float fc_maxspeed, out float vdist, out float vspeed)
+        {
+            vdist = 0;
+            vspeed = 0;
+            float desV = CalculateMaximumVelocity(Distance, flight_time, fc_acc);
+            if ((desV == -1) || (desV > fc_maxspeed)) //飞不到
+            {
+                vspeed = Distance / flight_time;
+                vdist = vspeed * curr_time;  //匀速距离
+                return 0;
+
+            }
+            float dect = desV / fc_acc;  // 总加速时间
+            float unit = flight_time - dect; //匀速段时间
+            float decD = (fc_acc * dect * dect) / 2;   //总加速距离
+
+            if (dect > flight_time) dect = flight_time;//约束时间
+            if (curr_time > dect)  //大于加速段时间--匀速
+            {
+                vspeed = (Distance - decD) / (flight_time - dect);
+                vdist = vspeed * (curr_time - dect);
+                vdist = vdist + decD; //总距离=匀速距离+减速距离
+                if (vdist > Distance) vdist = Distance; //约束距离
+
+            }
+            else  //加速段
+            {
+                vspeed = fc_acc * curr_time;
+                vdist = (fc_acc * curr_time * curr_time) / 2;   //加速距离
+            }
+            return 0;
+        }
+
+        //单算减速，不算加速的距离速度计算
+         float getspeeddist_V3(float flight_time, float Distance, float curr_time, float fc_acc, float fc_maxspeed, out float vdist, out float vspeed)
+        {
+            vdist = 0;
+            vspeed = 0;
+            float desV = CalculateMaximumVelocity(Distance, flight_time, fc_acc);
+            if ((desV == -1) || (desV > fc_maxspeed)) //飞不到
+            {
+                vspeed = Distance / flight_time;
+                vdist = vspeed * curr_time;  //匀速距离
+                return 0;
+
+            }
+            float dect = desV / fc_acc;  // 总减速时间
+            float unit = flight_time - dect; //匀速段时间
+            float decD = desV * dect - (fc_acc * dect * dect) / 2;   //总减速距离
+
+            if (dect > flight_time) dect = flight_time;//约束时间
+                                                       //匀速时间内
+            if (curr_time < unit)
+            {
+                vspeed = (Distance - decD) / (flight_time - dect);
+                vdist = vspeed * curr_time;
+            }
+            else
+            {
+                if (((flight_time - dect) * unit) == 0)
+                    vdist = 0;
+                else
+                    vdist = (Distance - decD) / (flight_time - dect) * unit;  //匀速距离
+                float currdect = curr_time - unit;  //减速时间
+                if (currdect >= 0)
+                {
+                    vspeed = desV - fc_acc * currdect;
+                    decD = desV * currdect - (fc_acc * currdect * currdect) / 2;   //减速距离
+                }
+                vdist = vdist + decD; //总距离=匀速距离+减速距离
+                if (vdist > Distance) vdist = Distance; //约束距离
+            }
+            return 0;
+        }
+
+
+
+         float getMinfligthtime(float Distance, float fc_acc, float fc_maxspeed)
+        {
+            float fDis = fabsf(Distance);
+            float facc = fabsf(fc_acc);
+
+            float realflytime = 0.0f;
+            //计算一半的距离
+            float hafdis = (fDis / 2);
+            //计算最大速度
+            float vel = (float)sqrt(2 * facc * hafdis);
+            //如果速度超过最大速度
+            if (vel > fc_maxspeed)
+                //使用最大速度
+                vel = fc_maxspeed;
+            //加速时间
+            float acctim = vel / facc;
+            //加速距离
+            float accdis = vel * vel / (2 * facc);
+            //匀速段时间
+            float vtime = (hafdis - accdis) / vel;
+            //到一半的时间
+            float haftime = acctim + vtime;
+            realflytime = haftime * 2;
+            return realflytime;
+        }
+
+        ///计算飞行距离和速度   单位--米，秒----
+        //返回 整个距离最大飞行速度
+        ///flight_time 总飞行时间  Distance 飞行总距离  curr_time 当前飞行时间  fc_acc加速度, fc_maxspeed最大速度  vdist 计算的应该飞的距离  vspeed 计算的当前时间应该速度
+        //========这是飞控正在使用的算法========
+         float getspeeddist_V1(float flight_time, float Distance, float curr_time, float fc_acc, float fc_maxspeed, out float vdist, out float vspeed)
+        {
+            float plan_flytime=_plan_flytime;
+            float desired_velocity=_desired_velocity;
+            float dist = 0;
+            //导航加速度 米/秒*秒  ---不使用导航库
+            float wpacc = fabsf(fc_acc);// g.fc_acc_xy;//1.5; 
+            //
+            float speed = 0;
+            //计算实时速度
+            if (curr_time <= (plan_flytime / 2.0))
+                speed = curr_time * wpacc;
+            else
+                //需要减速
+                speed = (plan_flytime - curr_time) * wpacc;
+            //不能大于目标速度
+            if (speed > desired_velocity)
+                speed = desired_velocity;
+            if (speed <= 0)
+                speed = 0;
+            vspeed = speed;
+            //计算实时距离
+            float V_start = 0.0f;
+            float V_end = 0.0f;
+            //加速段
+            float t1 = (desired_velocity - V_start) / wpacc;
+            //减速段
+            float t3 = (desired_velocity - V_end) / wpacc;
+            //平飞段
+            float t2 = plan_flytime - t1 - t3;
+            if (curr_time < t1)
+            {
+                dist = 0.5f * wpacc * curr_time * curr_time + curr_time * V_start;
+            }
+            else if (curr_time < t1 + t2)
+            {
+                dist = -0.5f * wpacc * t1 * t1 + (curr_time) * desired_velocity;
+            }
+            else
+            {
+                float t = curr_time - t1 - t2;
+                dist = -0.5f * wpacc * t1 * t1 + curr_time * desired_velocity - 0.5f * wpacc * t * t;
+            }
+
+            if (fabsf(dist) > fabsf(Distance))
+                vdist = Distance;
+            else
+            {
+                if (Distance < 0)
+                    vdist = -dist;
+                else vdist = dist;
+            }
+
+            return desired_velocity;
+        }
+
+         float _vspeed=0;
+
+         float _desired_velocity = 0;
+         float _plan_flytime = 0;
+
+         float initgetspeeddist_V1(float flight_time, float Distance, float fc_acc, float fc_maxspeed)
+        {
+            //计划飞行时间
+            float plan_flytime = flight_time;
+            //计算距离用绝对值
+            float fDis = fabsf(Distance);
+
+            //导航加速度 米/秒*秒  ---不使用导航库
+            float wpacc = fabsf(fc_acc);// g.fc_acc_xy;//1.5; 
+
+            //最大目标速度---米/秒
+            float desired_velocity = 0;
+
+
+            //计算最小飞行时间
+            float minflytime = getMinfligthtime(Distance, fc_acc, fc_maxspeed);
+            if (flight_time < minflytime)
+                plan_flytime = minflytime;// + 0.1f;
+
+
+            //根据总飞行时间和总距离计算飞行速度----起始和结束速度都是0，中间按匀加速和匀减速计算，没考虑加加速度
+            float delta = (0.5f * plan_flytime) * (0.5f * plan_flytime) - fDis / wpacc;
+            if (delta >= 0)
+            {
+                desired_velocity = (0.5f * plan_flytime - sqrt(delta)) / (1 / wpacc);
+                if (desired_velocity > fc_maxspeed)
+                {
+                    plan_flytime = minflytime;
+                    desired_velocity = fc_maxspeed;
+                }
+            }
+            else
+            {
+                desired_velocity = (0.5f * plan_flytime) / (1 / wpacc);
+            }
+            if (desired_velocity < 0.2f) desired_velocity = 0.2f;       //限制最小速度0.2米/秒
+            if (desired_velocity > fc_maxspeed) desired_velocity = fc_maxspeed;//限制最大速度10米/秒，应该参数化
+
+            _desired_velocity = desired_velocity;
+            _plan_flytime = plan_flytime;
+            return desired_velocity;
+        }
+
+
+        public  float initgetspeeddist(int flytype, float flight_time, float Distance, float fc_acc, float fc_maxspeed)
+        {
+            flytype = 0;
+            switch (flytype)
+            {
+                case 0: //匀速 
+                    _vspeed = Distance / flight_time;
+                    return 0;
+                case 1: //同时计算加减速 
+                    return initgetspeeddist_V1(flight_time, Distance, fc_acc, fc_maxspeed);
+                case 2:  //单加速 
+                    return 0;
+                case 3: //单减速
+                    return 0;
+                default:
+                 
+                    return 0;
+            }
+
+        }
+        public  float getspeeddist(int flytype, float flight_time, float Distance, float curr_time, float fc_acc, float fc_maxspeed, out float vdist, out float vspeed)
+        {
+            flytype = 0;
+            switch (flytype)
+            {
+                case 0: //匀速 
+                //case 1: //匀速 
+                    vspeed = _vspeed;
+                    vdist = vspeed * curr_time;
+                    return 0;
+               case 1: //同时计算加减速 
+                    return getspeeddist_V1(flight_time, Distance, curr_time, fc_acc, fc_maxspeed, out vdist, out vspeed);
+                case 2:  //单加速 
+                    return getspeeddist_V2(flight_time, Distance, curr_time, fc_acc, fc_maxspeed, out vdist, out vspeed);
+                case 3: //单减速
+                    return getspeeddist_V3(flight_time, Distance, curr_time, fc_acc, fc_maxspeed, out vdist, out vspeed);
+                default:
+                    vspeed = 0;
+                    vdist = 0;
+                    return 0;
+            }
+        }
+
+        }
+    }

PlaneGcsSdk_Shared/CopterControllers/KeyboardController.cs

@@ -39,7 +39,7 @@ namespace Plane.CopterControllers
                     base.IsEnabled = value;
                     if (value)
                     {
-                        // TODO: 林俊清, 20160303, 修复 KeyboardController。
+                        // TODO: 王海, 20160303, 修复 KeyboardController。
                         //_messenger.Register<KeyDownMessage>(this, OnKeyDown);
                         //_messenger.Register<KeyUpMessage>(this, OnKeyUp);
                     }

PlaneGcsSdk_Shared/CopterManagement/SingleCopterManager.cs

@@ -104,7 +104,7 @@ namespace Plane.CopterManagement
             return Copter.FlyToAsync(lat, lng);
         }
 
-        public Task FlyToAsync(double lat, double lng, float alt)
+        public Task FlyToAsync(double lat, double lng, float alt, float flytime = 0, int flytype = 0)
         {
             return Copter.FlyToAsync(lat, lng, alt);
         }
@@ -147,6 +147,10 @@ namespace Plane.CopterManagement
         {
             return Copter.StartEmergencyHoverAsync();
         }
+        public Task MissionStartAsync(int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat)
+        {
+            return Copter.MissionStartAsync(hour_utc, minute_utc, second_utc, Missionlng, Missionlat);
+        }
 
         public void StartMobileControl(int? millisecondsInterval = default(int?))
         {
@@ -191,7 +195,10 @@ namespace Plane.CopterManagement
             return Copter.InjectGpsDataAsync(data, length);
         }
 
-
+        public Task MotorTestAsync(int motor)
+        {
+            return Copter.TakeOffAsync(motor);
+        }
 
 
         public Task TakeOffAsync(float alt)
@@ -204,6 +211,11 @@ namespace Plane.CopterManagement
             return Copter.UnlockAsync();
         }
 
+        public Task LEDAsync()
+        {
+            return Copter.LEDAsync();
+        }
+
         protected virtual ICopter CreateCopter(string id, string name, IConnection connection)
         {
             return new PLCopter(connection, _uiSyncContext)

PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs

@@ -1,6 +1,8 @@
 using Plane.Geography;
 using System;
+using System.Collections.Generic;
 using System.ComponentModel;
+using System.Drawing;
 using System.Threading;
 using System.Threading.Tasks;
 
@@ -104,6 +106,7 @@ namespace Plane.Copters
         private double _FlightDistance;
 
         private double _FlightDistance2D;
+        private int _FlightControlMode;
 
         private TimeSpan _FlightTime;
 
@@ -117,12 +120,21 @@ namespace Plane.Copters
 
         private short _Heading;
 
+        private byte _CopterErrorCode=0;
+
+        private bool _CopterPreCheckPass=true;
+        private String _CopterPreCheckPassStr;
+
+        private String _CopterErrorString;
+
         private ulong _HeartbeatCount;
 
         private bool _IsAbsolutelyConnected;
 
         private bool _IsCheckingConnection;
 
+        private bool _CommModuleConnected;
+
         private bool _IsConnected;
 
         private bool _IsEmergencyHoverActive;
@@ -138,6 +150,7 @@ namespace Plane.Copters
         private double _Longitude;
 
         private FlightMode _Mode;
+        private FlightMode _CommModuleMode;
 
         private float _Pitch;
 
@@ -145,6 +158,12 @@ namespace Plane.Copters
 
         private byte _SatCount;
 
+        private byte[] _Retain = new byte[4];
+
+        private float _GroundAlt = 0;
+
+        private int _sim_update_int = 50;
+
         private CopterState _State;
 
         private string _StatusText;
@@ -155,17 +174,32 @@ namespace Plane.Copters
 
         private float _Voltage;
 
-        private CopterLocationType _LocationType= CopterLocationType.GPS;
-
         private float _Yaw;
 
-        private int _MissionStatus;
+        //初始时位置：使用地图Center
+        private double _RecordLat;   
+
+        private double _RecordLng;
+
+
+        private int _VirtualId;
+
+        private bool _DisplayVirtualId = false;
+
+        private bool _DisplayID = true;
+
+        private float _maxspeed_xy=3;
+        private float _maxspeed_down=3;
+        private float _maxspeed_up = 3;
+        private float _acc_z=1f;
+        private float _acc_xy=2.5f;
 
         #endregion Backing Fields
 
         public CopterImplSharedPart(SynchronizationContext uiSyncContext) : base(uiSyncContext)
         {
             PropertyChanged += CopterImplSharedPart_PropertyChanged;
+            /*
             Task.Run(async () =>
             {
                 while (true)
@@ -174,6 +208,7 @@ namespace Plane.Copters
                     await Task.Delay(_intervalToUpdateFlightTimeSpan).ConfigureAwait(false);
                 }
             });
+            
             Task.Run(async () =>
             {
                 while (true)
@@ -186,9 +221,10 @@ namespace Plane.Copters
                     {
                         StatusText = null;
                     }
-                    await Task.Delay(50);
+                    await Task.Delay(500);  //50
                 }
             });
+            
             Task.Run(async () =>
             {
                 var lastHeartbeatCount = HeartbeatCount;
@@ -215,7 +251,9 @@ namespace Plane.Copters
                         await Task.Delay(5000).ConfigureAwait(false);
                     }
                 }
+                
             });
+            */
         }
 
         public event EventHandler AltitudeChanged;
@@ -248,6 +286,54 @@ namespace Plane.Copters
             RC_CHANNELS_RAW = 35    // MAVLINK_MSG_ID_RC_CHANNELS_RAW
         }
 
+        private static readonly Dictionary<int, string> ErrorIdToString = new Dictionary<int, string>
+        {
+            {1, "未知异常"},
+            {2, "气压计异常"},
+            {3, "磁罗盘异常"},
+            {4, "GPS异常"},
+            {5, "惯性传感器异常"},
+            {6, "参数异常"},
+            {7, "遥控异常"},
+            {8, "飞控电压低"},
+            {9, "电池电压低"},
+            {10, "空速传感器失败"},
+            {11, "日志记录失败"},
+            {12, "安全开关未开"},
+            {13, "GPS配置异常"},
+            {14, "系统异常"},
+            {15, "任务异常"},
+            {16, "测距传感器异常"},
+            {17, "摄像头异常"},
+            {18, "混控异常"},
+            {19, "版本异常"},
+            {20, "FFT异常"},
+            {21, "陀螺仪无数据"},
+            {22, "陀螺仪没校准"},
+            {23, "加速计没数据"},
+            {24, "加速计没校准"},
+            {25, "加速计需要重启"},
+            {26, "加速计不一致"},
+            {27, "陀螺仪不一致"},
+            {28, "EKF需要定位"},
+            {29, "需要位置估计"},
+            {30, "GPS信号不好"},
+            {31, "EKF磁罗盘变动大"},
+            {32, "EKF位置变动大"},
+            {33, "EKF速度变动大"},
+            {34, "EKF高度变动大"},
+            {35, "需要高度估计"},
+            {36, "航姿没有心跳"},
+            {37, "磁罗盘没有心跳"},
+            {38, "安全开关没按"},
+        };
+        public  String getcoptererrorstr(byte errorcode)
+        {
+            String errorstr = "未知";
+            ErrorIdToString.TryGetValue(errorcode, out errorstr);
+            String retstr = "["+errorcode.ToString()+"]" + errorstr;
+            return retstr;
+        }
         public float AirSpeed
         {
             get { return _AirSpeed; }
@@ -331,7 +417,7 @@ namespace Plane.Copters
             get { return _DesiredChannel3; }
             set
             {
-                // 林俊清, 20160317, 紧急悬停时可调节高度。
+                // 王海, 20160317, 紧急悬停时可调节高度。
                 //if (!IsEmergencyHoverActive)
                 //{
                 if (Set(nameof(DesiredChannel3), ref _DesiredChannel3, value))
@@ -384,6 +470,26 @@ namespace Plane.Copters
             protected set { Set(nameof(Elevation), ref _Elevation, value); }
         }
 
+        public int VirtualId
+        {
+            get { return _VirtualId; }
+            set { Set(nameof(VirtualId), ref _VirtualId, value); RefreashLoc(); }
+        }
+
+        public bool DisplayVirtualId
+        {
+            get { return _DisplayVirtualId; }
+            set { Set(nameof(DisplayVirtualId), ref _DisplayVirtualId, value); RefreashLoc(); }
+        }
+
+        public bool DisplayID
+        {
+            get { return _DisplayID; }
+            set { Set(nameof(DisplayID), ref _DisplayID, value); RefreashLoc(); }
+        }
+
+
+
         /// <summary>
         /// 获取固件版本。
         /// </summary>
@@ -438,6 +544,15 @@ namespace Plane.Copters
             protected set { Set(nameof(FlightDistance2D), ref _FlightDistance2D, value); }
         }
 
+        //FlightControlMode=1表示同时到达，速度可变,用于新固件
+        //=0表示固定速度，用于老固件
+        public int FlightControlMode
+        {
+            get { return _FlightControlMode; }
+            set { Set(nameof(FlightControlMode), ref _FlightControlMode, value); }
+        }
+
+
         public TimeSpan FlightTimeSpan
         {
             get { return _FlightTime; }
@@ -462,6 +577,56 @@ namespace Plane.Copters
             protected set { Set(nameof(GpsHdop), ref _GpsHdop, value); }
         }
 
+        /// <summary>
+        /// 飞机回传的错误码，可知道飞机当前状态，比如是否加速计不一致，是否磁罗盘不一致等，是黄灯的具体代码。
+        /// </summary>
+        public byte CopterErrorCode
+        {
+            get { return _CopterErrorCode; }
+            protected set { Set(nameof(CopterErrorCode), ref _CopterErrorCode, value); }
+        }
+
+        /// <summary>
+        /// 飞机预检查是否通过。
+        /// </summary>
+        public bool CopterPreCheckPass
+        {
+            get { return _CopterPreCheckPass; }
+            protected set { 
+                Set(nameof(CopterPreCheckPass), ref _CopterPreCheckPass, value);
+                if (_CopterPreCheckPass)
+                    CopterPreCheckPassStr= "通过";
+                else
+                    CopterPreCheckPassStr= "异常";
+
+                
+            }
+        }
+
+        /// <summary>
+        /// 飞机预检查是否通过。
+        /// </summary>
+        public string CopterPreCheckPassStr
+        {
+            get
+            {
+                return _CopterPreCheckPassStr;
+            }
+            protected set { Set(nameof(CopterPreCheckPassStr), ref _CopterPreCheckPassStr, value); }
+
+        }
+
+
+
+        /// <summary>
+        /// 飞机回传的最后错误码转换的字符串，可知道飞机当前状态，比如是否加速计不一致，是否磁罗盘不一致等，是黄灯的具体代码。
+        /// </summary>
+        public String CopterErrorString
+        {
+            get { return _CopterErrorString; }
+            protected set { Set(nameof(CopterErrorString), ref _CopterErrorString, value); }
+        }
+
         public float GroundSpeed
         {
             get { return _GroundSpeed; }
@@ -510,6 +675,12 @@ namespace Plane.Copters
             protected set { Set(nameof(IsConnected), ref _IsConnected, value); }
         }
 
+        public bool CommModuleConnected
+        {
+            get { return _CommModuleConnected; }
+            set { Set(nameof(CommModuleConnected), ref _CommModuleConnected, value); }
+        }
+
         public bool IsEmergencyHoverActive
         {
             get { return _IsEmergencyHoverActive; }
@@ -562,6 +733,18 @@ namespace Plane.Copters
             protected set { Set(nameof(Longitude), ref _Longitude, value); }
         }
 
+        public double RecordLat
+        {
+            get { return _RecordLat; }
+            set { Set(nameof(RecordLat), ref _RecordLat, value); }
+        }
+
+        public double RecordLng
+        {
+            get { return _RecordLng; }
+            set { Set(nameof(RecordLng), ref _RecordLng, value); }
+        }
+
         /// <summary>
         /// 获取任务总数。
         /// </summary>
@@ -587,6 +770,84 @@ namespace Plane.Copters
             protected set { Set(nameof(SatCount), ref _SatCount, value); }
         }
 
+        public float RetainInt
+        {
+            get { return BitConverter.ToSingle(Retain,0); }
+        }
+
+        public float GroundAlt
+        {
+            get { return _GroundAlt; }
+            set {
+                Set(nameof(GroundAlt), ref _GroundAlt, value);
+                RaisePropertyChanged(nameof(GroundAlt));
+            }
+        }
+
+
+        public int sim_update_int
+        {
+            get { return _sim_update_int; }
+            set
+            {
+                Set(nameof(sim_update_int), ref _sim_update_int, value);
+            }
+        }
+
+
+        public float maxspeed_xy {
+            get { return _maxspeed_xy; }
+            set
+            {
+                Set(nameof(maxspeed_xy), ref _maxspeed_xy, value);
+            }
+        }
+
+        public float maxspeed_up
+        {
+            get { return _maxspeed_up; }
+            set
+            {
+                Set(nameof(maxspeed_up), ref _maxspeed_up, value);
+            }
+        }
+        public float maxspeed_down
+        {
+            get { return _maxspeed_down; }
+            set
+            {
+                Set(nameof(maxspeed_down), ref _maxspeed_down, value);
+            }
+        }
+        public float acc_z
+        {
+            get { return _acc_z; }
+            set
+            {
+                Set(nameof(acc_z), ref _acc_z, value);
+            }
+        }
+        public float acc_xy
+        {
+            get { return _acc_xy; }
+            set
+            {
+                Set(nameof(acc_xy), ref _acc_xy, value);
+            }
+        }
+
+
+
+        public byte[] Retain
+        {
+            get { return _Retain; }
+            protected set
+            {
+                Set(nameof(Retain), ref _Retain, value);
+                RaisePropertyChanged(nameof(RetainInt));
+            }
+        }
+
         public CopterState State
         {
             get { return _State; }
@@ -617,25 +878,81 @@ namespace Plane.Copters
             protected set { Set(nameof(Voltage), ref _Voltage, value); }
         }
 
-
-        public CopterLocationType LocationType
-        {
-            get { return _LocationType; }
-            protected set { Set(nameof(LocationType), ref _LocationType, value); }
-        }
-
         public float Yaw
         {
             get { return _Yaw; }
             protected set { Set(nameof(Yaw), ref _Yaw, value); }
         }
 
-        public int MissionStatus
+        public FlightMode CommModuleMode
         {
-            get { return _MissionStatus; }
-            set { Set(nameof(MissionStatus), ref _MissionStatus, value); }
+            get { return _CommModuleMode; }
+            protected set { Set(nameof(CommModuleMode), ref _CommModuleMode, value); }
         }
 
+        private byte _CommModuleVersion;
+        public byte CommModuleVersion
+        {
+            get { return _CommModuleVersion; }
+            protected set { Set(nameof(CommModuleVersion), ref _CommModuleVersion, value); }
+        }
+
+        private string _LEDColor;
+        public string LEDColor
+        {
+            get { return _LEDColor; }
+            set {
+                if (value!=_LEDColor)
+                {
+                    Set(nameof(LEDColor), ref _LEDColor, value);
+                    //强制刷新颜色--在刷新位置时才刷新颜色
+                    RefreashLoc();
+
+                }
+            }
+        }
+
+        private Color _LEDShowColor;
+        public Color LEDShowColor
+        {
+            get { return _LEDShowColor; }
+            set
+            {
+ 
+                Set(nameof(LEDShowColor), ref _LEDShowColor, value);
+
+            }
+        }
+
+
+        private int _LEDMode;
+        public int LEDMode
+        {
+            get { return _LEDMode; }
+            set
+            {
+                if (value != _LEDMode)
+                {
+                    Set(nameof(LEDMode), ref _LEDMode, value);
+                    //强制刷新颜色--在刷新位置时才刷新颜色
+                   // RefreashLoc();
+                }
+            }
+        }
+        private float _LEDInterval;
+        public float LEDInterval
+        {
+            get { return _LEDInterval; }
+            set
+            {
+                if (value != _LEDInterval)
+                {
+                    Set(nameof(LEDInterval), ref _LEDInterval, value);
+                    //强制刷新颜色--在刷新位置时才刷新颜色
+                   // RefreashLoc();
+                }
+            }
+        }
       
 
 #if PRIVATE
@@ -648,7 +965,7 @@ namespace Plane.Copters
             get { return _Mode; }
             set
             {
-                // 林俊清, 20160325, Mode 属性改成 internal 了，如果仍然用 Set 或者 RaisePropertyChanged 方法，会抛异常（"Property not found"）。现决定不引发 PropertyChanged 事件了。
+                // 王海, 20160325, Mode 属性改成 internal 了，如果仍然用 Set 或者 RaisePropertyChanged 方法，会抛异常（"Property not found"）。现决定不引发 PropertyChanged 事件了。
                 var changed = _Mode != value;
                 if (changed) _Mode = value;
 
@@ -702,67 +1019,16 @@ namespace Plane.Copters
             return FlyToAsync(lat, lng, Altitude);
         }
 
-        public Task FlyToAsync(double lat, double lng, float alt)
+        public Task FlyToAsync(double lat, double lng, float alt, float flytime = 0, int flytype = 0)
         {
             _shouldFollow = false;
             //State = CopterState.CommandMode;
-            return FlyToCoreAsync(lat, lng, alt);
+            return FlyToCoreAsync(lat, lng, alt, flytime, flytype);
         }
 
         public void Follow(IVisibleStatus target, bool keepYawDifference = false, bool keepFacingTarget = true, bool keep3DRelativeLocations = false)
         {
-            // 林俊清, 20160409, 在目前的实现中，跟随状态使用 GUIDED 模式。
           
-            if (!IsConnected || !IsUnlocked || this == target || IsEmergencyHoverActive) return;
-
-            _followTarget = target;
-            _followAltDifference = Altitude - target.Altitude;
-            _followDistance = (float)target.CalcDistance2D(this);
-            _followKeepYawDifference = keepYawDifference;
-            _followKeepFacingTarget = keepFacingTarget;
-            _followKeep3DRelativeLocations = keep3DRelativeLocations;
-            _followSelfDirectionFromTarget = (float)target.CalcDirection2D(this).RadToDeg();
-            _followTargetOriginalYaw = target.Yaw.NormalizeDirection();
-
-            if (State == CopterState.Following) return;
-
-            Task.Run(async () =>
-            {
-                State = CopterState.Following;
-                _shouldFollow = true;
-                while (IsConnected && _shouldFollow)
-                {
-                    // 计算飞行器应当处于什么位置，并调用 FlyTo 使其飞往彼处。
-
-                    var destination2D = _followTarget.CalcLatLngSomeMetersAway2D(_followKeepYawDifference ? (_followSelfDirectionFromTarget - _followTargetOriginalYaw + _followTarget.Yaw.NormalizeDirection()).NormalizeDirection() : _followSelfDirectionFromTarget, _followDistance);
-                    var destinationAlt = _followKeep3DRelativeLocations ? _followTarget.Altitude + _followAltDifference : Altitude;
-                    var destination = new PLLocation(destination2D.Latitude, destination2D.Longitude, destinationAlt);
-                    if (_followLastDestination == null || _followLastDestination.CalcDistance(destination) >= 1.5)
-                    {
-                        await FlyToCoreAsync(destination2D.Latitude, destination2D.Longitude, destinationAlt).ConfigureAwait(false);
-                        _followLastDestination = destination;
-                    }
-
-                    // 如果需要保持面对目标，计算并使用偏航和云台俯仰的期望值。
-
-                    if (_followKeepFacingTarget)
-                    {
-                        var yaw = (float)this.CalcDirection2D(_followTarget).RadToDeg();
-                        SetFieldAndRaisePropertyChanged(ref _DesiredYaw, yaw, nameof(DesiredYaw));
-
-                        var distance2DFromTargetToCopter = _followTarget.CalcDistance2D(this);
-                        var altDifferenceFromTargetToCopter = Altitude - _followTarget.Altitude;
-                        // 正前方值为 0，向下取正值，向上取负值。
-                        var gimbalPitchRad = Math.Atan2(altDifferenceFromTargetToCopter, distance2DFromTargetToCopter);
-                        // 正前方值为 1500，向下 80° 值为 1900。
-                        var ch7 = (ushort)((1900 - 1500) * gimbalPitchRad / 80F.DegToRad() + 1500);
-                        SetFieldAndRaisePropertyChanged(ref _DesiredChannel7, ch7, nameof(DesiredChannel7));
-                    }
-
-                    await Task.Delay(50).ConfigureAwait(false);
-                }
-                if (!IsConnected) _shouldFollow = false;
-            });
         }
 
         public async Task HoverAsync()
@@ -811,7 +1077,7 @@ namespace Plane.Copters
 
         public abstract Task SetChannelsAsync();
 
-        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) // 林俊清, 20150912, 将来如有需要再补上 TargetAlt, float? alt = null)
+        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) // 王海, 20150912, 将来如有需要再补上 TargetAlt, float? alt = null)
         {
             SetTargets(ch1, ch2, ch3, ch4, ch5, ch6, ch7, ch8, yaw);
 
@@ -826,8 +1092,8 @@ namespace Plane.Copters
             State = CopterState.HoverMode;
             IsEmergencyHoverActive = true;
 
-            // 林俊清, 20151028, 先 IsEmergencyHoverActive = true 再设置目标通道是为了防止别的线程正好在 IsEmergencyHoverActive == false 时设置通道。
-            // 林俊清, 20151019, 在紧急悬停状态，外部设置 DesiredChannel[1-4] 无效，只能如此设置。
+            // 王海, 20151028, 先 IsEmergencyHoverActive = true 再设置目标通道是为了防止别的线程正好在 IsEmergencyHoverActive == false 时设置通道。
+            // 王海, 20151019, 在紧急悬停状态，外部设置 DesiredChannel[1-4] 无效，只能如此设置。
             SetFieldAndRaisePropertyChanged(ref _DesiredChannel1, 1500, nameof(DesiredChannel1));
             SetFieldAndRaisePropertyChanged(ref _DesiredChannel2, 1500, nameof(DesiredChannel2));
             SetFieldAndRaisePropertyChanged(ref _DesiredChannel3, 1500, nameof(DesiredChannel3));
@@ -837,7 +1103,7 @@ namespace Plane.Copters
 
             switch (Mode)
             {
-                // 林俊清, 20151019, 波子说一律切 LOITER。
+                // 王海, 20151019, 波子说一律切 LOITER。
                 //case FlightMode.ALT_HOLD:
                 //    break;
                 //case FlightMode.LOITER:
@@ -969,7 +1235,7 @@ namespace Plane.Copters
             async Task<bool> SetModeAsync(FlightMode mode, int millisecondsTimeout = 5000)
         {
             if (_shouldFollow && mode != FlightMode.GUIDED) _shouldFollow = false;
-            // 林俊清, 20160317, 紧急悬停时可返航或降落。
+            // 王海, 20160317, 紧急悬停时可返航或降落。
             if (mode == FlightMode.RTL || mode == FlightMode.LAND)
             {
                 StopEmergencyHover();
@@ -1013,7 +1279,7 @@ namespace Plane.Copters
         /// <param name="lng">经度。</param>
         /// <param name="alt">高度。</param>
         /// <returns>表示此异步发送操作的 <see cref="Task{TResult}"/> 实例。</returns>
-        protected abstract Task FlyToCoreAsync(double lat, double lng, float alt);
+        protected abstract Task FlyToCoreAsync(double lat, double lng, float alt, float flytime = 0, int flytype = 0);
 
         protected void RaiseAltitudeChanged() => AltitudeChanged?.Invoke(this, EventArgs.Empty);
 
@@ -1043,14 +1309,33 @@ namespace Plane.Copters
 
         protected void RaiseLocationChanged() => LocationChanged?.Invoke(this, EventArgs.Empty);
 
-        protected void RaiseLocationChangedIfNeeded()
+        public void RefreashLoc()
         {
-            if (_lastChangedLocation == null || this.CalcDistance(_lastChangedLocation) >= 0.5 || DateTime.Now.AddMilliseconds(-500) >= _lastRaiseLocationChangedTime)
+            RaiseLocationChangedIfNeeded(true);
+        }
+
+        protected void RaiseLocationChangedIfNeeded(bool forcemk=false)
+        {
+            //强制刷新位置
+            if (forcemk)
+            {
+                RaiseLocationChanged();
+                return;
+            }
+            //减少计算量，在模拟飞机很多时花时间
+            bool EnRaise = true;
+            if (_lastChangedLocation != null)
+                EnRaise = (Altitude != _lastChangedLocation.Altitude) ||
+                          (Latitude != _lastChangedLocation.Latitude) ||
+                          (Longitude != _lastChangedLocation.Longitude);
+            //if (_lastChangedLocation == null || this.CalcDistance(_lastChangedLocation) >= 0.5 || DateTime.Now.AddMilliseconds(-500) >= _lastRaiseLocationChangedTime)
+            if (EnRaise)
             {
                 RaiseLocationChanged();
                 _lastChangedLocation = new PLLocation(this);
                 _lastRaiseLocationChangedTime = DateTime.Now;
             }
+            
         }
 
         protected void RaiseMissionItemReceived(MissionItemReceivedEventArgs e)
@@ -1138,7 +1423,7 @@ namespace Plane.Copters
                 case nameof(IsUnlocked):
                     if (IsUnlocked)
                     {
-                        // 林俊清, 20151029, 在解锁时重置起飞点、起飞时间、飞行距离、飞行时间、速度。
+                        // 王海, 20151029, 在解锁时重置起飞点、起飞时间、飞行距离、飞行时间、速度。
                         TakeOffPoint = new PLLocation { Latitude = Latitude, Longitude = Longitude, Altitude = Altitude };
                         _takeOffTime = DateTime.Now;
                         FlightDistance = 0;

PlaneGcsSdk_Shared/Copters/EHCopter.InternalCopterEventHandlers.cs

@@ -1,10 +1,14 @@
 using System;
+using System.Collections.Generic;
 using static Plane.Protocols.MAVLink;
 
 namespace Plane.Copters
 {
+
+
     public partial class PLCopter : CopterImplSharedPart
     {
+       
         private bool _fetchingFirmwareVersion;
 
         private void _internalCopter_ConnectionBroken(object sender, EventArgs e)
@@ -20,24 +24,41 @@ namespace Plane.Copters
             RaiseTextReceived(e);
         }
     
+            
         private void _internalCopter_ReceiveDataStreamEvent(object sender, byte StreamType)
         {
             switch (StreamType)
             {
                 case MAVLINK_MSG_ID_GPS_RAW_INT:    //需要
                     {
-                        Latitude = _internalCopter.lat;
-                        Longitude = _internalCopter.lng;
                         SatCount = _internalCopter.satcount;
                         GpsFixType = _internalCopter.gpsstatus.ToGpsFixType();
                         GpsHdop = _internalCopter.gpshdop;
-                        Elevation = _internalCopter.gpsalt;
+                        CopterErrorCode =_internalCopter.errorcode;
+                        CopterPreCheckPass = _internalCopter.precheckok;
+                        CopterErrorString = getcoptererrorstr(_internalCopter.errorcode);
+                        Altitude = _internalCopter.gpsalt;
+                        Retain = BitConverter.GetBytes(_internalCopter.retain);
                         
-                        if (IsGpsAccurate)
+                        Voltage = _internalCopter.battery_voltage;
+                        CommModuleMode = (FlightMode)_internalCopter.commModuleMode;
+                        CommModuleVersion = _internalCopter.commModuleVersion;
+                        IsUnlocked = _internalCopter.isUnlocked;
+                        //Yaw = _internalCopter.yaw;
+                        Heading = _internalCopter.heading;
+                        HeartbeatCount++;
+
+                        if (SatCount > 8)
                         {
+                            IsGpsAccurate = true;
+                            RecordLat = _internalCopter.lat;
+                            RecordLng = _internalCopter.lng;
+                        }
+
+                        Latitude = RecordLat;
+                        Longitude = RecordLng;
                         UpdateFlightDataIfNeeded();
                         RaiseLocationChangedIfNeeded();
-                        }
 
                         RaiseDataStreamReceived(PDataStreamType.GPS_RAW_INT);
 
@@ -86,7 +107,7 @@ namespace Plane.Copters
         {
             IsUnlocked = _internalCopter.armed;
             Mode = (FlightMode)_internalCopter.mode;
-            // 林俊清, 20160311, 将闲置的 apname 用作已经切过 GPS 模式的标志。
+            // 王海, 20160311, 将闲置的 apname 用作已经切过 GPS 模式的标志。
             HasSwitchedToGpsMode = (byte)_internalCopter.apname != 0;
             ++HeartbeatCount;
 

PlaneGcsSdk_Shared/Copters/EHCopter.Mission.cs

@@ -10,7 +10,7 @@ namespace Plane.Copters
 {
     public partial class PLCopter
     {
-        private static readonly IMission PRE_TAKE_OFF_MISSION = new Mission
+        public static readonly IMission PRE_TAKE_OFF_MISSION = new Mission
         {
             Command = FlightCommand.Waypoint,
             Sequence = 0
@@ -132,7 +132,7 @@ namespace Plane.Copters
             else _writeMissionListStopwatch.Restart();
 
             // 写任务总数。
-            await WriteMissionCountAsync((ushort)(missions.Count() + 2)).ConfigureAwait(false);
+            await WriteMissionCountAsync((ushort)(missions.Count() + 1)).ConfigureAwait(false);
 
             // 任务序号。
             ushort seq = 0;
@@ -146,7 +146,7 @@ namespace Plane.Copters
 
             // 写起飞前准备任务。
             await WriteMissionAsync(PRE_TAKE_OFF_MISSION).ConfigureAwait(false);
-
+/*
             seq++;
 
             // 等待飞控请求 seq 号任务。
@@ -158,6 +158,7 @@ namespace Plane.Copters
 
             // 写起飞任务。
             await WriteMissionAsync(TAKE_OFF_MISSION).ConfigureAwait(false);
+            */
 
             foreach (var mission in missions)
             {
@@ -239,9 +240,19 @@ namespace Plane.Copters
             req.autocontinue = 1;
 
             req.frame = (byte)frame;
+            if (mission.Command == FlightCommand.LEDControl)
+            {
+                req.x = mission.R;
+                req.y = mission.G;
+                req.z = mission.B;
+            }
+            else
+            {
                 req.x = (float)mission.Latitude;
                 req.y = (float)mission.Longitude;
                 req.z = mission.Altitude;
+            }
+           
 
             req.param1 = mission.Param1;
             req.param2 = mission.Param2;
@@ -250,6 +261,7 @@ namespace Plane.Copters
 
             req.seq = mission.Sequence;
             
+
             return _internalCopter.GeneratePacketAsync(MAVLINK_MSG_ID_MISSION_ITEM, req);
         }
 

PlaneGcsSdk_Shared/Copters/EmptyCopter.Mission.cs

@@ -24,5 +24,14 @@ namespace Plane.Copters
         {
             return Task.FromResult(true);
         }
+        public async Task DoCommandAsync(int actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7)
+        {
+
+            await Task.Delay(50).ConfigureAwait(false);
+
+
+        }
+
+
     }
 }

PlaneGcsSdk_Shared/Copters/EmptyCopter.cs

@@ -44,6 +44,11 @@ namespace Plane.Copters
             return TaskUtils.CompletedTask;
         }
 
+        public Task LEDAsync()
+        {
+            return TaskUtils.CompletedTask;
+        }
+
         public override Task SetChannelsAsync()
         {
             return TaskUtils.CompletedTask;
@@ -54,6 +59,24 @@ namespace Plane.Copters
             return TaskUtils.CompletedTask;
         }
 
+        public void GetCommunicationNumber(out int recnumber, out int sendnumber)
+        {
+            recnumber = 0;
+            sendnumber = 0;
+        }
+        public  Task DoCommandAckAsync(ushort command, byte result)
+        {
+            return TaskUtils.CompletedTask;
+        }
+
+
+        //重设数据量
+        public void ResetCommunicationNumber()
+        {
+            
+        }
+
+
         public Task SetParamAsync(string paramName, float value, int millisecondsTimeout = -1)
         {
             return TaskUtils.CompletedTask;
@@ -84,11 +107,6 @@ namespace Plane.Copters
             return TaskUtils.CompletedTask;
         }
 
-        public Task InitAsync()
-        {
-            return TaskUtils.CompletedTask;
-        }
-
         public Task StopPairingAsync()
         {
             return TaskUtils.CompletedTask;
@@ -98,12 +116,22 @@ namespace Plane.Copters
         {
             return TaskUtils.CompletedTask;
         }
-
+        public Task MissionStartAsync(int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat)
+        {
+            return TaskUtils.CompletedTask;
+        }
         public Task UnlockAsync()
         {
             return TaskUtils.CompletedTask;
         }
 
+        public Task MotorTestAsync(int motor)
+        {
+            return TaskUtils.CompletedTask;
+        }
+
+        
+
         public Task InjectGpsDataAsync(byte[] data, ushort length)
         {
             return TaskUtils.CompletedTask;
@@ -116,7 +144,7 @@ namespace Plane.Copters
             return Task.FromResult(true);
         }
 
-        protected override Task FlyToCoreAsync(double lat, double lng, float alt)
+        protected override Task FlyToCoreAsync(double lat, double lng, float alt, float flytime = 0, int flytype = 0)
         {
             return TaskUtils.CompletedTask;
         }

PlaneGcsSdk_Shared/Copters/FakeCopter.cs

@@ -2,9 +2,12 @@
 using Plane.Geography;
 using System;
 using System.Diagnostics;
+using System.Drawing;
 using System.Threading;
 using System.Threading.Tasks;
 using static Plane.Copters.Constants;
+using System.Windows.Media;
+using Plane.CopterControllers;
 
 namespace Plane.Copters
 {
@@ -18,21 +21,25 @@ namespace Plane.Copters
         /// 心跳间隔，单位为毫秒。
         /// </summary>
         private const int HEARTBEAT_INTERVAL = 200;
+        /// <summary>
+        /// 更新虚拟飞行器状态的时间间隔，单位为毫秒。
+        /// </summary>
+        static private int UPDATE_INTERVAL = 100;  //默认100， i7电脑 150可以跑1000架 100可以跑500架 50可以200-300架 
 
+        static private int UPDATE_INTERVAL_TEMP = 50;
         /// <summary>
         /// 在一个更新间隔中的最大移动距离。
         /// </summary>
-        private const float MAX_MOVE_IN_INTERVAL = MAX_VEL * UPDATE_INTERVAL / 1000;
+        private float MAX_MOVE_IN_INTERVAL = MAX_VEL * UPDATE_INTERVAL_TEMP / 1000;
 
         /// <summary>
         /// 高速模式下，在一个更新间隔中的最大移动距离。
         /// </summary>
-        private const float MAX_MOVE_IN_INTERVAL_FAST = MAX_MOVE_IN_INTERVAL * 4;
+        private float MAX_MOVE_IN_INTERVAL_FAST = MAX_VEL * UPDATE_INTERVAL_TEMP / 1000 * 4;  //MAX_MOVE_IN_INTERVAL*4
 
-        /// <summary>
-        /// 更新虚拟飞行器状态的时间间隔，单位为毫秒。
-        /// </summary>
-        private const int UPDATE_INTERVAL = 100;
+
+
+        // private int _UPDATE_INTERVAL = 50;
 
         /// <summary>
         /// 对飞行器的模拟是否正在运行。
@@ -52,12 +59,12 @@ namespace Plane.Copters
         /// <summary>
         /// 高速模式下，按比例缩放过的在一个更新间隔中的最大移动距离。
         /// </summary>
-        private float _scaledFastMaxMoveInInterval = MAX_MOVE_IN_INTERVAL_FAST;
+        private float _scaledFastMaxMoveInInterval = MAX_VEL * UPDATE_INTERVAL_TEMP / 1000 * 4;  //MAX_MOVE_IN_INTERVAL_FAST
 
         /// <summary>
         /// 按比例缩放过的在一个更新间隔中的最大移动距离。
         /// </summary>
-        private float _scaledMaxMoveInInterval = MAX_MOVE_IN_INTERVAL;
+        private float _scaledMaxMoveInInterval = MAX_VEL * UPDATE_INTERVAL_TEMP / 1000;  //MAX_MOVE_IN_INTERVAL
 
         /// <summary>
         /// 速度缩放比例。
@@ -73,6 +80,28 @@ namespace Plane.Copters
         /// FlyTo 的目标高度。
         /// </summary>
         private float _targetAlt;
+        //航点开始高度
+        private float _startAlt;
+
+        private float _Lng_delta;
+        private float _Lat_delta;
+
+        private float _flytime;
+        private DateTime _startTicks;
+
+        private float _distance_xy;
+        private float _distance_z;
+        private float _xy_speed;
+        private float _z_speed;
+        private int _flytype;
+        private float currflytime;
+        // 根据飞行时间计算飞行距离。
+        private float currdis_xy;
+        private float currdis_z;
+
+
+        // 目标点相对于开始位置的方向。
+        private double _direction;
 
         /// <summary>
         /// FlyTo 的目标纬度。
@@ -83,6 +112,15 @@ namespace Plane.Copters
         /// FlyTo 的目标经度。
         /// </summary>
         private double _targetLng;
+        /// <summary>
+        /// FlyTo 的目标纬度。
+        /// </summary>
+        private double _startLat;
+
+        /// <summary>
+        /// FlyTo 的目标经度。
+        /// </summary>
+        private double _startLng;
 
 
         /// <summary>
@@ -90,12 +128,50 @@ namespace Plane.Copters
         /// </summary>
         private bool _ShowLED;
 
+        private System.Drawing.ColorConverter rgbconverter;
+
+
+        private PLLocation _takeoffcentloc;
+        private PLLocation _taskcentloc;
+        private float _mindistance;
+        private float _rettime;
+        private bool _descending;
+        int Emergency_Retstatus = 0;
+        DateTime EmergencyRetTime;
+        //返航路径第一个航点
+        PLLocation Emergency_firstloc;
 
         /// <summary>
         /// 使用 <see cref="SynchronizationContext.Current"/> 创建 <see cref="FakeCopter"/> 实例。
         /// </summary>
         public FakeCopter() : this(SynchronizationContext.Current)
         {
+
+
+        }
+
+
+        new public int sim_update_int
+        {
+            get { return UPDATE_INTERVAL; }
+            set
+            {
+                //最大移动距离
+                MAX_MOVE_IN_INTERVAL = MAX_VEL * value / 1000;
+                //快速模式最大移动距离
+                MAX_MOVE_IN_INTERVAL_FAST = MAX_MOVE_IN_INTERVAL * 4;
+                //速度缩放后快速速度距离
+                _scaledFastMaxMoveInInterval = MAX_MOVE_IN_INTERVAL_FAST * _speedScale;
+                //速度缩放后速度距离
+                _scaledMaxMoveInInterval = MAX_MOVE_IN_INTERVAL * _speedScale;
+                Set(nameof(sim_update_int), ref UPDATE_INTERVAL, value);
+            }
+        }
+
+
+        public Task DoCommandAckAsync(ushort command, byte result)
+        {
+            return TaskUtils.CompletedTask;
         }
 
         /// <summary>
@@ -122,7 +198,7 @@ namespace Plane.Copters
                     case nameof(IsUnlocked):
                         if (IsUnlocked)
                         {
-                            // 林俊清, 20151029, 在解锁时上次计算速度点、上次计算速度时刻。
+                            // 王海, 20151029, 在解锁时上次计算速度点、上次计算速度时刻。
                             _lastCalcSpeedPoint = new PLLocation { Latitude = Latitude, Longitude = Longitude, Altitude = Altitude };
                             _lastCalcSpeedTime = DateTime.Now;
                         }
@@ -137,20 +213,32 @@ namespace Plane.Copters
             _scaledFastMaxMoveInInterval = MAX_MOVE_IN_INTERVAL_FAST * _speedScale;
 
             this.Connection = connection ?? EmptyConnection.Instance;
-
+            _isRunning = true;
             // 持续计算并更新虚拟飞行器的状态。
-            Task.Run(async () =>
+            // Task.Run(async () =>
+
+            Task.Factory.StartNew(async () =>
+
             {
-                while (true)
-                {
-                    if (_isRunning)
+                while (_isRunning)
                 {
                     Update();
-                    }
                     await TaskUtils.Delay(UPDATE_INTERVAL).ConfigureAwait(false);
                 }
-            });
+            }
+            , TaskCreationOptions.LongRunning
+            );
 
+
+            ++HeartbeatCount;
+            GpsFixType = GpsFixType.Fix3D;
+            GpsHdop = 1;
+            IsGpsAccurate = true;
+            HasSwitchedToGpsMode = true;
+            GC_xy = new GuidController();
+            GC_z = new GuidController();
+
+            /*
             // 持续假装收到飞行器发来的心跳。
             Task.Run(async () =>
             {
@@ -176,6 +264,7 @@ namespace Plane.Copters
                     await TaskUtils.Delay(HEARTBEAT_INTERVAL).ConfigureAwait(false);
                 }
             });
+            */
         }
 
         public IConnection Connection { get; set; }
@@ -183,6 +272,9 @@ namespace Plane.Copters
         public string Id { get; set; }
 
         private int ReturnToLaunchAltitude { get { return _takeOffTargetAltitude; } }
+        private GuidController GC_xy;
+        private GuidController GC_z;
+
 
         public Task ConnectAsync()
         {
@@ -199,15 +291,86 @@ namespace Plane.Copters
             IsCheckingConnection = false;
             return TaskUtils.CompletedTask;
         }
+        private int minretalt=15; //最低返航高度
+        private int minretalt_first = 25; //第一次返航高度
+        //返航位置1
+        private PLLocation EmergencyRetPLLocation1;
 
-        protected override Task FlyToCoreAsync(double lat, double lng, float alt)
+        private void BuildPath()
+        {
+            //计算当前位置和起飞中心点的距离
+            float dis = (float)this.CalcDistance(_takeoffcentloc.Latitude, _takeoffcentloc.Longitude, 0);
+            _rettime = (dis - _mindistance) * 1.0f;
+
+            //计算方向--角度
+            float takeoffdir =(float)_takeoffcentloc.CalcDirection2D(TakeOffPoint)* 57.29578f;
+            //计算起飞点距离
+            float takeoffdis = (float)TakeOffPoint.CalcDistance2D(_takeoffcentloc.Latitude, _takeoffcentloc.Longitude);
+
+            
+            ILocation2D  take1 = TakeOffPoint.CalcLatLngSomeMetersAway2D(takeoffdir, takeoffdis);
+            //第一次返航点
+            EmergencyRetPLLocation1 = new PLLocation(take1.Latitude, take1.Longitude, minretalt_first);
+
+
+
+
+
+
+         }
+        public Task EmergencyRetAsync(PLLocation takeoffcentloc, PLLocation taskcentloc, float mindistance, int rettime, bool descending)
+        {
+            _takeoffcentloc = takeoffcentloc;
+            _taskcentloc = taskcentloc;
+            _mindistance = mindistance;
+          //  _rettime = rettime;
+            _descending = descending;
+            Emergency_Retstatus = 0;
+            EmergencyRetTime = DateTime.Now;
+            //计算返航路径
+            BuildPath();
+            Mode = FlightMode.EMERG_RTL;
+            return TaskUtils.CompletedTask;
+        }
+
+        protected override Task FlyToCoreAsync(double lat, double lng, float alt, float flytime = 0, int flytype = 0)
         {
             if (!IsEmergencyHoverActive)
             {
                 _targetLat = lat;
                 _targetLng = lng;
                 _targetAlt = alt;
+                _startLat = Latitude;
+                _startLng = Longitude ;
+                _startAlt = Altitude;
+                _direction = this.CalcDirection2D(lat, lng);
+
+                _flytime = flytime*1000; //ms
+                _startTicks = DateTime.Now; //ms
+
+
+                _Lng_delta = (float)( Math.Sin(_direction) * GeographyUtils.CalcMetersToLngSpan(Latitude));
+                _Lat_delta =(float)( Math.Cos(_direction) * GeographyUtils.METERS_TO_LAT_SPAN);
+               
+                //计算xy和x方向距离
+                _distance_xy = (float)this.CalcDistance(lat, lng, Altitude);
+                _distance_z = alt - Altitude;
+                Console.WriteLine($"{this.Id}d:{_distance_xy},lat{this.Latitude},lng{this.Longitude },tlat:{lat}tlng:{lng}");
+
+                _flytype = flytype;
+                // _xy_speed = _distance_xy / _flytime;
+                // _z_speed = _distance_z / _flytime;
+                GC_xy.initgetspeeddist(_flytype, _flytime / 1000, _distance_xy, acc_xy, maxspeed_xy);
+                if (_targetAlt>Altitude )
+                    GC_z.initgetspeeddist(_flytype, _flytime / 1000, _distance_z, acc_z, maxspeed_up);
+                else
+                    GC_z.initgetspeeddist(_flytype, _flytime / 1000, _distance_z, acc_z, maxspeed_down);
+
+
+
                 Mode = FlightMode.GUIDED;
+
+
             }
             return TaskUtils.CompletedTask;
         }
@@ -219,7 +382,7 @@ namespace Plane.Copters
 
         public Task<float> GetParamAsync(string paramName, int millisecondsTimeout = Timeout.Infinite)
         {
-            // TODO: 林俊清, 20150806, 实现仿真的 GetParamAsync。
+            // TODO: 王海, 20150806, 实现仿真的 GetParamAsync。
             return Task.FromResult(0f);
         }
 
@@ -232,6 +395,16 @@ namespace Plane.Copters
             return TaskUtils.CompletedTask;
         }
 
+        public Task LEDAsync()
+        {
+            return TaskUtils.CompletedTask;
+        }
+
+        public Task MotorTestAsync(int motor)
+        {
+            return TaskUtils.CompletedTask;
+        }
+
         public override Task SetChannelsAsync()
         {
             Channel1 = DesiredChannel1 ?? Channel1;
@@ -260,9 +433,30 @@ namespace Plane.Copters
             return TaskUtils.CompletedTask;
         }
 
+        //得到收到的总数据量
+        public void GetCommunicationNumber(out int recnumber, out int sendnumber)
+        {
+            recnumber = 0;
+            sendnumber = 0;
+        }
+
+
+        //重设数据量
+        public void ResetCommunicationNumber()
+        {
+
+        }
+
+        public async Task DoCommandAsync(int actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7)
+        {
+
+            await Task.Delay(50).ConfigureAwait(false);
+           
+
+        }
         public Task SetParamAsync(string paramName, float value, int millisecondsTimeout = Timeout.Infinite)
         {
-            // TODO: 林俊清, 20150807, 实现仿真的 SetParamAsync。
+            // TODO: 王海, 20150807, 实现仿真的 SetParamAsync。
             return TaskUtils.CompletedTask;
         }
         public bool GetShowLEDAsync()
@@ -308,7 +502,7 @@ namespace Plane.Copters
             double? latitude = null,   //23.14973333,
             double? longitude = null,   //113.40974166,
             float? altitude = null,   //0,
-            string name = null,   //"林俊清的飞行器",
+            string name = null,   //"王海的飞行器",
             byte? batteryPer = null,   //10,
             short? heading = null,   //33,
             bool? isConnected = null,   //true,
@@ -335,6 +529,9 @@ namespace Plane.Copters
             if (flightDistance != null) FlightDistance = flightDistance.Value;
             if (flightDistance2D != null) FlightDistance2D = flightDistance2D.Value;
             if (flightTimeSpan != null) FlightTimeSpan = flightTimeSpan.Value;
+            CopterPreCheckPass = true;
+            //CopterErrorCode = 2;
+            //CopterErrorString = getcoptererrorstr(CopterErrorCode);
         }
 
         public Task StartPairingAsync()
@@ -343,12 +540,6 @@ namespace Plane.Copters
             return TaskUtils.CompletedTask;
         }
 
-
-        public Task InitAsync()
-        {
-            return TaskUtils.CompletedTask;
-        }
-
         public Task StopPairingAsync()
         {
             return TaskUtils.CompletedTask;
@@ -360,6 +551,15 @@ namespace Plane.Copters
             _takeOffTargetAltitude = (int)alt;
             await TakeOffAsync().ConfigureAwait(false);
         }
+        DateTime MissionStartTime;
+        public Task MissionStartAsync(int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat)
+        {
+            MissionStartTime = DateTime.Now;
+            TakeOffPoint = new PLLocation(this.Latitude, this.Longitude, 0);
+
+            return TaskUtils.CompletedTask;
+        }
+
 
         public async Task UnlockAsync()
         {
@@ -392,11 +592,15 @@ namespace Plane.Copters
 
         protected override void UpdateFlightDataIfNeeded()
         {
+            //计算飞机距离，没必要
+            /*
             if (!TakeOffPoint.IsNullOrEmpty())
             {
                 FlightDistance = TakeOffPoint.CalcDistance(this);
                 FlightDistance2D = TakeOffPoint.CalcDistance2D(this);
             }
+           
+
             if (FlightTimeSpan.TotalSeconds > 0)
             {
                 if (_lastCalcSpeedPoint.IsNullOrEmpty())
@@ -414,16 +618,18 @@ namespace Plane.Copters
                     if (movedDistance >= 3 || movedTime.TotalSeconds >= 3)
                     {
                         var airSpeed = movedDistance / movedTime.TotalSeconds;
-                        if (airSpeed < 100)  // 林俊清, 20151023, 速度过大时不正常，经纬度可能有错误。
+                        if (airSpeed < 100)  // 王海, 20151023, 速度过大时不正常，经纬度可能有错误。
                         {
                             AirSpeed = (float)airSpeed;
                         }
+                        //Windows.Messages.Message.Show(AirSpeed.ToString());
                         GroundSpeed = AirSpeed;
                         _lastCalcSpeedPoint = new PLLocation(this);
                         _lastCalcSpeedTime = DateTime.Now;
                     }
                 }
             }
+            */
         }
 
         private void MoveToPointImmediately(double lat, double lng, float alt)
@@ -440,6 +646,73 @@ namespace Plane.Copters
 
         }
 
+        public static System.Drawing.Color GetRandomColor()
+        {
+            Random rand = new Random();
+            return System.Drawing.Color.FromArgb(rand.Next(256), rand.Next(256), rand.Next(256));
+        }
+
+        private DateTime led_laston;
+        private System.Drawing.Color Led_color;
+
+        //更新显示颜色，根据设置的led参数
+        private  void   UpdateShowColor()
+        {
+            // 使用实例化的对象调用ConvertFromString
+            //LEDShowColor = (System.Drawing.Color)converter.ConvertFromString("#" + LEDColor);
+            // 创建ColorConverter实例用于颜色转换
+            if ( rgbconverter==null) rgbconverter=new System.Drawing.ColorConverter();
+            //简化版的颜色模拟
+            switch (LEDMode)
+            {
+                case 0:
+                        if (LEDColor != null)
+                        LEDShowColor = (System.Drawing.Color)rgbconverter.ConvertFromString("#" + LEDColor);
+                        else LEDShowColor = (System.Drawing.Color)rgbconverter.ConvertFromString("#FFFFFF");
+
+                    break;
+                //闪烁
+                case 1:
+                case 2:
+                case 3:
+                case 4:
+                case 5:
+                case 6:
+                case 7:
+                case 8:
+                case 9:
+                case 10:
+                case 11:
+                case 12:
+                case 13:
+                case 14:
+
+                    if (DateTime.Now.AddMilliseconds(-(LEDInterval*1000)) >= led_laston)
+                    {
+                        led_laston = DateTime.Now;
+                        if (LEDShowColor != System.Drawing.Color.Black)
+                        {
+                            LEDShowColor = System.Drawing.Color.Black;
+
+                        }
+                        else
+                        {
+                            if ((LEDMode == 1) || (LEDMode == 4) || (LEDMode == 5) || (LEDMode == 6) || (LEDMode == 7) || (LEDMode == 9) || (LEDMode == 11) || (LEDMode == 13))
+                                LEDShowColor = (System.Drawing.Color)rgbconverter.ConvertFromString("#" + LEDColor);
+                            if ((LEDMode == 2)||(LEDMode == 3) || (LEDMode == 8) || (LEDMode == 10) || (LEDMode == 12) || (LEDMode == 14))
+                                 LEDShowColor = GetRandomColor();
+                        }
+                    }
+
+                    break;
+              
+            }
+
+
+
+
+
+        }
 
         private bool ReachedDestination(double destLat, double destLng, float destAlt) => this.CalcDistance(destLat, destLng, destAlt) < 0.1;
 
@@ -474,6 +747,7 @@ namespace Plane.Copters
                             break;
 
                         case FlightMode.AUTO:
+                            /*
                             // 暂时只管起飞。以最大速度上升，直到达到目标高度。
                             if (Altitude < _takeOffTargetAltitude)
                             {
@@ -483,11 +757,17 @@ namespace Plane.Copters
                             {
                                 Mode = FlightMode.LOITER;
                             }
+                            */
                             break;
 
                         case FlightMode.GUIDED:
-                            // 林俊清, 20160317, 指点时也能体感控制若干通道。
-                            UpdateWithChannels();
+                            // 王海, 20160317, 指点时也能体感控制若干通道。
+                            //考虑不更新这个，好像没必要-xu
+                            //UpdateWithChannels();
+                            //UpdateWithDestination(_targetLat, _targetLng, _targetAlt);
+                            if (FlightControlMode==1)
+                                UpdateWithDestination_v2(_targetLat, _targetLng, _targetAlt);
+                            else
                                 UpdateWithDestination(_targetLat, _targetLng, _targetAlt);
                             break;
 
@@ -519,8 +799,8 @@ namespace Plane.Copters
                             break;
 
                         case FlightMode.LAND:
-                            // 林俊清, 20160317, 降落时也能体感控制若干通道。
-                            UpdateWithChannels();
+                            // 王海, 20160317, 降落时也能体感控制若干通道。
+                            //  UpdateWithChannels();
                             // 以最大速度降落，直到高度为 0。
                             if (Altitude > 0)
                             {
@@ -542,26 +822,94 @@ namespace Plane.Copters
 
                         case FlightMode.TOY:
                             break;
+                        //紧急返航
+                        case FlightMode.EMERG_RTL:
+                            switch(Emergency_Retstatus)
+                            {
+                                case 0: //等待返航
+                                    if ((DateTime.Now - EmergencyRetTime).TotalSeconds > _rettime)
+                                    {
+                                        Emergency_Retstatus = 1;
+                                        //平飞或降落随机距离
+                                       //直接返航
+                                        FlyToCoreAsync(TakeOffPoint.Latitude, TakeOffPoint.Longitude, minretalt);
+                                        //先到第一返航点再到起飞点上空
+                                        //FlyToCoreAsync(EmergencyRetPLLocation1.Latitude, EmergencyRetPLLocation1.Longitude, EmergencyRetPLLocation1.Altitude);
+                                        
+                                        //FlyToCoreAsync里面把模式改成了GUIDED，这里改回来
+                                        Mode = FlightMode.EMERG_RTL;
+                                    }
+                                     break;
+
+
+                                case 1: //返航阶段一:等待平飞或降落随机距离并到达,然后飞到起飞点上空
+
+                                    //直接返航
+                                    UpdateWithDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, minretalt);
+                                    if (ReachedDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, minretalt))
+                                    {
+                                        Mode = FlightMode.LAND;
+                                        //FlyToCoreAsync里面把模式改成了GUIDED，这里改回来
+                                        Emergency_Retstatus = 3;
+                                    }
+
+                                    /* // //先到第一返航点再到起飞点上空
+                                    UpdateWithDestination(EmergencyRetPLLocation1.Latitude, EmergencyRetPLLocation1.Longitude, EmergencyRetPLLocation1.Altitude);
+                                    if (ReachedDestination(EmergencyRetPLLocation1.Latitude, EmergencyRetPLLocation1.Longitude, EmergencyRetPLLocation1.Altitude))
+                                    {
+                                        FlyToCoreAsync(TakeOffPoint.Latitude, TakeOffPoint.Longitude, minretalt);
+                                        Mode = FlightMode.EMERG_RTL;
+                                        //FlyToCoreAsync里面把模式改成了GUIDED，这里改回来
+                                        Emergency_Retstatus = 2;
+                                    }
+                                    */
+
+                                    break;
+                                case 2: //返航阶段一:等待平飞或降落随机距离并到达,然后飞到起飞点上空
+
+                                    UpdateWithDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, minretalt);
+                                    if (ReachedDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, minretalt))
+                                    {
+                                        Mode = FlightMode.LAND;
+                                        //FlyToCoreAsync里面把模式改成了GUIDED，这里改回来
+                                        Emergency_Retstatus = 3;
+                                    }
+
+                                    break;
+                                case 3: //降落
+
+                                    break;
+
+                            }
+                            break;
 
                         default:
                             break;
                     }
-                    UpdateFlightDataIfNeeded();
+                    //UpdateFlightDataIfNeeded();
                 }
             }
             else
             {
-                // TODO: 林俊清, 20151228, 模拟空中锁定。
+                // TODO: 王海, 20151228, 模拟空中锁定。
                 // 锁定时直接把速度设为 0。
                 AirSpeed = 0;
             }
+
+          //  UpdateShowColor();
+
+
+
             _uiSyncContext.Post(() =>
             {
+                //位置变化需要在UI刷新
                 RaiseLocationChangedIfNeeded();
-                RaiseAltitudeChangedIfNeeded();
-
-                RaiseDataStreamReceived(PDataStreamType.GPS_RAW_INT);
-                RaiseAttitudeChanged();
+                //高度变化需要在UI刷新
+               // RaiseAltitudeChangedIfNeeded();
+                //GPS数据用于显示
+              //  RaiseDataStreamReceived(PDataStreamType.GPS_RAW_INT);
+                //不考虑姿态
+             //   RaiseAttitudeChanged();
             });
         }
 
@@ -627,41 +975,20 @@ namespace Plane.Copters
         {
             // 与目标点之间的距离。
             var distance = this.CalcDistance(lat, lng, alt);
-
             // 距离已经很近，直接移动到目标点。
             if (distance < _scaledMaxMoveInInterval)
             {
                 MoveToPointImmediately(lat, lng, alt);
                 return;
             }
-
             // 在空间中的移动距离。
             var move = _scaledMaxMoveInInterval;
-
             // 竖直方向的移动距离。
             var altDelta = (float)(move * (alt - Altitude) / distance);
             // 更新高度。
             Altitude += altDelta;
-
             // 目标点相对于当前位置的方向。
-            var direction = this.CalcDirection2D(lat, lng);
-
-            // 更新姿态。
-            if (Mode == FlightMode.RTL)
-            {
-                // 林俊清, 20160126, 目前飞行器只在返航时会旋转机头，面对飞行方向。
-                Yaw = (float)direction.RadToDeg();
-                Heading = (short)Yaw;
-                Roll = 0;
-                Pitch = MAX_TILT_IN_FLIGHT;
-            }
-            else
-            {
-                var directionDelta = direction - Heading.DegToRad();
-                Roll = MAX_TILT_IN_FLIGHT * (float)Math.Sin(directionDelta);
-                Pitch = MAX_TILT_IN_FLIGHT * (float)Math.Cos(directionDelta);
-            }
-
+            // var direction = this.CalcDirection2D(lat, lng);
             // 水平面上的移动距离。
             var moveInHorizontalPlane = Math.Sqrt(move * move - altDelta * altDelta);
             if (double.IsNaN(moveInHorizontalPlane))
@@ -669,10 +996,58 @@ namespace Plane.Copters
                 MoveToPointImmediately(lat, lng, alt);
                 return;
             }
+
             // 更新纬度。
-            Longitude += moveInHorizontalPlane * Math.Sin(direction) * GeographyUtils.CalcMetersToLngSpan(Latitude);
+            //Longitude += moveInHorizontalPlane * Math.Sin(direction) * GeographyUtils.CalcMetersToLngSpan(Latitude);
+            Longitude += moveInHorizontalPlane * _Lng_delta;
+            Latitude += moveInHorizontalPlane * _Lat_delta;
             // 更新经度。
-            Latitude += moveInHorizontalPlane * Math.Cos(direction) * GeographyUtils.METERS_TO_LAT_SPAN;
+            //Latitude += moveInHorizontalPlane * Math.Cos(direction) * GeographyUtils.METERS_TO_LAT_SPAN;
         }
+        //新版本小航点计算移动位置
+        private void UpdateWithDestination_v2(double lat, double lng, float alt)
+        {
+            //_flytime 总飞行时间 秒
+            //_startTicks   开始飞行时间ms
+            // _distance_xy  米
+            // _distance_z    米
+            //当前飞行时间
+            if ((lat == Latitude) && (lng == Longitude) && (alt == Altitude)) return;
+            currflytime =(float)( DateTime.Now -_startTicks).TotalMilliseconds;//实际飞行时间 ms
+            //超时直接移动到目标点
+            if (currflytime >= _flytime)
+            {
+                MoveToPointImmediately(lat, lng, alt);
+                return;
+            }
+            //if (currflytime > 13000)
+            //    return;
+            // 根据飞行时间计算飞行距离
+
+            float vspeed = 0;
+
+            GC_xy.getspeeddist(_flytype, _flytime / 1000, _distance_xy, currflytime / 1000, acc_xy,maxspeed_xy , out currdis_xy, out vspeed);
+
+            if (alt> Altitude)
+                GC_z.getspeeddist(_flytype, _flytime / 1000, _distance_z, currflytime / 1000, acc_z,maxspeed_up, out currdis_z, out vspeed);
+            else
+                GC_z.getspeeddist(_flytype, _flytime / 1000, _distance_z, currflytime / 1000, acc_z,maxspeed_down, out currdis_z, out vspeed);
+
+            Console.WriteLine($"{this.Id}：time:{currflytime / 1000}  to {currdis_z}");
+
+            // 距离已经很近，直接移动到目标点。           
+            if ((Math.Abs(_distance_xy-currdis_xy) < 0.1)&& (Math.Abs(_distance_z - currdis_z) < 0.1))
+            {
+                Console.WriteLine($"{this.Id}  to  tlat:{lat}tlng:{lng}");
+
+                MoveToPointImmediately(lat, lng, alt);
+                return;
+            }
+            // 更新位置
+            Altitude = _startAlt+ currdis_z;
+            Longitude  = _startLng  + currdis_xy*_Lng_delta;
+            Latitude = _startLat + currdis_xy *_Lat_delta;
+        }
+
     }
 }

PlaneGcsSdk_Shared/Copters/FlightMode.cs

@@ -9,11 +9,11 @@ namespace Plane.Copters
 #endif
         enum FlightMode
     {
-        // 林俊清，20150608：不可将以下枚举项重命名。
+        // 王海，20150608：不可将以下枚举项重命名。
 
         STABILIZE = 0,          // hold level position
 
-        ACRO = 1,           // rate control // 林俊清, 20160205, 特技模式：http://copter.ardupilot.cn/wiki/acro-mode/
+        ACRO = 1,           // rate control // 王海, 20160205, 特技模式：http://copter.ardupilot.cn/wiki/acro-mode/
 
         ALT_HOLD = 2,       // AUTO control
 
@@ -27,13 +27,15 @@ namespace Plane.Copters
 
         CIRCLE = 7,
 
-        POSITION = 8,   // 林俊清, 20160205, 位置模式：http://copter.ardupilot.cn/wiki/POSITION-mode/
+        POSITION = 8,   // 王海, 20160205, 位置模式：http://copter.ardupilot.cn/wiki/POSITION-mode/
 
         LAND = 9,               // AUTO control
 
-        OF_LOITER = 10, // 林俊清, 20160205, 光流悬停模式：http://copter.ardupilot.cn/wiki/loiter-mode/ 底部。
+        OF_LOITER = 10, // 王海, 20160205, 光流悬停模式：http://copter.ardupilot.cn/wiki/loiter-mode/ 底部。
 
-        TOY = 11
+        TOY = 11,
+
+        EMERG_RTL=12,   //紧急返航模式
     }
 
     internal static class FightModeExtensions

PlaneGcsSdk_Shared/Copters/Mission.cs

@@ -19,6 +19,9 @@ namespace Plane.Copters
         private float _Param3;
         private float _Param4;
         private ushort _Sequence;
+        private float _R;
+        private float _G;
+        private float _B;
 
         #endregion Backing Fields
 
@@ -107,13 +110,34 @@ namespace Plane.Copters
             set { Set(nameof(Sequence), ref _Sequence, value); }
         }
 
+        public float R
+        {
+            get { return _R; }
+            set { Set(nameof(R), ref _R, value); }
+        }
+
+        public float G
+        {
+            get { return _G; }
+            set { Set(nameof(G), ref _G, value); }
+        }
+
+        public float B
+        {
+            get { return _B; }
+            set { Set(nameof(B), ref _B, value); }
+        }
+
+        
+
         /// <summary>
         /// 创建降落任务。
         /// </summary>
         /// <returns>降落任务。</returns>
-        public static IMission CreateLandMission() => new Mission
+        public static IMission CreateLandMission(int waittime) => new Mission
         {
-            Command = FlightCommand.Land
+            Command = FlightCommand.Land,
+            Param1 = waittime,  //停留时间  s
         };
 
         /// <summary>
@@ -125,13 +149,16 @@ namespace Plane.Copters
             Command = FlightCommand.ReturnToLaunch
         };
 
+
+
+
         /// <summary>
         /// 创建航点任务。
         /// </summary>
         /// <param name="loc">航点目的地。</param>
         /// <returns>航点任务。</returns>
-        public static IMission CreateWaypointMission(ILocation loc) =>
-            CreateWaypointMission(loc.Latitude, loc.Longitude, loc.Altitude);
+      //  public static IMission CreateWaypointMission(ILocation loc) =>
+      //      CreateWaypointMission(loc.Latitude, loc.Longitude, loc.Altitude);
 
         /// <summary>
         /// 创建航点任务。
@@ -140,12 +167,44 @@ namespace Plane.Copters
         /// <param name="lng">目的地经度。</param>
         /// <param name="alt">目的地相巴拉圭高度。</param>
         /// <returns>航点任务。</returns>
-        public static IMission CreateWaypointMission(double lat, double lng, float alt) => new Mission
+        public static IMission CreateWaypointMission(int loitertime,int flytime, double lat, double lng, float alt) => new Mission
         {
             Command = FlightCommand.Waypoint,
+            Param1= loitertime,  //停留时间  s
+            Param2= flytime,   //飞行时间 s
             Latitude = lat,
             Longitude = lng,
             Altitude = alt
         };
+
+        public static IMission CreateTakeoffMission(int waittime,int flytime, double lat, double lng, float alt) => new Mission
+        {
+            Command = FlightCommand.TakeOff,
+            Param1 = waittime,  //起飞等待时间  s
+            Param2 = flytime,   //起飞飞行时间 s
+            Latitude = lat,
+            Longitude = lng,
+            Altitude = alt
+        };
+
+        public static IMission CreateLEDControlMission(int delay, int ledmode, float ledInterval, int ledtimes, byte red, byte green, byte blue) => new Mission
+        {
+            Command = FlightCommand.LEDControl,
+            Param1 = delay,     //灯光延时  单位:100ms
+            Param2 = ledmode,   //灯光模式  0.常亮 1.闪烁 2.随机闪烁(RGB无意义)
+            Param3 = ledInterval,   //闪烁延时  单位:100ms
+            Param4 = ledtimes,  //次数 (暂无意义)
+            R = red,            //Red
+            G = green,          //Green
+            B = blue            //Blue
+        };
+
+        public static IMission CreateChangeSpeedMission(float levelSpeed, float upSpeed, float downSpeed) => new Mission
+        {
+            Command = FlightCommand.ChangeSpeed,
+            Param1 = levelSpeed,    //水平速度
+            Param2 = upSpeed,       //上升速度
+            Param3 = downSpeed,     //下降速度
+        };
     }
 }

PlaneGcsSdk_Shared/Copters/PLCopter.cs

@@ -13,7 +13,7 @@ namespace Plane.Copters
     {
         private double _FlightDistance2D;
 
-        private PlaneCopter _internalCopter;
+        public PlaneCopter _internalCopter;
 
         private int _setModeCount = 0;
 
@@ -54,8 +54,6 @@ namespace Plane.Copters
             await _internalCopter.ConnectAsync().ConfigureAwait(false);
             IsConnected = _internalCopter.IsConnected;
             IsCheckingConnection = true;
-            //连接完成后做一些初始化的工作
-            await InitAsync();
         }
 
         public virtual async Task DisconnectAsync()
@@ -76,6 +74,11 @@ namespace Plane.Copters
             return await _internalCopter.GetParamAsync(paramName, millisecondsTimeout).ConfigureAwait(false);
         }
 
+        public async Task MotorTestAsync(int motor)
+        {
+            await _internalCopter.DoMotorTestAsync(motor, MAVLink.MOTOR_TEST_THROTTLE_TYPE.MOTOR_TEST_THROTTLE_PERCENT, 5 , 5).ConfigureAwait(false);
+        }
+
         /// <summary>
         /// 锁定飞行器。在调用之前务必判断高度及让使用者确认！
         /// </summary>
@@ -86,7 +89,13 @@ namespace Plane.Copters
                 await _internalCopter.DoARMAsync(false).ConfigureAwait(false);
             }
         }
-
+        public async Task MissionStartAsync(int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat)
+        {
+            if (IsUnlocked)
+            {
+                await _internalCopter.DoMissionStartAsync(hour_utc, minute_utc, second_utc, Missionlng, Missionlat).ConfigureAwait(false);
+            }
+        }
         public override async Task SetChannelsAsync()
         {
             await _internalCopter.SetChannelsAsync(
@@ -197,16 +206,35 @@ namespace Plane.Copters
                     await Task.Delay(5).ConfigureAwait(false);
                 }
         }
+
+        //得到收到的总数据量
+
+        public void GetCommunicationNumber(out int recnumber, out int sendnumber)
+        {
+            _internalCopter.GetCommunicationNumber(out recnumber,out sendnumber);
+        }
+
+
+        //重设数据量
+        public void ResetCommunicationNumber()
+        {
+            _internalCopter.ResetCommunicationNumber();
+        }
+
         public async Task SetParamAsync(string paramName, float paramValue, int millisecondsTimeout = Timeout.Infinite)
         {
             var stopwatch = Stopwatch.StartNew();
             while (true)
             {
-                for (int ii = 0; ii < 5; ii++)
+               /* for (int ii = 0; ii < 5; ii++)
                 {
                     await _internalCopter.SetParam2Async(paramName, paramValue).ConfigureAwait(false);
                     await Task.Delay(5).ConfigureAwait(false);
                 }
+                */
+
+                await _internalCopter.SetParam2Async(paramName, paramValue).ConfigureAwait(false);
+                await Task.Delay(5).ConfigureAwait(false);
 
                 int i = 0;
                 try
@@ -216,7 +244,7 @@ namespace Plane.Copters
                     //    await Task.Delay(5).ConfigureAwait(false);
                     //}
                     
-                    if (await _internalCopter.GetParamAsync(paramName, 1000) == paramValue)
+                   // if (await _internalCopter.GetParamAsync(paramName, millisecondsTimeout) == paramValue)
                     {
                         i = 1;
                     }
@@ -250,17 +278,6 @@ namespace Plane.Copters
             await _internalCopter.GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_SET_PAIR, packet).ConfigureAwait(false);
         }
 
-        public async Task InitAsync()
-        {
-            float Gpstype=  await _internalCopter.GetParamAsync("GPS_TYPE") ;
-            if (Gpstype == 15)
-                LocationType = CopterLocationType.RTK;
-            else
-                LocationType = CopterLocationType.GPS;
-        }
-
-
-
         public async Task StopPairingAsync()
         {
             if (!IsPairing) return;
@@ -275,8 +292,8 @@ namespace Plane.Copters
         public async Task TakeOffAsync(float alt)
         {
             var currentTakeOffCount = ++_takeOffCount;
-            // 林俊清, 20160312, 从固件版本 3.1.1（0x3101）开始支持直接解锁起飞命令。
-            if (FirmwareVersion >= 0x3101)
+            // 王海, 20160312, 从固件版本 3.1.1（0x3101）开始支持直接解锁起飞命令。
+            //if (FirmwareVersion >= 0x3101)
             {
                 await SetChannelsAsync(
                     ch1: 1500,
@@ -303,6 +320,7 @@ namespace Plane.Copters
                     }
                 });
             }
+            /*
             else
             {
                 await UnlockAsync().ConfigureAwait(false);
@@ -317,6 +335,7 @@ namespace Plane.Copters
                     }
                 }
             }
+            */
         }
 
         public async Task UnlockAsync()
@@ -329,13 +348,39 @@ namespace Plane.Copters
                     ch3: 1100,
                     ch4: 1500
                 ).ConfigureAwait(false);
-                if (await SetModeAsync(FlightMode.ALT_HOLD).ConfigureAwait(false))
+
+                if (await SetModeAsync(FlightMode.LOITER).ConfigureAwait(false))
                 {
                     await _internalCopter.DoARMAsync(true).ConfigureAwait(false);
                 }
             }
         }
 
+        public async Task DoCommandAckAsync(ushort command, byte result)
+        {
+            MAVLink.mavlink_command_ack_t req = new MAVLink.mavlink_command_ack_t();
+            req.command = command;
+            req.result = result;
+            await _internalCopter.GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_COMMAND_ACK, req);
+        }
+    
+
+        public async Task DoCommandAsync(int actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7)
+        {
+           await _internalCopter.DoCommandAsync((MAVLink.MAV_CMD)actionid, p1, p2, p3, p4, p5, p6, p7).ConfigureAwait(false);
+        }
+
+        public async Task LEDAsync()
+        {
+           
+            await Task.Run(async () =>
+            {
+                await _internalCopter.DoLEDAsync().ConfigureAwait(false);
+
+            }).ConfigureAwait(false);
+            //await _internalCopter.DoARMAsync(true).ConfigureAwait(false);
+        }
+
         /// <summary>
         /// 无论在不在紧急悬停状态下都可用的切换模式方法。
         /// </summary>
@@ -373,7 +418,7 @@ namespace Plane.Copters
             return !anotherSetModeActionCalled && Mode == mode;
         }
 
-        protected override async Task FlyToCoreAsync(double lat, double lng, float alt)
+        protected override async Task FlyToCoreAsync(double lat, double lng, float alt, float flytime = 0, int flytype = 0)
         {
             if (!IsEmergencyHoverActive)
             {

PlaneGcsSdk_Shared/Copters/PlaneCopter.NestedTypes.cs

@@ -46,7 +46,7 @@ namespace Plane.Copters
                     ((ICollection)bitArray).CopyTo(array, 0);
                     return array[0];
                 }
-                // 林俊清，20150703：BitArray 的构造函数的参数是数组长度，这里根本不对啊。
+                // 王海，20150703：BitArray 的构造函数的参数是数组长度，这里根本不对啊。
                 //set
                 //{
                 //    bitArray = new BitArray(value);

PlaneGcsSdk_Shared/Copters/PlaneCopter.PacketAnalysis.cs

@@ -373,5 +373,32 @@ namespace Plane.Copters
             heading = vfr.heading;
             RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
         }
+
+        /// <summary>
+        /// 飞控返回数据 处理通信模块发过来的28个字节数据
+        /// </summary>
+        /// <param name="buffer"></param>
+        public void AnalyzeCommMouldePositionIntPacket(byte[] buffer, byte version)
+        {
+            var info = buffer.ByteArrayToStructure<MavComm.comm_copter_info>(0);
+            lat = info.lat * 1.0e-7; 
+            lng = info.lng * 1.0e-7;
+            commModuleMode = (uint)info.control_mode;
+            gpsstatus = (byte)info.gps_status;
+            errorcode= (byte)info.error_code;
+            precheckok = info.rpecheck_fault == 0;
+            gpsalt = ((float)info.alt) / 1000;
+            satcount = info.gps_num_sats;
+            isUnlocked = info.lock_status == 1;
+
+            battery_voltage = ((float)info.battery_voltage) / 1000;
+
+            heading = (short)((info.heading / 100) % 360);
+            
+            commModuleVersion = version;
+
+            retain = info.retain;
+            RaiseReceiveDataStreamEventOnUIThread(MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT);
+        }
     }
 }

PlaneGcsSdk_Shared/Copters/PlaneCopter.cs

@@ -24,6 +24,8 @@ namespace Plane.Copters
     {
         uint iErrorData;
         ulong iDataCount;
+        private int _recnumber = 0;
+        private int _sendnumber = 0;
 
         public bool IsConnected { get { return Connection.IsOpen; } }
 
@@ -40,6 +42,10 @@ namespace Plane.Copters
         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; }
@@ -49,9 +55,14 @@ namespace Plane.Copters
 
         public bool useLocation { get; set; }
         public float gpsalt { get; set; }
+        public bool precheckok { get; set; }
+
         public byte gpsstatus { get; set; }
+        public byte errorcode { get; set; }
+
         public float gpshdop { get; set; }
         public byte satcount { get; set; }
+        public float retain { get; set; }
         public float groundspeed { get; set; }
         public float groundcourse { get; set; }
         public double lat { get; set; }
@@ -133,12 +144,21 @@ namespace Plane.Copters
             //停止所有数据流
             await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.ALL, 0).ConfigureAwait(false);
             //需要电池电压，电流，GPS数据
-
-            await RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM.EXTENDED_STATUS, 5).ConfigureAwait(false); //10 1电池电压、电流、电量,152-内存、62-导航控制输出、24-GPS数据、42-航点数据
+            /*
+             *  目前接收的数据是
+             *  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, 5).ConfigureAwait(false); //10 请求vfr airspeed(空速)、groundspeed(地速)、alt(高度)、climb(爬升率)、heading(头朝向)、throttle(实际油门) ---74
+            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 请求 通道数据
             //不需要原始数据
@@ -201,7 +221,7 @@ namespace Plane.Copters
                             autopilot = (byte)MAVLink.MAV_AUTOPILOT.GENERIC,
                             mavlink_version = 3,
                         };
-                        await SendPacketAsync(htb).ConfigureAwait(false);
+    //                    await SendPacketAsync(htb).ConfigureAwait(false);  //发心跳包
 #if DEBUG && LOG_PACKETS
                         if (!_sendHeartbeatStopwatch.IsRunning)
                         {
@@ -247,7 +267,7 @@ namespace Plane.Copters
                     {
                         PacketReceived?.Invoke(this, new PacketReceivedEventArgs(packet));
                         var handled = true;
-                        Debug.WriteLine(packet[5],"收到数据类型");
+                      //  Debug.WriteLine(packet[5],"收到数据类型");
                         switch (packet[5])
                         {
                             case MAVLink.MAVLINK_MSG_ID_HEARTBEAT: //0
@@ -332,7 +352,7 @@ namespace Plane.Copters
             return flightModes.ToList();
         }
 
-        // 林俊清, 20151029, 由于只在 myComm 循环中读取数据包，因此去掉了锁。
+        // 王海, 20151029, 由于只在 myComm 循环中读取数据包，因此去掉了锁。
         //volatile object readlock = new object();
 
 #if DEBUG && LOG_PACKETS
@@ -348,7 +368,7 @@ namespace Plane.Copters
         private byte[] _readBuffer = new byte[263];
 
         /// <summary>
-        /// 异步读取数据包。// 林俊清, 20151029, 去掉了锁，不要在除读数据包循环之外的地方调用此方法。
+        /// 异步读取数据包。// 王海, 20151029, 去掉了锁，不要在除读数据包循环之外的地方调用此方法。
         /// </summary>
         /// <returns>表示此异步操作的 <see cref="Task{TResult}"/> 实例，其结果为读取到的数据包。</returns>
         private async Task<byte[]> ReadPacketAsync()
@@ -359,26 +379,33 @@ namespace Plane.Copters
             }
 
             int length = 0;
-
+            int readnumber = 0;  //_recnumber;
             try
             {
-                if (await Connection.ReadAsync(_readBuffer, 0, 1) == 0)
+                readnumber = await Connection.ReadAsync(_readBuffer, 0, 1);
+                _recnumber += readnumber;
+                if (readnumber == 0)
                 {
                     return null;
                 }
                 if (_readBuffer[0] == MAVLink.MAVLINK_STX)
                 {
-                    if (await Connection.ReadAsync(_readBuffer, 1, 5) == 0)
+                    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
-                    if (await Connection.ReadAsync(_readBuffer, 6, length - 6) == 0)
+                    readnumber = await Connection.ReadAsync(_readBuffer, 6, length - 6);
+                    _recnumber += readnumber;
+                    if (readnumber == 0)
                     {
                         return null;
                     }
@@ -534,7 +561,7 @@ namespace Plane.Copters
             {
                 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];
 
@@ -584,17 +611,119 @@ namespace Plane.Copters
                 try
                 {
                     await Connection.WriteAsync(packet, 0, i);
+                   // Console.WriteLine("sendout:" + i);
+                    _sendnumber += i; //统计发送数量
                 }
                 catch
                 {
-                    // 林俊清, 20151031, 异常已通过 IConnection.ExceptionThrown 事件传到外部，此处吞掉就行了。
+                    // 王海, 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, 0, 0, 0, 0, 0, 0);
+            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)
@@ -607,6 +736,12 @@ namespace Plane.Copters
             ///<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)
         {
@@ -625,6 +760,8 @@ namespace Plane.Copters
 
             req.command = (ushort)actionid;
 
+           
+            Console.WriteLine("P1 = "  + p1);
             req.param1 = p1;
             req.param2 = p2;
             req.param3 = p3;
@@ -1104,7 +1241,7 @@ namespace Plane.Copters
                 return;
             }
 
-            //log.InfoFormat("Request stream {0} at {1} hz", Enum.Parse(typeof(MAV_DATA_STREAM), id.ToString()), hzrate);
+           // log.InfoFormat("Request stream {0} at {1} hz", Enum.Parse(typeof(MAV_DATA_STREAM), id.ToString()), hzrate);
             await GetDatastreamAsync(id, hzrate);
         }
 
@@ -1241,7 +1378,19 @@ namespace Plane.Copters
             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();
@@ -1329,16 +1478,18 @@ namespace Plane.Copters
         {
             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];
-                int copy = Math.Min(length - a * msglen, msglen);
-                Array.Copy(data, a * msglen, gps.data, 0, copy);
-                gps.len = (byte)copy;
+                Array.Copy(data, a * msglen, gps.data, 0, datalen);
+                gps.len = (byte)datalen;
                 gps.target_component = compid;
                 gps.target_system = sysid;
-                await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_INJECT_DATA, gps);
+                await GenerateRTKPacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_INJECT_DATA, gps, gps.len);
+               // Console.WriteLine("send:" + (ushort)gps.len);
             }
         }
 

PlaneGcsSdk_Shared/PlaneGcsSdk_Shared.projitems

@@ -9,10 +9,17 @@
     <Import_RootNamespace>Plane</Import_RootNamespace>
   </PropertyGroup>
   <ItemGroup>
+    <Compile Include="$(MSBuildThisFileDirectory)CommunicationManagement\CommModuleGenerateMavLink.cs" />
+    <Compile Include="$(MSBuildThisFileDirectory)CommunicationManagement\CommunicationReceiveCopterInfoEventArgs.cs" />
+    <Compile Include="$(MSBuildThisFileDirectory)CommunicationManagement\CommConnection.cs" />
+    <Compile Include="$(MSBuildThisFileDirectory)CommunicationManagement\CommModule.cs" />
+    <Compile Include="$(MSBuildThisFileDirectory)CommunicationManagement\CommModulePacketAnalysis.cs" />
+    <Compile Include="$(MSBuildThisFileDirectory)CommunicationManagement\CommWriteMissinState.cs" />
     <Compile Include="$(MSBuildThisFileDirectory)Communication\ExceptionThrownEventSource.cs" />
     <Compile Include="$(MSBuildThisFileDirectory)Communication\EmptyConnection.cs" />
     <Compile Include="$(MSBuildThisFileDirectory)Communication\CompositeConnection.cs" />
     <Compile Include="$(MSBuildThisFileDirectory)Communication\SerialPortConnection.cs" />
+    <Compile Include="$(MSBuildThisFileDirectory)Communication\TcpClientWithTimeout.cs" />
     <Compile Include="$(MSBuildThisFileDirectory)Communication\TcpConnectionBase.cs" />
     <Compile Include="$(MSBuildThisFileDirectory)Communication\TcpConnection.cs" />
     <Compile Include="$(MSBuildThisFileDirectory)Communication\TcpServerConnection.cs" />
@@ -27,6 +34,7 @@
     <Compile Include="$(MSBuildThisFileDirectory)CopterControllers\CopterControllerBase.cs" />
     <Compile Include="$(MSBuildThisFileDirectory)CopterControllers\CopterControllerManager.cs" />
     <Compile Include="$(MSBuildThisFileDirectory)CopterControllers\FlyToController.cs" />
+    <Compile Include="$(MSBuildThisFileDirectory)CopterControllers\GuidController.cs" />
     <Compile Include="$(MSBuildThisFileDirectory)CopterControllers\KeyboardController.cs" />
     <Compile Include="$(MSBuildThisFileDirectory)CopterManagement\CopterFactory.cs" />
     <Compile Include="$(MSBuildThisFileDirectory)CopterManagement\SingleCopterManager.cs" />
@@ -45,6 +53,7 @@
     <Compile Include="$(MSBuildThisFileDirectory)Copters\FakeCopter.cs" />
     <Compile Include="$(MSBuildThisFileDirectory)Copters\FlightMode.cs" />
     <Compile Include="$(MSBuildThisFileDirectory)Copters\Mission.cs" />
+    <Compile Include="$(MSBuildThisFileDirectory)Protocols\MavCommTypes.cs" />
     <Compile Include="$(MSBuildThisFileDirectory)Protocols\MavlinkCRC.cs" />
     <Compile Include="$(MSBuildThisFileDirectory)Protocols\MAVLinkTypes.cs" />
     <Compile Include="$(MSBuildThisFileDirectory)Protocols\MavlinkUtil_NET45.cs" />

PlaneGcsSdk_Shared/Protocols/MAVLinkTypes.cs

@@ -17,7 +17,9 @@ namespace Plane.Protocols
         public const int MAVLINK_LITTLE_ENDIAN = 1;
         public const int MAVLINK_BIG_ENDIAN = 0;
 
-        public const byte MAVLINK_STX = 254;
+        //协议头 飞控4.0以后 254 - 240  Mavlink头 FE 改为 F0 
+        //public const byte MAVLINK_STX = 254;
+        public const byte MAVLINK_STX = 240;
 
         public const byte MAVLINK_ENDIAN = MAVLINK_LITTLE_ENDIAN;
 
@@ -27,12 +29,16 @@ namespace Plane.Protocols
 
         public const bool MAVLINK_NEED_BYTE_SWAP = (MAVLINK_ENDIAN == MAVLINK_LITTLE_ENDIAN);
 
-        // 林俊清, 20150721: 消息长度表未被使用，而且没更新，删除。
+        // 王海, 20150721: 消息长度表未被使用，而且没更新，删除。
         //public static readonly byte[] MAVLINK_MESSAGE_LENGTHS = new byte[] { 9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16, 33, 25, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 44, 3, 9, 22, 12, 18, 34, 66, 98, 8, 48, 19, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0 };
 
-        public static readonly byte[] MAVLINK_MESSAGE_CRCS = new byte[] { 50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 167, 138, 234, 240, 47, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 59, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0 };
+        //public static readonly byte[] MAVLINK_MESSAGE_CRCS = new byte[] {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 167, 138, 234, 240, 47, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 59, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0 };
+        //public static readonly byte[] MAVLINK_MESSAGE_CRCS = new byte[] { 50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 167, 138, 234, 240, 47, 0, 0, 0, 0, 0, 0, 0, 72, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 59, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0 };
+        //public static readonly byte[] MAVLINK_MESSAGE_CRCS = new byte[] { 50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 167, 138, 234, 240, 47, 0, 0, 0, 0, 85, 0, 0, 72, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 59, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0 };
+        public static readonly byte[] MAVLINK_MESSAGE_CRCS = new byte[] { 50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 167, 138, 234, 240, 47, 0, 0, 0, 0, 85, 0, 0, 72, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 59, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 35, 35, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0 };
 
-        public static Type[] MAVLINK_MESSAGE_INFO = new Type[] { typeof(mavlink_heartbeat_t), typeof(mavlink_sys_status_t), typeof(mavlink_system_time_t), null, typeof(mavlink_ping_t), typeof(mavlink_change_operator_control_t), typeof(mavlink_change_operator_control_ack_t), typeof(mavlink_auth_key_t), null, null, null, typeof(mavlink_set_mode_t), null, null, null, null, null, null, null, null, typeof(mavlink_param_request_read_t), typeof(mavlink_param_request_list_t), typeof(mavlink_param_value_t), typeof(mavlink_param_set_t), typeof(mavlink_gps_raw_int_t), typeof(mavlink_gps_status_t), typeof(mavlink_scaled_imu_t), typeof(mavlink_raw_imu_t), typeof(mavlink_raw_pressure_t), typeof(mavlink_scaled_pressure_t), typeof(mavlink_attitude_t), typeof(mavlink_attitude_quaternion_t), typeof(mavlink_local_position_ned_t), typeof(mavlink_global_position_int_t), typeof(mavlink_rc_channels_scaled_t), typeof(mavlink_rc_channels_raw_t), typeof(mavlink_servo_output_raw_t), typeof(mavlink_mission_request_partial_list_t), typeof(mavlink_mission_write_partial_list_t), typeof(mavlink_mission_item_t), typeof(mavlink_mission_request_t), typeof(mavlink_mission_set_current_t), typeof(mavlink_mission_current_t), typeof(mavlink_mission_request_list_t), typeof(mavlink_mission_count_t), typeof(mavlink_mission_clear_all_t), typeof(mavlink_mission_item_reached_t), typeof(mavlink_mission_ack_t), typeof(mavlink_set_gps_global_origin_t), typeof(mavlink_gps_global_origin_t), typeof(mavlink_set_local_position_setpoint_t), typeof(mavlink_local_position_setpoint_t), typeof(mavlink_global_position_setpoint_int_t), typeof(mavlink_set_global_position_setpoint_int_t), typeof(mavlink_safety_set_allowed_area_t), typeof(mavlink_safety_allowed_area_t), typeof(mavlink_set_roll_pitch_yaw_thrust_t), typeof(mavlink_set_roll_pitch_yaw_speed_thrust_t), typeof(mavlink_roll_pitch_yaw_thrust_setpoint_t), typeof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t), typeof(mavlink_set_quad_motors_setpoint_t), typeof(mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t), typeof(mavlink_nav_controller_output_t), typeof(mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t), typeof(mavlink_state_correction_t), null, typeof(mavlink_request_data_stream_t), typeof(mavlink_data_stream_t), null, typeof(mavlink_manual_control_t), typeof(mavlink_rc_channels_override_t), null, null, null, typeof(mavlink_vfr_hud_t), null, typeof(mavlink_command_long_t), typeof(mavlink_command_ack_t), null, null, typeof(mavlink_roll_pitch_yaw_rates_thrust_setpoint_t), typeof(mavlink_manual_setpoint_t), null, null, null, null, null, null, null, typeof(mavlink_local_position_ned_system_global_offset_t), typeof(mavlink_hil_state_t), typeof(mavlink_hil_controls_t), typeof(mavlink_hil_rc_inputs_raw_t), null, null, null, null, null, null, null, typeof(mavlink_optical_flow_t), typeof(mavlink_global_vision_position_estimate_t), typeof(mavlink_vision_position_estimate_t), typeof(mavlink_vision_speed_estimate_t), typeof(mavlink_vicon_position_estimate_t), typeof(mavlink_highres_imu_t), null, null, null, null, typeof(mavlink_file_transfer_start_t), typeof(mavlink_file_transfer_dir_list_t), typeof(mavlink_file_transfer_res_t), null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, typeof(mavlink_battery_status_t), typeof(mavlink_setpoint_8dof_t), typeof(mavlink_setpoint_6dof_t), typeof(mavlink_sensor_offsets_t), typeof(mavlink_set_mag_offsets_t), typeof(mavlink_meminfo_t), typeof(mavlink_ap_adc_t), typeof(mavlink_digicam_configure_t), typeof(mavlink_digicam_control_t), typeof(mavlink_mount_configure_t), typeof(mavlink_mount_control_t), typeof(mavlink_mount_status_t), null, typeof(mavlink_fence_point_t), typeof(mavlink_fence_fetch_point_t), typeof(mavlink_fence_status_t), typeof(mavlink_ahrs_t), typeof(mavlink_simstate_t), typeof(mavlink_hwstatus_t), typeof(mavlink_radio_t), typeof(mavlink_limits_status_t), typeof(mavlink_wind_t), typeof(mavlink_data16_t), typeof(mavlink_data32_t), typeof(mavlink_data64_t), typeof(mavlink_data96_t), typeof(mavlink_rangefinder_t), null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, typeof(mavlink_rc_mobile_control_t), typeof(mavlink_set_pair_t), null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, typeof(mavlink_memory_vect_t), typeof(mavlink_debug_vect_t), typeof(mavlink_named_value_float_t), typeof(mavlink_named_value_int_t), typeof(mavlink_statustext_t), typeof(mavlink_debug_t), null };
+        //public static Type[] MAVLINK_MESSAGE_INFO = new Type[] { typeof(mavlink_heartbeat_t), typeof(mavlink_sys_status_t), typeof(mavlink_system_time_t), null, typeof(mavlink_ping_t), typeof(mavlink_change_operator_control_t), typeof(mavlink_change_operator_control_ack_t), typeof(mavlink_auth_key_t), null, null, null, typeof(mavlink_set_mode_t), null, null, null, null, null, null, null, null, typeof(mavlink_param_request_read_t), typeof(mavlink_param_request_list_t), typeof(mavlink_param_value_t), typeof(mavlink_param_set_t), typeof(mavlink_gps_raw_int_t), typeof(mavlink_gps_status_t), typeof(mavlink_scaled_imu_t), typeof(mavlink_raw_imu_t), typeof(mavlink_raw_pressure_t), typeof(mavlink_scaled_pressure_t), typeof(mavlink_attitude_t), typeof(mavlink_attitude_quaternion_t), typeof(mavlink_local_position_ned_t), typeof(mavlink_global_position_int_t), typeof(mavlink_rc_channels_scaled_t), typeof(mavlink_rc_channels_raw_t), typeof(mavlink_servo_output_raw_t), typeof(mavlink_mission_request_partial_list_t), typeof(mavlink_mission_write_partial_list_t), typeof(mavlink_mission_item_t), typeof(mavlink_mission_request_t), typeof(mavlink_mission_set_current_t), typeof(mavlink_mission_current_t), typeof(mavlink_mission_request_list_t), typeof(mavlink_mission_count_t), typeof(mavlink_mission_clear_all_t), typeof(mavlink_mission_item_reached_t), typeof(mavlink_mission_ack_t), typeof(mavlink_set_gps_global_origin_t), typeof(mavlink_gps_global_origin_t), typeof(mavlink_set_local_position_setpoint_t), typeof(mavlink_local_position_setpoint_t), typeof(mavlink_global_position_setpoint_int_t), typeof(mavlink_set_global_position_setpoint_int_t), typeof(mavlink_safety_set_allowed_area_t), typeof(mavlink_safety_allowed_area_t), typeof(mavlink_set_roll_pitch_yaw_thrust_t), typeof(mavlink_set_roll_pitch_yaw_speed_thrust_t), typeof(mavlink_roll_pitch_yaw_thrust_setpoint_t), typeof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t), typeof(mavlink_set_quad_motors_setpoint_t), typeof(mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t), typeof(mavlink_nav_controller_output_t), typeof(mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t), typeof(mavlink_state_correction_t), null, typeof(mavlink_request_data_stream_t), typeof(mavlink_data_stream_t), null, typeof(mavlink_manual_control_t), typeof(mavlink_rc_channels_override_t), null, null, null, typeof(mavlink_vfr_hud_t), null, typeof(mavlink_command_long_t), typeof(mavlink_command_ack_t), null, null, typeof(mavlink_roll_pitch_yaw_rates_thrust_setpoint_t), typeof(mavlink_manual_setpoint_t), null, null, null, null, null, null, null, typeof(mavlink_local_position_ned_system_global_offset_t), typeof(mavlink_hil_state_t), typeof(mavlink_hil_controls_t), typeof(mavlink_hil_rc_inputs_raw_t), null, null, null, null, null, null, null, typeof(mavlink_optical_flow_t), typeof(mavlink_global_vision_position_estimate_t), typeof(mavlink_vision_position_estimate_t), typeof(mavlink_vision_speed_estimate_t), typeof(mavlink_vicon_position_estimate_t), typeof(mavlink_highres_imu_t), null, null, null, null, typeof(mavlink_file_transfer_start_t), typeof(mavlink_file_transfer_dir_list_t), typeof(mavlink_file_transfer_res_t), null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, typeof(mavlink_battery_status_t), typeof(mavlink_setpoint_8dof_t), typeof(mavlink_setpoint_6dof_t), typeof(mavlink_sensor_offsets_t), typeof(mavlink_set_mag_offsets_t), typeof(mavlink_meminfo_t), typeof(mavlink_ap_adc_t), typeof(mavlink_digicam_configure_t), typeof(mavlink_digicam_control_t), typeof(mavlink_mount_configure_t), typeof(mavlink_mount_control_t), typeof(mavlink_mount_status_t), null, typeof(mavlink_fence_point_t), typeof(mavlink_fence_fetch_point_t), typeof(mavlink_fence_status_t), typeof(mavlink_ahrs_t), typeof(mavlink_simstate_t), typeof(mavlink_hwstatus_t), typeof(mavlink_radio_t), typeof(mavlink_limits_status_t), typeof(mavlink_wind_t), typeof(mavlink_data16_t), typeof(mavlink_data32_t), typeof(mavlink_data64_t), typeof(mavlink_data96_t), typeof(mavlink_rangefinder_t), null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, typeof(mavlink_rc_mobile_control_t), typeof(mavlink_set_pair_t), null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, typeof(mavlink_memory_vect_t), typeof(mavlink_debug_vect_t), typeof(mavlink_named_value_float_t), typeof(mavlink_named_value_int_t), typeof(mavlink_statustext_t), typeof(mavlink_debug_t), null };
+        public static Type[] MAVLINK_MESSAGE_INFO = new Type[] { typeof(mavlink_heartbeat_t), typeof(mavlink_sys_status_t), typeof(mavlink_system_time_t), null, typeof(mavlink_ping_t), typeof(mavlink_change_operator_control_t), typeof(mavlink_change_operator_control_ack_t), typeof(mavlink_auth_key_t), null, null, null, typeof(mavlink_set_mode_t), null, null, null, null, null, null, null, null, typeof(mavlink_param_request_read_t), typeof(mavlink_param_request_list_t), typeof(mavlink_param_value_t), typeof(mavlink_param_set_t), typeof(mavlink_gps_raw_int_t), typeof(mavlink_gps_status_t), typeof(mavlink_scaled_imu_t), typeof(mavlink_raw_imu_t), typeof(mavlink_raw_pressure_t), typeof(mavlink_scaled_pressure_t), typeof(mavlink_attitude_t), typeof(mavlink_attitude_quaternion_t), typeof(mavlink_local_position_ned_t), typeof(mavlink_global_position_int_t), typeof(mavlink_rc_channels_scaled_t), typeof(mavlink_rc_channels_raw_t), typeof(mavlink_servo_output_raw_t), typeof(mavlink_mission_request_partial_list_t), typeof(mavlink_mission_write_partial_list_t), typeof(mavlink_mission_item_t), typeof(mavlink_mission_request_t), typeof(mavlink_mission_set_current_t), typeof(mavlink_mission_current_t), typeof(mavlink_mission_request_list_t), typeof(mavlink_mission_count_t), typeof(mavlink_mission_clear_all_t), typeof(mavlink_mission_item_reached_t), typeof(mavlink_mission_ack_t), typeof(mavlink_set_gps_global_origin_t), typeof(mavlink_gps_global_origin_t), typeof(mavlink_set_local_position_setpoint_t), typeof(mavlink_local_position_setpoint_t), typeof(mavlink_global_position_setpoint_int_t), typeof(mavlink_set_global_position_setpoint_int_t), typeof(mavlink_safety_set_allowed_area_t), typeof(mavlink_safety_allowed_area_t), typeof(mavlink_set_roll_pitch_yaw_thrust_t), typeof(mavlink_set_roll_pitch_yaw_speed_thrust_t), typeof(mavlink_roll_pitch_yaw_thrust_setpoint_t), typeof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t), typeof(mavlink_set_quad_motors_setpoint_t), typeof(mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t), typeof(mavlink_nav_controller_output_t), typeof(mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t), typeof(mavlink_state_correction_t), null, typeof(mavlink_request_data_stream_t), typeof(mavlink_data_stream_t), null, typeof(mavlink_manual_control_t), typeof(mavlink_rc_channels_override_t), null, null, null, typeof(mavlink_vfr_hud_t), null, typeof(mavlink_command_long_t), typeof(mavlink_command_ack_t), null, null, typeof(mavlink_roll_pitch_yaw_rates_thrust_setpoint_t), typeof(mavlink_manual_setpoint_t), null, null, null, null, null, null, null, typeof(mavlink_local_position_ned_system_global_offset_t), typeof(mavlink_hil_state_t), typeof(mavlink_hil_controls_t), typeof(mavlink_hil_rc_inputs_raw_t), null, null, null, null, null, null, null, typeof(mavlink_optical_flow_t), typeof(mavlink_global_vision_position_estimate_t), typeof(mavlink_vision_position_estimate_t), typeof(mavlink_vision_speed_estimate_t), typeof(mavlink_vicon_position_estimate_t), typeof(mavlink_highres_imu_t), null, null, null, null, typeof(mavlink_file_transfer_start_t), typeof(mavlink_file_transfer_dir_list_t), typeof(mavlink_file_transfer_res_t), null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, typeof(mavlink_battery_status_t), typeof(mavlink_setpoint_8dof_t), typeof(mavlink_setpoint_6dof_t), typeof(mavlink_sensor_offsets_t), typeof(mavlink_set_mag_offsets_t), typeof(mavlink_meminfo_t), typeof(mavlink_ap_adc_t), typeof(mavlink_digicam_configure_t), typeof(mavlink_digicam_control_t), typeof(mavlink_mount_configure_t), typeof(mavlink_mount_control_t), typeof(mavlink_mount_status_t), null, typeof(mavlink_fence_point_t), typeof(mavlink_fence_fetch_point_t), typeof(mavlink_fence_status_t), typeof(mavlink_ahrs_t), typeof(mavlink_simstate_t), typeof(mavlink_hwstatus_t), typeof(mavlink_radio_t), typeof(mavlink_limits_status_t), typeof(mavlink_wind_t), typeof(mavlink_data16_t), typeof(mavlink_data32_t), typeof(mavlink_data64_t), typeof(mavlink_data96_t), typeof(mavlink_rangefinder_t), null, null, null, null, null, null, null, null, null, null, null, null, typeof(mavlink_msg_id_led_control), null, null, null, null, null, null, null, null, null, null, null, null, null, typeof(mavlink_rc_mobile_control_t), typeof(mavlink_set_pair_t), null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, typeof(mavlink_memory_vect_t), typeof(mavlink_debug_vect_t), typeof(mavlink_named_value_float_t), typeof(mavlink_named_value_int_t), typeof(mavlink_statustext_t), typeof(mavlink_debug_t), null };
 
         public const byte MAVLINK_VERSION = 2;
 
@@ -73,15 +79,15 @@ namespace Plane.Protocols
             ///<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>
             TAKEOFF = 22,
             /// <summary>
-            /// // 林俊清, 20160331, 来自郭老师，据说是传文件用的。
+            /// // 王海, 20160331, 来自王海1，据说是传文件用的。
             /// </summary>
             TELL_COPTER = 23,
             /// <summary>
-            /// // 林俊清, 20160331, 来自郭老师的邮件，作用未知。
+            /// // 王海, 20160331, 来自王海1的邮件，作用未知。
             /// </summary>
             FILE_TRANS_MODE = 24,
             /// <summary>
-            /// // 林俊清, 20160331, 来自郭老师的邮件，用于 RF 测试。
+            /// // 王海, 20160331, 来自王海1的邮件，用于 RF 测试。
             /// </summary>
             RF_TEST = 25,
             ///<summary> Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|  </summary>
@@ -168,6 +174,12 @@ namespace Plane.Protocols
             START_RX_PAIR = 500,
             ///<summary>  | </summary>
             ENUM_END = 501,
+            ///<summary> 校准指南针| </summary>
+            DO_START_MAG_CAL = 42424,
+            ///<summary> 放弃指南针校准| </summary>
+            DO_CANCEL_MAG_CAL = 42425,
+            ///<summary> 接受指南针校准|</summary>
+            DO_ACCEPT_MAG_CAL = 42426,
         };
 
         /** @brief  */
@@ -1354,7 +1366,7 @@ namespace Plane.Protocols
             /// <summary> Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) </summary>
             public byte type;
             /// <summary> Autopilot type / class. defined in MAV_AUTOPILOT ENUM
-            /// // 林俊清, 20160311, 亿航用作切过 GPS 模式的标志。</summary>
+            /// // 王海, 20160311, 亿航用作切过 GPS 模式的标志。</summary>
             public byte autopilot;
             /// <summary> System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h </summary>
             public byte base_mode;
@@ -3062,6 +3074,27 @@ namespace Plane.Protocols
 
         };
 
+        public const byte MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST = 183;
+        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)]
+        public struct mavlink_sversion_request
+        {
+            public Int16 version;
+        }
+
+        public const byte MAVLINK_MSG_ID_GPS_RTCM_DATA = 233;
+        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 182)]
+        ///<summary> RTCM message for injecting into the onboard GPS (used for DGPS) </summary>
+        public struct mavlink_gps_rtcm_data_t
+        {
+            /// <summary> LSB: 1 means message is fragmented, next 2 bits are the fragment ID, the remaining 5 bits are used for the sequence ID. Messages are only to be flushed to the GPS when the entire message has been reconstructed on the autopilot. The fragment ID specifies which order the fragments should be assembled into a buffer, while the sequence ID is used to detect a mismatch between different buffers. The buffer is considered fully reconstructed when either all 4 fragments are present, or all the fragments before the first fragment with a non full payload is received. This management is used to ensure that normal GPS operation doesn't corrupt RTCM data, and to recover from a unreliable transport delivery order. </summary>
+            public byte flags;
+            /// <summary> data length </summary>
+            public byte len;
+            /// <summary> RTCM message (may be fragmented) </summary>
+            [MarshalAs(UnmanagedType.ByValArray, SizeConst = 180)]
+            public byte[] data;
+
+        };
 
         public const byte MAVLINK_MSG_ID_MEMORY_VECT = 249;
         [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 36)]
@@ -3192,6 +3225,20 @@ namespace Plane.Protocols
             public int RxID;
         };
 
+
+        public const byte MAVLINK_MSG_ID_LED_CONTROL = 186;
+        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 29)]
+        public struct mavlink_msg_id_led_control
+        {
+            public byte target_system;
+            public byte target_component;
+            public byte instance;
+            public byte pattern;
+            public byte custom_len;
+            [MarshalAs(UnmanagedType.ByValArray, SizeConst = 24)]
+            public byte[] custom_bytes;
+        };
+
     }
     //#endif
 }

PlaneGcsSdk_Shared/Protocols/MavCommTypes.cs

@@ -0,0 +1,187 @@
+using System;
+using System.Collections.Generic;
+using System.Runtime.InteropServices;
+using System.Text;
+
+namespace Plane.Protocols
+{
+    /*
+     * 
+     * 通信模块协议
+     * 
+     */
+    public class MavComm
+    {
+        /// <summary>
+        /// 发送数据时的数据包头
+        /// </summary>
+        public const ushort COMM_SEND_PACKET_HEADER = 0x9551;
+        /// <summary>
+        /// 接受数据时的数据包头
+        /// </summary>
+        public const ushort COMM_RECEIVE_PACKET_HEADER = 0x7713;
+
+
+        #region 命令类型 2字节
+        //-----------------命令类型-----------------
+        /// <summary>
+        /// 查询状态
+        /// </summary>
+        public const short COMM_QUERY_STATUS = 0x00;
+
+        /// <summary>
+        /// 变更飞机总数及参与者
+        /// </summary>
+        public const short COMM_SET_MAV_COUNT = 0x10;
+
+        /// <summary>
+        /// 获取飞机详细信息
+        /// </summary>
+        public const short COMM_GET_COPTERS_COUNT = 0x20;
+
+        /// <summary>
+        /// 时间同步，不改变当前模式
+        /// </summary>
+        public const short COMM_ASYN_TIME = 0x30;
+
+        /// <summary>
+        /// 进入空闲模式
+        /// </summary>
+        public const short COMM_IDLE_MODE = 0x50;
+
+        /// <summary>
+        /// 进入搜索模式，命名编号targetID  memcpy(pdata,input,2);
+        /// </summary>
+        public const short COMM_SEARCH_MODE = 0x51;
+
+        /// <summary>
+        /// 进入航点下载模式 (写航点)
+        /// </summary>
+        public const short COMM_DOWNLOAD_MODE = 0x52;
+
+        /// <summary>
+        /// 下载模式通信
+        /// </summary>
+        public const short COMM_DOWNLOAD_COMM = 0x53;
+
+        /// <summary>
+        /// 进入飞行模式（无负载）
+        /// </summary>
+        public const short COMM_FLIGHT_MODE = 0x54;
+
+        /// <summary>
+        /// 进入飞行模式（有负载数据）
+        /// </summary>
+        public const short COMM_FLIGHT_LOAD_MODE = 0x55;
+
+
+        /// <summary>
+        /// 发送非针对飞控的内部控制指令
+        /// </summary>
+        public const short COMM_TO_MODULE = 0x5F;
+
+
+
+        /// <summary>
+        /// 通信模块
+        /// </summary>
+        public const short COMM_NOTIFICATION = 0x1234;
+
+        /// <summary>
+        /// 空中升级(更新通信模块飞机端)
+        /// </summary>
+        public const short COMM_UPDATE_COPTER_MODULE = 0xFD;
+
+
+
+        /// <summary>
+        /// 测试模块
+        /// </summary>
+        public const short COMM_TEST_MODULE = 0x3C;
+
+
+
+        #endregion 
+
+        public enum CommMode
+        {
+            IDLE = 0,
+            SEARCH = 1,
+            DOWNLOAD = 3,
+            FLIGHT = 4
+        }
+
+        public enum MessageType
+        {
+            STR = 0,
+            SCAN2 = 2,
+            SCAN3 = 3
+        }
+
+        //search 搜索模式相关
+        public const ushort SEARCH_COMPLETED = 0x0502;                  //搜索模式 扫描成功
+        public const ushort FREQUENCY_HOPPING_COMPLETED = 0x0503;       //搜索模式 跳频表更新成功
+        public const ushort SEARCH_FINDCOPTER = 0x0401;                 //搜索模式 找到指定目标
+        public const ushort FREQUENCY_HOPPING_ERROR = 0x0100;           //搜索模式 跳频表更新错误
+        public const ushort SEARCH_OUTTIME = 0xFFFF;                    //搜索模式 没有人应答   超时时间20S
+
+
+        public const ushort MISSION_SEND_COMPLETED = 0x0503;            //航点传输到飞控成功，可开始传输下个飞机航点信息, 
+                                                                        //需要再后续等待一个成功消息，才算完成   
+
+        public const ushort P2P_SEND_FAILED = 0x0600;                   //点对点通信发送失败 
+
+        public const ushort ERROR_CODE_START = 0x0100;
+        public const ushort ERROR_CODE_END = 0x03FF;
+
+
+        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)]
+        public struct comm_set_mav_count
+        {
+            public Int16 mav_count;    //飞机总数
+        };
+
+        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 4)]
+        public struct comm_update_copter_module
+        {
+            public Int16 mav_count;    //飞机总数
+            public Int16 update_code; 
+        };
+
+
+        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 8)]
+        public struct comm_asyn_time
+        {
+            public Int64 time_stamp;
+        };
+
+        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 28)]
+        public struct comm_copter_info
+        {
+            //   public Int32 control_mode;      //飞行模式(自动，悬停，定高，返航)      
+            public byte control_mode;         //飞行模式(自动，悬停，定高，返航)     
+            public byte rpecheck_fault;          //是否有 预解锁错误
+            public byte reserveddata1;        //保留   
+            public byte reserveddata2;        //保留
+ 
+
+            public UInt32 lat;              // 经度 in 1E7 degrees
+            public UInt32 lng;              // 维度 in 1E7 degrees
+            public float retain;            // 写参数序列号,返回版本号等不特定返回数据	
+            public Int32 alt;               // millimeters above home  
+
+
+            public byte gps_status;        //锁定类型 (无锁定,3D锁定，RTK浮点，RTK固定)   
+            public byte error_code;        //错误号，0表示无错误 --放到低位为了和之前兼容
+ 
+
+
+            public byte lock_status;        //以及是否解锁	
+            public byte gps_num_sats;       // 卫星数量 
+            public Int16 battery_voltage;   // 电池电压mV  
+            public UInt16 heading;          //方向角度
+
+        };
+
+    }
+}

PlaneGcsSdk_Shared/Protocols/MavlinkUtil.cs

@@ -26,7 +26,7 @@ namespace Plane.Protocols
         /// <returns>The newly created mavlink packet</returns>
         internal static TMavlinkPacket ByteArrayToStructure<TMavlinkPacket>(this byte[] bytearray, int startoffset = 6) where TMavlinkPacket : struct
         {
-            if (bytearray[0] == 'U')
+            if (bytearray[0] == 'U' && startoffset == 6 )
             {
                 throw new NotSupportedException("bytearray[0] == 'U' is not supported.");
                 //ByteArrayToStructureEndian(bytearray, ref obj, startoffset);
@@ -44,7 +44,7 @@ namespace Plane.Protocols
                     Marshal.Copy(bytearray, startoffset, i, len);
                     packet = Marshal.PtrToStructure<TMavlinkPacket>(i);
                 }
-                // 林俊清, 20151026, 改为不吞异常了。
+                // 王海, 20151026, 改为不吞异常了。
                 //catch
                 //{
                 //    //log.Error("ByteArrayToStructure FAIL", ex);

PlaneGcsSdk_Shared/Protocols/MavlinkUtil_NET45.cs

@@ -43,7 +43,7 @@ namespace Plane.Protocols
                     Marshal.Copy(bytearray, startoffset, i, len);
                     packet = MarshalEx.PtrToStructure<TMavlinkPacket>(i);
                 }
-                // 林俊清, 20151026, 改为不吞异常了。
+                // 王海, 20151026, 改为不吞异常了。
                 //catch
                 //{
                 //    //log.Error("ByteArrayToStructure FAIL", ex);

PlaneGcsSdk_Shared/TaskTools/HIL/Vector3.cs

@@ -39,13 +39,13 @@ namespace TaskTools.HIL
                                                   self.z);
         }
 
-        // 林俊清，20150702：删除 MissionPlanner.Utilities.dll。
+        // 王海，20150702：删除 MissionPlanner.Utilities.dll。
         //public static implicit operator Vector3(PointLatLngAlt a)
         //{
         //    return new Vector3(a.Lat,a.Lng,a.Alt);
         //}
 
-        // 林俊清，20150702：删除 GMap.NET.Core.dll（仅此处使用其 PointLatLng）。
+        // 王海，20150702：删除 GMap.NET.Core.dll（仅此处使用其 PointLatLng）。
 
         //public static implicit operator Vector3(PointLatLng a)
         //{