bool ReceviedPacket(MAVLink.MAVLinkMessage rawpacket)
{
if (rawpacket.msgid == (byte) MAVLink.MAVLINK_MSG_ID.TERRAIN_REQUEST)
{
MAVLink.mavlink_terrain_request_t packet =
rawpacket.ToStructure<MAVLink.mavlink_terrain_request_t>();
if (issending)
return false;
lastmessage = rawpacket;
lastrequest = packet;
log.Info("received TERRAIN_REQUEST " + packet.lat/1e7 + " " + packet.lon/1e7 + " space " +
packet.grid_spacing + " " + Convert.ToString((long) packet.mask, 2));
System.Threading.ThreadPool.QueueUserWorkItem(QueueSendGrid);
}
else if (rawpacket.msgid == (byte) MAVLink.MAVLINK_MSG_ID.TERRAIN_REPORT)
{
MAVLink.mavlink_terrain_report_t packet =
rawpacket.ToStructure<MAVLink.mavlink_terrain_report_t>();
log.Info("received TERRAIN_REPORT " + packet.lat/1e7 + " " + packet.lon/1e7 + " " + packet.loaded + " " +
packet.pending);
}
return false;
}
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 receivedPacket(MAVLink.MAVLinkMessage arg)
{
if (arg.msgid == (uint)MAVLink.MAVLINK_MSG_ID.STATUSTEXT)
{
var message = ASCIIEncoding.ASCII.GetString(arg.ToStructure<MAVLink.mavlink_statustext_t>().text);
UpdateUserMessage(message);
if (message.ToLower().Contains("calibration successful") ||
message.ToLower().Contains("calibration failed"))
{
try
{
Invoke((MethodInvoker)delegate
{
BUT_calib_accell.Text = Strings.Done;
BUT_calib_accell.Enabled = false;
});
_incalibrate = false;
MainV2.comPort.UnSubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.STATUSTEXT, receivedPacket);
}
catch
{
}
}
}
return true;
}
private bool ReceviedPacket(MAVLink.MAVLinkMessage packet)
{
if (packet.msgid == (byte) MAVLink.MAVLINK_MSG_ID.MAG_CAL_PROGRESS)
{
var mprog = packet.ToStructure<MAVLink.mavlink_mag_cal_progress_t>();
lock (this.mprog)
{
this.mprog.Add(mprog);
}
return true;
}
else if (packet.msgid == (byte) MAVLink.MAVLINK_MSG_ID.MAG_CAL_REPORT)
{
var mrep = packet.ToStructure<MAVLink.mavlink_mag_cal_report_t>();
lock (this.mrep)
{
this.mrep.Add(mrep);
}
return true;
}
return true;
}
bool ReceviedPacket(MAVLink.MAVLinkMessage packet)
{
if (packetcounttimer.Second != DateTime.Now.Second)
{
log.Info("packet count " + packetcount + " with data " + packetwithdata + " " + buffer.Size);
packetcount = 0;
packetwithdata = 0;
packetcounttimer = DateTime.Now;
}
packetcount++;
MAVLink.mavlink_serial_control_t item = packet.ToStructure<MAVLink.mavlink_serial_control_t>();
if (item.count == 0)
return true;
packetwithdata++;
Console.WriteLine(DateTime.Now.Millisecond + "data count " + item.count);
// ASCIIEncoding.ASCII.GetString(item.data, 0, item.count)
lock (buffer)
{
buffer.AllowOverflow = true;
buffer.Put(item.data, 0, item.count);
}
// we just received data, so do a request again
GetData(true);
return true;
}
static bool ReceviedPacket(MAVLink.MAVLinkMessage rawpacket)
{
if (rawpacket.msgid == (byte) MAVLink.MAVLINK_MSG_ID.SCALED_IMU2)
{
MAVLink.mavlink_scaled_imu2_t packet = rawpacket.ToStructure<MAVLink.mavlink_scaled_imu2_t>();
// filter dataset
string item = (int) (packet.xmag/div) + "," +
(int) (packet.ymag/div) + "," +
(int) (packet.zmag/div);
if (filtercompass2.ContainsKey(item))
{
filtercompass2[item] = (int) filtercompass2[item] + 1;
if ((int) filtercompass2[item] > 3)
return false;
}
else
{
filtercompass2[item] = 1;
}
// values - offsets are 0
float rawmx = packet.xmag;
float rawmy = packet.ymag;
float rawmz = packet.zmag;
// add data
lock (datacompass2)
{
if (rawmx == 0 || rawmy == 0 || rawmz == 0)
return true;
datacompass2.Add(new Tuple<float, float, float>(rawmx, rawmy, rawmz));
}
return true;
}
else if (rawpacket.msgid == (byte) MAVLink.MAVLINK_MSG_ID.SCALED_IMU3)
{
MAVLink.mavlink_scaled_imu3_t packet = rawpacket.ToStructure<MAVLink.mavlink_scaled_imu3_t>();
// filter dataset
string item = (int) (packet.xmag/div) + "," +
(int) (packet.ymag/div) + "," +
(int) (packet.zmag/div);
if (filtercompass3.ContainsKey(item))
{
filtercompass3[item] = (int) filtercompass3[item] + 1;
if ((int) filtercompass3[item] > 3)
return false;
}
else
{
filtercompass3[item] = 1;
}
// values - offsets are 0
float rawmx = packet.xmag;
float rawmy = packet.ymag;
float rawmz = packet.zmag;
// add data
lock (datacompass3)
{
if (rawmx == 0 || rawmy == 0 || rawmz == 0)
return true;
datacompass3.Add(new Tuple<float, float, float>(rawmx, rawmy, rawmz));
}
return true;
}
else if (rawpacket.msgid == (byte) MAVLink.MAVLINK_MSG_ID.RAW_IMU)
{
MAVLink.mavlink_raw_imu_t packet = rawpacket.ToStructure<MAVLink.mavlink_raw_imu_t>();
if (packet.xmag == 0 && packet.ymag == 0)
return false;
// filter dataset
string item = (int) (packet.xmag/div) + "," +
(int) (packet.ymag/div) + "," +
(int) (packet.zmag/div);
if (filtercompass1.ContainsKey(item))
{
filtercompass1[item] = (int) filtercompass1[item] + 1;
if ((int) filtercompass1[item] > 3)
return false;
}
else
{
filtercompass1[item] = 1;
}
// values
float rawmx = packet.xmag - (float) MainV2.comPort.MAV.cs.mag_ofs_x;
float rawmy = packet.ymag - (float) MainV2.comPort.MAV.cs.mag_ofs_y;
float rawmz = packet.zmag - (float) MainV2.comPort.MAV.cs.mag_ofs_z;
// add data
lock (datacompass1)
{
datacompass1.Add(new Tuple<float, float, float>(rawmx, rawmy, rawmz));
}
return true;
}
return true;
}
