[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/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/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 定位，卫星数不足导致定位不准时非常危险。

PlaneGcsSdk.Contract_Shared/Copters/ICopterAttribute.cs

@@ -14,5 +14,13 @@ namespace Plane.Copters
         /// </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
 {
@@ -187,5 +188,19 @@ namespace Plane.Copters
         /// 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/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/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_Private_NET45/Copters/FakeCopter.Private.cs

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

PlaneGcsSdk_Private_NET45/Copters/PLCopter.Private.cs

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

PlaneGcsSdk_Private_NET45/Copters/PlaneCopter.Private.cs

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

PlaneGcsSdk_Private_NET46/PlaneGcsSdk_Private_NET46.csproj

@@ -9,8 +9,9 @@
     <AppDesignerFolder>Properties</AppDesignerFolder>
     <RootNamespace>Plane</RootNamespace>
     <AssemblyName>PlaneGcsSdk_Private_NET46</AssemblyName>
-    <TargetFrameworkVersion>v4.6</TargetFrameworkVersion>
+    <TargetFrameworkVersion>v4.8</TargetFrameworkVersion>
     <FileAlignment>512</FileAlignment>
+    <TargetFrameworkProfile />
   </PropertyGroup>
   <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
     <DebugSymbols>true</DebugSymbols>
@@ -34,6 +35,7 @@
     <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" />

PlaneGcsSdk_Shared/Communication/TcpConnection.cs

@@ -13,6 +13,8 @@ namespace Plane.Communication
     public class TcpConnection : TcpConnectionBase
     {
         private string _remoteHostname;
+        const int tcpReceiveBufferSize = 20 * 1024;  //40 * 1024,
+        const int tcpReceiveTimeout = 1200;
 
         private int _remotePort;
 
@@ -22,8 +24,8 @@ namespace Plane.Communication
             _remotePort = remotePort;
             _client = new TcpClient
             {
-                ReceiveBufferSize = 40 * 1024,
-                ReceiveTimeout = 1200
+                ReceiveBufferSize = tcpReceiveBufferSize,
+                ReceiveTimeout = tcpReceiveTimeout
             };
         }
 
@@ -71,33 +73,53 @@ namespace Plane.Communication
             string logstr;
             if (!IsOpen)
             {
+                if (_client == null)
+                    CreateClientAndConnect();
                 try
                 {
-                    await _client.ConnectAsync(_remoteHostname, _remotePort).ConfigureAwait(false);
+                    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");
+                    }
                 }
-                //屏蔽掉异常处理
-                //CreateClientAndConnectAsync会new一个TcpClient并且重新连接  
-                //之前设置的TcpClient中Socket Bind会无效，在多网卡工作时会导致断线重连的时间特别长
                 catch (SocketException e)
                 {
-                    logstr= e.Message;
-                    //await CreateClientAndConnectAsync();
+                    logstr = e.Message;
+                    CloseClient(); // 关闭并清理客户端
                 }
                 catch (ObjectDisposedException)
                 {
-                    //await CreateClientAndConnectAsync();
+                    CloseClient(); // 关闭并清理客户端
+                }
+                catch (Exception)
+                {
+                    CloseClient(); // 处理其他可能的异常
+                   // throw; // 可选：重新抛出异常，以便调用者知道发生了错误
                 }
                 _isBroken = false;
             }
+            if (_client != null)
                 _stream = _client.GetStream();
         }
+        private void CloseClient()
+        {
+            _client?.Close(); // 如果 _client 不为 null，则关闭连接
+            _client = null; // 将 _client 设置为 null，以便垃圾回收器可以回收它
+        }
 
         private void CreateClientAndConnect()
         {
             _client = new TcpClient(_remoteHostname, _remotePort)
             {
-                ReceiveBufferSize = 40 * 1024,
-                ReceiveTimeout = 1200
+                ReceiveBufferSize = tcpReceiveBufferSize,
+                ReceiveTimeout = tcpReceiveTimeout
             };
         }
 
@@ -105,8 +127,8 @@ namespace Plane.Communication
         {
             _client = new TcpClient
             {
-                ReceiveBufferSize = 40 * 1024,
-                ReceiveTimeout = 1200
+                ReceiveBufferSize = tcpReceiveBufferSize,// 40 * 1024,
+                ReceiveTimeout = tcpReceiveTimeout// 1200
             };
             await _client.ConnectAsync(_remoteHostname, _remotePort).ConfigureAwait(false);
         }

PlaneGcsSdk_Shared/Communication/TcpConnectionBase.cs

