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