From d0513001715f8e41276efb14d18e39d25cb17bd8 Mon Sep 17 00:00:00 2001
From: pxzleo <pxzleo@126.com>
Date: Tue, 21 Nov 2023 21:18:03 +0800
Subject: [PATCH] =?UTF-8?q?1=E5=A2=9E=E5=8A=A0=E8=A7=82=E6=B5=8B=E8=80=85?=
 =?UTF-8?q?=E4=BD=8D=E7=BD=AE=E5=9B=BE=E6=A0=87=EF=BC=8C=E4=BF=AE=E6=94=B9?=
 =?UTF-8?q?=E5=90=8E=E7=9B=B4=E6=8E=A5=E5=8F=AF=E4=BB=A5=E4=BD=BF=E7=94=A8?=
 =?UTF-8?q?=202=E5=A2=9E=E5=8A=A0=E9=85=8D=E7=BD=AEFC=5FVER=5FNO=20=3D=203?=
 =?UTF-8?q?=20=E4=BD=BF=E7=94=A8=E5=B0=8F=E8=88=AA=E7=82=B9=E5=90=8C?=
 =?UTF-8?q?=E6=97=B6=E8=BE=BE=E5=88=B0=E6=A8=A1=E5=BC=8F=E8=AE=A1=E7=AE=97?=
 =?UTF-8?q?=E6=A8=A1=E6=8B=9F=E9=A3=9E=E8=A1=8C=203=E5=A2=9E=E5=8A=A0?=
 =?UTF-8?q?=E7=81=AF=E5=85=89=E6=A1=86=E6=9E=B6=EF=BC=8C=E5=8F=AF=E5=AE=9E?=
 =?UTF-8?q?=E7=8E=B0=E9=97=AA=E7=83=81=E7=AD=89=EF=BC=8C=E4=BD=86=E5=AE=9E?=
 =?UTF-8?q?=E9=99=85=E4=BD=BF=E7=94=A8=E5=A4=AA=E6=85=A2=EF=BC=8C=E6=9A=82?=
 =?UTF-8?q?=E6=97=B6=E4=B8=8D=E7=94=A8?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../PlaneGcsSdk.Contract_Private.csproj       |   8 +-
 .../ICopterActionsSharedByCopterManager.cs    |   3 +-
 .../Copters/ICopterStatus.cs                  |  15 ++
 .../Geography/GeographyUtils.cs               |  39 ++++
 .../PlaneGcsSdk_Private_NET46.csproj          |   1 +
 .../CopterManagement/SingleCopterManager.cs   |   2 +-
 .../Copters/CopterImplSharedPart.cs           | 121 ++++++-----
 PlaneGcsSdk_Shared/Copters/EmptyCopter.cs     |   2 +-
 PlaneGcsSdk_Shared/Copters/FakeCopter.cs      | 198 ++++++++++++++++--
 PlaneGcsSdk_Shared/Copters/PLCopter.cs        |   2 +-
 10 files changed, 313 insertions(+), 78 deletions(-)

diff --git a/PlaneGcsSdk.Contract_Private/PlaneGcsSdk.Contract_Private.csproj b/PlaneGcsSdk.Contract_Private/PlaneGcsSdk.Contract_Private.csproj
index 0583db7..bc14a6c 100644
--- a/PlaneGcsSdk.Contract_Private/PlaneGcsSdk.Contract_Private.csproj
+++ b/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.0\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. 
diff --git a/PlaneGcsSdk.Contract_Shared/Copters/ICopterActionsSharedByCopterManager.cs b/PlaneGcsSdk.Contract_Shared/Copters/ICopterActionsSharedByCopterManager.cs
index c655bd1..90f82b5 100644
--- a/PlaneGcsSdk.Contract_Shared/Copters/ICopterActionsSharedByCopterManager.cs
+++ b/PlaneGcsSdk.Contract_Shared/Copters/ICopterActionsSharedByCopterManager.cs
@@ -31,7 +31,8 @@ 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 =飞行时间 秒
+        Task FlyToAsync(double lat, double lng, float alt,float flytime=0);
 
         /// <summary>
         /// 切换到 <see cref="CopterState.HoverMode"/> 并悬停。此操作需要使用 GPS 定位，卫星数不足导致定位不准时非常危险。
diff --git a/PlaneGcsSdk.Contract_Shared/Copters/ICopterStatus.cs b/PlaneGcsSdk.Contract_Shared/Copters/ICopterStatus.cs
index 81554ab..07cebda 100644
--- a/PlaneGcsSdk.Contract_Shared/Copters/ICopterStatus.cs
+++ b/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; }
+
     }
 }
