[fix] 兼容老固件

没有改完，兼容老固件的rtcm需要
         1.gps.target_system =1
         2.不能多次发送
目前1还没做到，因为库里没法判断是新的还是老的

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

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/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

@@ -16,5 +16,11 @@ namespace Plane.Copters
         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/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_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

@@ -73,27 +73,34 @@ 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)
+                    {
+                        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;
                 }
                 catch (ObjectDisposedException)
                 {
-                    //await CreateClientAndConnectAsync();
                 }
                 _isBroken = false;
             }
             _stream = _client.GetStream();
         }
 
+
         private void CreateClientAndConnect()
         {
             _client = new TcpClient(_remoteHostname, _remotePort)

PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs

@@ -170,6 +170,7 @@ namespace Plane.CommunicationManagement
             {
                 try
                 {
+                    if (!Connection.IsOnline)
                         await Connection.OpenAsync().ConfigureAwait(false);
                 }
                 catch
@@ -179,6 +180,8 @@ namespace Plane.CommunicationManagement
             if (Connection.IsOnline)
             {
                 Message.Connect(true);
+                Message.Show($"通讯基站连接成功!");
+                times = 0;
                 SendQuery();
                 await StartReadingPacketsAsync();
             }
@@ -196,7 +199,7 @@ namespace Plane.CommunicationManagement
             {
                 long lastPacketCountNum = 0;
                 int faulttimes = 0; //等待没有数据次数
-                const int revtimeout = 1200; //等待超时ms
+                const int revtimeout = 2000; //等待超时ms  old=1200
                 while (CommOK)
                 {
                     if (Connection != null)
@@ -204,7 +207,7 @@ namespace Plane.CommunicationManagement
                         //发送心跳包-等待回复消息
                         await WriteCommPacketAsync(0, MavComm.COMM_QUERY_STATUS);
                       //  await SendQueryStatusPacketsAsync();
-                        await Task.Delay(revtimeout).ConfigureAwait(false);  //1200
+                        await Task.Delay(revtimeout).ConfigureAwait(false); 
                         //等待是否接受到返回的数据包，如果没有收到认为连结断开，重新联接
                         if (packetCountNum > lastPacketCountNum)
                         {
@@ -217,8 +220,8 @@ namespace Plane.CommunicationManagement
                         {
                             faulttimes++;
                             Message.Show("设备回复超时("+ revtimeout + "ms)" + faulttimes+"次");
-                            //超过3次没数据包返回就重连
-                            if (faulttimes > 3)
+                            //2次没数据包返回就重连
+                            if (faulttimes > 1)
                             {
                                 Message.Show("长时间未收到设备回复数据包");
                                 break;
@@ -239,7 +242,8 @@ namespace Plane.CommunicationManagement
         }
         */
 
-        int times = 1;
+        int times = 0;
+        //这里容易报空引用异常 ，没有连接通讯设备时
         private void Reconnect()
         {
            // Message.Show($"正在重新连接...");
@@ -247,7 +251,7 @@ namespace Plane.CommunicationManagement
             {
                 CloseConnection();
                 await Task.Delay(250).ConfigureAwait(false);
-                //Message.Show($"正在重连:次数{times++}");
+               // Message.Show($"正在重连:次数{++times}");
                 await Task.Delay(250).ConfigureAwait(false);
                 await ConnectAsync();
                 
@@ -307,7 +311,7 @@ namespace Plane.CommunicationManagement
                                     case (short)MavComm.MessageType.SCAN3:
                                         AnalyzeComm3RetrunPacket(copterNum, dealData);
                                         break;
-                                    case 4:
+                                    case 4:  //版本13通讯模块收到飞机返回的数据
                                         AnalyzeComm4RetrunPacket(copterNum, dealData);
                                         break;
                                 }

PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs

@@ -1,4 +1,5 @@
 using Plane.Copters;
+using Plane.Geography;
 using Plane.Protocols;
 using System;
 using System.Collections.Generic;
@@ -533,7 +534,7 @@ 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);
 
@@ -842,6 +843,32 @@ 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);
+            }
+            else
+            {
+                Windows.Messages.Message.Show($"未开发完成！！");
+            }
+
+
+
+        }
+
+
         public async Task GetCommsumAsync(IEnumerable<ICopter> copters = null)
         {
             if (UseTransModule)
@@ -1073,6 +1100,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>
@@ -1127,7 +1203,9 @@ namespace Plane.CommunicationManagement
         }
 
 
-        byte rtcm_tmpser = 0;
+        byte rtcm_tmpser = 0;  //用于通讯模块的
+        byte rtcm_Broadser = 0;//用于广播的
+
 
 
         public void BroadcastGpsDataAsync(byte[] data, ushort length)
@@ -1135,7 +1213,43 @@ namespace Plane.CommunicationManagement
             //广播发送RTCM数据采用专用数据可以一次发180个字节
             if (!Recomisopen)
                 return;
+            MAVLink.mavlink_gps_inject_data_t gps = new MAVLink.mavlink_gps_inject_data_t();
+            var msglen = 110;
+            int datalen = 0;
+            var len = (length % msglen) == 0 ? length / msglen : (length / msglen) + 1;
+            for (int a = 0; a < len; a++)
+            {
+                datalen = Math.Min(length - a * msglen, msglen);
+                gps.data = new byte[msglen];
+                Array.Copy(data, a * msglen, gps.data, 0, datalen);
+                //gps.data[0] = rtcm_Broadser++;
+                gps.len = (byte)datalen;
+                gps.target_component = rtcm_Broadser++;
+                //实测一旦收到数据都是正确的，张伟已经做过crc检验了，为了兼容性不再做了
+                //如果需要做，飞控里面需要把注释的代码放开，并且不再兼容之前的固件
+                gps.target_system = 1;// checkrtrcmsum(gps.data, (ushort)datalen);  
 
+                //   Console.WriteLine("                                              ["+ gps.target_component + "]总长: "+ length +" 发送第 "+ (a+1) + " 个包,长度: " + (ushort)datalen );
+
+                byte[] packet = GenerateRTKPacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_INJECT_DATA, gps, gps.len);
+
+                try
+                {
+                    RecomPort.Write(packet, 0, packet.Length);
+                }
+                catch (Exception ex)
+                {
+                    Windows.Messages.Message.Show("转发端口发送失败" + ex.Message);
+                }
+
+               // await Task.Delay(80).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到
+
+            }
+
+
+
+
+            /*
             MAVLink.mavlink_gps_rtcm_data_t gps = new mavlink_gps_rtcm_data_t();
             var msglen = 180;
 
@@ -1192,10 +1306,20 @@ namespace Plane.CommunicationManagement
                 }
             }
             inject_seq_no++;
-            
+            */
         }
 
 
+        // 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>
@@ -1221,7 +1345,9 @@ namespace Plane.CommunicationManagement
                     //gps.data[0] = rtcm_tmpser++;
                     gps.len = (byte)datalen;
                     gps.target_component = rtcm_tmpser++;
-                    gps.target_system = 1;
+                    //实测一旦收到数据都是正确的，张伟已经做过crc检验了，为了兼容性不再做了
+                    //如果需要做，飞控里面需要把注释的代码放开，并且不再兼容之前的固件
+                    gps.target_system = checkrtrcmsum(gps.data, (ushort)datalen);  //默认:1
 
                     //   Console.WriteLine("                                              ["+ gps.target_component + "]总长: "+ length +" 发送第 "+ (a+1) + " 个包,长度: " + (ushort)datalen );
 
@@ -1229,18 +1355,18 @@ namespace Plane.CommunicationManagement
 
                     await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
                   
-                    await Task.Delay(50).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到
+                    await Task.Delay(80).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到
 
-                    //重发一次，有序列号(target_component)飞机可以检测出来重复接收的
+                   // 重发一次，有序列号(target_component)飞机可以检测出来重复接收的
                     //需要新固件支持
                     // await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
-                    //  await Task.Delay(50).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到
+                    //await Task.Delay(80).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到
+
                     
-                    /*
                     //重发一次，有序列号(target_component)飞机可以检测出来重复接收的--需要新固件支持
-                    await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
-                    await Task.Delay(50).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到
-                    */
+                    //await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
+                   // await Task.Delay(80).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到
+                    
 
 
                 }

