public void send_packet(CRTPPacket pk) { long tick = DateTime.Now.Ticks; while (DateTime.Now.Ticks - tick < 20000000) { if (out_queue.Count == 0) { out_queue.Enqueue(pk); return; } } Console.WriteLine("RadioDriver: Could not send packet to copter"); }
public void run() { byte[] dataOut = new byte[] { 0xFF }; bool wait = false; int emptyCtr = 0; Crazyradio cradio = _radio_manager.enter(); for (int i = 0; i < 10; i++) { _radio_ack resp = cradio.send_packet(new byte[] { 0xFF, 0x05, 0x01 }); if (resp.data != null && resp.data.SequenceEqual(new byte[] { 0xFF, 0x05, 0x01 })) { _has_safelink = true; break; } } this.needs_resending = !this._has_safelink; _send_packet_safe(cradio, new byte[] { 0xF3 }); _send_packet_safe(cradio, new byte[] { 0x5D, 0x05 }); while (true) { _radio_ack ackStatus; cradio = _radio_manager.enter(); if (_has_safelink) { ackStatus = _send_packet_safe(cradio, dataOut); } else { ackStatus = cradio.send_packet(dataOut); } if (ackStatus == null) { Console.WriteLine("Dongle reported ACK status == null"); continue; } if (!ackStatus.ack) { _retry_before_disconnect--; if (_retry_before_disconnect == 0) { Console.WriteLine("Too many packets lost"); continue; } } _retry_before_disconnect = 100; byte[] data = ackStatus.data; if (data != null && data.Length > 0) { in_queue.Enqueue(new CRTPPacket(data[0], data.Skip(1).ToArray())); wait = false; emptyCtr = 0; } else { emptyCtr += 1; if (emptyCtr > 10) { emptyCtr = 10; wait = true; } else { wait = false; } } CRTPPacket outPacket = null; if (out_queue.Count > 0) { outPacket = out_queue.Dequeue(); } else { if (wait) { for (int i = 0; i < 10; i++) { Thread.Sleep(1); if (out_queue.Count > 0) { outPacket = out_queue.Dequeue(); break; } } } } if (outPacket != null) { List <byte> tmp = new List <byte>(outPacket.data.Length + 1) { outPacket.header }; tmp.AddRange(outPacket.data); dataOut = tmp.ToArray(); } else { dataOut = new byte[] { 0xFF }; } } }