加入任务整体旋转
加入自动识别RTK航点到达精度为0.6米，碰撞精度为0.5米
This commit is contained in:
parent
86856b6e42
commit
c85a17a324
@ -56,20 +56,41 @@ namespace Plane.FormationCreator
|
||||
return s;
|
||||
}
|
||||
|
||||
/**
|
||||
* 求两经纬度距离(google maps源码中)
|
||||
*
|
||||
* @param lon1
|
||||
* 第一点的经度
|
||||
* @param lat1
|
||||
* 第一点的纬度
|
||||
* @param lon2
|
||||
* 第二点的经度
|
||||
* @param lat2
|
||||
* 第二点的纬度
|
||||
* @return 两点距离，单位km
|
||||
* */
|
||||
public static double GetDistanceTwo(double lon1, double lat1, double lon2,
|
||||
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源码中)
|
||||
*
|
||||
* @param lon1
|
||||
* 第一点的经度
|
||||
* @param lat1
|
||||
* 第一点的纬度
|
||||
* @param lon2
|
||||
* 第二点的经度
|
||||
* @param lat2
|
||||
* 第二点的纬度
|
||||
* @return 两点距离，单位km
|
||||
* */
|
||||
public static double GetDistanceTwo(double lon1, double lat1, double lon2,
|
||||
double lat2)
|
||||
{
|
||||
double radLat1 = rad(lat1);
|
||||
|
||||
@ -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,10 +62,14 @@ 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) < 2; // added by ZJF
|
||||
}
|
||||
// return copter.DistanceTo(targetLat, targetLng, targetAlt) < 0.6; //到达航点精度
|
||||
|
||||
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米
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -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++)
|
||||
|
||||
@ -680,8 +680,87 @@ 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 事件导致飞行器重新出现在地图上。
|
||||
|
||||
}));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
@ -77,7 +77,11 @@
|
||||
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}"/>
|
||||
|
||||
|
||||
|
||||
</StackPanel>
|
||||
|
||||
Loading…
Reference in New Issue
Block a user