@@ -43,7 +43,10 @@ namespace Plane.Communication
             {
                 try
                 {
-                    return _client.Client != null && !((_client.Client.Poll(1000, SelectMode.SelectRead) && (_client.Client.Available == 0)) || !_client.Client.Connected);
+              //      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)
                 {

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/CommunicationManagement/CommModule.cs

@@ -1,6 +1,5 @@
 using Plane.Communication;
 using Plane.Protocols;
-using Plane.Windows.Messages;
 using System;
 using System.Collections.Generic;
 using System.Diagnostics;
@@ -12,17 +11,24 @@ 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";
+        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;
@@ -36,17 +42,32 @@ namespace Plane.CommunicationManagement
         /// </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()
         {
-            Connection = new TcpConnection(MODULE_IP, PORT);
+            LoadIPSet();
+            Connection = new TcpConnection(ServerIP, PORT);
             await ConnectOpenAsync();
         }
 
         public void Connect()
         {
+            LoadIPSet();
             Task.Run(async () =>
             {
-                Connection = new TcpConnection(MODULE_IP, PORT);
+                Connection = new TcpConnection(ServerIP, PORT);
                 await ConnectOpenAsync();
             }
             );
@@ -149,6 +170,7 @@ namespace Plane.CommunicationManagement
             {
                 try
                 {
+                    if (!Connection.IsOnline)
                         await Connection.OpenAsync().ConfigureAwait(false);
                 }
                 catch
@@ -158,6 +180,8 @@ namespace Plane.CommunicationManagement
             if (Connection.IsOnline)
             {
                 Message.Connect(true);
+                Message.Show($"通讯基站连接成功!");
+                times = 0;
                 SendQuery();
                 await StartReadingPacketsAsync();
             }
@@ -174,40 +198,60 @@ namespace Plane.CommunicationManagement
             Task.Run(async () =>
             {
                 long lastPacketCountNum = 0;
+                int faulttimes = 0; //等待没有数据次数
+                const int revtimeout = 2000; //等待超时ms  old=1200
                 while (CommOK)
                 {
                     if (Connection != null)
                     {
-                        await SendQueryStatusPacketsAsync();
-                        await Task.Delay(1200).ConfigureAwait(false);
-
+                        //发送心跳包-等待回复消息
+                        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("通信断开");
-
+                    }
+                }
+                Message.Show("主动断开连接，重连");
                 Reconnect();
             }).ConfigureAwait(false);
         }
 
+     /*  //直接调用不用封装函数了  
        private async Task SendQueryStatusPacketsAsync()
         {
             await WriteCommPacketAsync(0, MavComm.COMM_QUERY_STATUS);
-            await Task.Delay(100);
+            await Task.Delay(100);//100
         }
+        */
 
-        int times = 1;
+        int times = 0;
+        //这里容易报空引用异常 ，没有连接通讯设备时
         private void Reconnect()
         {
-            //Message.Show($"正在重新连接...");
+           // Message.Show($"正在重新连接...");
             Task.Run(async () =>
             {
                 CloseConnection();
                 await Task.Delay(250).ConfigureAwait(false);
-                //Message.Show($"正在重连:次数{times++}");
+               // Message.Show($"正在重连:次数{++times}");
                 await Task.Delay(250).ConfigureAwait(false);
                 await ConnectAsync();
                 
@@ -222,6 +266,12 @@ namespace Plane.CommunicationManagement
                 {
                     if (Connection == null || !Connection.IsOnline)
                     {
+                        if (Connection==null)
+                            Message.Show("空连接异常");
+                        else  if (!Connection.IsOnline)
+                        {
+                            Message.Show("检测到设备已断开连接");
+                        }
                         break;
                     }
 
@@ -261,7 +311,7 @@ namespace Plane.CommunicationManagement
                                     case (short)MavComm.MessageType.SCAN3:
                                         AnalyzeComm3RetrunPacket(copterNum, dealData);
                                         break;
-                                    case 4:
+                                    case 4:  //版本13通讯模块收到飞机返回的数据
                                         AnalyzeComm4RetrunPacket(copterNum, dealData);
                                         break;
                                 }
@@ -280,7 +330,8 @@ namespace Plane.CommunicationManagement
                     }
                 }
             }).ConfigureAwait(false);
-            Message.Show("连接断开");
+            Message.Show("退出读设备数据........");
+            //Reconnect();
         }
         
         private async Task<byte[]> ReadPacketAsync()
@@ -353,80 +404,57 @@ namespace Plane.CommunicationManagement
             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)
         {
-            
-            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 = 4;
-            led.custom_bytes = new byte[24];
-      
-            if (colorString == "") colorString = "330033";
-
-            Color color = (Color)ColorConverter.ConvertFromString("#" + colorString);
-
-            led.custom_bytes[0] = color.R;
-            led.custom_bytes[1] = color.G;
-            led.custom_bytes[2] = color.B;
-            led.custom_bytes[3] = 3;
-            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 < 14; times++)
-//             {
-//                 senddata = senddata.Concat(packet).ToArray();
-//             }
-
-//             byte[] myByteArray = Enumerable.Repeat((byte)0x08, 15).ToArray();
-// 
-//             senddata = senddata.Concat(myByteArray).ToArray();
-
+            byte[] packet = DoLEDCommandAsync(51, 0, 51);
             temp = !temp;
             while (temp)
             {
-                Message.Show("长度 = " + senddata.Length);
-                await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, senddata);
+                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)
         {
 
@@ -435,7 +463,7 @@ namespace Plane.CommunicationManagement
             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_len = 1;//命令类型——测试拉烟
             led.custom_bytes = new byte[24];
 
             led.custom_bytes[0] = (byte)channel;
@@ -489,6 +517,77 @@ namespace Plane.CommunicationManagement
             }
         }
 
