public FrameHeader (uint size, uint dataOffset, FrameHeaderType type, uint channelId) { Size = size; DataOffset = dataOffset; Type = type; ChannelId = channelId; }
public static FrameReadyEventArgs Create(FrameHeaderType header, byte[] data) { var args = new FrameReadyEventArgs() { Servos = new uint[26] }; var br = new BinaryReader(new MemoryStream(data)); Buffer.BlockCopy(data, 0, args.Servos, 0, args.Servos.Length * 4); br.BaseStream.Seek(args.Servos.Length * 4, SeekOrigin.Begin); if (header == FrameHeaderType.ESP32Debug) { args.Model.tlen.x = br.ReadDouble(); args.Model.tlen.y = br.ReadDouble(); args.Model.tlen.z = br.ReadDouble(); args.Model.pos.x = br.ReadDouble(); args.Model.pos.y = br.ReadDouble(); args.Model.pos.z = br.ReadDouble(); args.Model.rot.x = br.ReadDouble(); args.Model.rot.y = br.ReadDouble(); args.Model.rot.z = br.ReadDouble(); args.Model.turnedOn = br.ReadBoolean(); args.Model.cps = br.ReadInt32(); } return(args); }
public void SendFrame(FrameHeaderType header, byte[] data) { MemoryStream ms = new MemoryStream(); BinaryWriter bw = new BinaryWriter(ms); bw.Write((ushort)header); bw.Write((ushort)data.Length); bw.Write(data); bw.Write(Crc.Get_CRC16(data)); serialPort.SendMessage(ms.ToArray()); }
private ushort GetExpectedDataSize(FrameHeaderType header) { switch (header) { case FrameHeaderType.ESP32Debug: return(26 * 4 + 8 * 3 * 3 + 1 + 4); case FrameHeaderType.STM32Debug: return(26 * 4); } return(0); }
public void Loop() { if (sw.ElapsedMilliseconds < 10) { return; } if (frame_buf_len > 0) { var frame_br = new BinaryReader(new MemoryStream(frame_buf)); int frame_start_offset = 0; FrameHeaderType header = FrameHeaderType.Unknown; while (frame_start_offset < (frame_buf_len - 4)) { frame_br.BaseStream.Seek(frame_start_offset, SeekOrigin.Begin); header = (FrameHeaderType)frame_br.ReadUInt16(); if (header == FrameHeaderType.STM32Debug || header == FrameHeaderType.ESP32Debug) { break; } else { header = FrameHeaderType.Unknown; } frame_start_offset++; } if (header != FrameHeaderType.Unknown) { // align frame start with 0 start index in buffer MoveFrame(frame_start_offset, (ushort)frame_buf.Length); ushort expectedDataSize = GetExpectedDataSize(header); if (frame_buf_len >= expectedDataSize) { frame_br.BaseStream.Seek(2, SeekOrigin.Begin); ushort data_size = frame_br.ReadUInt16(); if (data_size == expectedDataSize) { var data = frame_br.ReadBytes(data_size); var expected_crc = Crc.Get_CRC16(data); var actual_crc = frame_br.ReadUInt16(); bool crc_valid = actual_crc == expected_crc; if (crc_valid) { OnFrameReady(this, FrameReadyEventArgsBuilder.Create(header, data)); frame_buf_len = 0; } } } } } sw.Restart(); }