PlaneGcsSdk_Shared/CommunicationManagement/CommModulePacketAnalysis.cs

@@ -203,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/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/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;
@@ -178,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)
@@ -270,6 +286,43 @@ namespace Plane.Copters
             RC_CHANNELS_RAW = 35    // MAVLINK_MSG_ID_RC_CHANNELS_RAW
         }
 
+        private static readonly Dictionary<int, string> ErrorIdToString = new Dictionary<int, string>
+        {
+            {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, "陀螺仪不一致"},
+
+        };
+        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; }
@@ -480,6 +533,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; }
@@ -504,6 +566,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; }
@@ -672,6 +784,48 @@ namespace Plane.Copters
         }
 
 
+        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
         {
@@ -737,11 +891,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
@@ -807,67 +1008,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()
@@ -1118,7 +1268,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);
 

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

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
 {
@@ -21,17 +24,18 @@ namespace Plane.Copters
         /// <summary>
         /// 更新虚拟飞行器状态的时间间隔，单位为毫秒。
         /// </summary>
-        private int UPDATE_INTERVAL = 50;  //默认100， i7电脑 150可以跑1000架 100可以跑500架 50可以200-300架 
+        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 float MAX_MOVE_IN_INTERVAL = MAX_VEL * 50 / 1000;
+        private float MAX_MOVE_IN_INTERVAL = MAX_VEL * UPDATE_INTERVAL_TEMP / 1000;
 
         /// <summary>
         /// 高速模式下，在一个更新间隔中的最大移动距离。
         /// </summary>
-        private  float MAX_MOVE_IN_INTERVAL_FAST = MAX_VEL * 50 / 1000 * 4;  //MAX_MOVE_IN_INTERVAL*4
+        private float MAX_MOVE_IN_INTERVAL_FAST = MAX_VEL * UPDATE_INTERVAL_TEMP / 1000 * 4;  //MAX_MOVE_IN_INTERVAL*4
 
 
 
@@ -55,12 +59,12 @@ namespace Plane.Copters
         /// <summary>
         /// 高速模式下，按比例缩放过的在一个更新间隔中的最大移动距离。
         /// </summary>
-        private float _scaledFastMaxMoveInInterval = MAX_VEL * 50 / 1000 * 4;  //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_VEL * 50 / 1000;  //MAX_MOVE_IN_INTERVAL
+        private float _scaledMaxMoveInInterval = MAX_VEL * UPDATE_INTERVAL_TEMP / 1000;  //MAX_MOVE_IN_INTERVAL
 
         /// <summary>
         /// 速度缩放比例。
@@ -76,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 的目标纬度。
@@ -86,6 +112,15 @@ namespace Plane.Copters
         /// FlyTo 的目标经度。
         /// </summary>
         private double _targetLng;
+        /// <summary>
+        /// FlyTo 的目标纬度。
+        /// </summary>
+        private double _startLat;
+
+        /// <summary>
+        /// FlyTo 的目标经度。
+        /// </summary>
+        private double _startLng;
 
 
         /// <summary>
@@ -93,14 +128,26 @@ 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)
         {
+
+
         }
 
 
