[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

@@ -1,74 +1,81 @@
-<?xml version="1.0" encoding="utf-8"?>
-<Project ToolsVersion="14.0" DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
-  <Import Project="$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props" Condition="Exists('$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props')" />
-  <PropertyGroup>
-    <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
-    <Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>
-    <ProjectGuid>{0111EB6E-72E3-499C-A3BA-022F5BBC4CAF}</ProjectGuid>
-    <OutputType>Library</OutputType>
-    <AppDesignerFolder>Properties</AppDesignerFolder>
-    <RootNamespace>Plane</RootNamespace>
-    <AssemblyName>PlaneGcsSdk_Private_NET46</AssemblyName>
-    <TargetFrameworkVersion>v4.6</TargetFrameworkVersion>
-    <FileAlignment>512</FileAlignment>
-  </PropertyGroup>
-  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
-    <DebugSymbols>true</DebugSymbols>
-    <DebugType>full</DebugType>
-    <Optimize>false</Optimize>
-    <OutputPath>bin\Debug\</OutputPath>
-    <DefineConstants>TRACE;DEBUG;PRIVATE</DefineConstants>
-    <ErrorReport>prompt</ErrorReport>
-    <WarningLevel>4</WarningLevel>
-  </PropertyGroup>
-  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
-    <DebugType>pdbonly</DebugType>
-    <Optimize>true</Optimize>
-    <OutputPath>bin\Release\</OutputPath>
-    <DefineConstants>TRACE;PRIVATE</DefineConstants>
-    <ErrorReport>prompt</ErrorReport>
-    <WarningLevel>4</WarningLevel>
-    <DocumentationFile>bin\Release\PlaneGcsSdk_Private_NET46.xml</DocumentationFile>
-  </PropertyGroup>
-  <ItemGroup>
-    <Reference Include="System" />
-    <Reference Include="System.Core" />
-    <Reference Include="System.Xml.Linq" />
-    <Reference Include="System.Data.DataSetExtensions" />
-    <Reference Include="Microsoft.CSharp" />
-    <Reference Include="System.Data" />
-    <Reference Include="System.Net.Http" />
-    <Reference Include="System.Xml" />
-    <Reference Include="WindowsBase" />
-  </ItemGroup>
-  <ItemGroup>
-    <Compile Include="..\PlaneGcsSdk_Private_NET45\Copters\CopterImplSharedPart.Private.cs">
-      <Link>Copters\CopterImplSharedPart.Private.cs</Link>
-    </Compile>
-    <Compile Include="..\PlaneGcsSdk_Private_NET45\Copters\PlaneCopter.Private.cs">
-      <Link>Copters\PlaneCopter.Private.cs</Link>
-    </Compile>
-    <Compile Include="..\PlaneGcsSdk_Private_NET45\Copters\PLCopter.Private.cs">
-      <Link>Copters\EHCopter.Private.cs</Link>
-    </Compile>
-    <Compile Include="..\PlaneGcsSdk_Private_NET45\Copters\FakeCopter.Private.cs">
-      <Link>Copters\FakeCopter.Private.cs</Link>
-    </Compile>
-    <Compile Include="Properties\AssemblyInfo.cs" />
-  </ItemGroup>
-  <ItemGroup>
-    <ProjectReference Include="..\PlaneGcsSdk.Contract_Private\PlaneGcsSdk.Contract_Private.csproj">
-      <Project>{47141894-ece3-48ca-8dcf-ca751bda231e}</Project>
-      <Name>PlaneGcsSdk.Contract_Private</Name>
-    </ProjectReference>
-  </ItemGroup>
-  <Import Project="..\PlaneGcsSdk_Shared\PlaneGcsSdk_Shared.projitems" Label="Shared" />
-  <Import Project="$(MSBuildToolsPath)\Microsoft.CSharp.targets" />
-  <!-- To modify your build process, add your task inside one of the targets below and uncomment it. 
-       Other similar extension points exist, see Microsoft.Common.targets.
-  <Target Name="BeforeBuild">
-  </Target>
-  <Target Name="AfterBuild">
-  </Target>
-  -->
+<?xml version="1.0" encoding="utf-8"?>
+<Project ToolsVersion="14.0" DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+  <Import Project="$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props" Condition="Exists('$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props')" />
+  <PropertyGroup>
+    <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
+    <Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>
+    <ProjectGuid>{0111EB6E-72E3-499C-A3BA-022F5BBC4CAF}</ProjectGuid>
+    <OutputType>Library</OutputType>
+    <AppDesignerFolder>Properties</AppDesignerFolder>
+    <RootNamespace>Plane</RootNamespace>
+    <AssemblyName>PlaneGcsSdk_Private_NET46</AssemblyName>
+    <TargetFrameworkVersion>v4.8</TargetFrameworkVersion>
+    <FileAlignment>512</FileAlignment>
+    <TargetFrameworkProfile />
+  </PropertyGroup>
+  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
+    <DebugSymbols>true</DebugSymbols>
+    <DebugType>full</DebugType>
+    <Optimize>false</Optimize>
+    <OutputPath>bin\Debug\</OutputPath>
+    <DefineConstants>TRACE;DEBUG;PRIVATE</DefineConstants>
+    <ErrorReport>prompt</ErrorReport>
+    <WarningLevel>4</WarningLevel>
+  </PropertyGroup>
+  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
+    <DebugType>pdbonly</DebugType>
+    <Optimize>true</Optimize>
+    <OutputPath>bin\Release\</OutputPath>
+    <DefineConstants>TRACE;PRIVATE</DefineConstants>
+    <ErrorReport>prompt</ErrorReport>
+    <WarningLevel>4</WarningLevel>
+    <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" />
+    <Reference Include="System.Data" />
+    <Reference Include="System.Net.Http" />
+    <Reference Include="System.Xml" />
+    <Reference Include="WindowsBase" />
+  </ItemGroup>
+  <ItemGroup>
+    <Compile Include="..\PlaneGcsSdk_Private_NET45\Copters\CopterImplSharedPart.Private.cs">
+      <Link>Copters\CopterImplSharedPart.Private.cs</Link>
+    </Compile>
+    <Compile Include="..\PlaneGcsSdk_Private_NET45\Copters\PlaneCopter.Private.cs">
+      <Link>Copters\PlaneCopter.Private.cs</Link>
+    </Compile>
+    <Compile Include="..\PlaneGcsSdk_Private_NET45\Copters\PLCopter.Private.cs">
+      <Link>Copters\EHCopter.Private.cs</Link>
+    </Compile>
+    <Compile Include="..\PlaneGcsSdk_Private_NET45\Copters\FakeCopter.Private.cs">
+      <Link>Copters\FakeCopter.Private.cs</Link>
+    </Compile>
+    <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>
+    </ProjectReference>
+  </ItemGroup>
+  <Import Project="..\PlaneGcsSdk_Shared\PlaneGcsSdk_Shared.projitems" Label="Shared" />
+  <Import Project="$(MSBuildToolsPath)\Microsoft.CSharp.targets" />
+  <!-- To modify your build process, add your task inside one of the targets below and uncomment it. 
+       Other similar extension points exist, see Microsoft.Common.targets.
+  <Target Name="BeforeBuild">
+  </Target>
+  <Target Name="AfterBuild">
+  </Target>
+  -->
 </Project>

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,92 +1,138 @@
-#if !NETFX_CORE
-
-using System;
-using System.Net.Sockets;
-using System.Threading.Tasks;
-
-namespace Plane.Communication
-{
-    /// <summary>
-    /// 提供作为 TCP 客户端通信的能力。
-    /// </summary>
-    public class TcpConnection : TcpConnectionBase
-    {
-        private string _remoteHostname;
-
-        private int _remotePort;
-
-        public TcpConnection(string remoteHostname, int remotePort = 5250)
-        {
-            _remoteHostname = remoteHostname;
-            _remotePort = remotePort;
-            _client = new TcpClient
-            {
-                ReceiveBufferSize = 40 * 1024,
-                ReceiveTimeout = 1200
-            };
-        }
-
-        public void Open()
-        {
-            if (!IsOpen)
-            {
-                try
-                {
-                    _client.Connect(_remoteHostname, _remotePort);
-                }
-                catch (SocketException)
-                {
-                    CreateClientAndConnect();
-                }
-                catch (ObjectDisposedException)
-                {
-                    CreateClientAndConnect();
-                }
-                _isBroken = false;
-            }
-            _stream = _client.GetStream();
-        }
-
-        public override async Task OpenAsync()
-        {
-            if (!IsOpen)
-            {
-                try
-                {
-                    await _client.ConnectAsync(_remoteHostname, _remotePort).ConfigureAwait(false);
-                }
-                catch (SocketException)
-                {
-                    await CreateClientAndConnectAsync();
-                }
-                catch (ObjectDisposedException)
-                {
-                    await CreateClientAndConnectAsync();
-                }
-                _isBroken = false;
-            }
-            _stream = _client.GetStream();
-        }
-
-        private void CreateClientAndConnect()
-        {
-            _client = new TcpClient(_remoteHostname, _remotePort)
-            {
-                ReceiveBufferSize = 40 * 1024,
-                ReceiveTimeout = 1200
-            };
-        }
-
-        private async Task CreateClientAndConnectAsync()
-        {
-            _client = new TcpClient
-            {
-                ReceiveBufferSize = 40 * 1024,
-                ReceiveTimeout = 1200
-            };
-            await _client.ConnectAsync(_remoteHostname, _remotePort).ConfigureAwait(false);
-        }
-    }
-}
-
-#endif
+#if !NETFX_CORE
+
+using System;
+using System.Net;
+using System.Net.Sockets;
+using System.Threading.Tasks;
+
+namespace Plane.Communication
+{
+    /// <summary>
+    /// 提供作为 TCP 客户端通信的能力。
+    /// </summary>
+    public class TcpConnection : TcpConnectionBase
+    {
+        private string _remoteHostname;
+        const int tcpReceiveBufferSize = 20 * 1024;  //40 * 1024,
+        const int tcpReceiveTimeout = 1200;
+
+        private int _remotePort;
+
+        public TcpConnection(string remoteHostname, int remotePort = 5250)
+        {
+            _remoteHostname = remoteHostname;
+            _remotePort = remotePort;
+            _client = new TcpClient
+            {
+                ReceiveBufferSize = tcpReceiveBufferSize,
+                ReceiveTimeout = tcpReceiveTimeout
+            };
+        }
+
+        public void Open()
+        {
+            if (!IsOpen)
+            {
+                try
+                {
+                    _client.Connect(_remoteHostname, _remotePort);
+                }
+                catch (SocketException)
+                {
+                    CreateClientAndConnect();
+                }
+                catch (ObjectDisposedException)
+                {
+                    CreateClientAndConnect();
+                }
+                _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
+                {
+                    var connectTask = _client.ConnectAsync(_remoteHostname, _remotePort);
+                    if (await Task.WhenAny(connectTask, Task.Delay(5000)) == connectTask)
+                    {
+                        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)
+                {
+                    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 = tcpReceiveBufferSize,
+                ReceiveTimeout = tcpReceiveTimeout
+            };
+        }
+
+        private async Task CreateClientAndConnectAsync()
+        {
+            _client = new TcpClient
+            {
+                ReceiveBufferSize = tcpReceiveBufferSize,// 40 * 1024,
+                ReceiveTimeout = tcpReceiveTimeout// 1200
+            };
+            await _client.ConnectAsync(_remoteHostname, _remotePort).ConfigureAwait(false);
+        }
+    }
+}
+
+#endif

PlaneGcsSdk_Shared/Communication/TcpConnectionBase.cs

@@ -24,7 +24,10 @@ namespace Plane.Communication
             {
                 try
                 {
-                    return _client.Available;
+                    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,8 +195,11 @@ 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/EHCopter.InternalCopterEventHandlers.cs

@@ -1,296 +1,317 @@
-using System;
-using static Plane.Protocols.MAVLink;
-
-namespace Plane.Copters
-{
-    public partial class PLCopter : CopterImplSharedPart
-    {
-        private bool _fetchingFirmwareVersion;
-
-        private void _internalCopter_ConnectionBroken(object sender, EventArgs e)
-        {
-            IsConnected = _internalCopter.IsConnected;
-        }
-
-        private void _internalCopter_GetLogDataEvent(string log)
-        {
-            //飞机消息日志，后面需要改为记录方式
-            StatusText =Name+":"+log;
-            var e = new MessageCreatedEventArgs { Message = StatusText };
-            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;
-
-                        if (IsGpsAccurate)
-                        {
-                            UpdateFlightDataIfNeeded();
-                            RaiseLocationChangedIfNeeded();
-                        }
-
-                        RaiseDataStreamReceived(PDataStreamType.GPS_RAW_INT);
-
-                        break;
-                    }
-                case MAVLINK_MSG_ID_ATTITUDE:  //无用
-                    {
-                        Roll = _internalCopter.roll;
-                        Pitch = _internalCopter.pitch;
-                        Yaw = _internalCopter.yaw;
-                        TimebootMs = _internalCopter.timebootms;
-                        RaiseAttitudeChanged();
-
-                        break;
-                    }
-                case MAVLINK_MSG_ID_RC_CHANNELS_RAW:   //只用于显示,可以不要
-                    {
-                        Channel1 = _internalCopter.ch1in;
-                        Channel2 = _internalCopter.ch2in;
-                        Channel3 = _internalCopter.ch3in;
-                        Channel4 = _internalCopter.ch4in;
-                        Channel5 = _internalCopter.ch5in;
-                        Channel6 = _internalCopter.ch6in;
-                        Channel7 = _internalCopter.ch7in;
-                        Channel8 = _internalCopter.ch8in;
-
-                        RaiseDataStreamReceived(PDataStreamType.RC_CHANNELS_RAW);
-
-                        break;
-                    }
-                case MAVLINK_MSG_ID_VFR_HUD:
-                    {
-                        Heading = _internalCopter.heading;  //有用
-                        Altitude = _internalCopter.alt;     //有用
-                        AirSpeed = _internalCopter.airspeed;   //没用
-                        GroundSpeed = _internalCopter.groundspeed;  //没用
-
-                        RaiseAltitudeChangedIfNeeded();
-
-                        break;
-                    }
-            }
-        }
-
-        private async void _internalCopter_ReceiveHeartBearEvent(object sender, uint iError, ulong iCount)
-        {
-            IsUnlocked = _internalCopter.armed;
-            Mode = (FlightMode)_internalCopter.mode;
-            // 林俊清, 20160311, 将闲置的 apname 用作已经切过 GPS 模式的标志。
-            HasSwitchedToGpsMode = (byte)_internalCopter.apname != 0;
-            ++HeartbeatCount;
-
-            IsCheckingConnection = false;
-            /*
-            if (FirmwareVersion == null && !_fetchingFirmwareVersion)
-            {
-                try
-                {
-                    _fetchingFirmwareVersion = true;
-                    FirmwareVersion = (int)await GetParamAsync("FIRMWARE_VERSION", 5000);
-                }
-                catch (TimeoutException)
-                {
-                    // 吞掉。
-                }
-                finally
-                {
-                    _fetchingFirmwareVersion = false;
-                }
-            }
-            */
-            RaiseHeartbeatReceived(new HeartbeatReceivedEventArgs(iCount));
-        }
-
-        private void _internalCopter_ReceiveSensorEvent(object sender)
-        {
-            RaiseSensorDataReceived(EventArgs.Empty);
-        }
-
-        private void _internalCopter_ReceiveSysStatusEvent(object sender, byte StreamType, bool GPSBadHealth, bool GyroBadHealth, bool AccelBadHealth, bool CompassBadHealth, bool BarometerBadHealth, bool canTakeOff)
-        {
-            Voltage = _internalCopter.battery_voltage / 1000;
-
-            if (_internalCopter.battery_remaining == 255)   // 剩余电量未知，需要计算。
-            {
-                CalcBatteryPer();
-            }
-            else
-            {
-                BatteryPer = _internalCopter.battery_remaining;
-            }
-
-            const int MIN_VERSION_WITH_CAN_TAKE_OFF_FIELD = 12308;
-            const int MIN_VERSION_OF_GENERATION2 = 0x3000;
-            if (FirmwareVersion != null)
-            {
-                if (FirmwareVersion.Value >= MIN_VERSION_WITH_CAN_TAKE_OFF_FIELD)
-                {
-                    // 飞控提供了 canTakeOff。
-                    IsGpsAccurate = canTakeOff;
-                }
-                else if (FirmwareVersion.Value >= MIN_VERSION_OF_GENERATION2)
-                {
-                    // 2.0 飞行器，飞控未提供 canTakeOff。
-                    IsGpsAccurate = SatCount >= 12;
-                }
-                else
-                {
-                    // 1.0 飞行器，飞控不提供 canTakeOff。
-                    IsGpsAccurate = SatCount >= 6;
-                }
-            }
-            else
-                IsGpsAccurate = SatCount >= 8;
-
-            FlightDistance2D = _internalCopter.FlightDistance2D;
-
-            RaiseSysStatusReceived(new SystemStatusReceivedEventArgs(GPSBadHealth, GyroBadHealth, AccelBadHealth, CompassBadHealth, BarometerBadHealth));
-        }
-
-        private void _internalCopter_PacketHandled(object sender, PlaneCopter.PacketReceivedEventArgs e)
-        {
-            switch (e.Packet[5])
-            {
-                case MAVLINK_MSG_ID_MISSION_COUNT:
-                    MissionCount = _internalCopter.WpCount;
-                    break;
-
-                default:
-                    break;
-            }
-        }
-
-        private void _internalCopter_UnhandledPacketReceived(object sender, PlaneCopter.PacketReceivedEventArgs e)
-        {
-            switch (e.Packet[5])
-            {
-                case MAVLINK_MSG_ID_MISSION_ACK:
-                    AnalyzeMissionAckPacket(e.Packet);
-                    break;
-
-                case MAVLINK_MSG_ID_MISSION_COUNT:
-                    AnalyzeMissionCountPacket(e.Packet);
-                    break;
-
-                case MAVLINK_MSG_ID_MISSION_ITEM:
-                    AnalyzeMissionItemPacket(e.Packet);
-                    break;
-
-                case MAVLINK_MSG_ID_MISSION_REQUEST:
-                    AnalyzeMissionRequestPacket(e.Packet);
-                    break;
-
-                case MAVLINK_MSG_ID_SET_PAIR:
-                    AnalyzeSetPairPacket(e.Packet);
-                    break;
-
-                default:
-                    break;
-            }
-        }
-
-#if PRIVATE
-        protected virtual void RegisterInternalCopterEventHandlers()
-#else
-
-        private void RegisterInternalCopterEventHandlers()
-#endif
-        {
-            _internalCopter.GetLogDataEvent += _internalCopter_GetLogDataEvent;
-            _internalCopter.ReceiveDataStreamEvent += _internalCopter_ReceiveDataStreamEvent;
-            _internalCopter.ReceiveHeartBearEvent += _internalCopter_ReceiveHeartBearEvent;
-            _internalCopter.ReceiveSensorEvent += _internalCopter_ReceiveSensorEvent;
-            _internalCopter.ReceiveSysStatusEvent += _internalCopter_ReceiveSysStatusEvent;
-            _internalCopter.ConnectionBroken += _internalCopter_ConnectionBroken;
-            _internalCopter.PacketHandled += _internalCopter_PacketHandled;
-            _internalCopter.UnhandledPacketReceived += _internalCopter_UnhandledPacketReceived;
-            _internalCopter.MessageCreated += _internalCopter_MessageCreated;
-        }
-
-        private void _internalCopter_MessageCreated(object sender, MessageCreatedEventArgs e)
-        {
-            var txte = new MessageCreatedEventArgs { Message = Name + ":" +e.Message };
-            RaiseTextReceived(txte);
-        }
-
-        #region 计算剩余电量
-
-        private int bPerTimes;
-        private int outBatteryPer;
-        private int[] tPerTimes = new int[20];
-        private int v_num;
-
-        /// <summary>
-        /// 计算电量并在条件满足时更新 <see cref="BatteryPer"/> 属性。
-        /// </summary>
-        private void CalcBatteryPer()
-        {
-            float volmax = 0f;
-            float volmin = 0f;
-            if (Voltage > 5 && Voltage < 9)
-            {
-                volmax = 8.2f;
-                volmin = 7f;
-            }
-            else if (Voltage >= 9
-                  && Voltage < 13.6)
-            {
-                volmax = 11.6f;
-                volmin = 10.2f;
-            }
-            else if (Voltage >= 13.6
-                  && Voltage < 17.2)
-            {
-                volmax = 16.3f;
-                volmin = 14.2f;
-            }
-            else if (Voltage >= 17.2
-                  && Voltage < 26.2)
-            {
-                volmax = 24.8f;
-                volmin = 21.2f;
-            }
-
-            int batteryPer = (int)Math.Round((Voltage - volmin) * 100 / (volmax - volmin));
-
-            if (batteryPer == -1 || volmax == 0 || volmin == 0)
-                return;
-
-            if (bPerTimes < 20)
-            {
-                tPerTimes[bPerTimes] = batteryPer;
-                bPerTimes += 1;
-            }
-            else
-            {
-                tPerTimes[v_num] = batteryPer;
-                v_num++;
-                if (v_num == 20)
-                    v_num = 0;
-            }
-            for (int i = 0; i < bPerTimes; i++)
-            {
-                outBatteryPer += tPerTimes[i];
-            }
-
-            outBatteryPer = outBatteryPer / bPerTimes;
-            if (outBatteryPer < BatteryPer && bPerTimes > 18)
-            {
-                BatteryPer = (byte)outBatteryPer;
-            }
-        }
-
-        #endregion 计算剩余电量
-    }
-}
+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)
+        {
+            IsConnected = _internalCopter.IsConnected;
+        }
+
+        private void _internalCopter_GetLogDataEvent(string log)
+        {
+            //飞机消息日志，后面需要改为记录方式
+            StatusText =Name+":"+log;
+            var e = new MessageCreatedEventArgs { Message = StatusText };
+            RaiseTextReceived(e);
+        }
+    
+            
+        private void _internalCopter_ReceiveDataStreamEvent(object sender, byte StreamType)
+        {
+            switch (StreamType)
+            {
+                case MAVLINK_MSG_ID_GPS_RAW_INT:    //需要
+                    {
+                        SatCount = _internalCopter.satcount;
+                        GpsFixType = _internalCopter.gpsstatus.ToGpsFixType();
+                        GpsHdop = _internalCopter.gpshdop;
+                        CopterErrorCode =_internalCopter.errorcode;
+                        CopterPreCheckPass = _internalCopter.precheckok;
+                        CopterErrorString = getcoptererrorstr(_internalCopter.errorcode);
+                        Altitude = _internalCopter.gpsalt;
+                        Retain = BitConverter.GetBytes(_internalCopter.retain);
+                        
+                        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);
+
+                        break;
+                    }
+                case MAVLINK_MSG_ID_ATTITUDE:  //无用
+                    {
+                        Roll = _internalCopter.roll;
+                        Pitch = _internalCopter.pitch;
+                        Yaw = _internalCopter.yaw;
+                        TimebootMs = _internalCopter.timebootms;
+                        RaiseAttitudeChanged();
+
+                        break;
+                    }
+                case MAVLINK_MSG_ID_RC_CHANNELS_RAW:   //只用于显示,可以不要
+                    {
+                        Channel1 = _internalCopter.ch1in;
+                        Channel2 = _internalCopter.ch2in;
+                        Channel3 = _internalCopter.ch3in;
+                        Channel4 = _internalCopter.ch4in;
+                        Channel5 = _internalCopter.ch5in;
+                        Channel6 = _internalCopter.ch6in;
+                        Channel7 = _internalCopter.ch7in;
+                        Channel8 = _internalCopter.ch8in;
+
+                        RaiseDataStreamReceived(PDataStreamType.RC_CHANNELS_RAW);
+
+                        break;
+                    }
+                case MAVLINK_MSG_ID_VFR_HUD:
+                    {
+                        Heading = _internalCopter.heading;  //有用
+                        Altitude = _internalCopter.alt;     //有用
+                        AirSpeed = _internalCopter.airspeed;   //没用
+                        GroundSpeed = _internalCopter.groundspeed;  //没用
+
+                        RaiseAltitudeChangedIfNeeded();
+
+                        break;
+                    }
+            }
+        }
+
+        private async void _internalCopter_ReceiveHeartBearEvent(object sender, uint iError, ulong iCount)
+        {
+            IsUnlocked = _internalCopter.armed;
+            Mode = (FlightMode)_internalCopter.mode;
+            // 王海, 20160311, 将闲置的 apname 用作已经切过 GPS 模式的标志。
+            HasSwitchedToGpsMode = (byte)_internalCopter.apname != 0;
+            ++HeartbeatCount;
+
+            IsCheckingConnection = false;
+            /*
+            if (FirmwareVersion == null && !_fetchingFirmwareVersion)
+            {
+                try
+                {
+                    _fetchingFirmwareVersion = true;
+                    FirmwareVersion = (int)await GetParamAsync("FIRMWARE_VERSION", 5000);
+                }
+                catch (TimeoutException)
+                {
+                    // 吞掉。
+                }
+                finally
+                {
+                    _fetchingFirmwareVersion = false;
+                }
+            }
+            */
+            RaiseHeartbeatReceived(new HeartbeatReceivedEventArgs(iCount));
+        }
+
+        private void _internalCopter_ReceiveSensorEvent(object sender)
+        {
+            RaiseSensorDataReceived(EventArgs.Empty);
+        }
+
+        private void _internalCopter_ReceiveSysStatusEvent(object sender, byte StreamType, bool GPSBadHealth, bool GyroBadHealth, bool AccelBadHealth, bool CompassBadHealth, bool BarometerBadHealth, bool canTakeOff)
+        {
+            Voltage = _internalCopter.battery_voltage / 1000;
+
+            if (_internalCopter.battery_remaining == 255)   // 剩余电量未知，需要计算。
+            {
+                CalcBatteryPer();
+            }
+            else
+            {
+                BatteryPer = _internalCopter.battery_remaining;
+            }
+
+            const int MIN_VERSION_WITH_CAN_TAKE_OFF_FIELD = 12308;
+            const int MIN_VERSION_OF_GENERATION2 = 0x3000;
+            if (FirmwareVersion != null)
+            {
+                if (FirmwareVersion.Value >= MIN_VERSION_WITH_CAN_TAKE_OFF_FIELD)
+                {
+                    // 飞控提供了 canTakeOff。
+                    IsGpsAccurate = canTakeOff;
+                }
+                else if (FirmwareVersion.Value >= MIN_VERSION_OF_GENERATION2)
+                {
+                    // 2.0 飞行器，飞控未提供 canTakeOff。
+                    IsGpsAccurate = SatCount >= 12;
+                }
+                else
+                {
+                    // 1.0 飞行器，飞控不提供 canTakeOff。
+                    IsGpsAccurate = SatCount >= 6;
+                }
+            }
+            else
+                IsGpsAccurate = SatCount >= 8;
+
+            FlightDistance2D = _internalCopter.FlightDistance2D;
+
+            RaiseSysStatusReceived(new SystemStatusReceivedEventArgs(GPSBadHealth, GyroBadHealth, AccelBadHealth, CompassBadHealth, BarometerBadHealth));
+        }
+
+        private void _internalCopter_PacketHandled(object sender, PlaneCopter.PacketReceivedEventArgs e)
+        {
+            switch (e.Packet[5])
+            {
+                case MAVLINK_MSG_ID_MISSION_COUNT:
+                    MissionCount = _internalCopter.WpCount;
+                    break;
+
+                default:
+                    break;
+            }
+        }
+
+        private void _internalCopter_UnhandledPacketReceived(object sender, PlaneCopter.PacketReceivedEventArgs e)
+        {
+            switch (e.Packet[5])
+            {
+                case MAVLINK_MSG_ID_MISSION_ACK:
+                    AnalyzeMissionAckPacket(e.Packet);
+                    break;
+
+                case MAVLINK_MSG_ID_MISSION_COUNT:
+                    AnalyzeMissionCountPacket(e.Packet);
+                    break;
+
+                case MAVLINK_MSG_ID_MISSION_ITEM:
+                    AnalyzeMissionItemPacket(e.Packet);
+                    break;
+
+                case MAVLINK_MSG_ID_MISSION_REQUEST:
+                    AnalyzeMissionRequestPacket(e.Packet);
+                    break;
+
+                case MAVLINK_MSG_ID_SET_PAIR:
+                    AnalyzeSetPairPacket(e.Packet);
+                    break;
+
+                default:
+                    break;
+            }
+        }
+
+#if PRIVATE
+        protected virtual void RegisterInternalCopterEventHandlers()
+#else
+
+        private void RegisterInternalCopterEventHandlers()
+#endif
+        {
+            _internalCopter.GetLogDataEvent += _internalCopter_GetLogDataEvent;
+            _internalCopter.ReceiveDataStreamEvent += _internalCopter_ReceiveDataStreamEvent;
+            _internalCopter.ReceiveHeartBearEvent += _internalCopter_ReceiveHeartBearEvent;
+            _internalCopter.ReceiveSensorEvent += _internalCopter_ReceiveSensorEvent;
+            _internalCopter.ReceiveSysStatusEvent += _internalCopter_ReceiveSysStatusEvent;
+            _internalCopter.ConnectionBroken += _internalCopter_ConnectionBroken;
+            _internalCopter.PacketHandled += _internalCopter_PacketHandled;
+            _internalCopter.UnhandledPacketReceived += _internalCopter_UnhandledPacketReceived;
+            _internalCopter.MessageCreated += _internalCopter_MessageCreated;
+        }
+
+        private void _internalCopter_MessageCreated(object sender, MessageCreatedEventArgs e)
+        {
+            var txte = new MessageCreatedEventArgs { Message = Name + ":" +e.Message };
+            RaiseTextReceived(txte);
+        }
+
+        #region 计算剩余电量
+
+        private int bPerTimes;
+        private int outBatteryPer;
+        private int[] tPerTimes = new int[20];
+        private int v_num;
+
+        /// <summary>
+        /// 计算电量并在条件满足时更新 <see cref="BatteryPer"/> 属性。
+        /// </summary>
+        private void CalcBatteryPer()
+        {
+            float volmax = 0f;
+            float volmin = 0f;
+            if (Voltage > 5 && Voltage < 9)
+            {
+                volmax = 8.2f;
+                volmin = 7f;
+            }
+            else if (Voltage >= 9
+                  && Voltage < 13.6)
+            {
+                volmax = 11.6f;
+                volmin = 10.2f;
+            }
+            else if (Voltage >= 13.6
+                  && Voltage < 17.2)
+            {
+                volmax = 16.3f;
+                volmin = 14.2f;
+            }
+            else if (Voltage >= 17.2
+                  && Voltage < 26.2)
+            {
+                volmax = 24.8f;
+                volmin = 21.2f;
+            }
+
+            int batteryPer = (int)Math.Round((Voltage - volmin) * 100 / (volmax - volmin));
+
+            if (batteryPer == -1 || volmax == 0 || volmin == 0)
+                return;
+
+            if (bPerTimes < 20)
+            {
+                tPerTimes[bPerTimes] = batteryPer;
+                bPerTimes += 1;
+            }
+            else
+            {
+                tPerTimes[v_num] = batteryPer;
+                v_num++;
+                if (v_num == 20)
+                    v_num = 0;
+            }
+            for (int i = 0; i < bPerTimes; i++)
+            {
+                outBatteryPer += tPerTimes[i];
+            }
+
+            outBatteryPer = outBatteryPer / bPerTimes;
+            if (outBatteryPer < BatteryPer && bPerTimes > 18)
+            {
+                BatteryPer = (byte)outBatteryPer;
+            }
+        }
+
+        #endregion 计算剩余电量
+    }
+}

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;
-            req.x = (float)mission.Latitude;
-            req.y = (float)mission.Longitude;
-            req.z = mission.Altitude;
+            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;
@@ -249,6 +260,7 @@ namespace Plane.Copters
             req.param4 = mission.Param4;
 
             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/FlightMode.cs

