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