@@ -114,15 +161,14 @@ namespace Plane.Copters
                 //快速模式最大移动距离
                 MAX_MOVE_IN_INTERVAL_FAST = MAX_MOVE_IN_INTERVAL * 4;
                 //速度缩放后快速速度距离
-                _scaledFastMaxMoveInInterval = MAX_MOVE_IN_INTERVAL_FAST* _speedScale;
+                _scaledFastMaxMoveInInterval = MAX_MOVE_IN_INTERVAL_FAST * _speedScale;
                 //速度缩放后速度距离
-                _scaledMaxMoveInInterval = MAX_MOVE_IN_INTERVAL* _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;
@@ -189,6 +235,8 @@ namespace Plane.Copters
             GpsHdop = 1;
             IsGpsAccurate = true;
             HasSwitchedToGpsMode = true;
+            GC_xy = new GuidController();
+            GC_z = new GuidController();
 
             /*
             // 持续假装收到飞行器发来的心跳。
@@ -224,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()
         {
@@ -240,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;
         }
@@ -407,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()
@@ -426,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;
         }
 
@@ -517,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;
 
@@ -551,6 +747,7 @@ namespace Plane.Copters
                             break;
 
                         case FlightMode.AUTO:
+                            /*
                             // 暂时只管起飞。以最大速度上升，直到达到目标高度。
                             if (Altitude < _takeOffTargetAltitude)
                             {
@@ -560,12 +757,17 @@ namespace Plane.Copters
                             {
                                 Mode = FlightMode.LOITER;
                             }
+                            */
                             break;
 
                         case FlightMode.GUIDED:
                             // 王海, 20160317, 指点时也能体感控制若干通道。
                             //考虑不更新这个，好像没必要-xu
                             //UpdateWithChannels();
+                            //UpdateWithDestination(_targetLat, _targetLng, _targetAlt);
+                            if (FlightControlMode==1)
+                                UpdateWithDestination_v2(_targetLat, _targetLng, _targetAlt);
+                            else
                                 UpdateWithDestination(_targetLat, _targetLng, _targetAlt);
                             break;
 
@@ -598,7 +800,7 @@ namespace Plane.Copters
 
                         case FlightMode.LAND:
                             // 王海, 20160317, 降落时也能体感控制若干通道。
-                            UpdateWithChannels();
+                            //  UpdateWithChannels();
                             // 以最大速度降落，直到高度为 0。
                             if (Altitude > 0)
                             {
@@ -620,11 +822,71 @@ 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
@@ -633,6 +895,11 @@ namespace Plane.Copters
                 // 锁定时直接把速度设为 0。
                 AirSpeed = 0;
             }
+
+          //  UpdateShowColor();
+
+
+
             _uiSyncContext.Post(() =>
             {
                 //位置变化需要在UI刷新
@@ -708,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))
@@ -754,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

@@ -33,7 +33,9 @@ namespace Plane.Copters
 
         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

@@ -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.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; }

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

@@ -18,8 +18,8 @@ namespace Plane.Protocols
         public const int MAVLINK_BIG_ENDIAN = 0;
 
         //协议头 飞控4.0以后 254 - 240  Mavlink头 FE 改为 F0 
-        public const byte MAVLINK_STX = 254;
-        //public const byte MAVLINK_STX = 240;
+        //public const byte MAVLINK_STX = 254;
+        public const byte MAVLINK_STX = 240;
 
         public const byte MAVLINK_ENDIAN = MAVLINK_LITTLE_ENDIAN;
 

PlaneGcsSdk_Shared/Protocols/MavCommTypes.cs

@@ -158,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 UInt16 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;          //方向角度
 
         };