@@ -1,84 +1,86 @@
-using System;
-
-namespace Plane.Copters
-{
-#if PRIVATE
-    public
-#else
-    internal
-#endif
-        enum FlightMode
-    {
-        // 林俊清，20150608：不可将以下枚举项重命名。
-
-        STABILIZE = 0,          // hold level position
-
-        ACRO = 1,           // rate control // 林俊清, 20160205, 特技模式：http://copter.ardupilot.cn/wiki/acro-mode/
-
-        ALT_HOLD = 2,       // AUTO control
-
-        AUTO = 3,           // AUTO control
-
-        GUIDED = 4,     // AUTO control
-
-        LOITER = 5,     // Hold a single location
-
-        RTL = 6,                // AUTO control
-
-        CIRCLE = 7,
-
-        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/ 底部。
-
-        TOY = 11
-    }
-
-    internal static class FightModeExtensions
-    {
-        public static string GetModeString(this FlightMode flightMode)
-        {
-            switch (flightMode)
-            {
-                case FlightMode.ALT_HOLD:
-                    return "ALT HOLD";
-
-                case FlightMode.POSITION:
-                    return "POS HOLD";
-
-                default:
-                    return Enum.GetName(typeof(FlightMode), flightMode);
-            }
-        }
-
-        public static bool NeedGps(this FlightMode flightMode)
-        {
-            switch (flightMode)
-            {
-                case FlightMode.AUTO:
-                case FlightMode.GUIDED:
-                case FlightMode.LOITER:
-                case FlightMode.RTL:
-                case FlightMode.CIRCLE:
-                case FlightMode.LAND:
-                case FlightMode.POSITION:
-                default:
-                    return true;
-
-                case FlightMode.STABILIZE:
-                case FlightMode.ACRO:
-                case FlightMode.ALT_HOLD:
-                case FlightMode.OF_LOITER:
-                case FlightMode.TOY:
-                    return false;
-            }
-        }
-
-        internal static PlaneCopter.ac2modes ToAC2Mode(this FlightMode flightMode)
-        {
-            return (PlaneCopter.ac2modes)flightMode;
-        }
-    }
-}
+using System;
+
+namespace Plane.Copters
+{
+#if PRIVATE
+    public
+#else
+    internal
+#endif
+        enum FlightMode
+    {
+        // 王海，20150608：不可将以下枚举项重命名。
+
+        STABILIZE = 0,          // hold level position
+
+        ACRO = 1,           // rate control // 王海, 20160205, 特技模式：http://copter.ardupilot.cn/wiki/acro-mode/
+
+        ALT_HOLD = 2,       // AUTO control
+
+        AUTO = 3,           // AUTO control
+
+        GUIDED = 4,     // AUTO control
+
+        LOITER = 5,     // Hold a single location
+
+        RTL = 6,                // AUTO control
+
+        CIRCLE = 7,
+
+        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/ 底部。
+
+        TOY = 11,
+
+        EMERG_RTL=12,   //紧急返航模式
+    }
+
+    internal static class FightModeExtensions
+    {
+        public static string GetModeString(this FlightMode flightMode)
+        {
+            switch (flightMode)
+            {
+                case FlightMode.ALT_HOLD:
+                    return "ALT HOLD";
+
+                case FlightMode.POSITION:
+                    return "POS HOLD";
+
+                default:
+                    return Enum.GetName(typeof(FlightMode), flightMode);
+            }
+        }
+
+        public static bool NeedGps(this FlightMode flightMode)
+        {
+            switch (flightMode)
+            {
+                case FlightMode.AUTO:
+                case FlightMode.GUIDED:
+                case FlightMode.LOITER:
+                case FlightMode.RTL:
+                case FlightMode.CIRCLE:
+                case FlightMode.LAND:
+                case FlightMode.POSITION:
+                default:
+                    return true;
+
+                case FlightMode.STABILIZE:
+                case FlightMode.ACRO:
+                case FlightMode.ALT_HOLD:
+                case FlightMode.OF_LOITER:
+                case FlightMode.TOY:
+                    return false;
+            }
+        }
+
+        internal static PlaneCopter.ac2modes ToAC2Mode(this FlightMode flightMode)
+        {
+            return (PlaneCopter.ac2modes)flightMode;
+        }
+    }
+}

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,17 +206,36 @@ 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

