public static void Initialize(CUnmanagedComm comm) { Comm = comm; Comm.OnPacket += Comm_OnPacket; dronePosition = new SGeoLoc() { lat = 32.168141, lng = 34.808755, alt = 0, altFromGround = 0, heading = 0 }; // Debug Serialization if (debugLoadPacketsFromFile) { // Load packets from file - Debug CMavlinkDeserialization.Initialize("mavlink-serialization.log"); //CMavlinkDeserialization.Initialize("Logs/cementary-mission.log"); CMavlinkDeserialization.OnPacket += CMavlinkDeserialization_OnPacket; } else { // Save packets to file - For later debug purposes CMavlinkSerialization.Initialize("mavlink-serialization.log"); } }
private static void Comm_OnPacket(System.IO.Stream stream) { object packet; byte tempSysId = 0; byte tempCompId = 0; // Save the packet to file if (!debugLoadPacketsFromFile) { CMavlinkSerialization.SavePacket(stream); } packet = mavlink.ReadPacket(stream, out tempSysId, out tempCompId); if ((sysID == 255) || (compID == 255)) { sysID = tempSysId; compID = tempCompId; } while (packet != null) { HandlePacket(packet); // Check if someone is waiting for a packet if (packetsWaitedOn.ContainsKey(packet.GetType())) { // Trigger packet packetsWaitedOn[packet.GetType()].Set(); } if (OnPacketReceived != null) { OnPacketReceived(packet, tempSysId, tempCompId); } packet = mavlink.ReadPacket(stream, out tempSysId, out tempCompId); } }