+
+       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>
@@ -523,39 +622,43 @@ namespace Plane.CommunicationManagement
 
             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);         //数据头
+            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);         //数据长度
+            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);         //流水号
+            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);       //飞机号
+            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);    //命令类型
+            Array.Copy(buffer_messageType, 0, packet, 8, 2);    //命令类型  2字节
 
             if (data != null)
-                Array.Copy(data, 0, packet, 10, data.Length);   //数据内容
+                Array.Copy(data, 0, packet, 10, data.Length);   //数据内容  10字节开始
 
-            byte[] buffer_CRC = checkCRC16(packet);
+            byte[] buffer_CRC = checkCRC16(packet);               //计算所有数据的crc
 
-            byte[] buffer_packet = new byte[packet.Length + 2];
+            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);
+            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);
@@ -570,10 +673,12 @@ namespace Plane.CommunicationManagement
 
         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;

PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs

@@ -1,13 +1,15 @@
 using Plane.Copters;
+using Plane.Geography;
 using Plane.Protocols;
-using Plane.Windows.Messages;
 using System;
 using System.Collections.Generic;
+using System.IO.Ports;
 using System.Linq;
 using System.Text;
 using System.Threading.Tasks;
 using System.Windows.Media;
 using static Plane.Copters.PlaneCopter;
+using static Plane.Protocols.MAVLink;
 
 namespace Plane.CommunicationManagement
 {
@@ -19,6 +21,24 @@ namespace Plane.CommunicationManagement
         private DateTime waitRtcmTime = DateTime.Now;
         private bool starttime = false;
         private bool rtcm_run = false;
+        private SerialPort RecomPort;
+      //  public bool Recomisopen = false;
+        private bool _Recomisopen = false;
+        public bool Recomisopen
+        {
+            get { return _Recomisopen; }
+            set
+            {
+                if (_Recomisopen != value)
+                {
+                    _Recomisopen = value;
+                    //_RtcmInfoViewModel.SetRTKStatestr();
+                }
+           }
+        }
+
+
+
         private IEnumerable<ICopter> _allcopters;
         //是否使用专用传输模块
         public bool UseTransModule = true;
@@ -185,7 +205,7 @@ namespace Plane.CommunicationManagement
             return GeneratePacket(MAVLink.MAVLINK_MSG_ID_PARAM_REQUEST_READ, req);
         }
 
-        private byte[] DoLEDCommandAsync()
+        private byte[] DoLEDCommandAsync(byte red,byte green,byte blue)
         {
             MAVLink.mavlink_msg_id_led_control led = new MAVLink.mavlink_msg_id_led_control();
             led.target_system = 1;
@@ -194,26 +214,36 @@ namespace Plane.CommunicationManagement
             led.pattern = 0;
             led.custom_len = 4;
             led.custom_bytes = new byte[24];
-            led.custom_bytes[0] = 255;
-            led.custom_bytes[1] = 255;
-            led.custom_bytes[2] = 255;
-            led.custom_bytes[3] = 3;
-
+            led.custom_bytes[0] = red;
+            led.custom_bytes[1] = green;
+            led.custom_bytes[2] = blue;
+            led.custom_bytes[3] = 3;  //持续亮的时间=1/3hz=0.33s
+            return GeneratePacket(MAVLink.MAVLINK_MSG_ID_LED_CONTROL, led);
+        }
+        private byte[] DoThrowoutCommandAsync()
+        {
+            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;                //长度是1个字节
+            led.custom_bytes = new byte[24];
+            led.custom_bytes[0] = (byte)10;  //10代表抛物
             return GeneratePacket(MAVLink.MAVLINK_MSG_ID_LED_CONTROL, led);
         }
 
 
         private byte[] DoTaskLEDCommandAsync(int ledmode, float ledInterval, int ledtimes, byte cR,byte cG,Byte cB)
         {
-            //ledInterval间隔单位秒，改为单位100ms
-            byte LEDInterval = (byte)(ledInterval*10);  //(byte)(10 / ledInterval);
-
+            //新版固件ledInterval间隔单位秒，传输为一个字节无法传输小数，所以*10改为100ms，可传输一位小数
+            byte LEDInterval = (byte)(ledInterval * 10);// (byte)(10 / ledrate);
             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 = 6;
+            led.custom_len = 6;  //测试LED灯光
             led.custom_bytes = new byte[24];
             led.custom_bytes[0] = cR;
             led.custom_bytes[1] = cG;
@@ -443,7 +473,7 @@ namespace Plane.CommunicationManagement
         public async Task DoMissionStartAsync(IEnumerable<ICopter> copters,int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat)
         {
             if (UseTransModule)
-                await DoMissionStartAsync_CMod(hour_utc, minute_utc, second_utc, Missionlng, Missionlat);
+                await DoMissionStartAsync_CMod(copters,hour_utc, minute_utc, second_utc, Missionlng, Missionlat);
             else await DoMissionStartAsync_Single(copters, hour_utc, minute_utc, second_utc, Missionlng, Missionlat); 
 
         }
@@ -463,10 +493,22 @@ namespace Plane.CommunicationManagement
         }
 
 
