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); }