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