273 lines
9.7 KiB
C#
273 lines
9.7 KiB
C#
// 读写任务序列的标准流程： http://qgroundcontrol.org/mavlink/waypoint_protocol
|
||
|
||
using System.Collections.Generic;
|
||
using System.Diagnostics;
|
||
using System.Linq;
|
||
using System.Threading.Tasks;
|
||
using static Plane.Protocols.MAVLink;
|
||
|
||
namespace Plane.Copters
|
||
{
|
||
public partial class PLCopter
|
||
{
|
||
private static readonly IMission PRE_TAKE_OFF_MISSION = new Mission
|
||
{
|
||
Command = FlightCommand.Waypoint,
|
||
Sequence = 0
|
||
};
|
||
|
||
private static readonly IMission TAKE_OFF_MISSION = new Mission
|
||
{
|
||
Command = FlightCommand.TakeOff,
|
||
Altitude = 10,
|
||
Sequence = 1
|
||
};
|
||
|
||
/// <summary>
|
||
/// 飞行器正在请求的任务序号。
|
||
/// </summary>
|
||
private ushort? _copterRequestingMissionSeq;
|
||
|
||
/// <summary>
|
||
/// 指示是否正在查询任务序列。
|
||
/// </summary>
|
||
private bool _isRequestingMissionList;
|
||
|
||
/// <summary>
|
||
/// 指示是否正在写入任务序列。
|
||
/// </summary>
|
||
private bool _isWritingMissionList;
|
||
|
||
/// <summary>
|
||
/// 向飞行器查询得到的任务序列。
|
||
/// </summary>
|
||
private List<IMission> _missionList;
|
||
|
||
/// <summary>
|
||
/// 用于判断查询任务序列操作超时的秒表。
|
||
/// </summary>
|
||
private Stopwatch _requestMissionListStopwatch;
|
||
|
||
/// <summary>
|
||
/// 写任务操作的结果。
|
||
/// </summary>
|
||
private MAV_MISSION_RESULT? _writeMissionListResult;
|
||
|
||
/// <summary>
|
||
/// 用于判断写入任务序列操作超时的秒表。
|
||
/// </summary>
|
||
private Stopwatch _writeMissionListStopwatch;
|
||
|
||
/// <summary>
|
||
/// 查询任务序列。若上一个操作未完成，本次结果为 null。
|
||
/// </summary>
|
||
/// <param name="millisecondsTimeout">超时时间。</param>
|
||
/// <returns>飞行器返回的任务序列。若上一个操作未完成，本次结果为 null；若超时，结果为不完整的序列（_mission.Count 小于 MissionCount）；若顺利完成，结果为完整的任务序列。</returns>
|
||
public async Task<IEnumerable<IMission>> RequestMissionListAsync(int millisecondsTimeout = 10000)
|
||
{
|
||
if (_isRequestingMissionList) return null;
|
||
_isRequestingMissionList = true;
|
||
|
||
MissionCount = null;
|
||
|
||
if (_missionList == null) _missionList = new List<IMission>();
|
||
else _missionList.Clear();
|
||
|
||
if (_requestMissionListStopwatch == null) _requestMissionListStopwatch = Stopwatch.StartNew();
|
||
else _requestMissionListStopwatch.Restart();
|
||
|
||
// 查询任务总数。
|
||
await _internalCopter.GetWPsAsync().ConfigureAwait(false);
|
||
|
||
// 异步等待 mavlink_mission_count_t 消息（收到后会赋值给 MissionCount）。
|
||
while (MissionCount == null)
|
||
{
|
||
if (_requestMissionListStopwatch.ElapsedMilliseconds >= millisecondsTimeout)
|
||
{
|
||
// 超时。
|
||
return EndRequestingMissionList();
|
||
}
|
||
await Task.Delay(50).ConfigureAwait(false);
|
||
}
|
||
|
||
for (ushort i = 0; i < MissionCount; i++)
|
||
{
|
||
// 查询 i 号任务。
|
||
await _internalCopter.GetWPAsync(i).ConfigureAwait(false);
|
||
|
||
// 异步等待 mavlink_mission_item_t 消息（收到后会把 Mission 对象放到 _missions 列表的合适位置）。
|
||
while (_missionList.Count <= i)
|
||
{
|
||
if (_requestMissionListStopwatch.ElapsedMilliseconds >= millisecondsTimeout)
|
||
{
|
||
// 超时。
|
||
return EndRequestingMissionList();
|
||
}
|
||
await Task.Delay(50).ConfigureAwait(false);
|
||
}
|
||
}
|
||
|
||
// 成功接收了全部任务，发送 Ack。
|
||
await SendMissionAckAsync();
|
||
|
||
// 清理各字段并返回任务序列。
|
||
return EndRequestingMissionList();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 写入任务序列。
|
||
/// </summary>
|
||
/// <param name="missions">任务序列。</param>
|
||
/// <param name="millisecondsTimeout">超时时间，单位为毫秒。</param>
|
||
/// <returns>若确定写入成功，返回 true；否则（上一个写任务操作未完成，或者飞控返回失败消息，或者超时）返回 false。</returns>
|
||
public async Task<bool> WriteMissionListAsync(IEnumerable<IMission> missions, int millisecondsTimeout = 10000)
|
||
{
|
||
if (_isWritingMissionList) return false;
|
||
|
||
_isWritingMissionList = true;
|
||
_writeMissionListResult = null;
|
||
_copterRequestingMissionSeq = null;
|
||
|
||
if (_writeMissionListStopwatch == null) _writeMissionListStopwatch = Stopwatch.StartNew();
|
||
else _writeMissionListStopwatch.Restart();
|
||
|
||
// 写任务总数。
|
||
await WriteMissionCountAsync((ushort)(missions.Count() + 1)).ConfigureAwait(false);
|
||
|
||
// 任务序号。
|
||
ushort seq = 0;
|
||
|
||
// 等待飞控请求 seq 号任务。
|
||
if (!await AwaitMissionRequestAsync(seq, millisecondsTimeout).ConfigureAwait(false))
|
||
{
|
||
// 超时。
|
||
return EndWritingMissions();
|
||
}
|
||
|
||
// 写起飞前准备任务。
|
||
await WriteMissionAsync(PRE_TAKE_OFF_MISSION).ConfigureAwait(false);
|
||
/*
|
||
seq++;
|
||
|
||
// 等待飞控请求 seq 号任务。
|
||
if (!await AwaitMissionRequestAsync(seq, millisecondsTimeout).ConfigureAwait(false))
|
||
{
|
||
// 超时。
|
||
return EndWritingMissions();
|
||
}
|
||
|
||
// 写起飞任务。
|
||
await WriteMissionAsync(TAKE_OFF_MISSION).ConfigureAwait(false);
|
||
*/
|
||
|
||
foreach (var mission in missions)
|
||
{
|
||
// 等待飞控请求 seq 号任务。
|
||
if (!await AwaitMissionRequestAsync(++seq, millisecondsTimeout).ConfigureAwait(false))
|
||
{
|
||
// 超时。
|
||
return EndWritingMissions();
|
||
}
|
||
mission.Sequence = seq;
|
||
await WriteMissionAsync(mission).ConfigureAwait(false);
|
||
}
|
||
|
||
// 等待飞控返回 Ack。
|
||
while (_writeMissionListResult == null && _writeMissionListStopwatch.ElapsedMilliseconds < millisecondsTimeout)
|
||
{
|
||
await Task.Delay(50).ConfigureAwait(false);
|
||
}
|
||
|
||
return EndWritingMissions();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 异步等待飞行器向地面站请求 seq 号任务。
|
||
/// </summary>
|
||
/// <param name="seq">任务序号。</param>
|
||
/// <param name="millisecondsTimeout">超时时间。</param>
|
||
/// <returns>若未超时并成功收到请求，返回 true；否则返回 false。</returns>
|
||
private async Task<bool> AwaitMissionRequestAsync(ushort seq, int millisecondsTimeout)
|
||
{
|
||
while (_copterRequestingMissionSeq != seq)
|
||
{
|
||
if (_writeMissionListStopwatch.ElapsedMilliseconds >= millisecondsTimeout)
|
||
{
|
||
return true;
|
||
}
|
||
await Task.Delay(50).ConfigureAwait(false);
|
||
}
|
||
return true;
|
||
}
|
||
|
||
private IEnumerable<IMission> EndRequestingMissionList()
|
||
{
|
||
_requestMissionListStopwatch.Stop();
|
||
_isRequestingMissionList = false;
|
||
return _missionList;
|
||
}
|
||
|
||
private bool EndWritingMissions()
|
||
{
|
||
_writeMissionListStopwatch.Stop();
|
||
_isWritingMissionList = false;
|
||
return _writeMissionListResult == MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED;
|
||
}
|
||
|
||
private async Task SendMissionAckAsync()
|
||
{
|
||
var ack = new mavlink_mission_ack_t
|
||
{
|
||
target_component = _internalCopter.compid,
|
||
target_system = _internalCopter.sysid,
|
||
type = (byte)MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED
|
||
};
|
||
await _internalCopter.GeneratePacketAsync(MAVLINK_MSG_ID_MISSION_ACK, ack).ConfigureAwait(false);
|
||
}
|
||
|
||
private Task WriteMissionAsync(IMission mission)
|
||
{
|
||
var frame = mission.Command == FlightCommand.Waypoint && mission.Sequence == 0 ? MAV_FRAME.GLOBAL : MAV_FRAME.GLOBAL_RELATIVE_ALT;
|
||
|
||
var req = new mavlink_mission_item_t();
|
||
|
||
req.target_system = _internalCopter.sysid;
|
||
req.target_component = _internalCopter.compid;
|
||
|
||
req.command = (byte)mission.Command;
|
||
|
||
req.current = 0;
|
||
req.autocontinue = 1;
|
||
|
||
req.frame = (byte)frame;
|
||
if (mission.Command == FlightCommand.LEDControl)
|
||
{
|
||
req.x = mission.R;
|
||
req.y = mission.G;
|
||
req.z = mission.B;
|
||
}
|
||
else
|
||
{
|
||
req.x = (float)mission.Latitude;
|
||
req.y = (float)mission.Longitude;
|
||
req.z = mission.Altitude;
|
||
}
|
||
|
||
|
||
req.param1 = mission.Param1;
|
||
req.param2 = mission.Param2;
|
||
req.param3 = mission.Param3;
|
||
req.param4 = mission.Param4;
|
||
|
||
req.seq = mission.Sequence;
|
||
|
||
return _internalCopter.GeneratePacketAsync(MAVLINK_MSG_ID_MISSION_ITEM, req);
|
||
}
|
||
|
||
private Task WriteMissionCountAsync(ushort count)
|
||
{
|
||
return _internalCopter.SetWPTotalAsync(count);
|
||
}
|
||
}
|
||
}
|