public Frame ReadFrame()
{
if (blen > 7)
{
int frameLen = 0;
frameLen = buffer[bstart + 3];
frameLen |= (buffer[bstart + 4] << 8);
frameLen |= (buffer[bstart + 5] << 16);
frameLen |= (buffer[bstart + 6] << 24);
if (blen >= frameLen)
{
// Process packet
var payload = new byte[frameLen - 7];
Buffer.BlockCopy(buffer, bstart + 7, payload, 0, frameLen - 7);
var type = buffer[bstart];
var id = buffer[bstart + 1];
var seq = buffer[bstart + 2];
var frame = new Frame(direction, (FrameType) type, id, seq, payload);
bstart += frameLen;
blen -= frameLen;
if (blen == 0)
{
bstart = 0;
}
else if (bstart > 0 && (bstart + blen) >= buffer.Length)
{
Buffer.BlockCopy(buffer, bstart, buffer, 0, blen);
bstart = 0;
}
return frame;
}
}
return null;
}
private void AddFrameToList(Frame f)
{
var a = new Action(() =>
{
var i = new ListViewItem(f.Time.ToString("0.###"));
i.SubItems.Add(f.Direction.ToString());
i.SubItems.Add(f.Type.ToString());
if (f.Type == FrameType.ACK)
{
i.ForeColor = Color.Purple;
}
else if (f.Direction == Frame.FrameDirection.ToDrone)
{
i.ForeColor = Color.Blue;
}
else if (f.Direction == Frame.FrameDirection.ToController)
{
i.ForeColor = Color.Black;
}
if (f.Id == 0)
{
i.SubItems.Add("PING");
i.ForeColor = Color.Orange;
}
else if (f.Id == 1)
{
i.SubItems.Add("PONG");
i.ForeColor = Color.DarkGreen;
}
else
{
i.SubItems.Add(f.Id.ToString());
}
i.SubItems.Add(f.Seq.ToString());
i.SubItems.Add(f.Data.Length.ToString());
if (f.Id >= 2 && (f.Type == FrameType.DATA_W_ACK
|| f.Type == FrameType.DATA_LL
|| f.Type == FrameType.DATA))
{
var proj = (PacketType)f.Data[0];
var c = Packet.GetPacketClass(proj, f.Data[1]);
ushort command = 0;
command = f.Data[2];
command |= (ushort)(f.Data[3] << 8);
var cmd = Packet.GetPacketCommand(proj, f.Data[1], command);
i.SubItems.Add(proj.ToString());
i.SubItems.Add(c);
i.SubItems.Add(cmd);
}
i.Tag = f;
lstPackets.Items.Add(i);
});
if (lstPackets.InvokeRequired)
lstPackets.Invoke(a);
else
a();
}
private void ProcessVideoFrame(Frame f)
{
try
{
ushort frameNum = f.Data[0];
frameNum |= (ushort)(f.Data[1] << 8);
if (frameNum != _currentFrameNum)
{
// Try to flush the previous frame
if (_currentFrameSize > 0)
{
var data = new byte[_currentFrameSize];
Buffer.BlockCopy(videoBuffer, 0, data, 0, _currentFrameSize);
var ev = OnVideoFrameReceived;
if (ev != null)
{
ev(this, new VideoFrameReceived()
{
Data = data,
FrameNum = _currentFrameNum
});
}
}
_currentFrameNum = frameNum;
_currentFrameSize = 0;
}
var flags = f.Data[2];
var fragNum = f.Data[3];
var fragmentsPerFrame = f.Data[4];
var flushFrame = (flags & 1) == 1; //when flush, dump frame already instead of assembly?
Debug.WriteLine("FrameNum={0}, FragmentNum={1}, # Fragments={2}", frameNum, fragmentsPerFrame, fragNum);
var offset = fragNum * MaxFragmentSize;
var dataLen = f.Data.Length - 5;
if (fragNum == fragmentsPerFrame - 1)
{
_currentFrameSize = (fragmentsPerFrame - 1) * MaxFragmentSize + dataLen;
Debug.WriteLine("Final frame, most likely not full size.");
}
else
{
if (dataLen != MaxFragmentSize)
{
Debug.WriteLine("Received non-full packet in between stream.");
}
}
Buffer.BlockCopy(f.Data, 5, videoBuffer, offset, dataLen);
}
catch (Exception ex)
{
Debug.WriteLine(ex.Message);
}
}
public RingBuffer(Frame.FrameDirection direction)
{
this.direction = direction;
}