diff --git a/PlaneGcsSdk.Contract_Shared/Geography/GeographyUtils.cs b/PlaneGcsSdk.Contract_Shared/Geography/GeographyUtils.cs
index 7088b02..802c43f 100644
--- a/PlaneGcsSdk.Contract_Shared/Geography/GeographyUtils.cs
+++ b/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>
         /// 计算空间中两点之间的距离，单位为米。
diff --git a/PlaneGcsSdk_Private_NET46/PlaneGcsSdk_Private_NET46.csproj b/PlaneGcsSdk_Private_NET46/PlaneGcsSdk_Private_NET46.csproj
index 78eddf5..b3d0704 100644
--- a/PlaneGcsSdk_Private_NET46/PlaneGcsSdk_Private_NET46.csproj
+++ b/PlaneGcsSdk_Private_NET46/PlaneGcsSdk_Private_NET46.csproj
@@ -34,6 +34,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" />
diff --git a/PlaneGcsSdk_Shared/CopterManagement/SingleCopterManager.cs b/PlaneGcsSdk_Shared/CopterManagement/SingleCopterManager.cs
index 29fbae6..a90a3a2 100644
--- a/PlaneGcsSdk_Shared/CopterManagement/SingleCopterManager.cs
+++ b/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)
         {
             return Copter.FlyToAsync(lat, lng, alt);
         }
diff --git a/PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs b/PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs
index 7c387e1..bb6e956 100644
--- a/PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs
+++ b/PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs
@@ -1,6 +1,7 @@
 using Plane.Geography;
 using System;
 using System.ComponentModel;
+using System.Drawing;
 using System.Threading;
 using System.Threading.Tasks;
 
@@ -104,6 +105,7 @@ namespace Plane.Copters
         private double _FlightDistance;
 
         private double _FlightDistance2D;
+        private int _FlightControlMode;
 
         private TimeSpan _FlightTime;
 
@@ -480,6 +482,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; }
@@ -737,12 +748,59 @@ namespace Plane.Copters
         {
             get { return _LEDColor; }
             set {
-                if (Set(nameof(LEDColor), ref _LEDColor, value))
+                if (value!=_LEDColor)
+                {
+                    Set(nameof(LEDColor), ref _LEDColor, value);
                     //强制刷新颜色--在刷新位置时才刷新颜色
-                     RefreashLoc();
+                    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
 #else
@@ -807,67 +865,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)
         {
             _shouldFollow = false;
             //State = CopterState.CommandMode;
-            return FlyToCoreAsync(lat, lng, alt);
+            return FlyToCoreAsync(lat, lng, alt, flytime);
         }
 
         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 +1125,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);
 
         protected void RaiseAltitudeChanged() => AltitudeChanged?.Invoke(this, EventArgs.Empty);
 
diff --git a/PlaneGcsSdk_Shared/Copters/EmptyCopter.cs b/PlaneGcsSdk_Shared/Copters/EmptyCopter.cs
index 23f81d0..e54d73d 100644
--- a/PlaneGcsSdk_Shared/Copters/EmptyCopter.cs
+++ b/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)
         {
             return TaskUtils.CompletedTask;
         }
diff --git a/PlaneGcsSdk_Shared/Copters/FakeCopter.cs b/PlaneGcsSdk_Shared/Copters/FakeCopter.cs
index 04016ac..256c545 100644
--- a/PlaneGcsSdk_Shared/Copters/FakeCopter.cs
+++ b/PlaneGcsSdk_Shared/Copters/FakeCopter.cs
@@ -2,10 +2,11 @@
 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;
 namespace Plane.Copters
 {
     /// <summary>
@@ -21,17 +22,18 @@ namespace Plane.Copters
         /// <summary>
         /// 更新虚拟飞行器状态的时间间隔，单位为毫秒。
         /// </summary>
-        static 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 * UPDATE_INTERVAL / 1000;
+        private float MAX_MOVE_IN_INTERVAL = MAX_VEL * UPDATE_INTERVAL_TEMP / 1000;
 
         /// <summary>
         /// 高速模式下，在一个更新间隔中的最大移动距离。
         /// </summary>
-        private  float MAX_MOVE_IN_INTERVAL_FAST = MAX_VEL * UPDATE_INTERVAL / 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 +57,12 @@ namespace Plane.Copters
         /// <summary>
         /// 高速模式下，按比例缩放过的在一个更新间隔中的最大移动距离。
         /// </summary>
-        private float _scaledFastMaxMoveInInterval = MAX_VEL * UPDATE_INTERVAL / 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 *  UPDATE_INTERVAL / 1000;  //MAX_MOVE_IN_INTERVAL
+        private float _scaledMaxMoveInInterval = MAX_VEL * UPDATE_INTERVAL_TEMP / 1000;  //MAX_MOVE_IN_INTERVAL
 
         /// <summary>
         /// 速度缩放比例。
@@ -76,6 +78,27 @@ 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 _distance_xy_delta;
+        private float _distance_z_delta;
+        private float currflytime;
+        // 根据飞行时间计算飞行距离。
+        private float currdis_xy;
+        private float currdis_z;
+
+
+        // 目标点相对于开始位置的方向。
+        private double _direction; 
 
         /// <summary>
         /// FlyTo 的目标纬度。
@@ -86,6 +109,15 @@ namespace Plane.Copters
         /// FlyTo 的目标经度。
         /// </summary>
         private double _targetLng;
+        /// <summary>
+        /// FlyTo 的目标纬度。
+        /// </summary>
+        private double _startLat;
+
+        /// <summary>
+        /// FlyTo 的目标经度。
+        /// </summary>
+        private double _startLng;
 
 
         /// <summary>
@@ -93,7 +125,7 @@ namespace Plane.Copters
         /// </summary>
         private bool _ShowLED;
 
-
+        private System.Drawing.ColorConverter rgbconverter;
 
 
         /// <summary>
@@ -101,6 +133,8 @@ namespace Plane.Copters
         /// </summary>
         public FakeCopter() : this(SynchronizationContext.Current)
         {
+
+
         }
 
 
@@ -122,7 +156,6 @@ namespace Plane.Copters
         }
 
 
