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>
/// 更新虚拟飞行器状态的时间间隔，单位为毫秒。 /// 更新虚拟飞行器状态的时间间隔，单位为毫秒。
/// </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; static private int UPDATE_INTERVAL_TEMP = 50;
/// <summary> /// <summary>
@ -35,7 +35,7 @@ namespace Plane.Copters
/// <summary> /// <summary>
/// 高速模式下，在一个更新间隔中的最大移动距离。 /// 高速模式下，在一个更新间隔中的最大移动距离。
/// </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> /// <summary>
/// FlyTo 的目标纬度。 /// FlyTo 的目标纬度。
@ -131,17 +131,26 @@ namespace Plane.Copters
private System.Drawing.ColorConverter rgbconverter; 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> /// <summary>
/// 使用 <see cref="SynchronizationContext.Current"/> 创建 <see cref="FakeCopter"/> 实例。 /// 使用 <see cref="SynchronizationContext.Current"/> 创建 <see cref="FakeCopter"/> 实例。
/// </summary> /// </summary>
public FakeCopter() : this(SynchronizationContext.Current) public FakeCopter() : this(SynchronizationContext.Current)
{ {
} }
new public int sim_update_int new public int sim_update_int
{ {
get { return UPDATE_INTERVAL; } get { return UPDATE_INTERVAL; }
set set
@ -151,9 +160,9 @@ namespace Plane.Copters
//快速模式最大移动距离 //快速模式最大移动距离
MAX_MOVE_IN_INTERVAL_FAST = MAX_MOVE_IN_INTERVAL * 4; 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); Set(nameof(sim_update_int), ref UPDATE_INTERVAL, value);
} }
} }
@ -207,19 +216,19 @@ namespace Plane.Copters
// 持续计算并更新虚拟飞行器的状态。 // 持续计算并更新虚拟飞行器的状态。
// Task.Run(async () => // Task.Run(async () =>
Task.Factory.StartNew(async () => Task.Factory.StartNew(async () =>
{ {
while (_isRunning) while (_isRunning)
{ {
Update(); Update();
await TaskUtils.Delay(UPDATE_INTERVAL).ConfigureAwait(false); await TaskUtils.Delay(UPDATE_INTERVAL).ConfigureAwait(false);
} }
} }
, TaskCreationOptions.LongRunning , TaskCreationOptions.LongRunning
); );
++HeartbeatCount; ++HeartbeatCount;
GpsFixType = GpsFixType.Fix3D; GpsFixType = GpsFixType.Fix3D;
GpsHdop = 1; GpsHdop = 1;
@ -281,6 +290,50 @@ namespace Plane.Copters
IsCheckingConnection = false; IsCheckingConnection = false;
return TaskUtils.CompletedTask; 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) 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; _takeOffTargetAltitude = (int)alt;
await TakeOffAsync().ConfigureAwait(false); await TakeOffAsync().ConfigureAwait(false);
} }
DateTime MissionStartTime;
public Task MissionStartAsync(int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat) 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; return TaskUtils.CompletedTask;
} }
@ -764,6 +821,49 @@ namespace Plane.Copters
case FlightMode.TOY: case FlightMode.TOY:
break; 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: default:
break; break;

170
PlaneGcsSdk_Shared/Copters/FlightMode.cs
View File

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