/// <summary> /// This function is called by the RF controller when data has been received. Here we identify what type of packet /// it is and instantiate an appropriate packet class to hold the data. The packet instance is then forwarded to /// all listeners of the PacketReceived event /// </summary> /// <param name="args"></param> private static void ProcessNewPacket(PacketReceivedEventArgs args) { int now = (int)sw.ElapsedTicks; TJPacket packet = args.Packet; switch (packet.PID) { case 0x00: packet = new TJStatePacket(packet.RawPacket); if (DragonflyStateUpdate != null) DragonflyStateUpdate(packet as TJStatePacket); break; case 0x01: packet = new TJState2Packet(packet.RawPacket); if (DragonflyState2Update != null) DragonflyState2Update(packet as TJState2Packet); break; case 0x02: // Scale factors packets break; case 0x3A: packet = new TJBootloaderResponsePacket(packet.RawPacket); if (DragonflyBootloaderUpdate != null) DragonflyBootloaderUpdate(packet as TJBootloaderResponsePacket); break; case 0x90: packet = new TJCameraPacket(packet.RawPacket); if (DragonflyCameraUpdate != null) DragonflyCameraUpdate(packet as TJCameraPacket); break; case 0x91: packet = new TJStartFramePacket(packet.RawPacket); if (DragonflyStartFrame != null) DragonflyStartFrame(packet as TJStartFramePacket); break; case 0x92: if (DragonflyEndFrame != null) DragonflyEndFrame(); break; default: break; } if (PacketReceived != null) PacketReceived(packet); // The following code is just to keep track of how many packets have been lost. int dif = 2; if (packet.Seq < lastSeqNum) { int seqdif = packet.Seq + 256 - lastSeqNum; if (seqdif != dif) { missed += seqdif / dif; } } else if (packet.Seq - lastSeqNum != dif) { missed += packet.Seq - lastSeqNum; } received++; if (received == 100) { Console.WriteLine("s/Packet: {0:0.00000}, missed: {1}", ((now - lastTime) / (double)received) / Stopwatch.Frequency, missed); lastTime = now; missed = 0; received = 0; } lastSeqNum = packet.Seq; }
/// <summary> /// This method keeps track of received packets in order to notify UpdateGraphs of when it should execute. /// </summary> void UpdateUIState2(TJState2Packet state) { long now = stopwatch.ElapsedMilliseconds; this.Dispatcher.BeginInvoke((Action)delegate() { if ((bool)cbxEnableGraphs2.IsChecked) { packet2IgnoreCount += 1; // See whether Aardvark is used if (TJDragonfly.getRFController() == 0) { bufferedStates2.AddLast(new Tuple<TJState2Packet, long>(state, now)); packet2IgnoreCount = 0; } else if (TJDragonfly.getRFController() == 1) { if (packet2IgnoreCount >= packetRateScaler) { bufferedStates2.AddLast(new Tuple<TJState2Packet, long>(state, now)); packet2IgnoreCount = 0; } } if (now - lastGraph2Update > graphUpdatePeriod) { lastGraph2Update = now; LinkedList<Tuple<TJState2Packet, long>> statesToAdd = bufferedStates2; this.Dispatcher.BeginInvoke((Action)delegate() { UpdateGraphs2(statesToAdd); }); bufferedStates2 = new LinkedList<Tuple<TJState2Packet, long>>(); } } }); }