加入任务整体旋转

加入自动识别RTK航点到达精度为0.6米，碰撞精度为0.5米

Plane.FormationCreator/CalculationLogLatDistance.cs

@@ -56,6 +56,27 @@ namespace Plane.FormationCreator
                 return s;
             }
 
+        public static void GetRotatePos(double Centlon, double Centlat,double Angle, double lon_in,
+                  double lat_in, out double lon_out, out double lat_out)
+        {
+
+            double lpDistance = GetDistanceOne(Centlon, Centlat,
+                                            lon_in,
+                                            lat_in);
+
+            MyLatLng mypos1, mypos2;
+            mypos1 = new CalculationLogLatDistance.MyLatLng(Centlon, Centlat);
+            mypos2 = new CalculationLogLatDistance.MyLatLng(lon_in , lat_in);
+            double lpAzimuth = CalculationLogLatDistance.getAngle(mypos1, mypos2);
+            ConvertDistanceToLogLat(Centlon, Centlat, lpDistance,
+                lpAzimuth + Angle, out lon_out, out lat_out);
+            return ;
+        }
+
+
+
+
+
         /** 
          * 求两经纬度距离(google maps源码中) 
          *  

Plane.FormationCreator/Formation/Extensions.cs

@@ -6,10 +6,18 @@ using System.Linq;
 using System.Text;
 using System.Threading.Tasks;
 
+
+
 namespace Plane.FormationCreator.Formation
 {
     static class Extensions
     {
+        public const float GpsArrivedDis = 2.0f; //GPS模式下航点到达精度
+        public const float RTKArrivedDis = 0.6f;  //RTK模式航点达到精度
+        public const float GpsCloseDis = 2.0f;    //GPS模式下碰撞检测最近距离
+        public const float RTKClosedDis = 0.5f;   //RTK模式下碰撞检测最近距离
+
+
         public static LatLng? GetCenter(this IEnumerable<ICopter> copters)
         {
             int count = 0;
@@ -54,9 +62,13 @@ namespace Plane.FormationCreator.Formation
 
         public static bool ArrivedTarget(this ICopter copter, double targetLat, double targetLng, float targetAlt)
         {
-           // return copter.DistanceTo(targetLat, targetLng, targetAlt) < 2;
+  //          return copter.DistanceTo(targetLat, targetLng, targetAlt) < 0.6;   //到达航点精度
-             return copter.DistanceTo(targetLat, targetLng, targetAlt) < 0.6;   //到达航点精度
+
-            // return copter.DistanceTo(targetLat, targetLng, targetAlt) < 2;  // added by ZJF
+            if (copter.GpsFixType == GpsFixType.RTKFIXED)
+                return copter.DistanceTo(targetLat, targetLng, targetAlt) < RTKArrivedDis;   //RTK到达航点精度0.6米
+            else
+                return copter.DistanceTo(targetLat, targetLng, targetAlt) < GpsArrivedDis;   //GPS到达航点精度2.0米
+  
     }
 
         public static bool ArrivedTargetInPlane(this ICopter copter, double targetLat, double targetLng)
@@ -71,7 +83,14 @@ namespace Plane.FormationCreator.Formation
 
         public static bool IsTooCloseTo(this ICopter copter, ICopter copter2)
         {
-            return copter.DistanceTo(copter2) < 0.5;  //最近距离2米
+            if (copter.GpsFixType == GpsFixType.RTKFIXED)
+                return copter.DistanceTo(copter2) < RTKClosedDis;  //最近距离0.5米
+            else
+                return copter.DistanceTo(copter2) < GpsCloseDis;  //最近距离2米
+
+
+
+
         }
     }
 }

Plane.FormationCreator/Formation/FlightTask_TakeOff.cs

@@ -23,6 +23,7 @@ namespace Plane.FormationCreator.Formation
             }
         }
 
+        //一架架起飞--未使用
         public async Task RunTakeOffTaskAsync()
         {
             // float takeOffAlt = 15;
@@ -60,7 +61,12 @@ namespace Plane.FormationCreator.Formation
             }
         }
 
-        // 几架飞机同时起飞，参数为takeOffCount
+        // 几架飞机同时起飞，参数为takeOffCount-----------------使用中
+        //起飞分三个阶段：1阶段分批起飞到15米(目前15米高度是飞控起飞航点决定)，上一批起飞超过5米下一批开始起飞，
+        //等待全部起飞完成后执行第二阶段，
+        //2阶段等待高度超过9米(可能已到达15米)然后平飞到第一个航点位置,高度为15米的位置，
+        //3阶段垂直上升到第一个航点指定高度
+        //
         public async Task MutilRunTakeOffTaskAsync()
         {
             int TaskCount = _flightTaskManager.Tasks.Count();
@@ -78,12 +84,13 @@ namespace Plane.FormationCreator.Formation
                 for (int i = 0; i < integerPart; i++)
                 {
                     var tasksTakeOff = new Task[takeOffCount];
+                    //执行n架同时起飞
                     for (int j = takeOffCount * i; j < takeOffCount * (i + 1); j++)
                     {
                         var info = infos[j];
 
                         int indexTmp = j - takeOffCount * i;
-                        if (info.takeOffStage == 0)   // 第一阶段：起飞到10m
+                        if (info.takeOffStage == 0)   // 第一阶段：起飞到15m
                         {
                             tasksTakeOff[indexTmp] = TakeOffTaskFlySingleCopterAsync(info);
                         }
@@ -92,8 +99,9 @@ namespace Plane.FormationCreator.Formation
                             tasksTakeOff[indexTmp] = Task.Run(async () => { await Task.Delay(1).ConfigureAwait(false); });
                         }
                     }
+                    //等待多架起飞任务-起飞高度超过5米完成
                     await Task.WhenAll(tasksTakeOff).ConfigureAwait(false);
-                    
+                    //加入已起飞飞机的第二第三阶段起飞任务--并不执行
                     for (int j = takeOffCount * i; j < takeOffCount * (i + 1); j++)
                     {
                         var info = infos[j];
@@ -110,7 +118,7 @@ namespace Plane.FormationCreator.Formation
                         var info = infos[j];
 
                         int indexTmp = j - takeOffCount * integerPart;
-                        if (info.takeOffStage == 0)   // 第一阶段：起飞到10m
+                        if (info.takeOffStage == 0)   // 第一阶段：起飞到15m
                         {
                             tasksTakeOff[indexTmp] = TakeOffTaskFlySingleCopterAsync(info);
                         }
@@ -119,14 +127,16 @@ namespace Plane.FormationCreator.Formation
                             tasksTakeOff[indexTmp] = Task.Run(async () => { await Task.Delay(1).ConfigureAwait(false); });
                         }
                     }
+                    //等待起飞超过5米
                     await Task.WhenAll(tasksTakeOff).ConfigureAwait(false);
                     
                     for (int j = integerPart * takeOffCount; j < takeOffCount * integerPart + residualPart; j++)
                     {
                         var info = infos[j];
-                        tasksTmp[j] = TakeOffSecondTaskAsync(info);   // 第二和第三阶段
+                        tasksTmp[j] = TakeOffSecondTaskAsync(info);   // 加入第二和第三阶段
                     }
                 }
+                //执行并等待所有的第二第三阶段起飞任务完成
                 await Task.WhenAll(tasksTmp).ConfigureAwait(false);
                 if (_flightTaskManager.IsPaused == false)
                 {
@@ -142,6 +152,7 @@ namespace Plane.FormationCreator.Formation
         
         private CopterManager _copterManager = ServiceLocator.Current.GetInstance<CopterManager>();
 
+        //执行第一阶段解锁起飞任务---使用中
         private async Task TakeOffTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info)
         {
             int copterIndex = SingleCopterInfos.IndexOf(info);
@@ -155,7 +166,11 @@ namespace Plane.FormationCreator.Formation
             {
                 return;
             }
+
+            //设置灯光
             await copter.SetShowLEDAsync(true);
+            
+            //开始解锁
             await copter.UnlockAsync();
             for (int i = 0; !copter.IsUnlocked; i++)
             {
@@ -175,11 +190,13 @@ namespace Plane.FormationCreator.Formation
 
                 await Task.Delay(25).ConfigureAwait(false);
             }
+            //////解锁完成
 
             // 为了返航，记录家的位置, 应该放在起飞命令
             info.TargetLat = info.Copter.Latitude;
             info.TargetLng = info.Copter.Longitude;
 
+            //开始起飞
             for (int i = 0; i < 5; i++)   // added by ZJF
             {
                 await copter.TakeOffAsync();
@@ -187,6 +204,7 @@ namespace Plane.FormationCreator.Formation
             }
 
             // while (copter.Altitude < 4 || copter.State == Copters.CopterState.TakingOff)
+            //低于5米任务一直等待
             while (copter.Altitude < 5)                 // 修改起飞逻辑，当高度达到5米时，下一架开始起飞
             {
                 if (_flightTaskManager.IsPaused == true)
@@ -206,6 +224,7 @@ namespace Plane.FormationCreator.Formation
 */
         }
 
