/// <summary> /// Returns the header portion of the given data /// </summary> /// <param name="data"></param> /// <returns></returns> private static Tuple <byte, byte, byte, UInt32> _read_header(byte[] data) { var Result = StructConverter.Unpack("<BBBI", data.SubArray(":7")); byte data_type = (byte)Result[0]; byte buffer_id = (byte)Result[1]; byte seq_no = (byte)Result[2]; UInt32 frame_size = (UInt32)Result[3]; return(Tuple.Create(data_type, buffer_id, seq_no, frame_size)); }
/// <summary> /// Returns true if the given command is a pcmd command and false otherwise /// </summary> /// <param name="cmd"></param> /// <returns></returns> public static bool _is_pcmd(byte[] cmd) { // BBHBbb: Header (7) + payload (7) if (cmd.Length != 14) { return(false); } var Result = StructConverter.Unpack("<BBH", cmd.SubArray("7:11")); return(Tuple.Create <byte, byte, UInt16>((byte)Result[0], (byte)Result[1], (UInt16)Result[2]).Equals(Tuple.Create <byte, byte, UInt16>(3, 0, 0))); }
private PacketHeader GetPacketHeader(byte[] binaryData) { var packetHeader = new PacketHeader(); try { var chunkData = binaryData.Skip(0).Take(1).ToArray(); var data = StructConverter.Unpack("b", chunkData); packetHeader.PacketType = Convert.ToInt32(data[0]); chunkData = binaryData.Skip(1).Take(2).ToArray(); data = StructConverter.Unpack("h", chunkData); packetHeader.PacketSubtype = Convert.ToInt16(data[0]); chunkData = binaryData.Skip(3).Take(1).ToArray(); data = StructConverter.Unpack("b", chunkData); packetHeader.StreamId = Convert.ToInt32(data[0]); chunkData = binaryData.Skip(4).Take(8).ToArray(); data = StructConverter.Unpack("d", chunkData); packetHeader.StartTime = Convert.ToDouble(data[0]); chunkData = binaryData.Skip(12).Take(8).ToArray(); data = StructConverter.Unpack("d", chunkData); packetHeader.EndTime = Convert.ToDouble(data[0]); chunkData = binaryData.Skip(20).Take(4).ToArray(); data = StructConverter.Unpack("I", chunkData); packetHeader.PacketSize = Convert.ToUInt32(data[0]); chunkData = binaryData.Skip(24).Take(4).ToArray(); data = StructConverter.Unpack("I", chunkData); packetHeader.ParameterSize = Convert.ToUInt32(data[0]); } catch (Exception ex) { Console.WriteLine(ex.Message); } return(packetHeader); }
private void _process_stream_frame(byte buffer_id, UInt32 frame_size, byte[] payload) { if (buffer_id == SumoConstantsCustom.NETWORK_DC_VIDEO_DATA_ID) { var Result = StructConverter.Unpack("<HBBB", payload.SubArray(":5")); UInt16 frame_no = (UInt16)Result[0]; byte frame_flags = (byte)Result[1]; byte frag_no = (byte)Result[2]; byte frags_per_frame = (byte)Result[3]; byte[] fragment = payload.SubArray("5:"); // We got a fragment for a different frame if (frame_no != this.current_videoframe_no) { // Reset frame number and fragment buffer this.current_videoframe_no = frame_no; this.VideoParts = new List <byte[]>(frags_per_frame); //{ null } * frags_per_frame; } if (VideoParts != null) { if (this.VideoParts.Count >= frag_no + 1) { if (this.VideoParts.ElementAt(frag_no) != null) { LOGGER.GetInstance.Debug(String.Format("[Video] Duplicate fragment | Frame: {0}, Fragment: {1}", frame_no, frag_no)); return; } } this.VideoParts.Add(fragment); // We've received the entire frame if (!this.VideoParts.Contains(null)) { lock (mutex_frames_Lock) { this.VideoFrames.Add(this.VideoParts.SelectMany(a => a).ToArray()); } } } } else if (buffer_id == SumoConstantsCustom.NETWORK_DC_SOUND_DATA_ID && _audioPlayer != null) { // Process Audio stream on Night/Race Drone //LOGGER.GetInstance.Debug(String.Format("Received Audio Stream ! ")); var Result = StructConverter.Unpack("<HBBB", payload.SubArray(":5")); UInt16 frame_no = (UInt16)Result[0]; byte frame_flags = (byte)Result[1]; byte frag_no = (byte)Result[2]; byte frags_per_frame = (byte)Result[3]; // Originaly 5: , I think first 4 byte is for Audio information... or frame Header ? I dont know... // maybe because Audio Frame = HEADER_SIZE = 16 + DATA_SIZE = 256 // From : https://github.com/Parrot-Developers/libARController/blob/master/JNI/java/com/parrot/arsdk/arcontroller/ARAudioFrame.java // https://github.com/Parrot-Developers/libARController/blob/master/Sources/ARCONTROLLER_AudioHeader.h // So Skip 16 first byte (5 frame header + 11 Audio Header) byte[] fragment = payload.SubArray("7:"); // Write it in the shared Stream _audioPlayer.OnDataReceived(fragment); // Debug /* * byte[] AudioHeader = payload; //.SubArray(":16"); * var r = StructConverter.Unpack("<QHHI", AudioHeader); * * using (var stream = new FileStream("AudioHeader.dat", FileMode.Append)) * { * stream.Write(fragment, 0, fragment.Length); * stream.Write(new byte[] { 0x0d, 0x0a }, 0, 2); * } */ } else { // Stream data from another low-latency buffer (maybe audio?) LOGGER.GetInstance.Debug(String.Format("Unknow ARStream | buffer: {0}, size: {1}", buffer_id, frame_size - 7)); return; } }
private void _process_frame(byte[] frame) { var Result = _read_header(frame); byte data_type = Result.Item1; byte buffer_id = Result.Item2; byte seq_no = Result.Item3; UInt32 frame_size = Result.Item4; byte[] payload = frame.SubArray("7:"); // We received an ACK for a packet we sent if (data_type == (byte)SumoConstantsCustom.ARNETWORKAL_FRAME_TYPE.ARNETWORKAL_FRAME_TYPE_ACK) { LOGGER.GetInstance.Debug("ACK packet received"); } else if (data_type == (byte)SumoConstantsCustom.ARNETWORKAL_FRAME_TYPE.ARNETWORKAL_FRAME_TYPE_DATA || data_type == (byte)SumoConstantsCustom.ARNETWORKAL_FRAME_TYPE.ARNETWORKAL_FRAME_TYPE_DATA_WITH_ACK) { // Frame requires an ACK, send one if (data_type == (byte)SumoConstantsCustom.ARNETWORKAL_FRAME_TYPE.ARNETWORKAL_FRAME_TYPE_DATA_WITH_ACK) { byte[] ack = _create_ack_packet(data_type, buffer_id, seq_no); this._sender.SendAck(ack); //LOGGER.GetInstance.Debug(String.Format("Sending ACK for {0} {1} {2} {3}", data_type, buffer_id, seq_no, frame_size)); } // We received a data packet var Result1 = StructConverter.Unpack("<BBH", payload.SubArray(":4")); byte cmd_project = (byte)Result1[0]; byte cmd_class = (byte)Result1[1]; UInt16 cmd_id = (UInt16)Result1[2]; if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.common_SettingsState_ProductNameChanged)) { // Product name Changed var name = Encoding.ASCII.GetString(payload.SubArray("4:")).Trim('\0'); LOGGER.GetInstance.Info(String.Format("Drone Name: {0}", name)); } else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.common_CommonState_CurrentDateChanged)) { var date = payload.SubArray("4:"); LOGGER.GetInstance.Info(String.Format("Date updated to: {0}", Encoding.ASCII.GetString(date))); } else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.common_CommonState_CurrentTimeChanged)) { var time = payload.SubArray("4:"); LOGGER.GetInstance.Info(String.Format("Time updated to: {0}", Encoding.ASCII.GetString(time))); } else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.common_CommonState_WifiSignalChanged)) { Int16 rssi = (Int16)StructConverter.Unpack("<h", payload.SubArray("4:"))[0]; _sumoInformations.Rssi = rssi; var evt = new SumoEventArgs(SumoEnumCustom.TypeOfEvents.RSSI, _sumoInformations); OnSumoEvents(evt); } else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.common_CommonState_BatteryStateChanged)) { var battery = payload.SubArray("4:5")[0]; BatteryLevel = battery; _sumoInformations.BatteryLevel = BatteryLevel; var BatEvt = new SumoEventArgs(SumoEnumCustom.TypeOfEvents.BatteryLevelEvent, _sumoInformations); OnSumoEvents(BatEvt); } else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.common_CommonState_AllStatesChanged)) { // All states have been sent var State = payload.SubArray("4:"); LOGGER.GetInstance.Debug(String.Format("All states have been sent: {0}", BitConverter.ToString(State))); } else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.common_CommonState_ProductModel)) { // Device model byte Model = payload.SubArray("4:")[0]; LOGGER.GetInstance.Info(String.Format("Device Model: {1} {0}", Model, (SumoEnumGenerated.ProductModel_model)Model).ToString()); // Add Capabilities _sumoInformations.BuildCapabilities((SumoEnumGenerated.ProductModel_model)Model); var CapEvt = new SumoEventArgs(SumoEnumCustom.TypeOfEvents.CapabilitiesChange, _sumoInformations); OnSumoEvents(CapEvt); } else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.common_HeadlightsState_intensityChanged)) { var HeadData = StructConverter.Unpack("<BB", payload.SubArray("4:")); LOGGER.GetInstance.Debug(String.Format("Headlight change: {0} {1}", HeadData[0], HeadData[1])); } else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.common_AudioState_AudioStreamingRunning)) { var AudioStateData = payload.SubArray("4:")[0]; if ((AudioStateData & (1 << 0)) != 0) { LOGGER.GetInstance.Info(String.Format("Drone is ready to stream to controller (Drone TX ON)")); if (_audioPlayer != null) { _audioPlayer.Start(); } } else if ((AudioStateData & (1 << 0)) == 0) { LOGGER.GetInstance.Info(String.Format("Drone has cancelled to stream to controller (Drone TX OFF)")); if (_audioPlayer != null) { _audioPlayer.Stop(); } } if ((AudioStateData & (1 << 1)) != 0) { LOGGER.GetInstance.Info(String.Format("Drone is ready to receive stream from Controller (Drone RX ON)")); } else if ((AudioStateData & (1 << 1)) == 0) { LOGGER.GetInstance.Info(String.Format("Drone has cancelled to receive stream from Controller (Drone RX OFF)")); } } else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.jpsumo_PilotingState_SpeedChanged)) { var Result2 = StructConverter.Unpack("<bh", payload.SubArray("4:")); sbyte speed = (sbyte)Result2[0]; Int16 real_speed = (Int16)Result2[1]; LOGGER.GetInstance.Debug(String.Format("Speed updated to {0} ({1} cm/s)", speed, real_speed)); _sumoInformations.Speed = speed; var evt = new SumoEventArgs(SumoEnumCustom.TypeOfEvents.SpeedChange, _sumoInformations); OnSumoEvents(evt); } else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.jpsumo_PilotingState_PostureChanged)) { var state = (Int32)StructConverter.Unpack("<i", payload.SubArray("4:"))[0]; LOGGER.GetInstance.Debug(String.Format("State of posture changed: {1} ({0})", state, (SumoEnumGenerated.PostureChanged_state)state).ToString()); _sumoInformations.Posture = (SumoEnumGenerated.PostureChanged_state)state; var evt = new SumoEventArgs(SumoEnumCustom.TypeOfEvents.PostureEvent, _sumoInformations); OnSumoEvents(evt); } else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.jpsumo_AnimationsState_JumpLoadChanged)) { var state = (Int32)StructConverter.Unpack("<i", payload.SubArray("4:"))[0]; LOGGER.GetInstance.Warn(String.Format("State of jump load changed: {1} ({0})", state, (SumoEnumGenerated.JumpLoadChanged_state)state).ToString()); } else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.jpsumo_AnimationsState_JumpTypeChanged)) { var state = (Int32)StructConverter.Unpack("<i", payload.SubArray("4:"))[0]; LOGGER.GetInstance.Warn(String.Format("State of jump type changed: {1} ({0})", state, (SumoEnumGenerated.JumpTypeChanged_state)state).ToString()); } else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.jpsumo_AnimationsState_JumpMotorProblemChanged)) { var state = (Int32)StructConverter.Unpack("<i", payload.SubArray("4:"))[0]; LOGGER.GetInstance.Error(String.Format("State about the jump motor problem: {1} ({0})", state, (SumoEnumGenerated.JumpMotorProblemChanged_error)state).ToString()); } else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.jpsumo_NetworkState_LinkQualityChanged)) { var linkQuality = (byte)StructConverter.Unpack("<B", payload.SubArray("4:"))[0]; _sumoInformations.LinkQuality = linkQuality; var evt = new SumoEventArgs(SumoEnumCustom.TypeOfEvents.LinkQuality, _sumoInformations); OnSumoEvents(evt); } else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.jpsumo_MediaStreamingState_VideoEnableChanged)) { var state = (Int32)StructConverter.Unpack("<i", payload.SubArray("4:"))[0]; LOGGER.GetInstance.Info(String.Format("Media streaming state is: {1}({0})", state, (SumoEnumGenerated.VideoStateChangedV2_state)state).ToString()); } else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.jpsumo_AudioSettingsState_MasterVolumeChanged)) { var VolumeState = (byte)payload.SubArray("4:")[0]; LOGGER.GetInstance.Debug(String.Format("Volume state is: {0}", VolumeState)); _sumoInformations.Volume = VolumeState; var evt = new SumoEventArgs(SumoEnumCustom.TypeOfEvents.VolumeChange, _sumoInformations); OnSumoEvents(evt); } else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.jpsumo_PilotingState_AlertStateChanged)) { var state = (Int32)StructConverter.Unpack("<i", payload.SubArray("4:"))[0]; _sumoInformations.Alert = (SumoEnumGenerated.AlertStateChanged_state)state; var evt = new SumoEventArgs(SumoEnumCustom.TypeOfEvents.AlertEvent, _sumoInformations); OnSumoEvents(evt); LOGGER.GetInstance.Debug(String.Format("AlertStateChanged: {0}", ((SumoEnumGenerated.AlertStateChanged_state)state).ToString())); } else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.jpsumo_NetworkSettingsState_WifiSelectionChanged)) { var Wifi = StructConverter.Unpack("<iiB", payload.SubArray("4:")); int WifiType = (int)Wifi[0]; int WifiBand = (int)Wifi[1]; byte WifiChannel = (byte)Wifi[2]; LOGGER.GetInstance.Info(String.Format("Wifi Type:{0} Band:{1} Channel:{2}", ((SumoEnumGenerated.WifiSelectionChanged_type)WifiType).ToString(), ((SumoEnumGenerated.WifiSelectionChanged_band)WifiBand).ToString(), WifiChannel)); _sumoInformations.WifiType = (SumoEnumGenerated.WifiSelectionChanged_type)WifiType; _sumoInformations.WifiBand = (SumoEnumGenerated.WifiSelectionChanged_band)WifiBand; _sumoInformations.WifiChannel = WifiChannel; var evt = new SumoEventArgs(SumoEnumCustom.TypeOfEvents.WifiChanged, _sumoInformations); OnSumoEvents(evt); } else if (Tuple.Create(cmd_project, cmd_class, cmd_id).Equals(SumoConstantsGenerated.jpsumo_AudioSettingsState_ThemeChanged)) { var AudioTheme = (byte)StructConverter.Unpack("<B", payload.SubArray("4:"))[0]; LOGGER.GetInstance.Info(String.Format("Audio Theme Changed: {0} ", ((SumoEnumGenerated.ThemeChanged_theme)AudioTheme).ToString())); } else { var data = payload.SubArray("4:"); LOGGER.GetInstance.Debug(String.Format("DataFrame | Project: {0}, Class: {1}, Id: {2} data: {3}", cmd_project, cmd_class, cmd_id, BitConverter.ToString(data))); } } else if (data_type == (byte)SumoConstantsCustom.ARNETWORKAL_FRAME_TYPE.ARNETWORKAL_FRAME_TYPE_DATA_LOW_LATENCY) { // We received an ARStream packet, process it this._process_stream_frame(buffer_id, frame_size, payload); } else { LOGGER.GetInstance.Warn(String.Format("Unknown header type: {0} {1} {2} {3}", data_type, buffer_id, seq_no, frame_size)); } }