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 (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
}
}
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 bool TryParseNavigationPacket(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 UInt32 navigationDataCheckSum;
public static bool TryParse(ref NavigationPacket packet, out NavdataBag navigationData)
{
navigationData = new NavdataBag();
navdata_tag_t navigationDataTag = 0;
UInt16 size = 0;
using (MemoryStream memoryStream = new MemoryStream(packet.Data))
{
BinaryReader binaryReader = new BinaryReader(memoryStream);
navigationData.navigationDataHeader.header = binaryReader.ReadUInt32();
navigationData.navigationDataHeader.ardrone_state = binaryReader.ReadUInt32();
navigationData.navigationDataHeader.sequence = binaryReader.ReadUInt32();
navigationData.navigationDataHeader.vision_defined = binaryReader.ReadUInt32();
memoryStream.Position = navigationDataHeaderSize;
if (navigationData.navigationDataHeader.header == NavdataHeader)
{
while (memoryStream.Position < memoryStream.Length)
{
navigationDataTag = (navdata_tag_t)binaryReader.ReadUInt16();
size = binaryReader.ReadUInt16();
switch (navigationDataTag)
{
case navdata_tag_t.NAVDATA_DEMO:
MapNavigationData(packet.Data, (int)(memoryStream.Position - 4), ref navigationData);
memoryStream.Position += size - 4;
break;
case navdata_tag_t.NAVDATA_CKS:
navigationData.cks.cks = binaryReader.ReadUInt32();
break;
case navdata_tag_t.NAVDATA_VISION_DETECT:
MapVisionDetect(packet.Data, (int)(memoryStream.Position - 4), ref navigationData);
memoryStream.Position += size - 4;
break;
case navdata_tag_t.NAVDATA_MAGNETO:
MapMagneto(packet.Data, (int)(memoryStream.Position - 4), ref navigationData);
memoryStream.Position += size - 4;
break;
case navdata_tag_t.NAVDATA_TIME:
navigationData.time.time = binaryReader.ReadUInt32();
break;
case navdata_tag_t.NAVDATA_RAW_MEASURES:
MapRawMeasures(packet.Data, (int)(memoryStream.Position - 4), ref navigationData);
break;
case navdata_tag_t.NAVDATA_ADC_DATA_FRAME:
MapAdcDataFrame(packet.Data, (int)(memoryStream.Position - 4), ref navigationData);
break;
case navdata_tag_t.NAVDATA_GYROS_OFFSETS:
MapGyrosOffsets(packet.Data, (int)(memoryStream.Position - 4), ref navigationData);
break;
case navdata_tag_t.NAVDATA_PHYS_MEASURES:
MapPhysMeasures(packet.Data, (int)(memoryStream.Position - 4), ref navigationData);
break;
case navdata_tag_t.NAVDATA_VISION_OF:
MapVisionOfTag(packet.Data, (int)(memoryStream.Position - 4), ref navigationData);
break;
default:
//Currently not used
memoryStream.Position += size - 4; // substract 4 bytes for Tag and Size field
break;
}
}
}
uint dataCheckSum = CalculateChecksum(packet.Data);
if (navigationData.cks.cks == dataCheckSum)
{
return(true);
}
}
return(false);
}