-        public async Task DoMissionStartAsync_CMod(int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat)
+        public async Task DoMissionStartAsync_CMod(IEnumerable<ICopter> copters,int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat)
         {
+
             byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.MISSION_START, hour_utc, minute_utc, second_utc, (float)Missionlng, (float)Missionlat, 0, -1);
-            await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
+            short copterId = 0;
+            byte[] batchPacket = null;
+            //部分开始任务
+            if (copters != null)
+                GetCopterIds(copters, out copterId, out batchPacket);
+            else
+            {
+                //只有全部飞机才广播
+                if (Recomisopen)
+                    await BroadcastSendAsync(packet);
+            }
+            await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
         }
 
 
@@ -479,7 +521,10 @@ namespace Plane.CommunicationManagement
             {
                 byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.MISSION_START, 0, 0, 0, 0, 0, 1, 0);
                 await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
-            }else
+                if (Recomisopen)
+                    await BroadcastSendAsync(packet);
+            }
+            else
                 Windows.Messages.Message.Show($"未开发完成！！");
         }
 
@@ -492,7 +537,10 @@ namespace Plane.CommunicationManagement
             {
                 byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.MISSION_START, hour_utc, minute_utc, second_utc, 0, 0, 2, 0);
                 await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
-            }else
+                if (Recomisopen)
+                    await BroadcastSendAsync(packet);
+            }
+            else
                 Windows.Messages.Message.Show($"未开发完成！！");
         }
 
@@ -511,12 +559,15 @@ namespace Plane.CommunicationManagement
 
                 byte[] packet1 = SetChannels(1500, 1500, 1100, 1500);
 
-                byte[] packet2 = SetModeAsync(FlightMode.ALT_HOLD);
+                byte[] packet2 = SetModeAsync(FlightMode.ALT_HOLD);   //ALT_HOLD  LOITER 
 
                 byte[] packet3 = DoARMAsync(true);
 
                 byte[] data = packet1.Concat(packet2).Concat(packet3).ToArray();
                 await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket).ConfigureAwait(false);
+                if (Recomisopen)
+                    await BroadcastSendAsync(data);
+
             }
             else
             {
@@ -528,6 +579,46 @@ namespace Plane.CommunicationManagement
                         await vcopter.UnlockAsync();
                 }
             }
+            //发送到广播模块--只针对全部飞机
+            //if (enrecom&&(copters == null))
+            {
+               // if (enrecom)
+
+
+            }
+        }
+
+
+        /// <summary>
+        /// 抛物
+        /// </summary>
+        /// <param name="copters">要解锁的飞机</param>
+        /// <returns></returns>
+        public void ThrowoutAsync(IEnumerable<ICopter> copters = null)
+        {
+            if (UseTransModule)
+            {
+                Task.Run(async () =>
+                {
+                    short copterId = 0;
+                    byte[] batchPacket = null;
+                    GetCopterIds(copters, out copterId, out batchPacket);
+
+                    byte[] packet = DoThrowoutCommandAsync();
+                    await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
+                });
+            }
+            else
+            {
+               /*
+                 Task.Run(async () =>
+                {
+                    foreach (var vcopter in copters)
+                        await vcopter.LEDAsync();
+                });
+                */
+
+            }
         }
 
         /// <summary>
@@ -545,8 +636,10 @@ namespace Plane.CommunicationManagement
                     byte[] batchPacket = null;
                     GetCopterIds(copters, out copterId, out batchPacket);
 
-                    byte[] packet = DoLEDCommandAsync();
+                    byte[] packet = DoLEDCommandAsync(255,255,255);
                     await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
+                    if (Recomisopen&&(copters == null))
+                        await BroadcastSendAsync(packet);
                 });
             }
             else
@@ -570,7 +663,7 @@ namespace Plane.CommunicationManagement
 
     */
 
-        public void LED_TaskAsync( int  ledmode, float ledInterval, int ledtimes, string ledRGB, ICopter copter )
+        public void LED_TaskAsync( int  ledmode, float ledInterval, int ledtimes, string ledRGB, ICopter copter,bool oldver=true )
         {
             if (UseTransModule)
             {
@@ -605,6 +698,8 @@ namespace Plane.CommunicationManagement
                 byte[] packet = SetParam2Async(paramname, value);
                 int packetNum = packet[2];
                 await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
+                if (Recomisopen && (copters == null))
+                    await BroadcastSendAsync(packet);
                 return packetNum;
             }
             else
@@ -786,6 +881,58 @@ namespace Plane.CommunicationManagement
             }
 
         }
