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