void VideoReceiver_DataReceived(object sender, DataReceivedEventArgs e)
{
// try
// {
videoImage.AddImageStream(e.Data);
// }
// catch (Exception ex)
// {
// drone.debug += ex.Message;
// }
}
public void DataReceived(object sender, DataReceivedEventArgs e)
{
if (e.Source.Address.Equals( this.drone.IPAddress))
{
MemoryStream ms = new MemoryStream(e.Data);
byte[] rawHeader = new byte[16]; //navData header size
ms.Read(rawHeader, 0 , rawHeader.Length);
unsafe { fixed (byte* header = &rawHeader[0]) { dataHeader = *(NavDataHeader*)header; } }
if (dataHeader.Header == 0x55667788)
{
SetStatus();
while (ms.Position < ms.Length)
{
byte[] rawOptionHeader = new byte[4];
ms.Read(rawOptionHeader, 0, rawOptionHeader.Length);
ushort tag = BitConverter.ToUInt16(rawOptionHeader,0);
ushort size = BitConverter.ToUInt16(rawOptionHeader,2);
byte[] rawOptionBody = new byte[size-4];
ms.Read(rawOptionBody, 0, rawOptionBody.Length);
switch (tag)
{
case 0xFFFF:
long chksum = BitConverter.ToUInt32(rawOptionBody,0);
break;
case 0: //navdata_demo
unsafe { fixed (byte* optionBody = &rawOptionBody[0]) { dataDemo = *(NavDataDemo*)optionBody; } }
OnNavDataReceived(EventArgs.Empty);
break;
case 1: //time tag
uint newTime = BitConverter.ToUInt32(rawOptionBody, 0);
int _dTime = (int)(newTime - timeStamp);
if (_dTime > 0)
dTime = _dTime;
timeStamp = newTime;
break;
default: // TODO: approx 20 more navdata options
break;
}
}
}
}
}
protected virtual void OnDataReceived(DataReceivedEventArgs e)
{
EventHandler<DataReceivedEventArgs> handler = DataReceived;
if (handler != null)
handler(this, e);
}
private void navDataReceived(object sender, DataReceivedEventArgs e)
{
for (int i=0; i<drones.Count; i++)
drones[i].NavData.DataReceived(sender, e);
}