+
+        public async Task  DoMissionEmergencyRetAsync(PLLocation takeoffcentloc,  PLLocation taskcentloc,  float mindistance,int rettime,bool descending)
+        {
+            Int32 param7 = rettime << 16;
+            //低16位中的第一位表示是否直接降低高度
+            if (descending)
+                param7 = param7 | 1;
+            if (UseTransModule)
+            {
+                byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.RETURN_TO_LAUNCH, 
+                        (float)takeoffcentloc.Latitude  * 10000000,
+                        (float)takeoffcentloc.Longitude * 10000000,//起飞高度不用传=0
+                         mindistance*100, 0, 0,0,0);
+                short copterId = 0;
+                byte[] batchPacket = null;
+                await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
+                if (Recomisopen)
+                    await BroadcastSendAsync(packet);
+            }
+            else
+            {
+                Windows.Messages.Message.Show($"未开发完成！！");
+            }
+
+
+
+        }
+
+
+        public async Task GetCommsumAsync(IEnumerable<ICopter> copters = null)
+        {
+            if (UseTransModule)
+            {
+                short copterId = 0;
+                byte[] batchPacket = null;
+                GetCopterIds(copters, out copterId, out batchPacket);
+
+                MAVLink.mavlink_sversion_request req = new MAVLink.mavlink_sversion_request();
+                req.version = 1;
+                byte[] packet = GeneratePacket(MAVLink.MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST, req);
+                await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
+            }
+            else
+            {
+
+                Windows.Messages.Message.Show($"未开发完成！！");
+            }
+
+        }
+
+
+        
         /// <summary>
         /// 降落
         /// </summary>
@@ -800,6 +947,8 @@ namespace Plane.CommunicationManagement
 
                 byte[] packet = SetModeAsync(FlightMode.LAND);
                 await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
+                if (Recomisopen&&(copters == null))
+                    await BroadcastSendAsync(packet);
             }
             else
             {
@@ -834,6 +983,8 @@ namespace Plane.CommunicationManagement
 
                 byte[] data = packet1.Concat(packet2).ToArray();
                 await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket).ConfigureAwait(false);
+                if (Recomisopen&&(copters == null))
+                    await BroadcastSendAsync(data);
             }
             else
             {
@@ -994,6 +1145,55 @@ namespace Plane.CommunicationManagement
             }
         }
 
+        /// <summary>
+        /// 重启飞控
+        /// </summary>
+        /// <param name="copterId"></param>
+        /// <returns></returns>
+        public async Task DoRestartFCAsync(IEnumerable<ICopter> copters = null)
+        {
+            if (UseTransModule)
+            {
+                short copterId = 0;
+                byte[] batchPacket = null;
+                GetCopterIds(copters, out copterId, out batchPacket);
+                //只校陀螺仪，详细说明见;https://mavlink.io/en/messages/common.html#MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN
+                byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN, 1, 1,1, 0, 0, 0, 0);
+                await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
+            }
+            else
+            {
+                if (copters != null)
+                    foreach (var vcopter in copters)
+                        await vcopter.DoCommandAsync((int)MAVLink.MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN, 1, 1, 1, 0, 0, 0, 0);
+
+            }
+        }
+
+        /// <summary>
+        /// 校准陀螺仪
+        /// </summary>
+        /// <param name="copterId"></param>
+        /// <returns></returns>
+        public async Task DoCalibrationPreflightAsync(IEnumerable<ICopter> copters = null)
+        {
+            if (UseTransModule)
+            {
+                short copterId = 0;
+                byte[] batchPacket = null;
+                GetCopterIds(copters, out copterId, out batchPacket);
+                //只校陀螺仪，详细说明见;https://mavlink.io/en/messages/common.html#MAV_CMD_PREFLIGHT_CALIBRATION
+                byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.PREFLIGHT_CALIBRATION, 1, 0, 0, 0, 0, 0, 0);
+                await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
+            }
+            else
+            {
+                if (copters != null)
+                    foreach (var vcopter in copters)
+                        await vcopter.DoCommandAsync((int)MAVLink.MAV_CMD.PREFLIGHT_CALIBRATION, 1, 0, 0, 0, 0, 0, 0);
+
+            }
+        }
         /// <summary>
         /// 校准指南针
         /// </summary>
@@ -1048,16 +1248,92 @@ namespace Plane.CommunicationManagement
         }
 
 
+        byte rtcm_tmpser = 0;  //用于通讯模块的
+        byte rtcm_Broadser = 0;//用于广播的
+
+        /// <summary>
+        /// 发送到广播端口
+        /// </summary>
+        /// <param name="packet">发送数据</param>
+        /// <param name="reopensend">发送失败是否要重新打开再发一次</param>
+        /// <returns></returns>
+        public async Task BroadcastSendAsync(byte[] packet,bool reopensend = false)
+        {
+            
+            bool sendok = false;
+            //并且没有最后打开的端口说明已经人为关闭了
+            if (last_reserialport == "")
+            {
+                return;
+            }
+            try
+            {
+                //防止阻塞，异步发送
+                await RecomPort.BaseStream.WriteAsync(packet, 0, packet.Length);
+                //RecomPort.Write(packet, 0, packet.Length);
+                sendok = true;
+            }
+            catch (Exception ex)
+            {
+                Windows.Messages.Message.Show("广播端口发送失败...");
+            }
+
+            if (!sendok&& reopensend)
+            {
+                //再次打开串口
+                try
+                {
+                    ReOpenRtcmserial();
+                    if (Recomisopen) Windows.Messages.Message.Show("广播端口打开成功!");
+                }
+                catch (Exception ex)
+                {
+                    Windows.Messages.Message.Show("再次打开串口失败" + ex.Message);
+                    return;
+                }
+                //再次发送
+                if (Recomisopen)
+                {
+                    try
+                    {
+                        await RecomPort.BaseStream.WriteAsync(packet, 0, packet.Length);
+                        //RecomPort.Write(packet, 0, packet.Length);
+                    }
+                    catch (Exception ex)
+                    {
+                        Windows.Messages.Message.Show("再次发送失败...");
+                    }
+                }
+
+            }
+        }
+
+        public async Task BroadcastbackupGpsDataAsync(byte[] packet)
+        {
+            await BroadcastSendAsync(packet,true);
+        }
+ 
+        // get sum crc 计算数据校验和
+        public byte checkrtrcmsum(byte[] data, ushort length)
+        {
+            byte rtcm_sumcrc = 0;
+            for ( int i=0; i< length; i++)
+            {
+                rtcm_sumcrc += data[i];
+            }
+            return rtcm_sumcrc; 
+        }
 
 
         /// <summary>