-
         public Task DoCommandAckAsync(ushort command, byte result)
         {
             return TaskUtils.CompletedTask;
@@ -241,14 +274,37 @@ namespace Plane.Copters
             return TaskUtils.CompletedTask;
         }
 
-        protected override Task FlyToCoreAsync(double lat, double lng, float alt)
+        protected override Task FlyToCoreAsync(double lat, double lng, float alt, float flytime = 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}");
+
+                _distance_xy_delta = _distance_xy / _flytime;
+                _distance_z_delta = _distance_z / _flytime;
+
+
                 Mode = FlightMode.GUIDED;
+
+
             }
             return TaskUtils.CompletedTask;
         }
@@ -517,6 +573,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;
 
@@ -566,7 +689,11 @@ namespace Plane.Copters
                             // 王海, 20160317, 指点时也能体感控制若干通道。
                             //考虑不更新这个，好像没必要-xu
                             //UpdateWithChannels();
-                            UpdateWithDestination(_targetLat, _targetLng, _targetAlt);
+                            //UpdateWithDestination(_targetLat, _targetLng, _targetAlt);
+                            if (FlightControlMode==1)
+                                UpdateWithDestination_v2(_targetLat, _targetLng, _targetAlt);
+                            else
+                                UpdateWithDestination(_targetLat, _targetLng, _targetAlt);
                             break;
 
                         case FlightMode.STABILIZE:
@@ -624,7 +751,7 @@ namespace Plane.Copters
                         default:
                             break;
                     }
-                    UpdateFlightDataIfNeeded();
+                    //UpdateFlightDataIfNeeded();
                 }
             }
             else
@@ -633,6 +760,11 @@ namespace Plane.Copters
                 // 锁定时直接把速度设为 0。
                 AirSpeed = 0;
             }
+
+          //  UpdateShowColor();
+
+
+
             _uiSyncContext.Post(() =>
             {
                 //位置变化需要在UI刷新
@@ -721,7 +853,7 @@ namespace Plane.Copters
             // 更新高度。
             Altitude += altDelta;
             // 目标点相对于当前位置的方向。
-             var direction = this.CalcDirection2D(lat, lng);
+            // var direction = this.CalcDirection2D(lat, lng);
             // 水平面上的移动距离。
             var moveInHorizontalPlane = Math.Sqrt(move * move - altDelta * altDelta);
             if (double.IsNaN(moveInHorizontalPlane))
@@ -729,10 +861,48 @@ 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;
+            // 根据飞行时间计算飞行距离
+            currdis_xy = _distance_xy_delta * currflytime;
+            currdis_z = _distance_z_delta * currflytime;
+            // 距离已经很近，直接移动到目标点。           
+            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;
+        }
+
     }
 }
diff --git a/PlaneGcsSdk_Shared/Copters/PLCopter.cs b/PlaneGcsSdk_Shared/Copters/PLCopter.cs
index c890284..2203513 100644
--- a/PlaneGcsSdk_Shared/Copters/PLCopter.cs
+++ b/PlaneGcsSdk_Shared/Copters/PLCopter.cs
@@ -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)
         {
             if (!IsEmergencyHoverActive)
             {