去掉不用的任务类型，只保留起飞，飞向和降落
This commit is contained in:
parent
d7df82f667
commit
e3f4e557dd
@ -156,27 +156,11 @@ namespace Plane.FormationCreator.Formation
|
|||||||
case FlightTaskType.FlyTo:
|
case FlightTaskType.FlyTo:
|
||||||
await RunFlyToTaskAsync().ConfigureAwait(false);
|
await RunFlyToTaskAsync().ConfigureAwait(false);
|
||||||
break;
|
break;
|
||||||
case FlightTaskType.Turn:
|
|
||||||
await RunTurnTaskAsync().ConfigureAwait(false);
|
|
||||||
break;
|
|
||||||
case FlightTaskType.Circle:
|
|
||||||
await RunCircleTaskAsync().ConfigureAwait(false);
|
|
||||||
break;
|
|
||||||
case FlightTaskType.SimpleCircle:
|
|
||||||
await RunSimpleCircleTaskAsync().ConfigureAwait(false);
|
|
||||||
break;
|
|
||||||
case FlightTaskType.Loiter:
|
|
||||||
await RunLoiterTimeTaskAsync().ConfigureAwait(false);
|
|
||||||
break;
|
|
||||||
case FlightTaskType.TakeOff:
|
case FlightTaskType.TakeOff:
|
||||||
//多架同时起飞
|
//多架同时起飞
|
||||||
//await MutilRunTakeOffTaskAsync().ConfigureAwait(false);
|
//await MutilRunTakeOffTaskAsync().ConfigureAwait(false);
|
||||||
await NewMutilRunTakeOffTaskAsync().ConfigureAwait(false);
|
await NewMutilRunTakeOffTaskAsync().ConfigureAwait(false);
|
||||||
break;
|
break;
|
||||||
case FlightTaskType.ReturnToLand: // Added by ZJF
|
|
||||||
//多架同时返航
|
|
||||||
await MutilRunReturnToLandTaskAsync().ConfigureAwait(false);
|
|
||||||
break;
|
|
||||||
case FlightTaskType.Land:
|
case FlightTaskType.Land:
|
||||||
await MutilLandTaskAsync().ConfigureAwait(false);
|
await MutilLandTaskAsync().ConfigureAwait(false);
|
||||||
break;
|
break;
|
||||||
@ -189,12 +173,6 @@ namespace Plane.FormationCreator.Formation
|
|||||||
public enum FlightTaskType
|
public enum FlightTaskType
|
||||||
{
|
{
|
||||||
FlyTo = 0,
|
FlyTo = 0,
|
||||||
Turn = 1,
|
|
||||||
Circle = 2,
|
|
||||||
Loiter = 3, // 加入悬停时间选项
|
|
||||||
// LEDAction = 4,
|
|
||||||
ReturnToLand = 4, // added by ZJF. 20160315
|
|
||||||
SimpleCircle = 5,
|
|
||||||
Land = 6,
|
Land = 6,
|
||||||
TakeOff = 100
|
TakeOff = 100
|
||||||
}
|
}
|
||||||
|
|||||||
@ -398,8 +398,6 @@ namespace Plane.FormationCreator.Formation
|
|||||||
|
|
||||||
|
|
||||||
var newSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForFlyToTask(copter, targetLatLng.Item1, targetLatLng.Item2, preSingleCopterInfo.TargetAlt);
|
var newSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForFlyToTask(copter, targetLatLng.Item1, targetLatLng.Item2, preSingleCopterInfo.TargetAlt);
|
||||||
newSingleCopterInfo.TargetHeading = lastSingleCopterInfo.TargetHeading;
|
|
||||||
newSingleCopterInfo.CenterDirectionDeg = lastSingleCopterInfo.TargetHeading;
|
|
||||||
newTask.SingleCopterInfos.Add(newSingleCopterInfo);
|
newTask.SingleCopterInfos.Add(newSingleCopterInfo);
|
||||||
|
|
||||||
}
|
}
|
||||||
@ -603,16 +601,7 @@ namespace Plane.FormationCreator.Formation
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// 将降落的阶段标志位都置位0
|
|
||||||
if (Tasks[i].TaskType == FlightTaskType.ReturnToLand)
|
|
||||||
{
|
|
||||||
var infos = Tasks[i].SingleCopterInfos;
|
|
||||||
for (int j = 0; j < infos.Count; j++)
|
|
||||||
{
|
|
||||||
var info = infos[j];
|
|
||||||
info.RTLStage = 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@ -884,88 +873,6 @@ namespace Plane.FormationCreator.Formation
|
|||||||
RaiseTaskAdded(lastTask, newTask);
|
RaiseTaskAdded(lastTask, newTask);
|
||||||
}
|
}
|
||||||
|
|
||||||
private void RestoreTurnTask(dynamic singleCopterInfos)
|
|
||||||
{
|
|
||||||
var copters = _copterManager.Copters;
|
|
||||||
if (!copters.Any()) return;
|
|
||||||
AppEx.Current.AppMode = AppMode.ModifyingTask;
|
|
||||||
var lastTask = Tasks.LastOrDefault();
|
|
||||||
var newTask = new FlightTask(FlightTaskType.Turn);
|
|
||||||
// TODO: 林俊清, 20150806, 处理实际飞行器数目与记录中数目不一致的情况。
|
|
||||||
for (int i = 0; i < copters.Count && i < singleCopterInfos.Count; i++)
|
|
||||||
{
|
|
||||||
var copter = copters[i];
|
|
||||||
var singleCopterInfoObj = singleCopterInfos[i];
|
|
||||||
var newSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForTurnTask(copter, new LatLng((double)singleCopterInfoObj.latOffset, (double)singleCopterInfoObj.lngOffset), (float)singleCopterInfoObj.targetAlt, (short)singleCopterInfoObj.targetHeading);
|
|
||||||
newTask.SingleCopterInfos.Add(newSingleCopterInfo);
|
|
||||||
}
|
|
||||||
Tasks.Add(newTask);
|
|
||||||
RaiseTaskAdded(lastTask, newTask);
|
|
||||||
SelectedTask = newTask;
|
|
||||||
SelectedTaskIndex = Tasks.Count - 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
private void RestoreCircleTask(dynamic singleCopterInfos)
|
|
||||||
{
|
|
||||||
var copters = _copterManager.Copters;
|
|
||||||
if (!copters.Any()) return;
|
|
||||||
AppEx.Current.AppMode = AppMode.ModifyingTask;
|
|
||||||
var lastTask = Tasks.LastOrDefault();
|
|
||||||
var newTask = new FlightTask(FlightTaskType.Circle);
|
|
||||||
// TODO: 林俊清, 20150806, 处理实际飞行器数目与记录中数目不一致的情况。
|
|
||||||
for (int i = 0; i < copters.Count && i < singleCopterInfos.Count; i++)
|
|
||||||
{
|
|
||||||
var copter = copters[i];
|
|
||||||
var singleCopterInfoObj = singleCopterInfos[i];
|
|
||||||
var newSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForCircleTask(copter, new LatLng((double)singleCopterInfoObj.latOffset, (double)singleCopterInfoObj.lngOffset), (float)singleCopterInfoObj.targetAlt, (short)singleCopterInfoObj.centerDirectionDeg, (int)singleCopterInfoObj.radius, (int)singleCopterInfoObj.rate, (int)singleCopterInfoObj.turns, (ushort)singleCopterInfoObj.channel3);
|
|
||||||
newTask.SingleCopterInfos.Add(newSingleCopterInfo);
|
|
||||||
}
|
|
||||||
Tasks.Add(newTask);
|
|
||||||
RaiseTaskAdded(lastTask, newTask);
|
|
||||||
SelectedTask = newTask;
|
|
||||||
SelectedTaskIndex = Tasks.Count - 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
private void RestoreSimpleCircleTask(dynamic singleCopterInfos)
|
|
||||||
{
|
|
||||||
var copters = _copterManager.Copters;
|
|
||||||
if (!copters.Any()) return;
|
|
||||||
AppEx.Current.AppMode = AppMode.ModifyingTask;
|
|
||||||
var lastTask = Tasks.LastOrDefault();
|
|
||||||
var newTask = new FlightTask(FlightTaskType.SimpleCircle);
|
|
||||||
// TODO: 林俊清, 20150806, 处理实际飞行器数目与记录中数目不一致的情况。
|
|
||||||
for (int i = 0; i < copters.Count && i < singleCopterInfos.Count; i++)
|
|
||||||
{
|
|
||||||
var copter = copters[i];
|
|
||||||
var singleCopterInfoObj = singleCopterInfos[i];
|
|
||||||
var newSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForSimpleCircleTask(copter, new LatLng((double)singleCopterInfoObj.latOffset, (double)singleCopterInfoObj.lngOffset), (float)singleCopterInfoObj.targetAlt, (int)singleCopterInfoObj.rate, (int)singleCopterInfoObj.turns, (ushort)singleCopterInfoObj.channel3);
|
|
||||||
newTask.SingleCopterInfos.Add(newSingleCopterInfo);
|
|
||||||
}
|
|
||||||
Tasks.Add(newTask);
|
|
||||||
RaiseTaskAdded(lastTask, newTask);
|
|
||||||
SelectedTask = newTask;
|
|
||||||
SelectedTaskIndex = Tasks.Count - 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
// added by ZJF
|
|
||||||
private void RestoreReturnToLandTask(IEnumerable<ICopter> copters,int retnumber,int rtlalt)
|
|
||||||
{
|
|
||||||
if (copters == null || !copters.Any()) return;
|
|
||||||
AppEx.Current.AppMode = AppMode.ModifyingTask;
|
|
||||||
var lastTask = Tasks.LastOrDefault();
|
|
||||||
|
|
||||||
var RTLTask = new FlightTask(FlightTaskType.ReturnToLand);
|
|
||||||
RTLTask.RetNumAttr = retnumber;
|
|
||||||
RTLTask.RTLAlt = rtlalt;
|
|
||||||
foreach (var copter in copters)
|
|
||||||
{
|
|
||||||
var newSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForReturnToLandTask(copter, targetAlt:10);
|
|
||||||
RTLTask.SingleCopterInfos.Add(newSingleCopterInfo);
|
|
||||||
}
|
|
||||||
Tasks.Add(RTLTask);
|
|
||||||
RaiseTaskAdded(lastTask, RTLTask);
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
private void RestoreTakeOffTask(byte takeOffTime, dynamic singleCopterInfos)
|
private void RestoreTakeOffTask(byte takeOffTime, dynamic singleCopterInfos)
|
||||||
{
|
{
|
||||||
@ -1015,43 +922,7 @@ namespace Plane.FormationCreator.Formation
|
|||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// added by ZJF
|
|
||||||
private void RestoreLoiterTimeTask(float LoiteTimeTmp, bool flashCheck, float flashCheckPeriod,
|
|
||||||
bool oneByOneCheck, float oneByOneCheckPeriod, string flashNameArray, string flashIndexArray,
|
|
||||||
bool ChangeYaw, float HeadYaw,
|
|
||||||
dynamic singleCopterInfos)
|
|
||||||
{
|
|
||||||
var copters = _copterManager.Copters;
|
|
||||||
if (copters == null || !copters.Any()) return;
|
|
||||||
AppEx.Current.AppMode = AppMode.ModifyingTask;
|
|
||||||
var lastTask = Tasks.LastOrDefault();
|
|
||||||
|
|
||||||
var loiterTimeTask = new FlightTask(FlightTaskType.Loiter)
|
|
||||||
{
|
|
||||||
LoiterTimeAttr = LoiteTimeTmp
|
|
||||||
};
|
|
||||||
loiterTimeTask.flashAttr = flashCheck;
|
|
||||||
loiterTimeTask.flashPeriodAttr = flashCheckPeriod;
|
|
||||||
loiterTimeTask.oneByOneAttr = oneByOneCheck;
|
|
||||||
loiterTimeTask.oneByOnePeriodAttr = oneByOneCheckPeriod;
|
|
||||||
loiterTimeTask.flashCopterNameArray = flashNameArray;
|
|
||||||
loiterTimeTask.flashCopterIndexArray = flashIndexArray;
|
|
||||||
loiterTimeTask.ChangeYaw = ChangeYaw;
|
|
||||||
loiterTimeTask.HeadYaw = HeadYaw;
|
|
||||||
|
|
||||||
// foreach (var copter in copters)
|
|
||||||
for (int i = 0; i < copters.Count; i++)
|
|
||||||
{
|
|
||||||
var copter = copters[i];
|
|
||||||
var singleCopterInfoObj = singleCopterInfos[i];
|
|
||||||
var newSingleCopterInfo = FlightTaskSingleCopterInfo.CreateForLoiterTimeTask(copter, new LatLng((double)singleCopterInfoObj.latOffset, (double)singleCopterInfoObj.lngOffset), (float)singleCopterInfoObj.targetAlt);
|
|
||||||
loiterTimeTask.SingleCopterInfos.Add(newSingleCopterInfo);
|
|
||||||
}
|
|
||||||
Tasks.Add(loiterTimeTask);
|
|
||||||
RaiseTaskAdded(lastTask, loiterTimeTask);
|
|
||||||
SelectedTask = loiterTimeTask;
|
|
||||||
SelectedTaskIndex = Tasks.Count - 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
//导出任务 (单位：米)
|
//导出任务 (单位：米)
|
||||||
public IEnumerable<object> ExportTasksToMeter()
|
public IEnumerable<object> ExportTasksToMeter()
|
||||||
@ -1101,95 +972,8 @@ namespace Plane.FormationCreator.Formation
|
|||||||
};
|
};
|
||||||
})
|
})
|
||||||
};
|
};
|
||||||
case FlightTaskType.Turn:
|
|
||||||
return new
|
|
||||||
{
|
|
||||||
type = type,
|
|
||||||
singleCopterInfos = task.SingleCopterInfos.Select(info =>
|
|
||||||
{
|
|
||||||
var offset = info.LatLngOffset;
|
|
||||||
return new
|
|
||||||
{
|
|
||||||
latOffset = offset.Lat,
|
|
||||||
lngOffset = offset.Lng,
|
|
||||||
targetAlt = info.TargetAlt,
|
|
||||||
targetHeading = info.TargetHeading
|
|
||||||
};
|
|
||||||
})
|
|
||||||
};
|
|
||||||
case FlightTaskType.Loiter: // 导出悬停任务
|
|
||||||
return new
|
|
||||||
{
|
|
||||||
type = type,
|
|
||||||
loiterTimeAttr = task.LoiterTimeAttr,
|
|
||||||
flashCheck = task.flashAttr,
|
|
||||||
flashCheckPeriod = task.flashPeriodAttr,
|
|
||||||
oneByOneCheck = task.oneByOneAttr,
|
|
||||||
oneByOneCheckPeriod = task.oneByOnePeriodAttr,
|
|
||||||
flashNameArray = task.flashCopterNameArray,
|
|
||||||
flashIndexArray = task.flashCopterIndexArray,
|
|
||||||
ChangeYaw = task.ChangeYaw,
|
|
||||||
HeadYaw = task.HeadYaw,
|
|
||||||
|
|
||||||
|
|
||||||
singleCopterInfos = task.SingleCopterInfos.Select(info =>
|
|
||||||
{
|
|
||||||
var offset = info.LatLngOffset;
|
|
||||||
return new
|
|
||||||
{
|
|
||||||
latOffset = offset.Lat,
|
|
||||||
lngOffset = offset.Lng,
|
|
||||||
targetAlt = info.TargetAlt
|
|
||||||
};
|
|
||||||
})
|
|
||||||
};
|
|
||||||
case FlightTaskType.Circle:
|
|
||||||
return new
|
|
||||||
{
|
|
||||||
type = type,
|
|
||||||
singleCopterInfos = task.SingleCopterInfos.Select(info =>
|
|
||||||
{
|
|
||||||
var offset = info.LatLngOffset;
|
|
||||||
return new
|
|
||||||
{
|
|
||||||
latOffset = offset.Lat,
|
|
||||||
lngOffset = offset.Lng,
|
|
||||||
targetAlt = info.TargetAlt,
|
|
||||||
centerDirectionDeg = info.CenterDirectionDeg,
|
|
||||||
radius = info.Radius,
|
|
||||||
rate = info.Rate,
|
|
||||||
turns = info.Turns,
|
|
||||||
channel3 = info.Channel3
|
|
||||||
};
|
|
||||||
})
|
|
||||||
};
|
|
||||||
case FlightTaskType.SimpleCircle:
|
|
||||||
return new
|
|
||||||
{
|
|
||||||
type = type,
|
|
||||||
singleCopterInfos = task.SingleCopterInfos.Select(info =>
|
|
||||||
{
|
|
||||||
var offset = info.LatLngOffset;
|
|
||||||
return new
|
|
||||||
{
|
|
||||||
latOffset = offset.Lat,
|
|
||||||
lngOffset = offset.Lng,
|
|
||||||
targetAlt = info.TargetAlt,
|
|
||||||
rate = info.Rate,
|
|
||||||
turns = info.Turns,
|
|
||||||
channel3 = info.Channel3
|
|
||||||
};
|
|
||||||
})
|
|
||||||
};
|
|
||||||
case FlightTaskType.ReturnToLand: // added by ZJF
|
|
||||||
return new
|
|
||||||
{
|
|
||||||
type = type,
|
|
||||||
rtlalt= task.RTLAlt,
|
|
||||||
retnumber = task.RetNumAttr
|
|
||||||
|
|
||||||
};
|
|
||||||
|
|
||||||
case FlightTaskType.Land:
|
case FlightTaskType.Land:
|
||||||
{
|
{
|
||||||
return new
|
return new
|
||||||
@ -1351,95 +1135,8 @@ namespace Plane.FormationCreator.Formation
|
|||||||
};
|
};
|
||||||
})
|
})
|
||||||
};
|
};
|
||||||
case FlightTaskType.Turn:
|
|
||||||
return new
|
|
||||||
{
|
|
||||||
type = type,
|
|
||||||
singleCopterInfos = task.SingleCopterInfos.Select(info =>
|
|
||||||
{
|
|
||||||
var offset = info.LatLngOffset;
|
|
||||||
return new
|
|
||||||
{
|
|
||||||
latOffset = offset.Lat,
|
|
||||||
lngOffset = offset.Lng,
|
|
||||||
targetAlt = info.TargetAlt,
|
|
||||||
targetHeading = info.TargetHeading
|
|
||||||
};
|
|
||||||
})
|
|
||||||
};
|
|
||||||
case FlightTaskType.Loiter: // 导出悬停任务
|
|
||||||
return new
|
|
||||||
{
|
|
||||||
type = type,
|
|
||||||
loiterTimeAttr = task.LoiterTimeAttr,
|
|
||||||
flashCheck = task.flashAttr,
|
|
||||||
flashCheckPeriod = task.flashPeriodAttr,
|
|
||||||
oneByOneCheck = task.oneByOneAttr,
|
|
||||||
oneByOneCheckPeriod = task.oneByOnePeriodAttr,
|
|
||||||
flashNameArray = task.flashCopterNameArray,
|
|
||||||
flashIndexArray = task.flashCopterIndexArray,
|
|
||||||
ChangeYaw = task.ChangeYaw,
|
|
||||||
HeadYaw = task.HeadYaw,
|
|
||||||
|
|
||||||
|
|
||||||
singleCopterInfos = task.SingleCopterInfos.Select(info =>
|
|
||||||
{
|
|
||||||
var offset = info.LatLngOffset;
|
|
||||||
return new
|
|
||||||
{
|
|
||||||
latOffset = offset.Lat,
|
|
||||||
lngOffset = offset.Lng,
|
|
||||||
targetAlt = info.TargetAlt
|
|
||||||
};
|
|
||||||
})
|
|
||||||
};
|
|
||||||
case FlightTaskType.Circle:
|
|
||||||
return new
|
|
||||||
{
|
|
||||||
type = type,
|
|
||||||
singleCopterInfos = task.SingleCopterInfos.Select(info =>
|
|
||||||
{
|
|
||||||
var offset = info.LatLngOffset;
|
|
||||||
return new
|
|
||||||
{
|
|
||||||
latOffset = offset.Lat,
|
|
||||||
lngOffset = offset.Lng,
|
|
||||||
targetAlt = info.TargetAlt,
|
|
||||||
centerDirectionDeg = info.CenterDirectionDeg,
|
|
||||||
radius = info.Radius,
|
|
||||||
rate = info.Rate,
|
|
||||||
turns = info.Turns,
|
|
||||||
channel3 = info.Channel3
|
|
||||||
};
|
|
||||||
})
|
|
||||||
};
|
|
||||||
case FlightTaskType.SimpleCircle:
|
|
||||||
return new
|
|
||||||
{
|
|
||||||
type = type,
|
|
||||||
singleCopterInfos = task.SingleCopterInfos.Select(info =>
|
|
||||||
{
|
|
||||||
var offset = info.LatLngOffset;
|
|
||||||
return new
|
|
||||||
{
|
|
||||||
latOffset = offset.Lat,
|
|
||||||
lngOffset = offset.Lng,
|
|
||||||
targetAlt = info.TargetAlt,
|
|
||||||
rate = info.Rate,
|
|
||||||
turns = info.Turns,
|
|
||||||
channel3 = info.Channel3
|
|
||||||
};
|
|
||||||
})
|
|
||||||
};
|
|
||||||
case FlightTaskType.ReturnToLand: // added by ZJF
|
|
||||||
return new
|
|
||||||
{
|
|
||||||
type = type,
|
|
||||||
rtlalt= task.RTLAlt,
|
|
||||||
retnumber = task.RetNumAttr
|
|
||||||
|
|
||||||
};
|
|
||||||
|
|
||||||
case FlightTaskType.Land:
|
case FlightTaskType.Land:
|
||||||
{
|
{
|
||||||
return new
|
return new
|
||||||
@ -1488,24 +1185,6 @@ namespace Plane.FormationCreator.Formation
|
|||||||
case FlightTaskType.FlyTo:
|
case FlightTaskType.FlyTo:
|
||||||
RestoreFlyToTask((bool)task.staggerRoutes, (int)task.flytoTime, (int)task.loiterTime, (string)task.taskname, task.singleCopterInfos, isMeter);
|
RestoreFlyToTask((bool)task.staggerRoutes, (int)task.flytoTime, (int)task.loiterTime, (string)task.taskname, task.singleCopterInfos, isMeter);
|
||||||
break;
|
break;
|
||||||
case FlightTaskType.Turn:
|
|
||||||
RestoreTurnTask(task.singleCopterInfos);
|
|
||||||
break;
|
|
||||||
case FlightTaskType.Circle:
|
|
||||||
RestoreCircleTask(task.singleCopterInfos);
|
|
||||||
break;
|
|
||||||
case FlightTaskType.Loiter:
|
|
||||||
RestoreLoiterTimeTask((float)task.loiterTimeAttr, (bool)task.flashCheck, (float)task.flashCheckPeriod,
|
|
||||||
(bool)task.oneByOneCheck, (float)task.oneByOneCheckPeriod, (string)task.flashNameArray,
|
|
||||||
(string)task.flashIndexArray, (bool)task.ChangeYaw, (float)task.HeadYaw, task.singleCopterInfos);
|
|
||||||
|
|
||||||
break;
|
|
||||||
case FlightTaskType.SimpleCircle:
|
|
||||||
RestoreSimpleCircleTask(task.singleCopterInfos);
|
|
||||||
break;
|
|
||||||
case FlightTaskType.ReturnToLand:
|
|
||||||
RestoreReturnToLandTask(copters, (int)task.retnumber, (int)task.rtlalt);
|
|
||||||
break;
|
|
||||||
case FlightTaskType.Land:
|
case FlightTaskType.Land:
|
||||||
RestoreLandTask(task.singleCopterInfos);
|
RestoreLandTask(task.singleCopterInfos);
|
||||||
break;
|
break;
|
||||||
@ -1843,24 +1522,7 @@ namespace Plane.FormationCreator.Formation
|
|||||||
case FlightTaskType.FlyTo:
|
case FlightTaskType.FlyTo:
|
||||||
RestoreFlyToTask((bool)task.staggerRoutes, (int)task.flytoTime, (int)task.loiterTime, (string)task.taskname, task.singleCopterInfos, isMeter);
|
RestoreFlyToTask((bool)task.staggerRoutes, (int)task.flytoTime, (int)task.loiterTime, (string)task.taskname, task.singleCopterInfos, isMeter);
|
||||||
break;
|
break;
|
||||||
case FlightTaskType.Turn:
|
|
||||||
RestoreTurnTask(task.singleCopterInfos);
|
|
||||||
break;
|
|
||||||
case FlightTaskType.Circle:
|
|
||||||
RestoreCircleTask(task.singleCopterInfos);
|
|
||||||
break;
|
|
||||||
case FlightTaskType.Loiter:
|
|
||||||
RestoreLoiterTimeTask( (float)task.loiterTimeAttr, (bool)task.flashCheck, (float)task.flashCheckPeriod,
|
|
||||||
(bool)task.oneByOneCheck, (float)task.oneByOneCheckPeriod, (string)task.flashNameArray,
|
|
||||||
(string)task.flashIndexArray, (bool)task.ChangeYaw, (float)task.HeadYaw, task.singleCopterInfos);
|
|
||||||
|
|
||||||
break;
|
|
||||||
case FlightTaskType.SimpleCircle:
|
|
||||||
RestoreSimpleCircleTask(task.singleCopterInfos);
|
|
||||||
break;
|
|
||||||
case FlightTaskType.ReturnToLand:
|
|
||||||
RestoreReturnToLandTask(copters, (int)task.retnumber, (int)task.rtlalt);
|
|
||||||
break;
|
|
||||||
case FlightTaskType.Land:
|
case FlightTaskType.Land:
|
||||||
RestoreLandTask(task.singleCopterInfos);
|
RestoreLandTask(task.singleCopterInfos);
|
||||||
break;
|
break;
|
||||||
@ -1883,39 +1545,7 @@ namespace Plane.FormationCreator.Formation
|
|||||||
this.SelectedTaskIndex = taskIndex;
|
this.SelectedTaskIndex = taskIndex;
|
||||||
this.SelectedTask = Tasks[taskIndex];
|
this.SelectedTask = Tasks[taskIndex];
|
||||||
|
|
||||||
// 在悬停任务时，左键waypoint为选中
|
|
||||||
// 在悬停任务模式，且LED控制被选中时
|
|
||||||
if ((this.SelectedTask.TaskType == FlightTaskType.Loiter) && (this.SelectedTask.flashAttr || this.SelectedTask.oneByOneAttr))
|
|
||||||
{
|
|
||||||
string copterNameStr = this.SelectedTask.flashCopterNameArray;
|
|
||||||
|
|
||||||
string[] copterArray = copterNameStr.Split(',');
|
|
||||||
bool flag = false;
|
|
||||||
for (int i = 0; i < copterArray.Length; i++)
|
|
||||||
{
|
|
||||||
if (copterArray[i].Equals(""))
|
|
||||||
continue;
|
|
||||||
if (copterArray[i].Equals(copter.Name))
|
|
||||||
{
|
|
||||||
flag = true;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
if (!flag)
|
|
||||||
{
|
|
||||||
if (copterArray[0].Equals(""))
|
|
||||||
{
|
|
||||||
this.SelectedTask.flashCopterNameArray += copter.Name;
|
|
||||||
SelectedTask.flashCopterIndexArray += Name2Index(copter.Name);
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
this.SelectedTask.flashCopterNameArray += "," + copter.Name;
|
|
||||||
SelectedTask.flashCopterIndexArray += "," + Name2Index(copter.Name);
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -1927,12 +1557,6 @@ namespace Plane.FormationCreator.Formation
|
|||||||
this.SelectedTaskIndex = taskIndex;
|
this.SelectedTaskIndex = taskIndex;
|
||||||
this.SelectedTask = Tasks[taskIndex];
|
this.SelectedTask = Tasks[taskIndex];
|
||||||
|
|
||||||
// 在悬停任务模式，且LED控制被选中时
|
|
||||||
if ((this.SelectedTask.TaskType == FlightTaskType.Loiter) && (this.SelectedTask.flashAttr || this.SelectedTask.oneByOneAttr))
|
|
||||||
{
|
|
||||||
this.SelectedTask.flashCopterNameArray = DeleteSelectedName(this.SelectedTask.flashCopterNameArray, copter.Name);
|
|
||||||
this.SelectedTask.flashCopterIndexArray = DeleteSelectedName(this.SelectedTask.flashCopterIndexArray, Name2Index(copter.Name).ToString());
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
private int Name2Index(string name) // 获取指定copter名字对应的序号
|
private int Name2Index(string name) // 获取指定copter名字对应的序号
|
||||||
@ -2132,11 +1756,7 @@ namespace Plane.FormationCreator.Formation
|
|||||||
{
|
{
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
// 在目前的起飞、降落逻辑，如果起飞或最后一个任务是返航降落，不要检查两架飞机距离过近. added by ZJF
|
|
||||||
bool RTLFlag = ( CurrentRunningTaskIndex == Tasks.Count()-1 ) && ( CurrentRunningTask.TaskType == FlightTaskType.ReturnToLand );
|
|
||||||
// bool RTLFlag = false;
|
|
||||||
//if ( (CurrentRunningTaskIndex != 0) && !RTLFlag)
|
|
||||||
{
|
|
||||||
double distance;
|
double distance;
|
||||||
foreach (var copter in _copterManager.Copters)
|
foreach (var copter in _copterManager.Copters)
|
||||||
{
|
{
|
||||||
@ -2150,7 +1770,7 @@ namespace Plane.FormationCreator.Formation
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
catch // 林俊清, 20151102, 通常是“集合已修改”异常。
|
catch // 林俊清, 20151102, 通常是“集合已修改”异常。
|
||||||
{
|
{
|
||||||
|
|||||||
@ -1,68 +0,0 @@
|
|||||||
using System;
|
|
||||||
using System.Collections.Generic;
|
|
||||||
using System.Linq;
|
|
||||||
using System.Text;
|
|
||||||
using System.Threading.Tasks;
|
|
||||||
using Plane.Copters;
|
|
||||||
|
|
||||||
namespace Plane.FormationCreator.Formation
|
|
||||||
{
|
|
||||||
public partial class FlightTaskSingleCopterInfo
|
|
||||||
{
|
|
||||||
public static FlightTaskSingleCopterInfo CreateForCircleTask(ICopter copter, LatLng latLngOffset, float targetAlt, short centerDirectionDeg, int radius, int rate, int turns, ushort channel3)
|
|
||||||
{
|
|
||||||
var info = new FlightTaskSingleCopterInfo(copter)
|
|
||||||
{
|
|
||||||
LatLngOffset = latLngOffset,
|
|
||||||
TargetAlt = targetAlt,
|
|
||||||
CenterDirectionDeg = centerDirectionDeg,
|
|
||||||
Radius = radius,
|
|
||||||
Rate = rate,
|
|
||||||
Turns = turns,
|
|
||||||
Channel3 = channel3
|
|
||||||
};
|
|
||||||
return info;
|
|
||||||
}
|
|
||||||
|
|
||||||
private short _CenterDirectionDeg;
|
|
||||||
public short CenterDirectionDeg
|
|
||||||
{
|
|
||||||
get { return _CenterDirectionDeg; }
|
|
||||||
set { Set(nameof(CenterDirectionDeg), ref _CenterDirectionDeg, value); }
|
|
||||||
}
|
|
||||||
|
|
||||||
private int _Radius = 1000;
|
|
||||||
/// <summary>
|
|
||||||
/// 圆的半径，单位为厘米。
|
|
||||||
/// </summary>
|
|
||||||
public int Radius
|
|
||||||
{
|
|
||||||
get { return _Radius; }
|
|
||||||
set { Set(nameof(Radius), ref _Radius, value); }
|
|
||||||
}
|
|
||||||
|
|
||||||
private int _Rate = 20;
|
|
||||||
/// <summary>
|
|
||||||
/// 转圈的角速度，单位为 deg/s，范围为 -90 到 90。
|
|
||||||
/// </summary>
|
|
||||||
public int Rate
|
|
||||||
{
|
|
||||||
get { return _Rate; }
|
|
||||||
set { Set(nameof(Rate), ref _Rate, value); }
|
|
||||||
}
|
|
||||||
|
|
||||||
private int _Turns = 1;
|
|
||||||
public int Turns
|
|
||||||
{
|
|
||||||
get { return _Turns; }
|
|
||||||
set { Set(nameof(Turns), ref _Turns, value); }
|
|
||||||
}
|
|
||||||
|
|
||||||
private ushort _Channel3 = 1500;
|
|
||||||
public ushort Channel3
|
|
||||||
{
|
|
||||||
get { return _Channel3; }
|
|
||||||
set { Set(nameof(Channel3), ref _Channel3, value); }
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,22 +0,0 @@
|
|||||||
using System;
|
|
||||||
using System.Collections.Generic;
|
|
||||||
using System.Linq;
|
|
||||||
using System.Text;
|
|
||||||
using System.Threading.Tasks;
|
|
||||||
using Plane.Copters;
|
|
||||||
|
|
||||||
namespace Plane.FormationCreator.Formation
|
|
||||||
{
|
|
||||||
public partial class FlightTaskSingleCopterInfo
|
|
||||||
{
|
|
||||||
public static FlightTaskSingleCopterInfo CreateForLoiterTimeTask(ICopter copter, LatLng latLngOffset, float targetAlt)
|
|
||||||
{
|
|
||||||
var info = new FlightTaskSingleCopterInfo(copter)
|
|
||||||
{
|
|
||||||
LatLngOffset = latLngOffset,
|
|
||||||
TargetAlt = targetAlt
|
|
||||||
};
|
|
||||||
return info;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,22 +0,0 @@
|
|||||||
using System;
|
|
||||||
using System.Collections.Generic;
|
|
||||||
using System.Linq;
|
|
||||||
using System.Text;
|
|
||||||
using System.Threading.Tasks;
|
|
||||||
using Plane.Copters;
|
|
||||||
|
|
||||||
namespace Plane.FormationCreator.Formation
|
|
||||||
{
|
|
||||||
public partial class FlightTaskSingleCopterInfo
|
|
||||||
{
|
|
||||||
// This file is added by ZJF
|
|
||||||
public static FlightTaskSingleCopterInfo CreateForReturnToLandTask(ICopter copter, float targetAlt)
|
|
||||||
{
|
|
||||||
var info = new FlightTaskSingleCopterInfo(copter)
|
|
||||||
{
|
|
||||||
TargetAlt = targetAlt
|
|
||||||
};
|
|
||||||
return info;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,25 +0,0 @@
|
|||||||
using System;
|
|
||||||
using System.Collections.Generic;
|
|
||||||
using System.Linq;
|
|
||||||
using System.Text;
|
|
||||||
using System.Threading.Tasks;
|
|
||||||
using Plane.Copters;
|
|
||||||
|
|
||||||
namespace Plane.FormationCreator.Formation
|
|
||||||
{
|
|
||||||
public partial class FlightTaskSingleCopterInfo
|
|
||||||
{
|
|
||||||
public static FlightTaskSingleCopterInfo CreateForSimpleCircleTask(ICopter copter, LatLng latLngOffset, float targetAlt, int rate, int turns, ushort channel3)
|
|
||||||
{
|
|
||||||
var info = new FlightTaskSingleCopterInfo(copter)
|
|
||||||
{
|
|
||||||
LatLngOffset = latLngOffset,
|
|
||||||
TargetAlt = targetAlt,
|
|
||||||
Rate = rate,
|
|
||||||
Turns = turns,
|
|
||||||
Channel3 = channel3
|
|
||||||
};
|
|
||||||
return info;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,30 +0,0 @@
|
|||||||
using System;
|
|
||||||
using System.Collections.Generic;
|
|
||||||
using System.Linq;
|
|
||||||
using System.Text;
|
|
||||||
using System.Threading.Tasks;
|
|
||||||
using Plane.Copters;
|
|
||||||
|
|
||||||
namespace Plane.FormationCreator.Formation
|
|
||||||
{
|
|
||||||
public partial class FlightTaskSingleCopterInfo
|
|
||||||
{
|
|
||||||
public static FlightTaskSingleCopterInfo CreateForTurnTask(ICopter copter, LatLng latLngOffset, float targetAlt, short targetHeading)
|
|
||||||
{
|
|
||||||
var info = new FlightTaskSingleCopterInfo(copter)
|
|
||||||
{
|
|
||||||
LatLngOffset = latLngOffset,
|
|
||||||
TargetAlt = targetAlt,
|
|
||||||
TargetHeading = targetHeading
|
|
||||||
};
|
|
||||||
return info;
|
|
||||||
}
|
|
||||||
|
|
||||||
private short _TargetHeading;
|
|
||||||
public short TargetHeading
|
|
||||||
{
|
|
||||||
get { return _TargetHeading; }
|
|
||||||
set { Set(nameof(TargetHeading), ref _TargetHeading, value); }
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,137 +0,0 @@
|
|||||||
using Plane.Copters;
|
|
||||||
using Plane.Geography;
|
|
||||||
using Plane.Logging;
|
|
||||||
using System;
|
|
||||||
using System.Collections.Generic;
|
|
||||||
using System.Linq;
|
|
||||||
using System.Text;
|
|
||||||
using System.Threading.Tasks;
|
|
||||||
|
|
||||||
namespace Plane.FormationCreator.Formation
|
|
||||||
{
|
|
||||||
public partial class FlightTask
|
|
||||||
{
|
|
||||||
public async Task RunCircleTaskAsync()
|
|
||||||
{
|
|
||||||
// 林俊清, 20150922, 首先一起调好方向。
|
|
||||||
await Task.WhenAll(
|
|
||||||
SingleCopterInfos.Select(info =>
|
|
||||||
{
|
|
||||||
int copterIndex = SingleCopterInfos.IndexOf(info);
|
|
||||||
// 当该飞机被标记时，跳过飞行任务
|
|
||||||
if ((bool)_copterManager.CopterStatus[copterIndex])
|
|
||||||
return Task.Run(()=> { });
|
|
||||||
info.TargetHeading = info.CenterDirectionDeg;
|
|
||||||
return TurnTaskFlySingleCopterAsync(info);
|
|
||||||
})
|
|
||||||
).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await Task.WhenAll(
|
|
||||||
SingleCopterInfos.Select(info => CircleTaskFlySingleCopterAsync(info))
|
|
||||||
).ConfigureAwait(false);
|
|
||||||
}
|
|
||||||
|
|
||||||
private async Task CircleTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info)
|
|
||||||
{
|
|
||||||
// 林俊清, 20150907, 目前不对 Circle 进行仿真。
|
|
||||||
if (info.Copter is FakeCopter)
|
|
||||||
{
|
|
||||||
await Task.Delay(1000);
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
int copterIndex = SingleCopterInfos.IndexOf(info);
|
|
||||||
// 当该飞机被标记时，跳过飞行任务
|
|
||||||
if ((bool)_copterManager.CopterStatus[copterIndex])
|
|
||||||
return;
|
|
||||||
|
|
||||||
await info.Copter.SetParamAsync("CIRCLE_RADIUS", info.Radius);
|
|
||||||
|
|
||||||
await info.Copter.SetParamAsync("CIRCLE_RATE", info.Rate);
|
|
||||||
|
|
||||||
var center = GeographyUtils.CalcLatLngSomeMetersAway2D(info.Copter.Latitude, info.Copter.Longitude, info.CenterDirectionDeg, 10);
|
|
||||||
|
|
||||||
await info.Copter.CircleAsync();
|
|
||||||
|
|
||||||
while (info.Copter.State != Copters.CopterState.Circling)
|
|
||||||
{
|
|
||||||
if (_flightTaskManager.IsPaused == true)
|
|
||||||
{
|
|
||||||
await info.Copter.HoverAsync();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
await Task.Delay(25).ConfigureAwait(false);
|
|
||||||
}
|
|
||||||
|
|
||||||
await info.Copter.SetChannelsAsync(ch3: info.Channel3);
|
|
||||||
|
|
||||||
for (int i = info.Turns; i > 0; i--)
|
|
||||||
{
|
|
||||||
//// 等待一段时间，让飞行器飞出一段距离。
|
|
||||||
//for (int i = 0; i < 3000 / 25; i++)
|
|
||||||
//{
|
|
||||||
// if (_flightTaskManager.IsPaused == true)
|
|
||||||
// {
|
|
||||||
// info.Copter.Hover();
|
|
||||||
// return;
|
|
||||||
// }
|
|
||||||
// await Task.Delay(25).ConfigureAwait(false);
|
|
||||||
//}
|
|
||||||
|
|
||||||
//var log = new StringBuilder(1024);
|
|
||||||
//var lastLogTime = DateTime.Now;
|
|
||||||
bool step1 = false;
|
|
||||||
while (true)
|
|
||||||
{
|
|
||||||
if (_flightTaskManager.IsPaused == true)
|
|
||||||
{
|
|
||||||
await info.Copter.HoverAsync();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
var angle = info.Radius == 0 ? NormalizeAngleDeg(info.Copter.Heading - info.CenterDirectionDeg) : GetNormalizedAngleDeg(info, center);
|
|
||||||
//log.AppendLine($"{angle} {step1}");
|
|
||||||
if (!step1)
|
|
||||||
{
|
|
||||||
if (angle > 100 && angle < 330)
|
|
||||||
{
|
|
||||||
step1 = true;
|
|
||||||
//log.AppendLine($"step1 is true now.");
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else if (angle > 340 || angle < 5)
|
|
||||||
{
|
|
||||||
//log.AppendLine("break");
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
//if (lastLogTime.AddSeconds(10) < DateTime.Now)
|
|
||||||
//{
|
|
||||||
// Logger.Instance.Log(log.ToString());
|
|
||||||
// log.Clear();
|
|
||||||
// lastLogTime = DateTime.Now;
|
|
||||||
//}
|
|
||||||
await Task.Delay(25).ConfigureAwait(false);
|
|
||||||
}
|
|
||||||
//Logger.Instance.Log(log.ToString());
|
|
||||||
}
|
|
||||||
|
|
||||||
await info.Copter.HoverAsync();
|
|
||||||
}
|
|
||||||
|
|
||||||
private static double GetNormalizedAngleDeg(FlightTaskSingleCopterInfo info, Tuple<double, double> center)
|
|
||||||
{
|
|
||||||
var angle = info.Copter.InPlaneDirectionToDeg(center.Item1, center.Item2) - info.CenterDirectionDeg;
|
|
||||||
return NormalizeAngleDeg(angle);
|
|
||||||
}
|
|
||||||
|
|
||||||
private static double NormalizeAngleDeg(double angle)
|
|
||||||
{
|
|
||||||
while (true)
|
|
||||||
{
|
|
||||||
if (angle < 0) angle += 360;
|
|
||||||
else if (angle >= 360) angle -= 360;
|
|
||||||
else break;
|
|
||||||
}
|
|
||||||
return angle;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,723 +0,0 @@
|
|||||||
using System;
|
|
||||||
using System.Collections.Generic;
|
|
||||||
using System.Linq;
|
|
||||||
using System.Text;
|
|
||||||
using System.Threading.Tasks;
|
|
||||||
|
|
||||||
namespace Plane.FormationCreator.Formation
|
|
||||||
{
|
|
||||||
public partial class FlightTask
|
|
||||||
{
|
|
||||||
private float _LoiterTimeAttr = 0.0f;
|
|
||||||
public float LoiterTimeAttr
|
|
||||||
{
|
|
||||||
get { return _LoiterTimeAttr; }
|
|
||||||
set
|
|
||||||
{
|
|
||||||
Set(nameof(LoiterTimeAttr), ref _LoiterTimeAttr, value);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// 改变机头
|
|
||||||
private bool _ChangeYaw = false;
|
|
||||||
public bool ChangeYaw
|
|
||||||
{
|
|
||||||
get { return _ChangeYaw; }
|
|
||||||
set
|
|
||||||
{
|
|
||||||
Set(nameof(ChangeYaw), ref _ChangeYaw, value);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// 机头方向0为北，顺时针360
|
|
||||||
private float _HeadYaw = 1.0f;
|
|
||||||
public float HeadYaw
|
|
||||||
{
|
|
||||||
get { return _HeadYaw; }
|
|
||||||
set
|
|
||||||
{
|
|
||||||
Set(nameof(HeadYaw), ref _HeadYaw, value);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// LED灯闪烁
|
|
||||||
private bool _flashAttr = false;
|
|
||||||
public bool flashAttr
|
|
||||||
{
|
|
||||||
get { return _flashAttr; }
|
|
||||||
set
|
|
||||||
{
|
|
||||||
Set(nameof(flashAttr), ref _flashAttr, value);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// LED闪烁次数
|
|
||||||
private float _flashPeriodAttr = 1.0f;
|
|
||||||
public float flashPeriodAttr
|
|
||||||
{
|
|
||||||
get { return _flashPeriodAttr; }
|
|
||||||
set
|
|
||||||
{
|
|
||||||
Set(nameof(flashPeriodAttr), ref _flashPeriodAttr, value);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
private string Name2Index(string name) // 获取指定copter名字对应的序号
|
|
||||||
{
|
|
||||||
int index = -1;
|
|
||||||
for (int i = 0; i < SingleCopterInfos.Count; i++)
|
|
||||||
{
|
|
||||||
if (name.Equals(SingleCopterInfos[i].Copter.Name))
|
|
||||||
{
|
|
||||||
return i.ToString();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return index.ToString();
|
|
||||||
}
|
|
||||||
|
|
||||||
// 闪烁的飞机名字编号
|
|
||||||
private string _flashCopterNameArray = "";
|
|
||||||
public string flashCopterNameArray
|
|
||||||
{
|
|
||||||
get { return _flashCopterNameArray; }
|
|
||||||
set
|
|
||||||
{
|
|
||||||
Set(nameof(flashCopterNameArray), ref _flashCopterNameArray, value);
|
|
||||||
|
|
||||||
flashCopterIndexArray = "";
|
|
||||||
|
|
||||||
string[] copterArray = flashCopterNameArray.Split(',');
|
|
||||||
|
|
||||||
for (int i = 0; i < copterArray.Length; i++)
|
|
||||||
{
|
|
||||||
flashCopterIndexArray += Name2Index(copterArray[i])+',';
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// 闪烁的飞机序号编号
|
|
||||||
private string _flashCopterIndexArray = "";
|
|
||||||
public string flashCopterIndexArray
|
|
||||||
{
|
|
||||||
get { return _flashCopterIndexArray; }
|
|
||||||
set
|
|
||||||
{
|
|
||||||
Set(nameof(flashCopterIndexArray), ref _flashCopterIndexArray, value);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// LED走马灯
|
|
||||||
private bool _oneByOneAttr = false;
|
|
||||||
public bool oneByOneAttr
|
|
||||||
{
|
|
||||||
get { return _oneByOneAttr; }
|
|
||||||
set
|
|
||||||
{
|
|
||||||
Set(nameof(oneByOneAttr), ref _oneByOneAttr, value);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// LED走马灯次数
|
|
||||||
private float _oneByOnePeriodAttr = 1.0f;
|
|
||||||
public float oneByOnePeriodAttr
|
|
||||||
{
|
|
||||||
get { return _oneByOnePeriodAttr; }
|
|
||||||
set
|
|
||||||
{
|
|
||||||
Set(nameof(oneByOnePeriodAttr), ref _oneByOnePeriodAttr, value);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
private int[] columnFirst = {0, 1, 2, 3, 4 };
|
|
||||||
private int[] columnSecond = { 5, 6, 7, 8, 9 };
|
|
||||||
private int[] columnThird = { 10, 11, 12, 13, 14 };
|
|
||||||
private int[] columnFourth = { 15, 16, 17, 18, 19 };
|
|
||||||
private int[] columnFive = { 20, 21, 22, 23, 24 };
|
|
||||||
private int[] columnSix = { 25, 26, 27, 28, 29 };
|
|
||||||
|
|
||||||
private int[] rowFirst = { 0, 5, 10, 15, 20, 25 };
|
|
||||||
private int[] rowSecond = { 1, 6, 11, 16, 21, 26 };
|
|
||||||
private int[] rowThird = { 2, 7, 12, 17, 22, 27 };
|
|
||||||
private int[] rowFourth = { 3, 8, 13, 18, 23, 28 };
|
|
||||||
private int[] rowFive = { 4, 9, 14, 19, 24, 29 };
|
|
||||||
|
|
||||||
private int[] rowFirstForEight = { 0, 1, 2, 3 };
|
|
||||||
private int[] rowSecondForEight = { 4, 5, 6, 7 };
|
|
||||||
|
|
||||||
private int[] doubleColumnFirst = { 0, 1, 2, 3, 4, 25, 26, 27, 28, 29 };
|
|
||||||
private int[] doubleColumnSecond = { 5, 6, 7, 8, 9, 20, 21, 22, 23, 24 };
|
|
||||||
private int[] doubleColumnThird = { 10, 11, 12, 13, 14, 15, 16, 17, 18, 19 };
|
|
||||||
|
|
||||||
private int[] circleFirst = { 0, 1, 2, 3, 4, 9, 14, 19, 24, 29, 28, 27, 26, 25, 20, 15, 10, 5 };
|
|
||||||
private int[] circleSecond = { 6, 7, 8, 13, 18, 23, 22, 21, 16, 11 };
|
|
||||||
private int[] circleThird = { 12, 17 };
|
|
||||||
|
|
||||||
private int[] lineFirst = { 4 };
|
|
||||||
private int[] lineSecond = { 3, 9 };
|
|
||||||
private int[] lineThird = { 2, 8, 14 };
|
|
||||||
private int[] lineFourth = { 1, 7, 13, 19 };
|
|
||||||
private int[] lineFive = { 0, 6, 12, 18, 24 };
|
|
||||||
private int[] lineSix = { 5, 11, 17, 23, 29 };
|
|
||||||
private int[] lineSeven = { 10, 16, 22, 28 };
|
|
||||||
private int[] lineEight = { 15, 21, 27 };
|
|
||||||
private int[] lineNine = { 20, 26 };
|
|
||||||
private int[] lineTen = { 25 };
|
|
||||||
|
|
||||||
public async Task RunLoiterTimeTaskAsync()
|
|
||||||
{
|
|
||||||
|
|
||||||
if (ChangeYaw)
|
|
||||||
{
|
|
||||||
await Task.WhenAll(SingleCopterInfos.Select(info => SetCopterYawAsync(info,HeadYaw)));
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// 判断flashPeriodAttr, oneByOnePeriodAttr的值，执行不同的闪烁模式
|
|
||||||
// 当flashPeriodAttr小于10，正常闪烁，即没有预制闪烁模式
|
|
||||||
// 当flashPeriodAttr大于等于10，小于20，执行预制闪烁模式
|
|
||||||
// 当flashPeriodAttr大于等于20， 改变飞控闪烁模式，以oneByOnePeriodAttr作为周期值
|
|
||||||
|
|
||||||
//LEDFlashPlanAsync需要30架飞机
|
|
||||||
if ((flashPeriodAttr >= 10.0f) && (flashPeriodAttr < 20.0f))
|
|
||||||
{
|
|
||||||
await LEDFlashPlanAsync();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
//需要固件支持
|
|
||||||
if (flashPeriodAttr >= 20.0f)
|
|
||||||
{
|
|
||||||
await LEDFlashParaModifyPlanAsync();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
var infos = SingleCopterInfos;
|
|
||||||
if (flashAttr) // LED闪烁显示效果
|
|
||||||
{
|
|
||||||
if (flashCopterIndexArray.Equals(""))
|
|
||||||
return;
|
|
||||||
string[] copterArray = flashCopterIndexArray.Split(',');
|
|
||||||
|
|
||||||
var tasks_selected = new Task[copterArray.Length];
|
|
||||||
|
|
||||||
// LED灯全灭
|
|
||||||
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false)));
|
|
||||||
await Task.Delay(1000).ConfigureAwait(false);
|
|
||||||
|
|
||||||
// 选中的飞机LED灯全亮
|
|
||||||
for (int i = 0; i < copterArray.Length; i++)
|
|
||||||
{
|
|
||||||
int index = int.Parse(copterArray[i]);
|
|
||||||
var info = infos[index];
|
|
||||||
|
|
||||||
tasks_selected[i] = await Task.Factory.StartNew(async () =>
|
|
||||||
{
|
|
||||||
var internalInfo = info;
|
|
||||||
await LEDFlashTaskFlySingleCopterAsync(internalInfo, true);
|
|
||||||
});
|
|
||||||
}
|
|
||||||
await Task.WhenAll(tasks_selected).ConfigureAwait(false);
|
|
||||||
await Task.Delay(3000).ConfigureAwait(false);
|
|
||||||
// LED灯全亮
|
|
||||||
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, true)));
|
|
||||||
//延时等待
|
|
||||||
//判断是否下一步
|
|
||||||
DateTime start = DateTime.Now;
|
|
||||||
|
|
||||||
while (true)
|
|
||||||
{
|
|
||||||
if (_flightTaskManager.IsPaused == true)
|
|
||||||
{
|
|
||||||
await Task.WhenAll(SingleCopterInfos.Select(info => (info.Copter.HoverAsync())));
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
DateTime end = DateTime.Now;
|
|
||||||
TimeSpan ts = end - start;
|
|
||||||
//等待指定时间
|
|
||||||
if (ts.TotalMilliseconds > LoiterTimeAttr * 1000)
|
|
||||||
break;
|
|
||||||
await Task.Delay(20).ConfigureAwait(false);
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
}
|
|
||||||
else if (oneByOneAttr) // LED走马灯显示效果
|
|
||||||
{
|
|
||||||
if (flashCopterNameArray.Equals(""))
|
|
||||||
return;
|
|
||||||
string[] copterArray = flashCopterIndexArray.Split(',');
|
|
||||||
//LED全灭
|
|
||||||
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false)));
|
|
||||||
await Task.Delay(1000).ConfigureAwait(false);
|
|
||||||
|
|
||||||
// LED灯一个接一个全亮
|
|
||||||
for (int i = 0; i < copterArray.Length; i++)
|
|
||||||
{
|
|
||||||
if (copterArray[i] == "") break;
|
|
||||||
int index = int.Parse(copterArray[i]);
|
|
||||||
var info = infos[index];
|
|
||||||
await LEDFlashTaskFlySingleCopterAsync(info, true);
|
|
||||||
await Task.Delay(200).ConfigureAwait(false);
|
|
||||||
}
|
|
||||||
|
|
||||||
await Task.Delay(1000).ConfigureAwait(false);
|
|
||||||
|
|
||||||
//////////////////全部闪烁一次
|
|
||||||
// LED灯全亮
|
|
||||||
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, true)));
|
|
||||||
await Task.Delay(200).ConfigureAwait(false);
|
|
||||||
// LED灯全灭
|
|
||||||
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false)));
|
|
||||||
await Task.Delay(200).ConfigureAwait(false);
|
|
||||||
/////////////////////////////////////
|
|
||||||
//////////////////全部闪烁一次
|
|
||||||
// LED灯全亮
|
|
||||||
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, true)));
|
|
||||||
await Task.Delay(200).ConfigureAwait(false);
|
|
||||||
// LED灯全灭
|
|
||||||
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false)));
|
|
||||||
await Task.Delay(200).ConfigureAwait(false);
|
|
||||||
/////////////////////////////////////
|
|
||||||
//////////////////全部闪烁一次
|
|
||||||
// LED灯全亮
|
|
||||||
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, true)));
|
|
||||||
await Task.Delay(200).ConfigureAwait(false);
|
|
||||||
// LED灯全灭
|
|
||||||
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false)));
|
|
||||||
await Task.Delay(200).ConfigureAwait(false);
|
|
||||||
/////////////////////////////////////
|
|
||||||
|
|
||||||
|
|
||||||
// LED灯全亮
|
|
||||||
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, true)));
|
|
||||||
//延时等待
|
|
||||||
|
|
||||||
//判断是否下一步
|
|
||||||
DateTime start = DateTime.Now;
|
|
||||||
while (true)
|
|
||||||
{
|
|
||||||
if (_flightTaskManager.IsPaused == true)
|
|
||||||
{
|
|
||||||
await Task.WhenAll(SingleCopterInfos.Select(info => (info.Copter.HoverAsync())));
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
DateTime end = DateTime.Now;
|
|
||||||
TimeSpan ts = end - start;
|
|
||||||
//等待指定时间
|
|
||||||
if (ts.TotalMilliseconds > LoiterTimeAttr * 1000)
|
|
||||||
break;
|
|
||||||
await Task.Delay(20).ConfigureAwait(false);
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
}
|
|
||||||
else // 没有LED显示效果只等待
|
|
||||||
{
|
|
||||||
|
|
||||||
//判断是否下一步
|
|
||||||
DateTime start = DateTime.Now;
|
|
||||||
|
|
||||||
while (true)
|
|
||||||
{
|
|
||||||
if (_flightTaskManager.IsPaused == true)
|
|
||||||
{
|
|
||||||
await Task.WhenAll(SingleCopterInfos.Select(info => (info.Copter.HoverAsync())));
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
DateTime end = DateTime.Now;
|
|
||||||
TimeSpan ts = end - start;
|
|
||||||
//等待指定时间
|
|
||||||
if (ts.TotalMilliseconds > LoiterTimeAttr * 1000)
|
|
||||||
break;
|
|
||||||
await Task.Delay(20).ConfigureAwait(false);
|
|
||||||
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
private async Task LEDFlashParaModifyPlanAsync()
|
|
||||||
{
|
|
||||||
/*
|
|
||||||
await LEDFlashParaModifyAsync(rowFirstForEight);
|
|
||||||
await Task.Delay(500).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await LEDFlashParaModifyAsync(rowSecondForEight);
|
|
||||||
await Task.Delay(500).ConfigureAwait(false);
|
|
||||||
*/
|
|
||||||
|
|
||||||
int copterCount = SingleCopterInfos.Count();
|
|
||||||
int[] intArray = new int[copterCount];
|
|
||||||
for (int ii=0; ii<copterCount; ii++)
|
|
||||||
{
|
|
||||||
intArray[ii] = ii;
|
|
||||||
}
|
|
||||||
await LEDFlashParaModifyAsync(intArray);
|
|
||||||
|
|
||||||
/*
|
|
||||||
await LEDFlashParaModifyAsync(rowFirst);
|
|
||||||
await Task.Delay(500).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await LEDFlashParaModifyAsync(rowSecond);
|
|
||||||
await Task.Delay(500).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await LEDFlashParaModifyAsync(rowThird);
|
|
||||||
await Task.Delay(500).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await LEDFlashParaModifyAsync(rowFourth);
|
|
||||||
await Task.Delay(500).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await LEDFlashParaModifyAsync(rowFive);
|
|
||||||
await Task.Delay(500).ConfigureAwait(false);
|
|
||||||
*/
|
|
||||||
}
|
|
||||||
|
|
||||||
private async Task LEDFlashParaModifyAsync(int[] LEDArray)
|
|
||||||
{
|
|
||||||
var infos = SingleCopterInfos;
|
|
||||||
|
|
||||||
var tasks_selected = new Task[LEDArray.Length];
|
|
||||||
for (int i = 0; i < LEDArray.Length; i++)
|
|
||||||
{
|
|
||||||
var info = infos[LEDArray[i]];
|
|
||||||
|
|
||||||
tasks_selected[i] = await Task.Factory.StartNew(async () =>
|
|
||||||
{
|
|
||||||
var internalInfo = info;
|
|
||||||
await LEDParaModifySingleCopterAsync(internalInfo, oneByOnePeriodAttr);
|
|
||||||
});
|
|
||||||
}
|
|
||||||
await Task.WhenAll(tasks_selected).ConfigureAwait(false);
|
|
||||||
}
|
|
||||||
private async Task SetCopterYawAsync(FlightTaskSingleCopterInfo info, float headyaw)
|
|
||||||
{
|
|
||||||
var copter = info.Copter;
|
|
||||||
|
|
||||||
// await Task.Delay(200).ConfigureAwait(false);
|
|
||||||
|
|
||||||
int copterIndex = SingleCopterInfos.IndexOf(info);
|
|
||||||
// 当该飞机被标记时，悬停并跳过飞行任务
|
|
||||||
if ((bool)_copterManager.CopterStatus[copterIndex])
|
|
||||||
{
|
|
||||||
await info.Copter.HoverAsync();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
var tasks = new Task[1];
|
|
||||||
tasks[0] = Task.Run(async () =>
|
|
||||||
{
|
|
||||||
try
|
|
||||||
{
|
|
||||||
await copter.SetMobileControlAsync(yaw: headyaw);
|
|
||||||
|
|
||||||
while (!copter.ArrivedHeading((short)headyaw))
|
|
||||||
{
|
|
||||||
if (_flightTaskManager.IsPaused == true)
|
|
||||||
{
|
|
||||||
await info.Copter.HoverAsync();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
await Task.Delay(25).ConfigureAwait(false);
|
|
||||||
// 当该飞机被标记时，悬停并跳过飞行任务
|
|
||||||
if ((bool)_copterManager.CopterStatus[copterIndex])
|
|
||||||
{
|
|
||||||
await info.Copter.HoverAsync();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
catch (TimeoutException ex)
|
|
||||||
{
|
|
||||||
_logger.Log($"SetYAWtimeout, {ex.Message}, CopterId: {copter.Id}。");
|
|
||||||
}
|
|
||||||
});
|
|
||||||
await Task.WhenAll(tasks).ConfigureAwait(false);
|
|
||||||
}
|
|
||||||
private async Task LEDParaModifySingleCopterAsync(FlightTaskSingleCopterInfo info, float count)
|
|
||||||
{
|
|
||||||
var copter = info.Copter;
|
|
||||||
|
|
||||||
int copterIndex = SingleCopterInfos.IndexOf(info);
|
|
||||||
// 当该飞机被标记时，悬停并跳过飞行任务
|
|
||||||
if ((bool)_copterManager.CopterStatus[copterIndex])
|
|
||||||
{
|
|
||||||
await info.Copter.HoverAsync();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
var tasks = new Task[1];
|
|
||||||
tasks[0] = Task.Run(async () =>
|
|
||||||
{
|
|
||||||
try
|
|
||||||
{
|
|
||||||
if (count == 1.0)
|
|
||||||
await copter.SetShowLEDAsync(true);
|
|
||||||
else
|
|
||||||
await copter.SetShowLEDAsync(false);
|
|
||||||
//await copter.SetParamAsync("NTF_SHOWLED", count, 5000);
|
|
||||||
}
|
|
||||||
catch (TimeoutException ex)
|
|
||||||
{
|
|
||||||
_logger.Log($"SetShowLEDAsync 超时, {ex.Message}, CopterId: {copter.Id}。");
|
|
||||||
}
|
|
||||||
});
|
|
||||||
|
|
||||||
/*
|
|
||||||
var tasks = new Task[2];
|
|
||||||
tasks[0] = Task.Run(async () =>
|
|
||||||
{
|
|
||||||
try
|
|
||||||
{
|
|
||||||
await copter.SetParamAsync("NOTI_GPSLED", count, 5000);
|
|
||||||
}
|
|
||||||
catch (TimeoutException ex)
|
|
||||||
{
|
|
||||||
_logger.Log($"NOTI_GPSLED 超时, {ex.Message}, CopterId: {copter.Id}。");
|
|
||||||
}
|
|
||||||
});
|
|
||||||
tasks[1] = Task.Run(async () =>
|
|
||||||
{
|
|
||||||
try
|
|
||||||
{
|
|
||||||
await copter.SetParamAsync("NOTI_ARMLED", count, 5000);
|
|
||||||
}
|
|
||||||
catch (TimeoutException ex)
|
|
||||||
{
|
|
||||||
_logger.Log($"NOTI_ARMLED 超时, {ex.Message}, CopterId: {copter.Id}。");
|
|
||||||
}
|
|
||||||
});
|
|
||||||
*/
|
|
||||||
await Task.WhenAll(tasks).ConfigureAwait(false);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* isOn为true，打开灯；isOn为false，关闭灯
|
|
||||||
*/
|
|
||||||
private async Task LEDFlashTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info, bool isOn)
|
|
||||||
{
|
|
||||||
var copter = info.Copter;
|
|
||||||
|
|
||||||
int copterIndex = SingleCopterInfos.IndexOf(info);
|
|
||||||
// 当该飞机被标记时，悬停并跳过飞行任务
|
|
||||||
if ((bool)_copterManager.CopterStatus[copterIndex])
|
|
||||||
{
|
|
||||||
await info.Copter.HoverAsync();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
// float gpsLed = await c.GetParamAsync("NOTI_GPSLED");
|
|
||||||
|
|
||||||
/*
|
|
||||||
float ledControl = 0.0f; //关灯
|
|
||||||
if (isOn)
|
|
||||||
{
|
|
||||||
ledControl = 1.0f; //长亮
|
|
||||||
}
|
|
||||||
*/
|
|
||||||
var tasks = new Task[1];
|
|
||||||
tasks[0] = Task.Run(async () =>
|
|
||||||
{
|
|
||||||
try
|
|
||||||
{
|
|
||||||
await copter.SetShowLEDAsync(isOn);
|
|
||||||
// await copter.SetParamAsync("NTF_SHOWLED", ledControl, 5000);
|
|
||||||
}
|
|
||||||
catch (TimeoutException ex)
|
|
||||||
{
|
|
||||||
_logger.Log($"SetShowLED 超时, {ex.Message}, CopterId: {copter.Id}。");
|
|
||||||
}
|
|
||||||
});
|
|
||||||
|
|
||||||
/*
|
|
||||||
var tasks = new Task[2];
|
|
||||||
tasks[0] = Task.Run(async () =>
|
|
||||||
{
|
|
||||||
try
|
|
||||||
{
|
|
||||||
await copter.SetParamAsync("NOTI_GPSLED", ledControl, 5000);
|
|
||||||
}
|
|
||||||
catch (TimeoutException ex)
|
|
||||||
{
|
|
||||||
_logger.Log($"NOTI_GPSLED 超时, {ex.Message}, CopterId: {copter.Id}。");
|
|
||||||
}
|
|
||||||
});
|
|
||||||
tasks[1] = Task.Run(async () =>
|
|
||||||
{
|
|
||||||
try
|
|
||||||
{
|
|
||||||
await copter.SetParamAsync("NOTI_ARMLED", ledControl, 5000);
|
|
||||||
}
|
|
||||||
catch (TimeoutException ex)
|
|
||||||
{
|
|
||||||
_logger.Log($"NOTI_ARMLED 超时, {ex.Message}, CopterId: {copter.Id}。");
|
|
||||||
}
|
|
||||||
});
|
|
||||||
*/
|
|
||||||
await Task.WhenAll(tasks).ConfigureAwait(false);
|
|
||||||
}
|
|
||||||
|
|
||||||
private async Task LEDFlashPlanAsync()
|
|
||||||
{
|
|
||||||
|
|
||||||
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false)));
|
|
||||||
await Task.Delay(500).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await LEDColumnFlashAsync();
|
|
||||||
await Task.Delay(500).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false)));
|
|
||||||
await Task.Delay(500).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await LEDDoubleColumnFlashAsync();
|
|
||||||
await Task.Delay(500).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false)));
|
|
||||||
await Task.Delay(500).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await LEDLineFlashAsync();
|
|
||||||
await Task.Delay(500).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false)));
|
|
||||||
await Task.Delay(500).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await LEDCircleFlashAsync();
|
|
||||||
await Task.Delay(500).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false)));
|
|
||||||
await Task.Delay(200).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, true)));
|
|
||||||
await Task.Delay(200).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, false)));
|
|
||||||
await Task.Delay(200).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await Task.WhenAll(SingleCopterInfos.Select(info => LEDFlashTaskFlySingleCopterAsync(info, true)));
|
|
||||||
await Task.Delay(200).ConfigureAwait(false);
|
|
||||||
}
|
|
||||||
|
|
||||||
// 一排一排闪烁 需要30架飞机，6排，一排5架，一排排闪灯
|
|
||||||
private async Task LEDColumnFlashAsync()
|
|
||||||
{
|
|
||||||
await LEDArrayFlashAsync(columnFirst);
|
|
||||||
await Task.Delay(200).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await LEDArrayFlashAsync(columnSecond);
|
|
||||||
await Task.Delay(200).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await LEDArrayFlashAsync(columnThird);
|
|
||||||
await Task.Delay(200).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await LEDArrayFlashAsync(columnFourth);
|
|
||||||
await Task.Delay(200).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await LEDArrayFlashAsync(columnFive);
|
|
||||||
await Task.Delay(200).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await LEDArrayFlashAsync(columnSix);
|
|
||||||
await Task.Delay(200).ConfigureAwait(false);
|
|
||||||
}
|
|
||||||
|
|
||||||
// 以圆圈收缩形式显示
|
|
||||||
private async Task LEDCircleFlashAsync()
|
|
||||||
{
|
|
||||||
|
|
||||||
await LEDArrayFlashAsync(circleFirst);
|
|
||||||
await Task.Delay(200).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await LEDArrayFlashAsync(circleSecond);
|
|
||||||
await Task.Delay(200).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await LEDArrayFlashAsync(circleThird);
|
|
||||||
await Task.Delay(200).ConfigureAwait(false);
|
|
||||||
}
|
|
||||||
|
|
||||||
// 两列同时闪烁
|
|
||||||
private async Task LEDDoubleColumnFlashAsync()
|
|
||||||
{
|
|
||||||
|
|
||||||
await LEDArrayFlashAsync(doubleColumnFirst);
|
|
||||||
await Task.Delay(200).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await LEDArrayFlashAsync(doubleColumnSecond);
|
|
||||||
await Task.Delay(200).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await LEDArrayFlashAsync(doubleColumnThird);
|
|
||||||
await Task.Delay(200).ConfigureAwait(false);
|
|
||||||
}
|
|
||||||
|
|
||||||
// 斜线方式闪烁
|
|
||||||
private async Task LEDLineFlashAsync()
|
|
||||||
{
|
|
||||||
|
|
||||||
await LEDArrayFlashAsync(lineFirst);
|
|
||||||
await Task.Delay(200).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await LEDArrayFlashAsync(lineSecond);
|
|
||||||
await Task.Delay(200).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await LEDArrayFlashAsync(lineThird);
|
|
||||||
await Task.Delay(200).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await LEDArrayFlashAsync(lineFourth);
|
|
||||||
await Task.Delay(200).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await LEDArrayFlashAsync(lineFive);
|
|
||||||
await Task.Delay(200).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await LEDArrayFlashAsync(lineSix);
|
|
||||||
await Task.Delay(200).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await LEDArrayFlashAsync(lineSeven);
|
|
||||||
await Task.Delay(200).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await LEDArrayFlashAsync(lineEight);
|
|
||||||
await Task.Delay(200).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await LEDArrayFlashAsync(lineNine);
|
|
||||||
await Task.Delay(200).ConfigureAwait(false);
|
|
||||||
|
|
||||||
await LEDArrayFlashAsync(lineTen);
|
|
||||||
await Task.Delay(200).ConfigureAwait(false);
|
|
||||||
}
|
|
||||||
|
|
||||||
private async Task LEDArrayFlashAsync(int[] LEDArray)
|
|
||||||
{
|
|
||||||
var infos = SingleCopterInfos;
|
|
||||||
|
|
||||||
var tasks_selected = new Task[LEDArray.Length];
|
|
||||||
for (int i = 0; i < LEDArray.Length; i++)
|
|
||||||
{
|
|
||||||
var info = infos[LEDArray[i]];
|
|
||||||
|
|
||||||
tasks_selected[i] = await Task.Factory.StartNew(async () =>
|
|
||||||
{
|
|
||||||
var internalInfo = info;
|
|
||||||
await LEDFlashTaskFlySingleCopterAsync(internalInfo, true);
|
|
||||||
});
|
|
||||||
}
|
|
||||||
await Task.WhenAll(tasks_selected).ConfigureAwait(false);
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,489 +0,0 @@
|
|||||||
using System;
|
|
||||||
using System.Collections.Generic;
|
|
||||||
using System.Linq;
|
|
||||||
using System.Text;
|
|
||||||
using System.Threading.Tasks;
|
|
||||||
|
|
||||||
namespace Plane.FormationCreator.Formation
|
|
||||||
{
|
|
||||||
public partial class FlightTask
|
|
||||||
{
|
|
||||||
private float _RTLAlt = 20.0f;
|
|
||||||
public float RTLAlt
|
|
||||||
{
|
|
||||||
get { return _RTLAlt; }
|
|
||||||
set { Set(nameof(RTLAlt), ref _RTLAlt, value); }
|
|
||||||
}
|
|
||||||
private int _RetNumAttr = 1;
|
|
||||||
public int RetNumAttr
|
|
||||||
{
|
|
||||||
get { return _RetNumAttr; }
|
|
||||||
set
|
|
||||||
{
|
|
||||||
Set(nameof(RetNumAttr), ref _RetNumAttr, value);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
public async Task RunReturnToLandTaskAsync()
|
|
||||||
{
|
|
||||||
int TaskCount = _flightTaskManager.Tasks.Count();
|
|
||||||
if (TaskCount > 1)
|
|
||||||
{
|
|
||||||
var infos = SingleCopterInfos;
|
|
||||||
// var infos = SingleCopterInfos.OrderByDescending(i => i.TargetAlt).ToList();
|
|
||||||
var tasks = new Task[infos.Count];
|
|
||||||
var tasksTmp = new Task[infos.Count];
|
|
||||||
for (int i = 0; i < infos.Count; i++)
|
|
||||||
{
|
|
||||||
var info = infos[i];
|
|
||||||
|
|
||||||
// var copter = info.Copter;
|
|
||||||
if (info.RTLStage <= 1) // 当前阶段小于等于1时进入
|
|
||||||
{
|
|
||||||
await ReturnToLandTaskFlySingleCopterAsync(info);
|
|
||||||
}
|
|
||||||
|
|
||||||
tasksTmp[i] = ReturnToLandSecondTaskAsync(info);
|
|
||||||
|
|
||||||
}
|
|
||||||
await Task.WhenAll(tasksTmp).ConfigureAwait(false);
|
|
||||||
if (_flightTaskManager.IsPaused == false)
|
|
||||||
{
|
|
||||||
for (int i = 0; i < infos.Count; i++)
|
|
||||||
{
|
|
||||||
var info = infos[i];
|
|
||||||
info.RTLStage = 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
public async Task MutilRunReturnToLandTaskAsync()
|
|
||||||
{
|
|
||||||
int TaskCount = _flightTaskManager.Tasks.Count();
|
|
||||||
if (TaskCount > 1)
|
|
||||||
{
|
|
||||||
var infos = SingleCopterInfos;
|
|
||||||
|
|
||||||
int copterCount = infos.Count;
|
|
||||||
int integerPart = copterCount / RetNumAttr;
|
|
||||||
int residualPart = copterCount % RetNumAttr;
|
|
||||||
var tasksTmp = new Task[infos.Count];
|
|
||||||
//整数部分执行返航降落
|
|
||||||
for (int i = 0; i < integerPart; i++)
|
|
||||||
{
|
|
||||||
var tasksRetutn = new Task[RetNumAttr*2];
|
|
||||||
//设置返航任务
|
|
||||||
for (int j = RetNumAttr * i; j < RetNumAttr * (i + 1); j++)
|
|
||||||
{
|
|
||||||
var info = infos[j];
|
|
||||||
|
|
||||||
int indexTmp = j - RetNumAttr * i;
|
|
||||||
|
|
||||||
// var copter = info.Copter;
|
|
||||||
if (info.RTLStage <= 1) // 当前阶段小于等于1时进入
|
|
||||||
{
|
|
||||||
tasksRetutn[indexTmp] = ReturnToLandTaskFlySingleCopterAsync(info);
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
tasksRetutn[indexTmp] = Task.Run(async () => { await Task.Delay(1).ConfigureAwait(false); });
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
//设置降落任务
|
|
||||||
if (i > 0)
|
|
||||||
{
|
|
||||||
|
|
||||||
for (int j = RetNumAttr * (i - 1); j < RetNumAttr * (i); j++)
|
|
||||||
{
|
|
||||||
var info = infos[j];
|
|
||||||
|
|
||||||
int indexTmp = RetNumAttr+ j - RetNumAttr * (i-1);
|
|
||||||
|
|
||||||
// var copter = info.Copter;
|
|
||||||
if (info.RTLStage == 2) // 当前阶段小于等于1时进入
|
|
||||||
{
|
|
||||||
tasksRetutn[indexTmp] = ReturnToLandSecondTaskAsync(info);
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
tasksRetutn[indexTmp] = Task.Run(async () => { await Task.Delay(1).ConfigureAwait(false); });
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
//第一波返航，降落任务为空
|
|
||||||
for (int j = RetNumAttr * (i +1); j < RetNumAttr * (i+2); j++)
|
|
||||||
{
|
|
||||||
int indexTmp = j - RetNumAttr * i;
|
|
||||||
tasksRetutn[indexTmp] = Task.Run(async () => { await Task.Delay(1).ConfigureAwait(false); });
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
}
|
|
||||||
//开始执行这波返航和上波降落
|
|
||||||
await Task.WhenAll(tasksRetutn).ConfigureAwait(false);
|
|
||||||
//上波降落后设置为0
|
|
||||||
if (_flightTaskManager.IsPaused == false)
|
|
||||||
{
|
|
||||||
if (i > 0)
|
|
||||||
{
|
|
||||||
for (int j = RetNumAttr * (i - 1); j < RetNumAttr * (i); j++)
|
|
||||||
{
|
|
||||||
var info = infos[j];
|
|
||||||
info.RTLStage = 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
if (residualPart > 0)
|
|
||||||
{
|
|
||||||
var tasksreturn = new Task[residualPart+ RetNumAttr];
|
|
||||||
//余数部分开始返航
|
|
||||||
for (int j = integerPart * RetNumAttr; j < RetNumAttr * integerPart + residualPart; j++)
|
|
||||||
{
|
|
||||||
var info = infos[j];
|
|
||||||
int indexTmp = j - RetNumAttr * integerPart;
|
|
||||||
// var copter = info.Copter;
|
|
||||||
if (info.RTLStage <= 1) // 当前阶段小于等于1时进入
|
|
||||||
{
|
|
||||||
tasksreturn[indexTmp] = ReturnToLandTaskFlySingleCopterAsync(info);
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
tasksreturn[indexTmp] = Task.Run(async () => { await Task.Delay(1).ConfigureAwait(false); });
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
//无余数的整数部分最后一波开始降落
|
|
||||||
// var tasksLastland = new Task[RetNumAttr];
|
|
||||||
for (int j = RetNumAttr * (integerPart - 1); j < RetNumAttr * (integerPart); j++)
|
|
||||||
{
|
|
||||||
var info = infos[j];
|
|
||||||
|
|
||||||
int indexTmp = RetNumAttr + j - RetNumAttr * integerPart;
|
|
||||||
|
|
||||||
// var copter = info.Copter;
|
|
||||||
if (info.RTLStage == 2) // 当前阶段小于等于1时进入
|
|
||||||
{
|
|
||||||
tasksreturn[residualPart + indexTmp] = ReturnToLandSecondTaskAsync(info);
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
tasksreturn[residualPart+indexTmp] = Task.Run(async () => { await Task.Delay(1).ConfigureAwait(false); });
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
//开始执行降落和返航
|
|
||||||
await Task.WhenAll(tasksreturn).ConfigureAwait(false);
|
|
||||||
|
|
||||||
//降落后设置为0
|
|
||||||
if (_flightTaskManager.IsPaused == false)
|
|
||||||
{
|
|
||||||
|
|
||||||
for (int j = RetNumAttr * (integerPart - 1); j < RetNumAttr * (integerPart); j++)
|
|
||||||
{
|
|
||||||
var info = infos[j];
|
|
||||||
info.RTLStage = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
//最后余数部分降落
|
|
||||||
|
|
||||||
var tasksRetutn1 = new Task[residualPart];
|
|
||||||
for (int j = integerPart * RetNumAttr; j < RetNumAttr * integerPart + residualPart; j++)
|
|
||||||
{
|
|
||||||
var info = infos[j];
|
|
||||||
int indexTmp = j - RetNumAttr * integerPart;
|
|
||||||
// var copter = info.Copter;
|
|
||||||
if (info.RTLStage == 2) // 当前阶段小于等于1时进入
|
|
||||||
{
|
|
||||||
tasksRetutn1[indexTmp] = ReturnToLandSecondTaskAsync(info);
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
tasksRetutn1[indexTmp] = Task.Run(async () => { await Task.Delay(1).ConfigureAwait(false); });
|
|
||||||
}
|
|
||||||
}
|
|
||||||
await Task.WhenAll(tasksRetutn1).ConfigureAwait(false);
|
|
||||||
|
|
||||||
if (_flightTaskManager.IsPaused == false)
|
|
||||||
{
|
|
||||||
|
|
||||||
for (int j = integerPart * RetNumAttr; j < RetNumAttr * integerPart + residualPart; j++)
|
|
||||||
{
|
|
||||||
var info = infos[j];
|
|
||||||
info.RTLStage = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
|
|
||||||
|
|
||||||
//无余数的整数部分最后一波开始降落
|
|
||||||
var tasksLastland = new Task[RetNumAttr];
|
|
||||||
for (int j = RetNumAttr * (integerPart - 1); j < RetNumAttr * (integerPart); j++)
|
|
||||||
{
|
|
||||||
var info = infos[j];
|
|
||||||
|
|
||||||
int indexTmp = RetNumAttr + j - RetNumAttr * integerPart;
|
|
||||||
|
|
||||||
// var copter = info.Copter;
|
|
||||||
if (info.RTLStage == 2) // 当前阶段小于等于1时进入
|
|
||||||
{
|
|
||||||
tasksLastland[indexTmp] = ReturnToLandSecondTaskAsync(info);
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
tasksLastland[indexTmp] = Task.Run(async () => { await Task.Delay(1).ConfigureAwait(false); });
|
|
||||||
}
|
|
||||||
}
|
|
||||||
await Task.WhenAll(tasksLastland).ConfigureAwait(false);
|
|
||||||
|
|
||||||
//降落后设置为0
|
|
||||||
if (_flightTaskManager.IsPaused == false)
|
|
||||||
{
|
|
||||||
|
|
||||||
for (int j = RetNumAttr * (integerPart - 1); j < RetNumAttr * (integerPart); j++)
|
|
||||||
{
|
|
||||||
var info = infos[j];
|
|
||||||
info.RTLStage = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
//原地下降高度到15米，然后返回起飞点
|
|
||||||
private async Task ReturnToLandTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info)
|
|
||||||
{
|
|
||||||
float takeOffAlt = RTLAlt;
|
|
||||||
int TaskCount = _flightTaskManager.Tasks.Count();
|
|
||||||
|
|
||||||
int copterIndex = SingleCopterInfos.IndexOf(info);
|
|
||||||
var copter = info.Copter;
|
|
||||||
var copterPreviousTask = _flightTaskManager.Tasks[TaskCount-2].SingleCopterInfos[copterIndex]; // 倒数第二步的目标位置
|
|
||||||
|
|
||||||
// 当该飞机被标记时，悬停并跳过飞行任务
|
|
||||||
if ((bool)_copterManager.CopterStatus[copterIndex])
|
|
||||||
{
|
|
||||||
await info.Copter.HoverAsync();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
DateTime dtNow = DateTime.Now;
|
|
||||||
DateTime dtLastTime = DateTime.Now;
|
|
||||||
TimeSpan ts = dtNow - dtLastTime;
|
|
||||||
// 第一阶段：垂直飞行
|
|
||||||
if (info.RTLStage == 0)
|
|
||||||
{
|
|
||||||
for (int j = 0; j < 3; j++)
|
|
||||||
{
|
|
||||||
await info.Copter.FlyToAsync(copterPreviousTask.TargetLat, copterPreviousTask.TargetLng, takeOffAlt);
|
|
||||||
}
|
|
||||||
|
|
||||||
while (!info.Copter.ArrivedTarget(copterPreviousTask.TargetLat, copterPreviousTask.TargetLng, takeOffAlt))
|
|
||||||
{
|
|
||||||
if (_flightTaskManager.IsPaused == true)
|
|
||||||
{
|
|
||||||
await info.Copter.HoverAsync();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
await Task.Delay(25).ConfigureAwait(false);
|
|
||||||
|
|
||||||
dtNow = DateTime.Now;
|
|
||||||
ts = dtNow - dtLastTime;
|
|
||||||
if (ts.TotalMilliseconds > 1000)
|
|
||||||
{
|
|
||||||
for (int j = 0; j < 2; j++)
|
|
||||||
{
|
|
||||||
await info.Copter.FlyToAsync(copterPreviousTask.TargetLat, copterPreviousTask.TargetLng, takeOffAlt);
|
|
||||||
}
|
|
||||||
dtLastTime = dtNow;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 当该飞机被标记时，悬停并跳过飞行任务
|
|
||||||
if ((bool)_copterManager.CopterStatus[copterIndex])
|
|
||||||
{
|
|
||||||
await info.Copter.HoverAsync();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
info.RTLStage++;
|
|
||||||
}
|
|
||||||
|
|
||||||
dtNow = DateTime.Now;
|
|
||||||
dtLastTime = DateTime.Now;
|
|
||||||
ts = dtNow - dtLastTime;
|
|
||||||
|
|
||||||
var copterFirstTask = _flightTaskManager.Tasks[0].SingleCopterInfos[copterIndex]; // 第一步记录的家位置
|
|
||||||
// 第二阶段：水平飞行
|
|
||||||
if (info.RTLStage == 1)
|
|
||||||
{
|
|
||||||
for (int j = 0; j < 3; j++)
|
|
||||||
{
|
|
||||||
await info.Copter.FlyToAsync(copterFirstTask.TargetLat, copterFirstTask.TargetLng, takeOffAlt);
|
|
||||||
}
|
|
||||||
|
|
||||||
while (!info.Copter.ArrivedTarget(copterFirstTask.TargetLat, copterFirstTask.TargetLng, takeOffAlt))
|
|
||||||
{
|
|
||||||
if (_flightTaskManager.IsPaused == true)
|
|
||||||
{
|
|
||||||
await info.Copter.HoverAsync();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
await Task.Delay(25).ConfigureAwait(false);
|
|
||||||
|
|
||||||
dtNow = DateTime.Now;
|
|
||||||
ts = dtNow - dtLastTime;
|
|
||||||
if (ts.TotalMilliseconds > 1000)
|
|
||||||
{
|
|
||||||
for (int j = 0; j < 2; j++)
|
|
||||||
{
|
|
||||||
await info.Copter.FlyToAsync(copterFirstTask.TargetLat, copterFirstTask.TargetLng, takeOffAlt);
|
|
||||||
}
|
|
||||||
dtLastTime = dtNow;
|
|
||||||
}
|
|
||||||
// 当该飞机被标记时，悬停并跳过飞行任务
|
|
||||||
if ((bool)_copterManager.CopterStatus[copterIndex])
|
|
||||||
{
|
|
||||||
await info.Copter.HoverAsync();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
info.RTLStage++;
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
//在起飞点上空从15米降落到4米，然后切land模式
|
|
||||||
private async Task ReturnToLandSecondTaskAsync(FlightTaskSingleCopterInfo info)
|
|
||||||
{
|
|
||||||
// await Task.Run(async () =>
|
|
||||||
// {
|
|
||||||
int copterIndex = SingleCopterInfos.IndexOf(info);
|
|
||||||
|
|
||||||
// 当该飞机被标记时，悬停并跳过飞行任务
|
|
||||||
if ((bool)_copterManager.CopterStatus[copterIndex])
|
|
||||||
{
|
|
||||||
await info.Copter.HoverAsync();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
DateTime dtNow = DateTime.Now;
|
|
||||||
DateTime dtLastTime = DateTime.Now;
|
|
||||||
TimeSpan ts = dtNow - dtLastTime;
|
|
||||||
|
|
||||||
float landAlt = 4.0f; // 飞机降落到4m再切land模式
|
|
||||||
|
|
||||||
var copterFirstTask = _flightTaskManager.Tasks[0].SingleCopterInfos[copterIndex]; // 第一步记录的家位置
|
|
||||||
// 第三阶段
|
|
||||||
if (info.RTLStage == 2)
|
|
||||||
{
|
|
||||||
for (int j = 0; j < 3; j++)
|
|
||||||
{
|
|
||||||
await info.Copter.FlyToAsync(copterFirstTask.TargetLat, copterFirstTask.TargetLng, landAlt);
|
|
||||||
}
|
|
||||||
|
|
||||||
while (!info.Copter.ArrivedTarget(copterFirstTask.TargetLat, copterFirstTask.TargetLng, landAlt))
|
|
||||||
{
|
|
||||||
if (_flightTaskManager.IsPaused == true)
|
|
||||||
{
|
|
||||||
await info.Copter.HoverAsync();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
await Task.Delay(25).ConfigureAwait(false);
|
|
||||||
|
|
||||||
dtNow = DateTime.Now;
|
|
||||||
ts = dtNow - dtLastTime;
|
|
||||||
if (ts.TotalMilliseconds > 1000)
|
|
||||||
{
|
|
||||||
for (int j = 0; j < 2; j++)
|
|
||||||
{
|
|
||||||
await info.Copter.FlyToAsync(copterFirstTask.TargetLat, copterFirstTask.TargetLng, landAlt);
|
|
||||||
}
|
|
||||||
dtLastTime = dtNow;
|
|
||||||
}
|
|
||||||
// 当该飞机被标记时，悬停并跳过飞行任务
|
|
||||||
if ((bool)_copterManager.CopterStatus[copterIndex])
|
|
||||||
{
|
|
||||||
await info.Copter.HoverAsync();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
info.RTLStage++;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 切到land模式
|
|
||||||
if (_flightTaskManager.IsPaused == true)
|
|
||||||
{
|
|
||||||
await info.Copter.HoverAsync();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
var copter = info.Copter;
|
|
||||||
if (info.RTLStage == 3) // 下降过程
|
|
||||||
{
|
|
||||||
for (int j = 0; j < 5; j++) // added by ZJF
|
|
||||||
{
|
|
||||||
// await copter.ReturnToLaunchAsync();
|
|
||||||
await copter.LandAsync(); // 修改为降落模式
|
|
||||||
await Task.Delay(10);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// });
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,24 +0,0 @@
|
|||||||
using System;
|
|
||||||
using System.Collections.Generic;
|
|
||||||
using System.Linq;
|
|
||||||
using System.Text;
|
|
||||||
using System.Threading.Tasks;
|
|
||||||
|
|
||||||
namespace Plane.FormationCreator.Formation
|
|
||||||
{
|
|
||||||
public partial class FlightTask
|
|
||||||
{
|
|
||||||
public async Task RunSimpleCircleTaskAsync()
|
|
||||||
{
|
|
||||||
var center = SingleCopterInfos.Select(info => info.Copter).GetCenter().Value;
|
|
||||||
|
|
||||||
// 林俊清, 20150921, 自动计算圆心方向和半径，然后直接调用普通画圈的方法。
|
|
||||||
foreach (var info in SingleCopterInfos)
|
|
||||||
{
|
|
||||||
info.CenterDirectionDeg = (short)info.Copter.InPlaneDirectionToDeg(center.Lat, center.Lng);
|
|
||||||
info.Radius = (int)(info.Copter.InPlaneDistanceTo(center.Lat, center.Lng) * 100);
|
|
||||||
}
|
|
||||||
await RunCircleTaskAsync().ConfigureAwait(false);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,41 +0,0 @@
|
|||||||
using Plane.Copters;
|
|
||||||
using Plane.Logging;
|
|
||||||
using System;
|
|
||||||
using System.Collections.Generic;
|
|
||||||
using System.Linq;
|
|
||||||
using System.Text;
|
|
||||||
using System.Threading.Tasks;
|
|
||||||
|
|
||||||
namespace Plane.FormationCreator.Formation
|
|
||||||
{
|
|
||||||
public partial class FlightTask
|
|
||||||
{
|
|
||||||
public async Task RunTurnTaskAsync()
|
|
||||||
{
|
|
||||||
await Task.WhenAll(
|
|
||||||
SingleCopterInfos.Select(info => TurnTaskFlySingleCopterAsync(info))
|
|
||||||
).ConfigureAwait(false);
|
|
||||||
}
|
|
||||||
|
|
||||||
private async Task TurnTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info)
|
|
||||||
{
|
|
||||||
int copterIndex = SingleCopterInfos.IndexOf(info);
|
|
||||||
// 当该飞机被标记时，跳过飞行任务
|
|
||||||
if ((bool)_copterManager.CopterStatus[copterIndex])
|
|
||||||
return;
|
|
||||||
|
|
||||||
await info.Copter.SetMobileControlAsync(yaw: info.TargetHeading);
|
|
||||||
while (!info.Copter.ArrivedHeading(info.TargetHeading))
|
|
||||||
{
|
|
||||||
if (_flightTaskManager.IsPaused == true)
|
|
||||||
{
|
|
||||||
await info.Copter.HoverAsync();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
await Task.Delay(25).ConfigureAwait(false);
|
|
||||||
}
|
|
||||||
|
|
||||||
await info.Copter.HoverAsync();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -156,13 +156,7 @@
|
|||||||
<Compile Include="Formation\Copter.cs" />
|
<Compile Include="Formation\Copter.cs" />
|
||||||
<Compile Include="Formation\FlightTaskSingleCopterInfo_Land.cs" />
|
<Compile Include="Formation\FlightTaskSingleCopterInfo_Land.cs" />
|
||||||
<Compile Include="Formation\FlightTaskSingleCopterInfo_LED.cs" />
|
<Compile Include="Formation\FlightTaskSingleCopterInfo_LED.cs" />
|
||||||
<Compile Include="Formation\FlightTaskSingleCopterInfo_LoiterTime.cs" />
|
|
||||||
<Compile Include="Formation\FlightTaskSingleCopterInfo_ReturnToLand.cs" />
|
|
||||||
<Compile Include="Formation\FlightTaskSingleCopterInfo_SimpleCircle.cs" />
|
|
||||||
<Compile Include="Formation\FlightTaskSingleCopterInfo_Turn.cs" />
|
|
||||||
<Compile Include="Formation\FlightTaskSingleCopterInfo_TakeOff.cs" />
|
<Compile Include="Formation\FlightTaskSingleCopterInfo_TakeOff.cs" />
|
||||||
<Compile Include="Formation\FlightTask_Circle.cs" />
|
|
||||||
<Compile Include="Formation\FlightTaskSingleCopterInfo_Circle.cs" />
|
|
||||||
<Compile Include="Formation\Extensions.cs" />
|
<Compile Include="Formation\Extensions.cs" />
|
||||||
<Compile Include="Formation\FlightTask.cs" />
|
<Compile Include="Formation\FlightTask.cs" />
|
||||||
<Compile Include="Formation\FlightTaskSingleCopterInfo.cs" />
|
<Compile Include="Formation\FlightTaskSingleCopterInfo.cs" />
|
||||||
@ -170,10 +164,6 @@
|
|||||||
<Compile Include="Formation\FlightTask_FlyTo.cs" />
|
<Compile Include="Formation\FlightTask_FlyTo.cs" />
|
||||||
<Compile Include="Formation\FlightTaskSingleCopterInfo_FlyTo.cs" />
|
<Compile Include="Formation\FlightTaskSingleCopterInfo_FlyTo.cs" />
|
||||||
<Compile Include="Formation\FlightTask_Land.cs" />
|
<Compile Include="Formation\FlightTask_Land.cs" />
|
||||||
<Compile Include="Formation\FlightTask_LoiterTime.cs" />
|
|
||||||
<Compile Include="Formation\FlightTask_ReturnToLand.cs" />
|
|
||||||
<Compile Include="Formation\FlightTask_SimpleCircle.cs" />
|
|
||||||
<Compile Include="Formation\FlightTask_Turn.cs" />
|
|
||||||
<Compile Include="Formation\FormationController.cs" />
|
<Compile Include="Formation\FormationController.cs" />
|
||||||
<Compile Include="Formation\CopterManager.cs" />
|
<Compile Include="Formation\CopterManager.cs" />
|
||||||
<Compile Include="Formation\GroupManager.cs" />
|
<Compile Include="Formation\GroupManager.cs" />
|
||||||
|
|||||||
@ -55,7 +55,7 @@ namespace Plane.Util
|
|||||||
|
|
||||||
public event EventHandler ObsMessage;
|
public event EventHandler ObsMessage;
|
||||||
public event EventHandler BasePosMessage;
|
public event EventHandler BasePosMessage;
|
||||||
public event EventHandler EphMessage;
|
|
||||||
|
|
||||||
public int length
|
public int length
|
||||||
{
|
{
|
||||||
|
|||||||
@ -28,7 +28,7 @@ namespace Plane.FormationCreator.ViewModels
|
|||||||
{
|
{
|
||||||
// serialport
|
// serialport
|
||||||
internal static IConnection Rtkport ;
|
internal static IConnection Rtkport ;
|
||||||
private static System.Threading.Thread thrtk;
|
|
||||||
private static bool trkthreadrun = false;
|
private static bool trkthreadrun = false;
|
||||||
|
|
||||||
private static bool rtcmthreadrun = false;
|
private static bool rtcmthreadrun = false;
|
||||||
@ -1371,11 +1371,6 @@ namespace Plane.FormationCreator.ViewModels
|
|||||||
_flightTaskManager.Tasks[j].SingleCopterInfos[i].TargetAlt - groudAlt)
|
_flightTaskManager.Tasks[j].SingleCopterInfos[i].TargetAlt - groudAlt)
|
||||||
);
|
);
|
||||||
break;
|
break;
|
||||||
case FlightTaskType.Loiter:
|
|
||||||
break;
|
|
||||||
case FlightTaskType.ReturnToLand:
|
|
||||||
missions.Add(Mission.CreateReturnToLaunchMission());
|
|
||||||
break;
|
|
||||||
|
|
||||||
case FlightTaskType.Land:
|
case FlightTaskType.Land:
|
||||||
missions.AddRange(CreateLEDMissions(_flightTaskManager.Tasks[j].SingleCopterInfos[i].LEDInfos));
|
missions.AddRange(CreateLEDMissions(_flightTaskManager.Tasks[j].SingleCopterInfos[i].LEDInfos));
|
||||||
|
|||||||
@ -248,8 +248,7 @@ namespace Plane.FormationCreator.ViewModels
|
|||||||
for (int i = _startindex; i <= _endindex; i++)
|
for (int i = _startindex; i <= _endindex; i++)
|
||||||
{
|
{
|
||||||
|
|
||||||
if ((_flightTaskManager.Tasks[i].TaskType == FlightTaskType.TakeOff) ||
|
if ((_flightTaskManager.Tasks[i].TaskType == FlightTaskType.TakeOff))
|
||||||
(_flightTaskManager.Tasks[i].TaskType == FlightTaskType.ReturnToLand))
|
|
||||||
continue;
|
continue;
|
||||||
|
|
||||||
|
|
||||||
@ -1124,13 +1123,7 @@ public ICommand VerticlAlignmentCommand
|
|||||||
{
|
{
|
||||||
|
|
||||||
|
|
||||||
if ((_flightTaskManager.Tasks[i].TaskType == FlightTaskType.Loiter) && (_flightTaskManager.Tasks[i].ChangeYaw))
|
for (int j = 0; j < _flightTaskManager.Tasks[i].SingleCopterInfos.Count; j++)
|
||||||
{
|
|
||||||
_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;
|
copterlng=_flightTaskManager.Tasks[i].SingleCopterInfos[j].TargetLng;
|
||||||
copterlat = _flightTaskManager.Tasks[i].SingleCopterInfos[j].TargetLat;
|
copterlat = _flightTaskManager.Tasks[i].SingleCopterInfos[j].TargetLat;
|
||||||
@ -1430,8 +1423,7 @@ public ICommand VerticlAlignmentCommand
|
|||||||
for (int i = _startindex; i <= _endindex; i++)
|
for (int i = _startindex; i <= _endindex; i++)
|
||||||
{
|
{
|
||||||
|
|
||||||
if ((_flightTaskManager.Tasks[i].TaskType == FlightTaskType.TakeOff) ||
|
if ((_flightTaskManager.Tasks[i].TaskType == FlightTaskType.TakeOff))
|
||||||
(_flightTaskManager.Tasks[i].TaskType == FlightTaskType.ReturnToLand))
|
|
||||||
continue;
|
continue;
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user