private void OnNavdataPacketAcquired(NavigationPacket packet)
{
if (NavigationPacketAcquired != null)
NavigationPacketAcquired(packet);
UpdateNavigationData(packet);
}
private void MessageReceived(DatagramSocket socket, DatagramSocketMessageReceivedEventArgs eventArguments)
{
DataReader reader;
try
{
reader = eventArguments.GetDataReader();
}
catch (Exception ex)
{
Debug.WriteLine("MessageReceived:" + ex);
return;
}
uint dataLength = reader.UnconsumedBufferLength;
byte[] data = new byte[dataLength];
reader.ReadBytes(data);
var packet = new NavigationPacket
{
Timestamp = DateTime.UtcNow.Ticks,
Data = data
};
UpdateNavigationData(packet);
_TimeoutStopWatch.Restart();
}
private void UpdateNavigationData(NavigationPacket packet)
{
NavigationData navigationData;
if (TryParseNavigationPacket(ref packet, out navigationData))
{
_DroneClient.NavigationData = navigationData;
_DroneClient.NavigationDataViewModel.Update(navigationData);
if (navigationData.Masks.HasFlag(def_ardrone_state_mask_t.ARDRONE_COM_WATCHDOG_MASK))
{
_DroneClient.PostCommand(Command.Watchdog());
}
if (navigationData.Masks.HasFlag(def_ardrone_state_mask_t.ARDRONE_COMMAND_MASK))
{
if (consecutiveCommandMask >= 5)
{
_DroneClient.FlushConfigCommands();
consecutiveCommandMask = 0;
}
else
{
consecutiveCommandMask++;
}
}
else
{
consecutiveCommandMask = 0;
}
//TODO gérer les autres MASKS (LOW Battery, Too much wind) pour la gestion d'alertes
}
}
public static unsafe bool TryParse(ref NavigationPacket packet, out NavdataBag navigationData)
{
byte[] data = packet.Data;
navigationData = new NavdataBag();
if (data.Length < sizeof(navdata_t))
return(false);
fixed(byte *pData = &data[0])
{
navdata_t navdata = *(navdata_t *)pData;
if (navdata.header == NavdataHeader)
{
navigationData.ardrone_state = navdata.ardrone_state;
int offset = sizeof(navdata_t);
while (offset < data.Length)
{
var option = (navdata_option_t *)(pData + offset);
ProcessOption(option, ref navigationData);
offset += option->size;
}
uint dataCheckSum = CalculateChecksum(data);
if (navigationData.cks.cks == dataCheckSum)
{
return(true);
}
}
}
return(false);
}
public static unsafe bool TryParse(ref NavigationPacket packet, out NavdataBag navigationData)
{
byte[] data = packet.Data;
navigationData = new NavdataBag();
if (data.Length < sizeof (navdata_t))
return false;
fixed (byte* pData = &data[0])
{
navdata_t navdata = *(navdata_t*) pData;
if (navdata.header == NavdataHeader)
{
navigationData.ardrone_state = navdata.ardrone_state;
int offset = sizeof (navdata_t);
while (offset < data.Length)
{
var option = (navdata_option_t*) (pData + offset);
ProcessOption(option, ref navigationData);
offset += option->size;
}
uint dataCheckSum = CalculateChecksum(data);
if (navigationData.cks.cks == dataCheckSum)
{
return true;
}
}
}
return false;
}
/// <summary>
/// 线程解析数据
/// </summary>
/// <param name="token"></param>
/// <returns></returns>
protected override void Loop(CancellationToken token)
{
//创立文件流
using (var stream = new FileStream(_path, FileMode.Open))
//创立包解析类
using (var reader = new PacketReader(stream))
{
while (stream.Position < stream.Length && token.IsCancellationRequested == false)
{
PacketType packetType = reader.ReadPacketType();
switch (packetType)
{
case PacketType.Navigation:
NavigationPacket navigationPacket = reader.ReadNavigationPacket();
_navigationPacketAcquired(navigationPacket);
break;
case PacketType.Video:
VideoPacket videoPacket = reader.ReadVideoPacket();
_videoPacketAcquired(videoPacket);
break;
default:
throw new ArgumentOutOfRangeException();
}
}
}
if (OnFileEnd != null)
{
OnFileEnd();
}
}
protected override void Loop(CancellationToken token)
{
using (var udpClient = new UdpClient(NavdataPort))
{
udpClient.Connect(_configuration.DroneHostname, NavdataPort);
SendKeepAliveSignal(udpClient);
var droneEp = new IPEndPoint(IPAddress.Any, NavdataPort);
Stopwatch swKeepAlive = Stopwatch.StartNew();
Stopwatch swNavdataTimeout = Stopwatch.StartNew();
while (token.IsCancellationRequested == false &&
swNavdataTimeout.ElapsedMilliseconds < NavdataTimeout)
{
if (udpClient.Available > 0)
{
byte[] data = udpClient.Receive(ref droneEp);
var packet = new NavigationPacket
{
Timestamp = DateTime.UtcNow.Ticks,
Data = data
};
_navigationPacketAcquired(packet);
swNavdataTimeout.Restart();
}
if (swKeepAlive.ElapsedMilliseconds > KeepAliveTimeout)
{
SendKeepAliveSignal(udpClient);
swKeepAlive.Restart();
}
Thread.Sleep(5);
}
}
}
private void OnNavigationPacketAcquired(NavigationPacket packet)
{
if (_packetRecorderWorker != null && _packetRecorderWorker.IsAlive)
{
_packetRecorderWorker.EnqueuePacket(packet);
}
_navigationPacket = packet;
}
/// <summary>
/// 写导航数据文件
/// NavigationData
/// </summary>
/// <param name="navigationPacket"></param>
/// <param name="navigationData"></param>
/// <returns></returns>
private void Write(NavigationPacket navigationPacket, NavigationData navigationData)
{
//_navigationDataWriter.WriteLine(navigationPacket.Timestamp + " " + navigationData.Yaw
// + " " + navigationData.Pitch + " " + navigationData.Roll + " " + navigationData.Altitude
// + " " + navigationData.Velocity.X + " " + navigationData.Velocity.Y + " " + navigationData.Velocity.Z);
_navigationDataWriter.WriteLine(navigationData.GPS.latitude + " " + navigationData.GPS.longitude + " " +
navigationData.GPS.latFuse + " " + navigationData.GPS.lonFuse);
_navigationDataWriter.Flush();
}
private void OnNavdataPacketAcquired(NavigationPacket packet)
{
if (NavigationPacketAcquired != null)
{
NavigationPacketAcquired(packet);
}
UpdateNavigationData(packet);
}
protected override void Loop(CancellationToken token)
{
_isAcquiring = false;
using (var udpClient = new UdpClient(NavdataPort))
try
{
udpClient.Connect(_configuration.DroneHostname, NavdataPort);
SendKeepAliveSignal(udpClient);
var remoteEp = new IPEndPoint(IPAddress.Any, NavdataPort);
Stopwatch swKeepAlive = Stopwatch.StartNew();
Stopwatch swNavdataTimeout = Stopwatch.StartNew();
while (token.IsCancellationRequested == false &&
swNavdataTimeout.ElapsedMilliseconds < NavdataTimeout)
{
if (udpClient.Available == 0)
{
Thread.Sleep(1);
}
else
{
byte[] data = udpClient.Receive(ref remoteEp);
var packet = new NavigationPacket
{
Timestamp = DateTime.UtcNow.Ticks,
Data = data
};
swNavdataTimeout.Restart();
_isAcquiring = true;
_onAcquisitionStarted();
_packetAcquired(packet);
}
if (swKeepAlive.ElapsedMilliseconds > KeepAliveTimeout)
{
SendKeepAliveSignal(udpClient);
swKeepAlive.Restart();
}
}
}
catch (Exception e)
{
}
finally
{
if (_isAcquiring)
{
_isAcquiring = false;
_onAcquisitionStopped();
}
}
}
private void OnNavigationPacketAcquired(NavigationPacket packet)
{
if (_navigationData.State == NavigationState.Unknown)
_requestedState = RequestedState.Initialize;
if (NavigationPacketAcquired != null)
NavigationPacketAcquired(packet);
UpdateNavigationData(packet);
}
public NavigationPacket ReadNavigationPacket()
{
var packet = new NavigationPacket();
packet.Timestamp = ReadInt64();
int dataSize = ReadInt32();
packet.Data = ReadBytes(dataSize);
return(packet);
}
public static void OnNavigationPacketAcquired(NavigationPacket packet)
{
//check if the worker is valid then send the nav packet to the worker.
if (_packetRecorderWorker != null && _packetRecorderWorker.IsAlive)
{
_packetRecorderWorker.EnqueuePacket(packet);
}
_navigationPacket = packet;
}
public static NavigationPacket ReadNavigationPacket(BinaryReader reader)
{
var packet = new NavigationPacket();
packet.Timestamp = reader.ReadInt64();
int dataSize = reader.ReadInt32();
packet.Data = reader.ReadBytes(dataSize);
return(packet);
}
public static bool TryParse(ref NavigationPacket packet, out NavigationData navigationData)
{
navigationData = new NavigationData();
NavdataBag navdataBag;
if (NavdataBagParser.TryParse(ref packet, out navdataBag))
{
navigationData = NavdataConverter.ToNavigationData(navdataBag);
return true;
}
return false;
}
protected override void Loop(CancellationToken token)
{
_isAcquiring = false;
using (var udpClient = new UdpClient(NavdataPort))
try
{
udpClient.Connect(_configuration.DroneHostname, NavdataPort);
SendKeepAliveSignal(udpClient);
var remoteEp = new IPEndPoint(IPAddress.Any, NavdataPort);
Stopwatch swKeepAlive = Stopwatch.StartNew();
Stopwatch swNavdataTimeout = Stopwatch.StartNew();
while (token.IsCancellationRequested == false && swNavdataTimeout.ElapsedMilliseconds < NavdataTimeout)
{
if (udpClient.Available == 0)
{
Thread.Sleep(1);
}
else
{
byte[] data = udpClient.Receive(ref remoteEp);
var packet = new NavigationPacket
{
Timestamp = DateTime.UtcNow.Ticks,
Data = data
};
swNavdataTimeout.Reset();
swNavdataTimeout.Start();
_isAcquiring = true;
_onAcquisitionStarted();
_packetAcquired(packet);
}
if (swKeepAlive.ElapsedMilliseconds > KeepAliveTimeout)
{
SendKeepAliveSignal(udpClient);
swKeepAlive.Reset();
swKeepAlive.Start();
}
}
}
finally
{
if (_isAcquiring)
{
_isAcquiring = false;
_onAcquisitionStopped();
}
}
}
public static bool TryParse(ref NavigationPacket packet, out NavigationData navigationData)
{
navigationData = new NavigationData();
NavdataBag navdataBag;
if (NavdataBagParser.TryParse(ref packet, out navdataBag))
{
navigationData = NavdataConverter.ToNavigationData(navdataBag);
return(true);
}
return(false);
}
private void UpdateNavigationData(NavigationPacket packet)
{
NavigationData navigationData;
if (NavigationPacketParser.TryParse(ref packet, out navigationData))
{
OnNavigationDataAcquired(navigationData);
_navigationData = navigationData;
ProcessStateTransitions(navigationData.State);
}
}
private void UpdateNavigationData(NavigationPacket packet)
{
NavigationData navigationData;
if (NavigationPacketParser.TryParse(ref packet, out navigationData))
{
OnNavigationDataAcquired(navigationData);
_navigationData = navigationData;
ProcessStateTransitions(navigationData.State);
}
}
private void OnNavigationPacketAcquired(NavigationPacket obj)
{
if (_fileWriter != null && _isWriter)
{
_fileWriter.EnqueuePacket(obj);
}
if (_isWriter)
{
_totalWriteStream.WriteLine("**Nav**");
_totalWriteStream.WriteLine("Nav:" + obj.Timestamp);
_totalWriteStream.WriteLine();
// _totalWriteStream.Flush();
}
}
private void OnNavigationPacketAcquired(NavigationPacket packet)
{
if (_navigationData.State == NavigationState.Unknown)
{
_requestedState = RequestedState.Initialize;
}
if (NavigationPacketAcquired != null)
{
NavigationPacketAcquired(packet);
}
UpdateNavigationData(packet);
}
/// <summary>
/// 写导航数据文件
/// NavgationBag
/// </summary>
/// <param name="navigationPacket"></param>
/// <returns></returns>
private void Write(NavigationPacket navigationPacket)
{
using (FileStream navFileStream = new FileStream(_navdataDir + @"/" + navigationPacket.Timestamp + ".nav", FileMode.OpenOrCreate))
{
using (StreamWriter navWriter = new StreamWriter(navFileStream))
{
NavdataBag navdataBag;
if (navigationPacket.Data != null && NavdataBagParser.TryParse(ref navigationPacket, out navdataBag))
{
DumpBranch(navWriter, navdataBag);
}
}
}
}
private void UpdateNavigationData(NavigationPacket packet)
{
NavigationData navigationData;
if (NavigationPacketParser.TryParse(ref packet, out navigationData))
{
_navigationData = navigationData;
ProcessRequestedState();
if (NavigationDataUpdated != null)
{
NavigationDataUpdated(_navigationData);
}
}
}
/// <summary>
/// This method will handle every packet that's coming from the drone.
/// </summary>
/// <param name="packet"></param>
private void OnNavigationPacketAcquired(NavigationPacket packet)
{
_navigationPacket = packet;
if (_navigationData != null)
{
if (!File.Exists("log.txt"))
{
File.Create("log.txt");
Console.WriteLine("No file");
}
else
{
File.AppendAllText("log.txt", _navigationData.ToString());
}
}
else
{
Console.WriteLine("No navigation data");
}
}
private void UpdateNavigationData(NavigationPacket packet)
{
NavigationData navigationData;
if (NavigationPacketParser.TryParse(ref packet, out navigationData))
{
_navigationData = navigationData;
ProcessRequestedState();
if (NavigationDataUpdated != null)
NavigationDataUpdated(_navigationData);
}
}
private void OnNavigationPacketAcquired(NavigationPacket obj)
{
// do nothing
}
public static void WriteNavigationPacket(BinaryWriter writer, NavigationPacket packet)
{
writer.Write(packet.Timestamp);
writer.Write(packet.Data.Length);
writer.Write(packet.Data);
}
/// <summary>
/// 线程
/// 用来与飞机通讯获取数据
/// </summary>
/// <param name="token"></param>
/// <returns></returns>
protected override void Loop(CancellationToken token)
{
_isAcquiring = false;
//新建UDP客户端端口
//using (var udpClient = new UdpClient(NavdataPort))
//测试不绑定本地端口 有一个会看不到导航数据
using (var udpClient = new UdpClient())
try
{
//与飞机连接
udpClient.Connect(_configuration.DroneHostname, NavdataPort);
//发数据确认连接可行
SendKeepAliveSignal(udpClient);
//接收任何ip传入NavdataPort的数据
//var remoteEp = new IPEndPoint(IPAddress.Any, NavdataPort);
//只接受固定ip
var remoteEp = new IPEndPoint(IPAddress.Parse(_configuration.DroneHostname), NavdataPort);
//定位超时时间的变量
Stopwatch swKeepAlive = Stopwatch.StartNew();
Stopwatch swNavdataTimeout = Stopwatch.StartNew();
while (token.IsCancellationRequested == false && swNavdataTimeout.ElapsedMilliseconds < NavdataTimeout)
{
//udp客户端不可以使用时让线程休眠
if (udpClient.Available == 0)
{
Thread.Sleep(1);
}
else
{
//接收数据，阻塞式
byte[] data = udpClient.Receive(ref remoteEp);
//创建新的packet
var packet = new NavigationPacket
{
Timestamp = DateTime.UtcNow.Ticks,
Data = data
};
//重启超时计时器
swNavdataTimeout.Restart();
//设置正在获取状态为真
_isAcquiring = true;
_onAcquisitionStarted();
//发送以获取到的包
_packetAcquired(packet);
}
if (swKeepAlive.ElapsedMilliseconds > KeepAliveTimeout)
{
SendKeepAliveSignal(udpClient);
swKeepAlive.Restart();
}
}
}
finally
{
//若之前状态为正在获取
//就将其关闭，并发送事件
if (_isAcquiring)
{
_isAcquiring = false;
_onAcquisitionStopped();
}
}
}
public void Write(NavigationPacket packet)
{
Write(packet.Timestamp);
Write(packet.Data.Length);
Write(packet.Data);
}
public void EnqueuePacket(NavigationPacket packet)
{
_packetQueue.Enqueue(packet);
}
