public NavData(ARDrone Drone)
{
this.drone = Drone;
this.drone.StatusChanged += new EventHandler<DroneStatusChangedEventArgs>(DroneStatusChanged);
statusData = new bool[32];
dataHeader = new NavDataHeader();
dataDemo = new NavDataDemo();
timeStamp = 0;
dTime = 66;
initTimer = new System.Timers.Timer(200);
initTimer.AutoReset = true;
initTimer.Elapsed += delegate { SendInit(); };
}
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;
}
}
}
}
}