@@ -1,377 +1,404 @@
-//#define LOG_PACKETS
-
-using Plane.Protocols;
-using System;
-using System.Diagnostics;
-using System.Text;
-using System.Threading.Tasks;
-using TaskTools.Utilities;
-
-#if LOG_PACKETS
-
-using System.Diagnostics;
-
-#endif
-
-namespace Plane.Copters
-{
-    partial class PlaneCopter
-    {
-        private void AnalyzeAttitudePacket(byte[] buffer)
-        {
-            var att = buffer.ByteArrayToStructure<MAVLink.mavlink_attitude_t>(6);
-            roll = att.roll * rad2deg;
-            pitch = att.pitch * rad2deg;
-            yaw = att.yaw * rad2deg;
-            timebootms = att.time_boot_ms;
-            RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
-        }
-
-        private void AnalyzeGlobalPositionIntPacket(byte[] buffer)
-        {
-            var loc = buffer.ByteArrayToStructure<MAVLink.mavlink_global_position_int_t>(6);
-            useLocation = true;
-            if (loc.lat == 0 && loc.lon == 0)
-            {
-                useLocation = false;
-            }
-            else
-            {
-                gpsalt = loc.alt / 1000.0f;
-                lat = loc.lat / 10000000.0;
-                lng = loc.lon / 10000000.0;
-            }
-            RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
-        }
-
-#if LOG_GPS_RAW_INT
-        private Stopwatch _lastGpsRawIntPacketTime = new Stopwatch();
-        private TimeSpan _lastGpsRawIntPacketTimeSpan;
-#endif
-
-        private void AnalyzeGpsRawIntPacket(byte[] buffer)
-        {
-#if LOG_GPS_RAW_INT
-            if (!_lastGpsRawIntPacketTime.IsRunning)
-            {
-                _lastGpsRawIntPacketTime.Start();
-            }
-            else
-            {
-                var elapsed = _lastGpsRawIntPacketTime.Elapsed;
-                Debug.WriteLine(elapsed - _lastGpsRawIntPacketTimeSpan, "----------------------------GpsRawInt");
-                _lastGpsRawIntPacketTimeSpan = elapsed;
-            }
-#endif
-            var gps = buffer.ByteArrayToStructure<MAVLink.mavlink_gps_raw_int_t>(6);
-            long mylat = BitConverter.ToInt32(buffer, 14);
-            long mylong = BitConverter.ToInt32(buffer, 18);
-            //if (!useLocation)
-            //{
-            lat = gps.lat * 1.0e-7;
-            lng = gps.lon * 1.0e-7;
-            //}
-            gpsalt = gps.alt * 1.0e-7f; // using vfr as includes baro calc
-            gpsstatus = gps.fix_type; //0-1: no fix, 2: 2D fix, 3: 3D fix
-            gpshdop = (float)Math.Round(gps.eph / 100.0, 2);
-            satcount = gps.satellites_visible;
-            groundspeed = gps.vel * 1.0e-2f;
-            groundcourse = gps.cog * 1.0e-2f;
-            RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
-        }
-
-#if LOG_PACKETS
-        private DateTime _lastHeartbeatTime = DateTime.MinValue;
-#endif
-
-        private void AnalyzeHeartbeatPacket(byte[] buffer)
-        {
-#if LOG_PACKETS
-            var now = DateTime.Now;
-            if (_lastHeartbeatTime != DateTime.MinValue)
-            {
-                Debug.WriteLine(now - _lastHeartbeatTime, "心跳间隔");
-            }
-            _lastHeartbeatTime = now;
-#endif
-            DataTimeOut = 0;
-            var hb = buffer.ByteArrayToStructure<MAVLink.mavlink_heartbeat_t>(6);
-            mavlinkversion = hb.mavlink_version;
-            aptype = (MAVLink.MAV_TYPE)hb.type;
-            apname = (MAVLink.MAV_AUTOPILOT)hb.autopilot;
-            sysid = buffer[3];
-            compid = buffer[4];
-            recvpacketcount = buffer[2];
-            armed = (hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED) == (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED;
-            failsafe = hb.system_status == (byte)MAVLink.MAV_STATE.CRITICAL;
-            mode = hb.custom_mode;
-
-            var handler = ReceiveHeartBearEvent;
-            if (handler != null)
-            {
-                RunOnUIThread(() =>
-                {
-                    handler(this, iErrorData, iDataCount);
-                });
-            }
-
-            if (_autoRequestData && mavlinkversion == 3 && SendReq == false)
-            {
-                SendReq = true;
-                Task.Run(GetCopterDataAsync);
-            }
-        }
-
-        private void AnalyzeMemInfoPacket(byte[] buffer)
-        {
-            var mem = buffer.ByteArrayToStructure<MAVLink.mavlink_meminfo_t>(6);
-            freemem = mem.freemem;
-            brklevel = mem.brkval;
-            RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
-        }
-
-        private void AnalyzeMissionCurrentPacket(byte[] buffer)
-        {
-            var wpcur = buffer.ByteArrayToStructure<MAVLink.mavlink_mission_current_t>(6);
-            RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
-        }
-
-        private void AnalyzeNavControllerOutputPacket(byte[] buffer)
-        {
-            var nav = buffer.ByteArrayToStructure<MAVLink.mavlink_nav_controller_output_t>(6);
-            nav_roll = nav.nav_roll;
-            nav_pitch = nav.nav_pitch;
-            nav_bearing = nav.nav_bearing;
-            target_bearing = nav.target_bearing;
-            wp_dist = nav.wp_dist;
-            alt_error = nav.alt_error;
-            aspd_error = nav.aspd_error / 100.0f;
-            xtrack_error = nav.xtrack_error;
-            RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
-        }
-
-        private void AnalyzeParamValuePacket(byte[] buffer)
-        {
-            var par = buffer.ByteArrayToStructure<MAVLink.mavlink_param_value_t>(6);
-            param_total = (par.param_count);
-            string paramID = Encoding.ASCII.GetString(par.param_id);
-            int pos = paramID.IndexOf('\0');
-            if (pos != -1)
-            {
-                paramID = paramID.Substring(0, pos);
-            }
-            try
-            {
-                param[paramID] = (par.param_value);
-                param_types[paramID] = (MAVLink.MAV_PARAM_TYPE)par.param_type;
-            }
-            catch (IndexOutOfRangeException ex)
-            {
-                Debug.WriteLine(ex);
-            }
-
-            var handler = ReceiveParamEvent;
-            if (handler != null)
-            {
-                RunOnUIThread(() =>
-                {
-                    handler(this, paramID, par.param_index, par.param_count);
-                });
-            }
-        }
-
-        private void AnalyzeRadioPacket(byte[] buffer)
-        {
-            var radio = buffer.ByteArrayToStructure<MAVLink.mavlink_radio_t>(6);
-            //Debug.Print("fix:" + radio.fixedp + " noise:" + radio.noise + " remnoise:" + radio.remnoise + " remrssi:" + radio.remrssi + " rssi:" + radio.rssi + " rxerr:" + radio.rxerrors + " txbuf:" + radio.txbuf);
-            remrssi = radio.remrssi;
-            rssi = radio.rssi;
-            RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
-        }
-
-        private void AnalyzeRawImuPacket(byte[] buffer)
-        {
-            var imu = buffer.ByteArrayToStructure<MAVLink.mavlink_raw_imu_t>(6);
-
-            gx = imu.xgyro;
-            gy = imu.ygyro;
-            gz = imu.zgyro;
-
-            ax = imu.xacc;
-            ay = imu.yacc;
-            az = imu.zacc;
-
-            mx = imu.xmag;
-            my = imu.ymag;
-            mz = imu.zmag;
-
-            var handler = ReceiveSensorEvent;
-            if (handler != null)
-            {
-                RunOnUIThread(() =>
-                {
-                    handler(this);
-                });
-            }
-        }
-
-        private void AnalyzeRCChannelsRawPacket(byte[] buffer)
-        {
-            var rcin = buffer.ByteArrayToStructure<MAVLink.mavlink_rc_channels_raw_t>(6);
-
-            ch1in = rcin.chan1_raw;
-            ch2in = rcin.chan2_raw;
-            ch3in = rcin.chan3_raw;
-            ch4in = rcin.chan4_raw;
-            ch5in = rcin.chan5_raw;
-            ch6in = rcin.chan6_raw;
-            ch7in = rcin.chan7_raw;
-            ch8in = rcin.chan8_raw;
-            if (bInitChannel == false)
-            {
-                bInitChannel = true;
-            }
-
-            //percent
-            rxrssi = (float)((rcin.rssi / 255.0) * 100.0);
-
-            RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
-        }
-
-        private void AnalyzeScaledImu2Packet(byte[] buffer)
-        {
-            var imu2 = buffer.ByteArrayToStructure<MAVLink.mavlink_scaled_imu2_t>(6);
-            gx2 = imu2.xgyro;
-            gy2 = imu2.ygyro;
-            gz2 = imu2.zgyro;
-
-            ax2 = imu2.xacc;
-            ay2 = imu2.yacc;
-            az2 = imu2.zacc;
-
-            mx2 = imu2.xmag;
-            my2 = imu2.ymag;
-            mz2 = imu2.zmag;
-
-            var handler = ReceiveScaleImu2Event;
-            if (handler != null)
-            {
-                RunOnUIThread(() => handler(this));
-            }
-        }
-
-        private void AnalyzeSensorOffsetsPacket(byte[] buffer)
-        {
-            var sensofs = buffer.ByteArrayToStructure<MAVLink.mavlink_sensor_offsets_t>(6);
-
-            mag_ofs_x = sensofs.mag_ofs_x;
-            mag_ofs_y = sensofs.mag_ofs_y;
-            mag_ofs_z = sensofs.mag_ofs_z;
-            mag_declination = sensofs.mag_declination;
-
-            raw_press = sensofs.raw_press;
-            raw_temp = sensofs.raw_temp;
-
-            gyro_cal_x = sensofs.gyro_cal_x;
-            gyro_cal_y = sensofs.gyro_cal_y;
-            gyro_cal_z = sensofs.gyro_cal_z;
-
-            accel_cal_x = sensofs.accel_cal_x;
-            accel_cal_y = sensofs.accel_cal_y;
-            accel_cal_z = sensofs.accel_cal_z;
-
-            var handler = ReceiveSensorEvent;
-            if (handler != null)
-            {
-                RunOnUIThread(() =>
-                {
-                    handler(this);
-                });
-            }
-        }
-
-        private void AnalyzeStatusTextPacket(byte[] buffer)
-        {
-            var stext = buffer.ByteArrayToStructure<MAVLink.mavlink_statustext_t>(6);
-            var length = Array.IndexOf<byte>(stext.text, 0);
-            var log = Encoding.ASCII.GetString(stext.text, 0, length);
-            var handler = GetLogDataEvent;
-            if (handler != null)
-            {
-                RunOnUIThread(() => handler(log));
-            }
-        }
-
-        private void AnalyzeSysStatusPacket(byte[] buffer)
-        {
-            var sysstatus = buffer.ByteArrayToStructure<MAVLink.mavlink_sys_status_t>(6);
-            battery_voltage = sysstatus.voltage_battery;
-            battery_remaining = sysstatus.battery_remaining;
-            current_battery = sysstatus.current_battery / 100.0f;
-
-            var sensors_enabled = new Mavlink_Sensors(sysstatus.onboard_control_sensors_enabled);
-            var sensors_health = new Mavlink_Sensors(sysstatus.onboard_control_sensors_health);
-
-            if (sensors_health.gps != sensors_enabled.gps)
-            {
-                bGPSBadHealth = true;
-            }
-            else
-            {
-                bGPSBadHealth = false;
-            }
-            if (sensors_health.gyro != sensors_enabled.gyro)
-            {
-                bGyroBadHealth = true;
-            }
-            else
-            {
-                bGyroBadHealth = false;
-            }
-            if (sensors_health.accelerometer != sensors_enabled.accelerometer)
-            {
-                bAccel = true;
-            }
-            else
-            {
-                bAccel = false;
-            }
-            if (sensors_health.compass != sensors_enabled.compass)
-            {
-                bCompass = true;
-            }
-            else
-            {
-                bCompass = false;
-            }
-            if (sensors_health.barometer != sensors_enabled.barometer)
-            {
-                bBarometer = true;
-            }
-            else
-            {
-                bBarometer = false;
-            }
-            FlightDistance2D = sysstatus.errors_count2;
-            var handler = ReceiveSysStatusEvent;
-            if (handler != null)
-            {
-                RunOnUIThread(() =>
-                {
-                    handler(this, buffer[5], bGPSBadHealth, bGyroBadHealth, bAccel, bCompass, bBarometer, sensors_health.CanTakeOff);
-                });
-            }
-        }
-
-        private void AnalyzeVfrHudPacket(byte[] buffer)
-        {
-            var vfr = buffer.ByteArrayToStructure<MAVLink.mavlink_vfr_hud_t>(6);
-            groundspeed = vfr.groundspeed;
-            airspeed = vfr.airspeed;
-            altorigin = vfr.alt; // this might include baro
-            ch3percent = vfr.throttle;
-            heading = vfr.heading;
-            RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
-        }
-    }
-}
+//#define LOG_PACKETS
+
+using Plane.Protocols;
+using System;
+using System.Diagnostics;
+using System.Text;
+using System.Threading.Tasks;
+using TaskTools.Utilities;
+
+#if LOG_PACKETS
+
+using System.Diagnostics;
+
+#endif
+
+namespace Plane.Copters
+{
+    partial class PlaneCopter
+    {
+        private void AnalyzeAttitudePacket(byte[] buffer)
+        {
+            var att = buffer.ByteArrayToStructure<MAVLink.mavlink_attitude_t>(6);
+            roll = att.roll * rad2deg;
+            pitch = att.pitch * rad2deg;
+            yaw = att.yaw * rad2deg;
+            timebootms = att.time_boot_ms;
+            RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
+        }
+
+        private void AnalyzeGlobalPositionIntPacket(byte[] buffer)
+        {
+            var loc = buffer.ByteArrayToStructure<MAVLink.mavlink_global_position_int_t>(6);
+            useLocation = true;
+            if (loc.lat == 0 && loc.lon == 0)
+            {
+                useLocation = false;
+            }
+            else
+            {
+                gpsalt = loc.alt / 1000.0f;
+                lat = loc.lat / 10000000.0;
+                lng = loc.lon / 10000000.0;
+            }
+            RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
+        }
+
+#if LOG_GPS_RAW_INT
+        private Stopwatch _lastGpsRawIntPacketTime = new Stopwatch();
+        private TimeSpan _lastGpsRawIntPacketTimeSpan;
+#endif
+
+        private void AnalyzeGpsRawIntPacket(byte[] buffer)
+        {
+#if LOG_GPS_RAW_INT
+            if (!_lastGpsRawIntPacketTime.IsRunning)
+            {
+                _lastGpsRawIntPacketTime.Start();
+            }
+            else
+            {
+                var elapsed = _lastGpsRawIntPacketTime.Elapsed;
+                Debug.WriteLine(elapsed - _lastGpsRawIntPacketTimeSpan, "----------------------------GpsRawInt");
+                _lastGpsRawIntPacketTimeSpan = elapsed;
+            }
+#endif
+            var gps = buffer.ByteArrayToStructure<MAVLink.mavlink_gps_raw_int_t>(6);
+            long mylat = BitConverter.ToInt32(buffer, 14);
+            long mylong = BitConverter.ToInt32(buffer, 18);
+            //if (!useLocation)
+            //{
+            lat = gps.lat * 1.0e-7;
+            lng = gps.lon * 1.0e-7;
+            //}
+            gpsalt = gps.alt * 1.0e-7f; // using vfr as includes baro calc
+            gpsstatus = gps.fix_type; //0-1: no fix, 2: 2D fix, 3: 3D fix
+            gpshdop = (float)Math.Round(gps.eph / 100.0, 2);
+            satcount = gps.satellites_visible;
+            groundspeed = gps.vel * 1.0e-2f;
+            groundcourse = gps.cog * 1.0e-2f;
+            RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
+        }
+
+#if LOG_PACKETS
+        private DateTime _lastHeartbeatTime = DateTime.MinValue;
+#endif
+
+        private void AnalyzeHeartbeatPacket(byte[] buffer)
+        {
+#if LOG_PACKETS
+            var now = DateTime.Now;
+            if (_lastHeartbeatTime != DateTime.MinValue)
+            {
+                Debug.WriteLine(now - _lastHeartbeatTime, "心跳间隔");
+            }
+            _lastHeartbeatTime = now;
+#endif
+            DataTimeOut = 0;
+            var hb = buffer.ByteArrayToStructure<MAVLink.mavlink_heartbeat_t>(6);
+            mavlinkversion = hb.mavlink_version;
+            aptype = (MAVLink.MAV_TYPE)hb.type;
+            apname = (MAVLink.MAV_AUTOPILOT)hb.autopilot;
+            sysid = buffer[3];
+            compid = buffer[4];
+            recvpacketcount = buffer[2];
+            armed = (hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED) == (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED;
+            failsafe = hb.system_status == (byte)MAVLink.MAV_STATE.CRITICAL;
+            mode = hb.custom_mode;
+
+            var handler = ReceiveHeartBearEvent;
+            if (handler != null)
+            {
+                RunOnUIThread(() =>
+                {
+                    handler(this, iErrorData, iDataCount);
+                });
+            }
+
+            if (_autoRequestData && mavlinkversion == 3 && SendReq == false)
+            {
+                SendReq = true;
+                Task.Run(GetCopterDataAsync);
+            }
+        }
+
+        private void AnalyzeMemInfoPacket(byte[] buffer)
+        {
+            var mem = buffer.ByteArrayToStructure<MAVLink.mavlink_meminfo_t>(6);
+            freemem = mem.freemem;
+            brklevel = mem.brkval;
+            RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
+        }
+
+        private void AnalyzeMissionCurrentPacket(byte[] buffer)
+        {
+            var wpcur = buffer.ByteArrayToStructure<MAVLink.mavlink_mission_current_t>(6);
+            RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
+        }
+
+        private void AnalyzeNavControllerOutputPacket(byte[] buffer)
+        {
+            var nav = buffer.ByteArrayToStructure<MAVLink.mavlink_nav_controller_output_t>(6);
+            nav_roll = nav.nav_roll;
+            nav_pitch = nav.nav_pitch;
+            nav_bearing = nav.nav_bearing;
+            target_bearing = nav.target_bearing;
+            wp_dist = nav.wp_dist;
+            alt_error = nav.alt_error;
+            aspd_error = nav.aspd_error / 100.0f;
+            xtrack_error = nav.xtrack_error;
+            RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
+        }
+
+        private void AnalyzeParamValuePacket(byte[] buffer)
+        {
+            var par = buffer.ByteArrayToStructure<MAVLink.mavlink_param_value_t>(6);
+            param_total = (par.param_count);
+            string paramID = Encoding.ASCII.GetString(par.param_id);
+            int pos = paramID.IndexOf('\0');
+            if (pos != -1)
+            {
+                paramID = paramID.Substring(0, pos);
+            }
+            try
+            {
+                param[paramID] = (par.param_value);
+                param_types[paramID] = (MAVLink.MAV_PARAM_TYPE)par.param_type;
+            }
+            catch (IndexOutOfRangeException ex)
+            {
+                Debug.WriteLine(ex);
+            }
+
+            var handler = ReceiveParamEvent;
+            if (handler != null)
+            {
+                RunOnUIThread(() =>
+                {
+                    handler(this, paramID, par.param_index, par.param_count);
+                });
+            }
+        }
+
+        private void AnalyzeRadioPacket(byte[] buffer)
+        {
+            var radio = buffer.ByteArrayToStructure<MAVLink.mavlink_radio_t>(6);
+            //Debug.Print("fix:" + radio.fixedp + " noise:" + radio.noise + " remnoise:" + radio.remnoise + " remrssi:" + radio.remrssi + " rssi:" + radio.rssi + " rxerr:" + radio.rxerrors + " txbuf:" + radio.txbuf);
+            remrssi = radio.remrssi;
+            rssi = radio.rssi;
+            RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
+        }
+
+        private void AnalyzeRawImuPacket(byte[] buffer)
+        {
+            var imu = buffer.ByteArrayToStructure<MAVLink.mavlink_raw_imu_t>(6);
+
+            gx = imu.xgyro;
+            gy = imu.ygyro;
+            gz = imu.zgyro;
+
+            ax = imu.xacc;
+            ay = imu.yacc;
+            az = imu.zacc;
+
+            mx = imu.xmag;
+            my = imu.ymag;
+            mz = imu.zmag;
+
+            var handler = ReceiveSensorEvent;
+            if (handler != null)
+            {
+                RunOnUIThread(() =>
+                {
+                    handler(this);
+                });
+            }
+        }
+
+        private void AnalyzeRCChannelsRawPacket(byte[] buffer)
+        {
+            var rcin = buffer.ByteArrayToStructure<MAVLink.mavlink_rc_channels_raw_t>(6);
+
+            ch1in = rcin.chan1_raw;
+            ch2in = rcin.chan2_raw;
+            ch3in = rcin.chan3_raw;
+            ch4in = rcin.chan4_raw;
+            ch5in = rcin.chan5_raw;
+            ch6in = rcin.chan6_raw;
+            ch7in = rcin.chan7_raw;
+            ch8in = rcin.chan8_raw;
+            if (bInitChannel == false)
+            {
+                bInitChannel = true;
+            }
+
+            //percent
+            rxrssi = (float)((rcin.rssi / 255.0) * 100.0);
+
+            RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
+        }
+
+        private void AnalyzeScaledImu2Packet(byte[] buffer)
+        {
+            var imu2 = buffer.ByteArrayToStructure<MAVLink.mavlink_scaled_imu2_t>(6);
+            gx2 = imu2.xgyro;
+            gy2 = imu2.ygyro;
+            gz2 = imu2.zgyro;
+
+            ax2 = imu2.xacc;
+            ay2 = imu2.yacc;
+            az2 = imu2.zacc;
+
+            mx2 = imu2.xmag;
+            my2 = imu2.ymag;
+            mz2 = imu2.zmag;
+
+            var handler = ReceiveScaleImu2Event;
+            if (handler != null)
+            {
+                RunOnUIThread(() => handler(this));
+            }
+        }
+
+        private void AnalyzeSensorOffsetsPacket(byte[] buffer)
+        {
+            var sensofs = buffer.ByteArrayToStructure<MAVLink.mavlink_sensor_offsets_t>(6);
+
+            mag_ofs_x = sensofs.mag_ofs_x;
+            mag_ofs_y = sensofs.mag_ofs_y;
+            mag_ofs_z = sensofs.mag_ofs_z;
+            mag_declination = sensofs.mag_declination;
+
+            raw_press = sensofs.raw_press;
+            raw_temp = sensofs.raw_temp;
+
+            gyro_cal_x = sensofs.gyro_cal_x;
+            gyro_cal_y = sensofs.gyro_cal_y;
+            gyro_cal_z = sensofs.gyro_cal_z;
+
+            accel_cal_x = sensofs.accel_cal_x;
+            accel_cal_y = sensofs.accel_cal_y;
+            accel_cal_z = sensofs.accel_cal_z;
+
+            var handler = ReceiveSensorEvent;
+            if (handler != null)
+            {
+                RunOnUIThread(() =>
+                {
+                    handler(this);
+                });
+            }
+        }
+
+        private void AnalyzeStatusTextPacket(byte[] buffer)
+        {
+            var stext = buffer.ByteArrayToStructure<MAVLink.mavlink_statustext_t>(6);
+            var length = Array.IndexOf<byte>(stext.text, 0);
+            var log = Encoding.ASCII.GetString(stext.text, 0, length);
+            var handler = GetLogDataEvent;
+            if (handler != null)
+            {
+                RunOnUIThread(() => handler(log));
+            }
+        }
+
+        private void AnalyzeSysStatusPacket(byte[] buffer)
+        {
+            var sysstatus = buffer.ByteArrayToStructure<MAVLink.mavlink_sys_status_t>(6);
+            battery_voltage = sysstatus.voltage_battery;
+            battery_remaining = sysstatus.battery_remaining;
+            current_battery = sysstatus.current_battery / 100.0f;
+
+            var sensors_enabled = new Mavlink_Sensors(sysstatus.onboard_control_sensors_enabled);
+            var sensors_health = new Mavlink_Sensors(sysstatus.onboard_control_sensors_health);
+
+            if (sensors_health.gps != sensors_enabled.gps)
+            {
+                bGPSBadHealth = true;
+            }
+            else
+            {
+                bGPSBadHealth = false;
+            }
+            if (sensors_health.gyro != sensors_enabled.gyro)
+            {
+                bGyroBadHealth = true;
+            }
+            else
+            {
+                bGyroBadHealth = false;
+            }
+            if (sensors_health.accelerometer != sensors_enabled.accelerometer)
+            {
+                bAccel = true;
+            }
+            else
+            {
+                bAccel = false;
+            }
+            if (sensors_health.compass != sensors_enabled.compass)
+            {
+                bCompass = true;
+            }
+            else
+            {
+                bCompass = false;
+            }
+            if (sensors_health.barometer != sensors_enabled.barometer)
+            {
+                bBarometer = true;
+            }
+            else
+            {
+                bBarometer = false;
+            }
+            FlightDistance2D = sysstatus.errors_count2;
+            var handler = ReceiveSysStatusEvent;
+            if (handler != null)
+            {
+                RunOnUIThread(() =>
+                {
+                    handler(this, buffer[5], bGPSBadHealth, bGyroBadHealth, bAccel, bCompass, bBarometer, sensors_health.CanTakeOff);
+                });
+            }
+        }
+
+        private void AnalyzeVfrHudPacket(byte[] buffer)
+        {
+            var vfr = buffer.ByteArrayToStructure<MAVLink.mavlink_vfr_hud_t>(6);
+            groundspeed = vfr.groundspeed;
+            airspeed = vfr.airspeed;
+            altorigin = vfr.alt; // this might include baro
+            ch3percent = vfr.throttle;
+            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/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)
         //{