紧急返航功能，策略不对，临时保存
This commit is contained in:
parent
f9814532f5
commit
9c2238479f
@ -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)
|
||||
|
||||
@ -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>
|
||||
@ -35,7 +35,7 @@ namespace Plane.Copters
|
||||
/// <summary>
|
||||
/// 高速模式下，在一个更新间隔中的最大移动距离。
|
||||
/// </summary>
|
||||
private float MAX_MOVE_IN_INTERVAL_FAST = MAX_VEL * UPDATE_INTERVAL_TEMP / 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
|
||||
|
||||
|
||||
|
||||
@ -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>
|
||||
@ -141,7 +150,7 @@ namespace Plane.Copters
|
||||
}
|
||||
|
||||
|
||||
new public int sim_update_int
|
||||
new public int sim_update_int
|
||||
{
|
||||
get { return UPDATE_INTERVAL; }
|
||||
set
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
@ -207,17 +216,17 @@ namespace Plane.Copters
|
||||
// 持续计算并更新虚拟飞行器的状态。
|
||||
// Task.Run(async () =>
|
||||
|
||||
Task.Factory.StartNew(async () =>
|
||||
Task.Factory.StartNew(async () =>
|
||||
|
||||
{
|
||||
{
|
||||
while (_isRunning)
|
||||
{
|
||||
Update();
|
||||
await TaskUtils.Delay(UPDATE_INTERVAL).ConfigureAwait(false);
|
||||
}
|
||||
}
|
||||
, TaskCreationOptions.LongRunning
|
||||
);
|
||||
, TaskCreationOptions.LongRunning
|
||||
);
|
||||
|
||||
|
||||
++HeartbeatCount;
|
||||
@ -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;
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user