-        ///广播Rtk
+        ///广播Rtk               //-------------使用中的RTK广播函数------------
         /// </summary>
         /// <param name="data"></param>
         /// <param name="length"></param>
         /// <returns></returns>
-        public async Task InjectGpsDataAsync(byte[] data, ushort length)
+        public async Task InjectGpsDataAsync(byte[] data, ushort length,bool enrecom)
         {
+            //由于通讯模块限制一次只能发110个字节
             if (UseTransModule)
             {
                 MAVLink.mavlink_gps_inject_data_t gps = new MAVLink.mavlink_gps_inject_data_t();
@@ -1069,13 +1345,35 @@ namespace Plane.CommunicationManagement
                     datalen = Math.Min(length - a * msglen, msglen);
                     gps.data = new byte[msglen];
                     Array.Copy(data, a * msglen, gps.data, 0, datalen);
+                    //gps.data[0] = rtcm_tmpser++;
                     gps.len = (byte)datalen;
-                    gps.target_component = 1;
-                    gps.target_system = 1;
+                    gps.target_component = rtcm_tmpser++;
+                    //实测一旦收到数据都是正确的，张伟已经做过crc检验了，为了兼容性不再做了
+                    //如果需要做，飞控里面需要把注释的代码放开，并且不再兼容之前的固件
+                    gps.target_system = checkrtrcmsum(gps.data, (ushort)datalen);  //默认:1
+
+                    //   Console.WriteLine("                                              ["+ gps.target_component + "]总长: "+ length +" 发送第 "+ (a+1) + " 个包,长度: " + (ushort)datalen );
+
                     byte[] packet = GenerateRTKPacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_INJECT_DATA, gps, gps.len);
+
+                    //发送到通讯模块
                     await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
+                    //发送到广播端口作为备用数据源
+                    if (enrecom)
+                        await BroadcastbackupGpsDataAsync(packet);
+                    await Task.Delay(80).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到
+
+                    // 重发一次，有序列号(target_component)飞机可以检测出来重复接收的
+                    //需要新固件支持
+                    await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
+                    //发送到广播端口作为备用数据源
+                    if (enrecom)
+                        await BroadcastbackupGpsDataAsync(packet);
+
+                    await Task.Delay(80).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到
                 }
-            }else
+            }
+            else
             {
                 if (_allcopters != null)
                     foreach (var vcopter in _allcopters)
@@ -1094,7 +1392,7 @@ namespace Plane.CommunicationManagement
         public async Task InjectGpsRTCMDataAsync(byte[] data, int length)
         {
             MAVLink.mavlink_gps_rtcm_data_t gps = new MAVLink.mavlink_gps_rtcm_data_t();
-            var msglen = 180;
+            var msglen = 180;  //飞机端是180，之前也是180和通讯盒可能也有关系
 
             //             if (length > msglen * 4)
             //                 log.Error("Message too large " + length);
@@ -1102,6 +1400,9 @@ namespace Plane.CommunicationManagement
             // number of packets we need, including a termination packet if needed
             var nopackets = (length % msglen) == 0 ? length / msglen + 1 : (length / msglen) + 1;
 
+           // if (nopackets>1)
+               // Plane.Windows.Messages.Message.Show("   RTCM_Mut:  " + length);
+
             if (nopackets >= 4)
                 nopackets = 4;
 
@@ -1134,10 +1435,16 @@ namespace Plane.CommunicationManagement
                 gps.len = (byte)copy;
 
                 byte[] packet = GeneratePacket(MAVLink.MAVLINK_MSG_ID_GPS_RTCM_DATA, gps);
+
+                // packet[2]
+                //Plane.Windows.Messages.Message.Show("["+ packet[2]+"]RTCM:"+ gps.len);
+
+               // Console.WriteLine("RTCM:" + gps.len+ "[" + data[0]+","+ data[1] + "," + data[2] + "," + data[3] + "," + data[4]+"]");
+
                 //Windows.Messages.Message.Show($"{DateTime.Now.ToString("HH:mm:ss:fff")} 单次长度 = {packet.Length}");
                 //老版本在这发送，新版本在StartRtcmLoop里面一次性打包4个180字节发送了
                 await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
-
+                /*  //长RTCP数据发送还没开发完
                 lock (lock_rtcm)
                 {
                     rtcm_packets.Add(packet);
@@ -1147,6 +1454,7 @@ namespace Plane.CommunicationManagement
                         starttime = true;
                     }
                 }
+                */
 
             }
         }
