private CrtpResponse Send(CrtpMessage outMessage)
{
var content = outMessage.Message;
// Adds 1bit counter to CRTP header to guarantee that no ack(downlink)
// payload are lost and no uplink packet are duplicated.
// The caller should resend packet if not acked(ie.same as with a
// direct call to crazyradio.send_packet)
if (_safeLink)
{
content[0] &= 0xF3;
content[0] |= (byte)(_curr_up << 3 | _curr_down << 2);
}
var result = new CrtpResponse(_crazyradioDriver.SendData(content));
if (_safeLink)
{
if (result.Ack && ((result.Content.Header & 0x04) == (_curr_down << 2)))
{
_curr_down = (byte)(1 - _curr_down);
}
if (result.Ack)
{
_curr_up = (byte)(1 - _curr_up);
}
}
return(result);
}
private void TrackLinkQuality(CrtpResponse response)
{
var retry = 10 - response.Retry;
_retries.Enqueue(retry);
_retry_sum += retry;
while (_retries.Count > 100)
{
var oldest = _retries.Dequeue();
_retry_sum -= oldest;
}
var linkQuality = ((float)_retry_sum / _retries.Count) * 10;
LinkQuality?.Invoke(this, new LinkQualityEventArgs(linkQuality));
}
private void CommunicationLoop()
{
_retryBeforeDisconnect = NumberOfRetries;
_emptyCounter = 0;
_waitTime = 0;
var emptyMessage = new CrtpMessage(0xff, new byte[0]);
var outMessage = emptyMessage;
// Try up to 10 times to enable the safelink mode
TryEnableSafeLink();
_comStarted.Set();
_log.Info("Communication with crazyfly started.");
while (_isRunning)
{
CrtpResponse response = null;
try
{
response = Send(outMessage);
}
catch (Exception ex)
{
_log.Error("error sending message", ex);
LinkError?.Invoke(this, new LinkErrorEventArgs("failed to send: " + ex));
}
// Analyse the in data packet ...
if (response == null)
{
_log.Info("Dongle reported ACK status == None");
continue;
}
TrackLinkQuality(response);
if (!response.Ack)
{
UpdateRetryCount();
if (_retryBeforeDisconnect > 0)
{
continue;
}
// else try a next packet to send.
}
_retryBeforeDisconnect = NumberOfRetries;
// after we managed to send the message, set the next one to the ping message again.
outMessage = emptyMessage;
if (response.HasContent)
{
_waitTime = 0;
_emptyCounter = 0;
lock (_lock)
{
_log.Debug($"incoming queue count: {_incoming.Count}; enqueue for {response.Content.Port} / {response.Content.Channel}");
_incoming.Enqueue(response.Content);
while (_incoming.Count > _maxInqueue)
{
// dequue old messages which are not processed and therefore stale.
var old = _incoming.Dequeue();
_log.Warn($"Too many old message not processed, drop for port: {old.Port}.");
}
_waitForInqueue.Set();
}
}
else
{
_emptyCounter += 1;
if (_emptyCounter > 10)
{
_emptyCounter = 10;
// Relaxation time if the last 10 packet where empty
_waitTime = 10;
}
else
{
// send more ack messages to get more responses and don't wait for
// user out messages; start waiting only after 10 empty messages received.
_waitTime = 0;
}
}
_waitForOutqueue.Wait(_waitTime);
lock (_lock)
{
if (_outgoing.Count > 0)
{
outMessage = _outgoing.Dequeue();
}
if (_outgoing.Count == 0)
{
_waitForOutqueue.Reset();
}
}
}
_log.Debug("send loop ended");
}
