group/Plane.Sdk3
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
pxzleo 2024-01-17 22:37:53 +08:00
parent f9814532f5
commit 9c2238479f
3 changed files with 1675 additions and 1542 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2921
PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs
View File

File diff suppressed because it is too large Load Diff

126
PlaneGcsSdk_Shared/Copters/FakeCopter.cs
View File

@ -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
@ -101,7 +101,7 @@ namespace Plane.Copters
// 目标点相对于开始位置的方向。
private double _direction;
private double _direction;
/// <summary>
/// FlyTo 的目标纬度。
@ -131,17 +131,26 @@ 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>
public FakeCopter() : this(SynchronizationContext.Current)
{
}
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,19 +216,19 @@ 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;
GpsFixType = GpsFixType.Fix3D;
GpsHdop = 1;
@ -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;

170
PlaneGcsSdk_Shared/Copters/FlightMode.cs
View File

@ -1,84 +1,86 @@
using System;
namespace Plane.Copters
{
#if PRIVATE
public
#else
internal
#endif
enum FlightMode
{
// 王海，20150608：不可将以下枚举项重命名。
STABILIZE = 0, // hold level position
ACRO = 1, // rate control // 王海, 20160205, 特技模式：http://copter.ardupilot.cn/wiki/acro-mode/
ALT_HOLD = 2, // AUTO control
AUTO = 3, // AUTO control
GUIDED = 4, // AUTO control
LOITER = 5, // Hold a single location
RTL = 6, // AUTO control
CIRCLE = 7,
POSITION = 8, // 王海, 20160205, 位置模式：http://copter.ardupilot.cn/wiki/POSITION-mode/
LAND = 9, // AUTO control
OF_LOITER = 10, // 王海, 20160205, 光流悬停模式：http://copter.ardupilot.cn/wiki/loiter-mode/ 底部。
TOY = 11
}
internal static class FightModeExtensions
{
public static string GetModeString(this FlightMode flightMode)
{
switch (flightMode)
{
case FlightMode.ALT_HOLD:
return "ALT HOLD";
case FlightMode.POSITION:
return "POS HOLD";
default:
return Enum.GetName(typeof(FlightMode), flightMode);
}
}
public static bool NeedGps(this FlightMode flightMode)
{
switch (flightMode)
{
case FlightMode.AUTO:
case FlightMode.GUIDED:
case FlightMode.LOITER:
case FlightMode.RTL:
case FlightMode.CIRCLE:
case FlightMode.LAND:
case FlightMode.POSITION:
default:
return true;
case FlightMode.STABILIZE:
case FlightMode.ACRO:
case FlightMode.ALT_HOLD:
case FlightMode.OF_LOITER:
case FlightMode.TOY:
return false;
}
}
internal static PlaneCopter.ac2modes ToAC2Mode(this FlightMode flightMode)
{
return (PlaneCopter.ac2modes)flightMode;
}
}
}
using System;
namespace Plane.Copters
{
#if PRIVATE
public
#else
internal
#endif
enum FlightMode
{
// 王海，20150608：不可将以下枚举项重命名。
STABILIZE = 0, // hold level position
ACRO = 1, // rate control // 王海, 20160205, 特技模式：http://copter.ardupilot.cn/wiki/acro-mode/
ALT_HOLD = 2, // AUTO control
AUTO = 3, // AUTO control
GUIDED = 4, // AUTO control
LOITER = 5, // Hold a single location
RTL = 6, // AUTO control
CIRCLE = 7,
POSITION = 8, // 王海, 20160205, 位置模式：http://copter.ardupilot.cn/wiki/POSITION-mode/
LAND = 9, // AUTO control
OF_LOITER = 10, // 王海, 20160205, 光流悬停模式：http://copter.ardupilot.cn/wiki/loiter-mode/ 底部。
TOY = 11,
EMERG_RTL=12, //紧急返航模式
}
internal static class FightModeExtensions
{
public static string GetModeString(this FlightMode flightMode)
{
switch (flightMode)
{
case FlightMode.ALT_HOLD:
return "ALT HOLD";
case FlightMode.POSITION:
return "POS HOLD";
default:
return Enum.GetName(typeof(FlightMode), flightMode);
}
}
public static bool NeedGps(this FlightMode flightMode)
{
switch (flightMode)
{
case FlightMode.AUTO:
case FlightMode.GUIDED:
case FlightMode.LOITER:
case FlightMode.RTL:
case FlightMode.CIRCLE:
case FlightMode.LAND:
case FlightMode.POSITION:
default:
return true;
case FlightMode.STABILIZE:
case FlightMode.ACRO:
case FlightMode.ALT_HOLD:
case FlightMode.OF_LOITER:
case FlightMode.TOY:
return false;
}
}
internal static PlaneCopter.ac2modes ToAC2Mode(this FlightMode flightMode)
{
return (PlaneCopter.ac2modes)flightMode;
}
}
}