@@ -1174,6 +1482,67 @@ namespace Plane.CommunicationManagement
             await Task.Delay(1);
         }
 
+        public string last_reserialport ="";
+        public void ReOpenRtcmserial()
+        {
+            CloseResendRtcmserial();
+            OpenResendRtcmserial(last_reserialport);
+
+        }
+
+        public void CloseResendRtcmserial(bool cleanport=false)
+        {
+            if (Recomisopen)
+            {
+                RecomPort.Close();
+                RecomPort.Dispose();
+            }
+            Recomisopen = false;
+            //清除端口
+            if (cleanport)
+                last_reserialport="";
+
+        }
+
+        public string BoardPortStatusStr
+        {
+            get { 
+                if (Recomisopen)
+                    return "广播端口已打开"; 
+                else
+                    return "广播端口未打开";
+            }
+            
+        }
+
+
+
+        public bool OpenResendRtcmserial(string reserialport)
+        {
+            RecomPort = new SerialPort(reserialport);
+            RecomPort.BaudRate = 57600;
+            RecomPort.Parity = Parity.None;
+            RecomPort.StopBits = StopBits.One;
+            RecomPort.DataBits = 8;
+            RecomPort.Handshake = Handshake.None;
+            RecomPort.WriteTimeout = 500; // 设置写入超时为500毫秒,防止Close时候卡死
+
+
+            try
+            {
+                RecomPort.Open();
+                Recomisopen = true;
+                last_reserialport = reserialport;
+            }
+            catch (Exception ex)
+            {
+              //  Alert.Show("转发端口打开失败" + ex.Message);
+            }
+ 
+            return Recomisopen;
+        }
+
+
 
 
         /// <summary>

PlaneGcsSdk_Shared/CommunicationManagement/CommModulePacketAnalysis.cs

@@ -104,12 +104,20 @@ namespace Plane.CommunicationManagement
                     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;
             }
         }
 
@@ -195,7 +203,7 @@ namespace Plane.CommunicationManagement
                 //0B110
                 case 0xC000 >> 13:
                 //0B111
-                case 0xE000 >> 13:
+                case 0xE000 >> 13: //最后正在用的版本
                     CommunicationReceived?.Invoke(this, new CommunicationReceiveCopterInfoEventArgs(copterId, packet, version));
                     break;
             }

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);
         }

PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs

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

PlaneGcsSdk_Shared/Copters/EHCopter.InternalCopterEventHandlers.cs

@@ -1,10 +1,14 @@
 using System;
+using System.Collections.Generic;
 using static Plane.Protocols.MAVLink;
 
 namespace Plane.Copters
 {
+
+
     public partial class PLCopter : CopterImplSharedPart
     {
+       
         private bool _fetchingFirmwareVersion;
 
         private void _internalCopter_ConnectionBroken(object sender, EventArgs e)
@@ -20,6 +24,7 @@ namespace Plane.Copters
             RaiseTextReceived(e);
         }
     
+            
         private void _internalCopter_ReceiveDataStreamEvent(object sender, byte StreamType)
         {
             switch (StreamType)
@@ -29,6 +34,9 @@ namespace Plane.Copters
                         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);
                         
@@ -99,7 +107,7 @@ namespace Plane.Copters
         {
             IsUnlocked = _internalCopter.armed;
             Mode = (FlightMode)_internalCopter.mode;
-            // 林俊清, 20160311, 将闲置的 apname 用作已经切过 GPS 模式的标志。
+            // 王海, 20160311, 将闲置的 apname 用作已经切过 GPS 模式的标志。
             HasSwitchedToGpsMode = (byte)_internalCopter.apname != 0;
             ++HeartbeatCount;
 

PlaneGcsSdk_Shared/Copters/EmptyCopter.cs

@@ -144,7 +144,7 @@ namespace Plane.Copters
             return Task.FromResult(true);
         }
 
-        protected override Task FlyToCoreAsync(double lat, double lng, float alt)
+        protected override Task FlyToCoreAsync(double lat, double lng, float alt, float flytime = 0, int flytype = 0)
         {
             return TaskUtils.CompletedTask;
         }

PlaneGcsSdk_Shared/Copters/FakeCopter.cs

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

PlaneGcsSdk_Shared/Copters/FlightMode.cs

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

PlaneGcsSdk_Shared/Copters/PLCopter.cs

@@ -292,7 +292,7 @@ namespace Plane.Copters
         public async Task TakeOffAsync(float alt)
         {
             var currentTakeOffCount = ++_takeOffCount;
-            // 林俊清, 20160312, 从固件版本 3.1.1（0x3101）开始支持直接解锁起飞命令。
+            // 王海, 20160312, 从固件版本 3.1.1（0x3101）开始支持直接解锁起飞命令。
             //if (FirmwareVersion >= 0x3101)
             {
                 await SetChannelsAsync(
@@ -349,7 +349,7 @@ namespace Plane.Copters
                     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);
                 }
@@ -418,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

@@ -375,7 +375,7 @@ namespace Plane.Copters
         }
 
         /// <summary>
-        /// 处理通信模块发过来的数据
+        /// 飞控返回数据 处理通信模块发过来的28个字节数据
         /// </summary>
         /// <param name="buffer"></param>
         public void AnalyzeCommMouldePositionIntPacket(byte[] buffer, byte version)
@@ -385,6 +385,8 @@ namespace Plane.Copters
             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;

PlaneGcsSdk_Shared/Copters/PlaneCopter.cs

@@ -55,7 +55,11 @@ namespace Plane.Copters
 
         public bool useLocation { get; set; }
         public float gpsalt { get; set; }
+        public bool precheckok { get; set; }
+
         public byte gpsstatus { get; set; }
+        public byte errorcode { get; set; }
+
         public float gpshdop { get; set; }
         public byte satcount { get; set; }
         public float retain { get; set; }
@@ -348,7 +352,7 @@ namespace Plane.Copters
             return flightModes.ToList();
         }
 
