紧急返航功能，策略不对，临时保存

PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs

@@ -1,4 +1,5 @@
 using Plane.Copters;
+using Plane.Geography;
 using Plane.Protocols;
 using System;
 using System.Collections.Generic;
@@ -842,6 +843,36 @@ 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.MISSION_START, 
+                        (float)takeoffcentloc.Latitude  * 1000,
+                        (float)takeoffcentloc.Longitude * 1000,//起飞高度不用传=0
+                         (float)taskcentloc.Latitude * 1000,
+                        (float)taskcentloc.Longitude * 1000,
+                               taskcentloc.Altitude,
+                        mindistance, param7
+                        );
+                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)

PlaneGcsSdk_Shared/Copters/FakeCopter.cs

@@ -24,7 +24,7 @@ namespace Plane.Copters
         /// <summary>
         /// 更新虚拟飞行器状态的时间间隔，单位为毫秒。
         /// </summary>
-        static private int UPDATE_INTERVAL =100;  //默认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>
@@ -131,6 +131,15 @@ namespace Plane.Copters
         private System.Drawing.ColorConverter rgbconverter;
 
 
+        private PLLocation _takeoffcentloc;
+        private PLLocation _taskcentloc;
+        private float _mindistance;
+        private int _rettime;
+        private bool _descending;
+        int Emergency_Retstatus = 0;
+        //返航路径第一个航点
+        PLLocation Emergency_firstloc;
+
         /// <summary>
         /// 使用 <see cref="SynchronizationContext.Current"/> 创建 <see cref="FakeCopter"/> 实例。
         /// </summary>
@@ -151,9 +160,9 @@ 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);
             }
         }
@@ -281,6 +290,50 @@ namespace Plane.Copters
             IsCheckingConnection = false;
             return TaskUtils.CompletedTask;
         }
+        private void BuildPath()
+        {
+            float taralt;
+            //返航高度参数
+            float retalt = 15;
+
+            if (_descending)
+            {
+                if (Altitude <= retalt) taralt = Altitude;
+                else
+                {
+                    float maxalt = Altitude - retalt;
+                    //降低一个随机高度，随机高度介于0和maxalt之间，也就是目标高度不要低于retalt, 防止飞机在同一高度飞行
+                    taralt = (float)(Altitude - (new Random().NextDouble() * maxalt));
+                }
+                Emergency_firstloc = new PLLocation(this.Latitude,this.Longitude, taralt);
+            }
+            else
+            {
+               //计算当前位置和起飞点的距离
+                float dis = (float)this.CalcDistance(TakeOffPoint.Latitude, TakeOffPoint.Longitude,Altitude);
+                //随机水平飞行距离
+                float maxdis = (float)new Random().NextDouble() * dis;
+
+
+
+
+            }
+
+           
+        }
+        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;
+            //计算返航路径
+            BuildPath();
+            Mode = FlightMode.EMERG_RTL;
+            return TaskUtils.CompletedTask;
+        }
 
         protected override Task FlyToCoreAsync(double lat, double lng, float alt, float flytime = 0, int flytype = 0)
         {
@@ -497,8 +550,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(Missionlat, Missionlng, 0);
+
             return TaskUtils.CompletedTask;
         }
 
@@ -764,6 +821,49 @@ namespace Plane.Copters
 
                         case FlightMode.TOY:
                             break;
+                        //紧急返航
+                        case FlightMode.EMERG_RTL:
+                            switch(Emergency_Retstatus)
+                            {
+                                case 0: //等待返航
+                                    if ((DateTime.Now- MissionStartTime).TotalSeconds > _rettime)
+                                    {
+                                        Emergency_Retstatus = 1;
+                                        //平飞或降落随机距离
+                                        FlyToCoreAsync(Emergency_firstloc.Latitude, Emergency_firstloc.Longitude, Emergency_firstloc.Altitude);
+                                        //FlyToCoreAsync里面把模式改成了GUIDED，这里改回来
+                                        Mode = FlightMode.EMERG_RTL;
+                                    }
+
+                                   
+                                    break;
+                                case 1: //返航阶段一:等待平飞或降落随机距离并到达,然后飞到起飞点上空
+                                        
+                                    UpdateWithDestination(Emergency_firstloc.Latitude, Emergency_firstloc.Longitude, Emergency_firstloc.Altitude);
+                                    if (ReachedDestination(Emergency_firstloc.Latitude, Emergency_firstloc.Longitude, Emergency_firstloc.Altitude))
+                                    {
+                                        FlyToCoreAsync(TakeOffPoint.Latitude, TakeOffPoint.Longitude, Emergency_firstloc.Altitude);
+                                        //FlyToCoreAsync里面把模式改成了GUIDED，这里改回来
+                                        Mode = FlightMode.EMERG_RTL;
+                                        Emergency_Retstatus = 2;
+                                    }
+                                  
+                                    break;
+                                case 2: //返航阶段二:等待到达起飞点上空,然后降落
+
+                                    UpdateWithDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, Emergency_firstloc.Altitude);
+                                    if (ReachedDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, Emergency_firstloc.Altitude))
+                                    {
+                                        FlyToCoreAsync(TakeOffPoint.Latitude, TakeOffPoint.Longitude, Emergency_firstloc.Altitude);
+                                        //FlyToCoreAsync里面把模式改成了GUIDED，这里改回来
+                                        Mode = FlightMode.LAND;
+                                        Emergency_Retstatus = 3;
+                                    }
+                                    break;
+                                case 3: //降落
+                                    break;
+                            }
+                            break;
 
                         default:
                             break;

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