+        //执行第二第三阶段起飞任务-------------使用中
         private async Task TakeOffSecondTaskAsync(FlightTaskSingleCopterInfo info)
         {
             float takeOffAlt = 15;
@@ -217,7 +236,7 @@ namespace Plane.FormationCreator.Formation
                 return;
             // await Task.Run(async () =>
             // {
-            // 修改起飞逻辑，当高度达到5米时，下一架开始起飞
+            // 小于9米时等待----按起飞高度算此时可能已到达15米起飞高度
             if (info.takeOffStage == 0)
             {
                 while (info.Copter.Altitude < 9)
@@ -239,17 +258,20 @@ namespace Plane.FormationCreator.Formation
             DateTime dtLastTime = DateTime.Now;
             TimeSpan ts = dtNow - dtLastTime;
 
-            // 第二阶段：水平飞行
+            // 第二阶段：超过9米开始水平飞行
             if (info.takeOffStage == 1)
             {
+                //切换到guided模式
                 await info.Copter.GuidAsync();
+                //设置灯光为航点要求的灯光
                 await info.Copter.SetShowLEDAsync(copterNextTask.FlytoShowLED );
+                //异步执行飞到第一个航点高度固定为15米的任务
                 for (int j = 0; j < 3; j++)
                 {
                     await info.Copter.FlyToAsync(copterNextTask.TargetLat, copterNextTask.TargetLng, takeOffAlt);
                     await Task.Delay(10).ConfigureAwait(false);
                 }
-
+                //直到到达第一个航点并高度15米
                 while (!info.Copter.ArrivedTarget(copterNextTask.TargetLat, copterNextTask.TargetLng, takeOffAlt))
                 {
                     if (_flightTaskManager.IsPaused == true)
@@ -261,6 +283,7 @@ namespace Plane.FormationCreator.Formation
 
                     dtNow = DateTime.Now;
                     ts = dtNow - dtLastTime;
+                    //每2秒再异步执行一次到航点高度15米的任务
                     if (ts.TotalMilliseconds > 2000)
                     {
                         for (int j = 0; j < 2; j++)
@@ -273,22 +296,23 @@ namespace Plane.FormationCreator.Formation
                 }
                 if (_flightTaskManager.IsPaused == false)
                 {
-                    info.takeOffStage++;
+                    info.takeOffStage++;   //当前变为2
                 }
             }
 
             dtNow = DateTime.Now;
             dtLastTime = DateTime.Now;
             ts = dtNow - dtLastTime;
-            // 第三阶段：垂直飞行
+            // 第三阶段：从第一个航点位置15米垂直飞行
             if (info.takeOffStage == 2)
             {
+                //执行飞到第一个航点高度为设定目标高度
                 for (int j = 0; j < 3; j++)
                 {
                     await info.Copter.FlyToAsync(copterNextTask.TargetLat, copterNextTask.TargetLng, copterNextTask.TargetAlt);
                     await Task.Delay(10).ConfigureAwait(false);
                 }
-
+                //直到到达第一个航点
                 while (!info.Copter.ArrivedTarget(copterNextTask.TargetLat, copterNextTask.TargetLng, copterNextTask.TargetAlt))
                 {
                     if (_flightTaskManager.IsPaused == true)
@@ -300,6 +324,7 @@ namespace Plane.FormationCreator.Formation
 
                     dtNow = DateTime.Now;
                     ts = dtNow - dtLastTime;
+                    //每2秒发一次第一个航点飞行任务
                     if (ts.TotalMilliseconds > 2000)
                     {
                         for (int j = 0; j < 2; j++)

Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs

@@ -681,7 +681,86 @@ public ICommand VerticlAlignmentCommand
             }
         }
 
+        //任务整体旋转
+        private ICommand _TaskRotateCommand;
+        public ICommand TaskRotateCommand
+        {
+            get
+            {
+                return _TaskRotateCommand ?? (_TaskRotateCommand = new RelayCommand<int>(async RotateLine =>
+                {
+                double centlng = 0;
+                double centlat = 0;
                     
+                    if (_copterManager.Copters.Count > 0)
+                    {
+                        centlng = _copterManager.Copters[0].Longitude;
+                        centlat = _copterManager.Copters[0].Latitude;
+
+                        //虚拟飞机一起旋转
+                        if ("FakeCopter" == _copterManager.Copters[0].GetType().Name)
+                        {
+                            for (int i = 0; i < _copterManager.Copters.Count; i++)
+                            {
+                                FakeCopter tempcopter = (FakeCopter)_copterManager.Copters[i];
+                                double lng_out = 0;
+                                double lat_out = 0;
+                                CalculationLogLatDistance.GetRotatePos(centlng, centlat, (double)RotateLine,
+                                    tempcopter.Longitude,
+                                    tempcopter.Latitude,
+                                    out lng_out, out  lat_out
+                                    );
+                                tempcopter.SetProperties(
+                                                    latitude: lat_out,
+                                                    longitude: lng_out
+                                    );
+
+                            }
+                        }
+
+
+
+
+
+                        double copterlng = 0;
+                        double copterlat = 0;
+                        double lng_out1 = 0;
+                        double lat_out1 = 0;
+                        for (int i = 0; i < _flightTaskManager.Tasks.Count; i++)
+                        {
+
+
+                            if ((_flightTaskManager.Tasks[i].TaskType == FlightTaskType.Loiter) && (_flightTaskManager.Tasks[i].ChangeYaw))
+                            {
+                                _flightTaskManager.Tasks[i].HeadYaw += RotateLine;
+                                _flightTaskManager.Tasks[i].HeadYaw = _flightTaskManager.Tasks[i].HeadYaw % 360;
+                            }
+
+                            for (int j = 0; j < _flightTaskManager.Tasks[i].SingleCopterInfos.Count; j++)
+                            {
+                                copterlng=_flightTaskManager.Tasks[i].SingleCopterInfos[j].TargetLng;
+                                copterlat = _flightTaskManager.Tasks[i].SingleCopterInfos[j].TargetLat;
+
+                                CalculationLogLatDistance.GetRotatePos(centlng, centlat, (double)RotateLine,
+                                    copterlng,
+                                    copterlat,
+                                    out lng_out1, out lat_out1
+                                    );
+
+                                _flightTaskManager.Tasks[i].SingleCopterInfos[j].TargetLng = lng_out1;
+                                _flightTaskManager.Tasks[i].SingleCopterInfos[j].TargetLat = lat_out1;
+                            }
+
+                        }
+
+
+                    }
+
+                    await Task.Delay(100);  // 如果不等待一段时间，很可能会再触发 DataStreamReceived 事件导致飞行器重新出现在地图上。
+
+                }));
+            }
+        }
 
 
 

Plane.FormationCreator/Views/ModifyTaskView.xaml

@@ -77,6 +77,10 @@
                      Text="0"
                      VerticalContentAlignment="Center" />
             <TextBlock Text="度" Margin="0, 10, 5, 0"/>
+            <Button Content="任务整体旋转"
+                    Margin="0,5,5,0"
+                    Command="{Binding TaskRotateCommand}" 
+                    CommandParameter="{Binding ElementName=txtAlignmentLine, Path=Text}"/>