-        // 林俊清, 20151029, 由于只在 myComm 循环中读取数据包，因此去掉了锁。
+        // 王海, 20151029, 由于只在 myComm 循环中读取数据包，因此去掉了锁。
         //volatile object readlock = new object();
 
 #if DEBUG && LOG_PACKETS
@@ -364,7 +368,7 @@ namespace Plane.Copters
         private byte[] _readBuffer = new byte[263];
 
         /// <summary>
-        /// 异步读取数据包。// 林俊清, 20151029, 去掉了锁，不要在除读数据包循环之外的地方调用此方法。
+        /// 异步读取数据包。// 王海, 20151029, 去掉了锁，不要在除读数据包循环之外的地方调用此方法。
         /// </summary>
         /// <returns>表示此异步操作的 <see cref="Task{TResult}"/> 实例，其结果为读取到的数据包。</returns>
         private async Task<byte[]> ReadPacketAsync()
@@ -612,7 +616,7 @@ namespace Plane.Copters
                 }
                 catch
                 {
-                    // 林俊清, 20151031, 异常已通过 IConnection.ExceptionThrown 事件传到外部，此处吞掉就行了。
+                    // 王海, 20151031, 异常已通过 IConnection.ExceptionThrown 事件传到外部，此处吞掉就行了。
                 }
             }
         }
@@ -688,7 +692,7 @@ namespace Plane.Copters
                 }
                 catch
                 {
-                    // 林俊清, 20151031, 异常已通过 IConnection.ExceptionThrown 事件传到外部，此处吞掉就行了。
+                    // 王海, 20151031, 异常已通过 IConnection.ExceptionThrown 事件传到外部，此处吞掉就行了。
                 }
             }
         }

PlaneGcsSdk_Shared/PlaneGcsSdk_Shared.projitems

@@ -34,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" />

PlaneGcsSdk_Shared/Protocols/MAVLinkTypes.cs

@@ -17,7 +17,7 @@ namespace Plane.Protocols
         public const int MAVLINK_LITTLE_ENDIAN = 1;
         public const int MAVLINK_BIG_ENDIAN = 0;
 
-        //飞控4.0以后 254 - 240
+        //协议头 飞控4.0以后 254 - 240  Mavlink头 FE 改为 F0 
         //public const byte MAVLINK_STX = 254;
         public const byte MAVLINK_STX = 240;
 
@@ -29,7 +29,7 @@ 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 };
@@ -79,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>
@@ -1366,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;

PlaneGcsSdk_Shared/Protocols/MavCommTypes.cs

@@ -74,6 +74,14 @@ namespace Plane.Protocols
         /// </summary>
         public const short COMM_FLIGHT_LOAD_MODE = 0x55;
 
+
+        /// <summary>
+        /// 发送非针对飞控的内部控制指令
+        /// </summary>
+        public const short COMM_TO_MODULE = 0x5F;
+
+
+
         /// <summary>
         /// 通信模块
         /// </summary>
@@ -84,6 +92,15 @@ namespace Plane.Protocols
         /// </summary>
         public const short COMM_UPDATE_COPTER_MODULE = 0xFD;
 
+
+
+        /// <summary>
+        /// 测试模块
+        /// </summary>
+        public const short COMM_TEST_MODULE = 0x3C;
+
+
+
         #endregion 
 
         public enum CommMode
@@ -141,18 +158,28 @@ namespace Plane.Protocols
         [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 28)]
         public struct comm_copter_info
         {
-            public Int32 control_mode;
-            public UInt32 lat;
-            public UInt32 lng;
-            public float retain;
-            public Int32 alt;
+            //   public Int32 control_mode;      //飞行模式(自动，悬停，定高，返航)      
+            public byte control_mode;         //飞行模式(自动，悬停，定高，返航)     
+            public byte rpecheck_fault;          //是否有 预解锁错误
+            public byte reserveddata1;        //保留   
+            public byte reserveddata2;        //保留
  
-            public Int16 gps_status;
 
-            public byte lock_status;
-            public byte gps_num_sats;
-            public Int16 battery_voltage;
-            public Int16 heading;
+            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

@@ -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)
         //{