private bool ReceviedPacket(MAVLink.MAVLinkMessage arg)
{
if (arg.msgid == (byte)MAVLink.MAVLINK_MSG_ID.DATA_TRANSMISSION_HANDSHAKE)
{
var packet = arg.ToStructure <MAVLink.mavlink_data_transmission_handshake_t>();
msgDataTransmissionHandshake = packet;
imageBuffer.Close();
imageBuffer = new MemoryStream((int)packet.size);
}
else if (arg.msgid == (byte)MAVLink.MAVLINK_MSG_ID.ENCAPSULATED_DATA)
{
var packet = arg.ToStructure <MAVLink.mavlink_encapsulated_data_t>();
msgEncapsulatedData = packet;
int start = msgDataTransmissionHandshake.payload * msgEncapsulatedData.seqnr;
int left = (int)msgDataTransmissionHandshake.size - start;
var writeamount = Math.Min(msgEncapsulatedData.data.Length, left);
imageBuffer.Seek(start, SeekOrigin.Begin);
imageBuffer.Write(msgEncapsulatedData.data, 0, writeamount);
// we have a complete image
if ((msgEncapsulatedData.seqnr + 1) == msgDataTransmissionHandshake.packets)
{
using (
Bitmap bmp = new Bitmap(msgDataTransmissionHandshake.width, msgDataTransmissionHandshake.height,
PixelFormat.Format8bppIndexed))
{
SetGrayscalePalette(bmp);
var bitmapData = bmp.LockBits(new Rectangle(Point.Empty, bmp.Size), ImageLockMode.ReadWrite,
bmp.PixelFormat);
if (imageBuffer.Length > msgDataTransmissionHandshake.size)
{
return(true);
}
var buffer = imageBuffer.ToArray();
Marshal.Copy(buffer, 0, bitmapData.Scan0, buffer.Length);
bmp.UnlockBits(bitmapData);
if (newImage != null)
{
newImage(this, new ImageEventHandle(bmp));
}
//bmp.Save("test.bmp", ImageFormat.Bmp);
}
}
}
return(true);
}
private bool ReceviedPacket(MAVLink.MAVLinkMessage arg)
{
if (arg.msgid == (byte)MAVLink.MAVLINK_MSG_ID.DATA_TRANSMISSION_HANDSHAKE)
{
var packet = arg.ToStructure <MAVLink.mavlink_data_transmission_handshake_t>();
msgDataTransmissionHandshake = packet;
imageBuffer.Close();
imageBuffer = new MemoryStream((int)packet.size);
}
else if (arg.msgid == (byte)MAVLink.MAVLINK_MSG_ID.ENCAPSULATED_DATA)
{
var packet = arg.ToStructure <MAVLink.mavlink_encapsulated_data_t>();
msgEncapsulatedData = packet;
int start = msgDataTransmissionHandshake.payload * msgEncapsulatedData.seqnr;
int left = (int)msgDataTransmissionHandshake.size - start;
var writeamount = Math.Min(msgEncapsulatedData.data.Length, left);
imageBuffer.Seek(start, SeekOrigin.Begin);
imageBuffer.Write(msgEncapsulatedData.data, 0, writeamount);
// we have a complete image
if ((msgEncapsulatedData.seqnr + 1) == msgDataTransmissionHandshake.packets)
{
using (
Bitmap bmp = new Bitmap(msgDataTransmissionHandshake.width, msgDataTransmissionHandshake.height))
{
SetGrayscalePalette(bmp);
if (imageBuffer.Length > msgDataTransmissionHandshake.size)
{
return(true);
}
var buffer = imageBuffer.ToArray();
int a = 0;
foreach (var b in buffer)
{
bmp.SetPixel(a % bmp.Width, (int)(a / bmp.Width), bmp.Palette.Entries[b]);
a++;
}
if (newImage != null)
{
newImage(this, new ImageEventHandle(bmp));
}
//bmp.Save("test.bmp", ImageFormat.Bmp);
}
}
}
return(true);
}
private bool ReceviedPacket(MAVLink.MAVLinkMessage arg)
{
if (arg.msgid == (byte) MAVLink.MAVLINK_MSG_ID.DATA_TRANSMISSION_HANDSHAKE)
{
var packet = arg.ToStructure<MAVLink.mavlink_data_transmission_handshake_t>();
msgDataTransmissionHandshake = packet;
imageBuffer.Close();
imageBuffer = new MemoryStream((int)packet.size);
}
else if (arg.msgid == (byte)MAVLink.MAVLINK_MSG_ID.ENCAPSULATED_DATA)
{
var packet = arg.ToStructure<MAVLink.mavlink_encapsulated_data_t>();
msgEncapsulatedData = packet;
int start = msgDataTransmissionHandshake.payload * msgEncapsulatedData.seqnr;
int left = (int)msgDataTransmissionHandshake.size - start;
var writeamount = Math.Min(msgEncapsulatedData.data.Length, left);
imageBuffer.Seek(start, SeekOrigin.Begin);
imageBuffer.Write(msgEncapsulatedData.data, 0, writeamount);
// we have a complete image
if ((msgEncapsulatedData.seqnr+1) == msgDataTransmissionHandshake.packets)
{
using (
Bitmap bmp = new Bitmap(msgDataTransmissionHandshake.width, msgDataTransmissionHandshake.height,
PixelFormat.Format8bppIndexed))
{
SetGrayscalePalette(bmp);
var bitmapData = bmp.LockBits(new Rectangle(Point.Empty, bmp.Size), ImageLockMode.ReadWrite,
bmp.PixelFormat);
if (imageBuffer.Length > msgDataTransmissionHandshake.size)
return true;
var buffer = imageBuffer.ToArray();
Marshal.Copy(buffer, 0, bitmapData.Scan0, buffer.Length);
bmp.UnlockBits(bitmapData);
if (newImage != null)
newImage(this, new ImageEventHandle(bmp));
//bmp.Save("test.bmp", ImageFormat.Bmp);
}
}
}
return true;
}
