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);
}
internal void Notify(CrtpMessage crtpMessage)
{
// copy handlers so that we don't need to keep lock during notify to protect handler list against modification.
var handlersToNotify = new List <CrtpEventCallback>();
lock (_lock)
{
handlersToNotify.AddRange(_eventCallbacksAll);
IList <CrtpEventCallback> handlers;
if (!_eventCallbacksSpecificPort.TryGetValue(crtpMessage.Port, out handlers))
{
return;
}
handlersToNotify.AddRange(handlers);
}
foreach (var handler in handlersToNotify)
{
try
{
handler.Invoke(crtpMessage);
}
catch (Exception ex)
{
_log.Error("error while notifying receiver", ex);
}
}
}
/// <summary>
/// Enable/disable the client side X-mode.When enabled this recalculates
/// the setpoints before sending them to the Crazyflie.
/// </summary>
public void SetContinousWave(bool enabled)
{
var msg = new CrtpMessage((byte)CrtpPort.PLATFORM, PLATFORM_COMMAND_CHANNEL,
new byte[] { 0, Convert.ToByte(enabled) });
_communicator.SendMessage(msg);
}
protected override void StartLoadToc()
{
var message = new CrtpMessage((byte)CrtpPort.LOGGING,
(byte)LogChannel.CHAN_SETTINGS, new byte[] { (byte)LogConfigCommand.CMD_RESET_LOGGING });
_communicator.SendMessageExcpectAnswer(message, new byte[] { message.Data[0] });
}
internal void RegisterExpectation(CrtpMessage message, Func <CrtpMessage, bool> checkFunction, TimeSpan timeout)
{
lock (_lock)
{
_registeredMessageExceptions.Add(new WaitForMessageRequest(message, checkFunction, NotifyTimeout, timeout));
}
}
// the event loop listen on the incoming messages and processed them.
private void EventLoop()
{
_eventLoopStarted.Set();
while (_isRunning)
{
_waitForInqueue.Wait(1);
CrtpMessage inMessage = null;
lock (_lock)
{
if (_incoming.Count > 0)
{
inMessage = _incoming.Dequeue();
_log.Debug($"Dequeued message for {inMessage.Channel} / {inMessage.Port}");
}
if (_incoming.Count == 0)
{
_waitForInqueue.Reset();
}
}
if (inMessage == null)
{
continue;
}
_crtpEventRegistry.Notify(inMessage);
}
_log.Debug("Event loop ended");
}
internal void StartAlreadyAdded()
{
var msg = new CrtpMessage((byte)CrtpPort.LOGGING,
(byte)Logger.LogChannel.CHAN_SETTINGS,
new byte[] { (byte)Logger.LogConfigCommand.CMD_START_LOGGING, Identifier.Value, (byte)((PeriodInMs / 10) & 0xFF) });
_communicator.SendMessageExcpectAnswer(msg, msg.Data.Take(2).ToArray());
}
internal WaitForMessageRequest(CrtpMessage request, Func <CrtpMessage, bool> checkFunction, Action <WaitForMessageRequest> notifyTimeout, TimeSpan timeout)
{
Request = request;
_checkFunction = checkFunction;
var timerInterValInMs = (int)Math.Round(timeout.TotalMilliseconds);
_waitTimer = new Timer(RequestTimeout, null, timerInterValInMs, timerInterValInMs);
_notifyTimeout = notifyTimeout;
}
private void PlatformMessageReceived(CrtpMessage message)
{
if (message.Channel == VERSION_COMMAND_CHANNEL &&
message.Data[0] == VERSION_GET_PROTOCOL_COMMAND)
{
ProtocolVersion = message.Data[1];
_log.Info("received protocol version: " + ProtocolVersion);
_waitForProtocolResult.Set();
}
}
private int RequestProtocolVersion()
{
// Sending a sink request to detect if the connected Crazyflie
// supports protocol versioning
var msg = new CrtpMessage((byte)CrtpPort.LINKCTRL, LINKSERVICE_SOURCE_CHANNEL,
new byte[] { VERSION_GET_PROTOCOL_COMMAND });
_communicator.SendMessage(msg);
if (!_waitForProtocolResult.WaitOne(5000))
{
_log.Warn("failed to retrieve protocol version (timeout)");
}
return(ProtocolVersion);
}
/// <summary>
/// Callback for newly arrived packets
/// </summary>
private void ParamMessageReceived(CrtpMessage message)
{
_log.Debug($"received parameter value result message for channel {message.Channel}");
if (message.Channel == (byte)ParamConfigurator.ParamChannel.READ_CHANNEL ||
message.Channel == (byte)ParamConfigurator.ParamChannel.WRITE_CHANNEL)
{
ParameterReceivedEventArgs notificationReceived = null;
ParameterStoredEventArgs notificationStored = null;
lock (_lock)
{
ushort forId = (_useV2 ? BitConverter.ToUInt16(message.Data.Take(2).ToArray(), 0) : message.Data.First());
_log.Debug($"Check for existing requests for received parameter value for id {forId}.");
if (_requests.Any())
{
var request = _requests.Peek();
if (request.ForParamId == forId && request.RequestMessage.Channel == message.Channel)
{
// remove now from the queue and notify processing thread to take next one.
_log.Debug($"Dequeue current parameter request as fullfilled. Id: {forId}, channel: {message.Channel}");
_requests.Dequeue();
_waitForResponse.Set();
}
if (message.Channel == (byte)ParamConfigurator.ParamChannel.READ_CHANNEL)
{
_log.Debug($"received parameter value result for param {forId}");
// for version 2, it seems that we need to skip 3 bytes (2 for id, 1 for something else).
notificationReceived = new ParameterReceivedEventArgs(forId, message.Data.Skip(_useV2 ? 3 : 1).ToArray());
}
if (message.Channel == (byte)ParamConfigurator.ParamChannel.WRITE_CHANNEL)
{
_log.Info($"stored parameter value result for param {forId}");
notificationStored = new ParameterStoredEventArgs(forId);
}
}
else
{
_log.Warn($"no macthing request found for parameter {forId}");
}
}
if (notificationReceived != null)
{
ParameterReceived?.Invoke(this, notificationReceived);
}
if (notificationStored != null)
{
ParameterStored?.Invoke(this, notificationStored);
}
}
}
/// <summary>
/// Delete this entry in the Crazyflie
/// </summary>
internal void Delete()
{
if (Identifier != null)
{
_log.Debug($"LogEntry: Sending delete logging for block id={Identifier}");
var msg = new CrtpMessage((byte)CrtpPort.LOGGING,
(byte)Logger.LogChannel.CHAN_SETTINGS,
new byte[] { (byte)Logger.LogConfigCommand.CMD_DELETE_BLOCK, Identifier.Value });
_communicator.SendMessageExcpectAnswer(msg, msg.Data.Take(2).ToArray());
}
else
{
_log.Warn("Delete block, but no block registered");
}
}
/// <summary>
/// Stop the logging for this entry
/// </summary>
internal void Stop()
{
if (Identifier != null)
{
_log.Debug($"Sending stop logging for block id={Identifier}");
var msg = new CrtpMessage((byte)CrtpPort.LOGGING,
(byte)Logger.LogChannel.CHAN_SETTINGS,
new byte[] { (byte)Logger.LogConfigCommand.CMD_STOP_LOGGING, Identifier.Value });
_communicator.SendMessageExcpectAnswer(msg, msg.Data.Take(2).ToArray());
}
else
{
_log.Warn("Stopping block, but no block registered");
}
}
public void SendMessage(CrtpMessage message)
{
lock (_lock)
{
_log.Debug($"enqueue message for port {message.Port} / channel {message.Channel}; number of messages waiting: {_outgoing.Count}");
// ensure that outqueue doesn't get too big. Drop older packets if more than one already waiting
// so that we have at most 100 packets in queue.
while (_outgoing.Count > _maxOutqueue)
{
// the older messages are at the beginning, so dequeue until we have the most recent on top.
var old = _outgoing.Dequeue();
_log.Warn($"too many messages in out queue, drop for port {old.Port}");
}
_outgoing.Enqueue(message);
_waitForOutqueue.Set();
}
}
private void SendTocInfoRequest()
{
byte[] content;
if (_useV2)
{
content = new byte[] { (byte)TocCommand.CMD_TOC_INFO_V2 };
}
else
{
content = new byte[] { (byte)TocCommand.CMD_TOC_INFO };
}
var msg = new CrtpMessage(_port, TOC_CHANNEL,
content);
_communicator.SendMessageExcpectAnswer(msg, new [] { msg.Data[0] });
}
/// <summary>
/// Handle a newly arrived packet
/// </summary>
private void TocPacketReceived(CrtpMessage message)
{
if (message.Channel != TOC_CHANNEL)
{
return;
}
var payload = message.Data.Skip(1).ToArray();
if (_fetchState == FetchState.GET_TOC_INFO)
{
HandleGetTocInfo(payload);
}
else if (_fetchState == FetchState.GET_TOC_ELEMENT)
{
// Always add new element, but only request new if it's not the
// last one.
HandleGetTocElement(payload);
}
}
/// <summary>
/// Request information about a specific item in the TOC
/// </summary>
private void RequestTocElement(ushort index)
{
_log.Debug($"Requesting index {index} on port {_port}");
byte[] content;
if (_useV2)
{
content = new byte[] { (byte)TocCommand.CMD_TOC_ITEM_V2,
(byte)(index & 0x0ff), (byte)((index >> 8) & 0x0ff) };
}
else
{
content = new byte[] { (byte)TocCommand.CMD_TOC_ELEMENT, (byte)(index & 0xff) };
}
var msg = new CrtpMessage(_port, TOC_CHANNEL,
content);
_communicator.SendMessageExcpectAnswer(msg, msg.Data.Take(content.Length).ToArray());
}
private void CrtServiceMessageReceived(CrtpMessage message)
{
if (message.Channel == LINKSERVICE_SOURCE_CHANNEL)
{
// If the sink contains a magic string, get the protocol version,
// otherwise -1
if (message.Data.Length >= 18 && _encoder.GetString(message.Data.Take(18).ToArray()) ==
"Bitcraze Crazyflie")
{
var msg = new CrtpMessage((byte)CrtpPort.PLATFORM, VERSION_COMMAND_CHANNEL,
new byte[] { 0 }); // get protocol = 0
_communicator.SendMessage(msg);
}
else
{
_log.Info("not received Bitcraze Crazyflie from service message");
ProtocolVersion = -1;
_waitForProtocolResult.Set();
}
}
}
private Func <CrtpMessage, bool> CheckMatch(CrtpMessage messageToSend, byte[] startResponseContent)
{
return(message =>
{
if (message.Port != messageToSend.Port ||
message.Channel != messageToSend.Channel ||
message.Data.Length < startResponseContent.Length)
{
return false;
}
for (int i = 0; i < startResponseContent.Length; i++)
{
if (startResponseContent[i] != message.Data[i])
{
return false;
}
}
return true;
});
}
/// <summary>
/// Callback for newly arrived packets with TOC information
/// </summary>
private void LogPacketReceived(CrtpMessage message)
{
if (message.Channel == (byte)LogChannel.CHAN_SETTINGS)
{
var cmd = message.Data[0];
var id = message.Data[1];
var block = FindBlock(id);
var errorStatus = message.Data[2];
if (cmd == (byte)LogConfigCommand.CMD_CREATE_BLOCK ||
cmd == (byte)LogConfigCommand.CMD_CREATE_BLOCK_V2)
{
HandleCreateBlock(id, block, errorStatus);
}
else if (cmd == (byte)LogConfigCommand.CMD_START_LOGGING)
{
HandleStartLoggingCommand(id, block, errorStatus);
}
else if (cmd == (byte)LogConfigCommand.CMD_STOP_LOGGING)
{
HandleStopLoggingCommand(id, block, errorStatus);
}
else if (cmd == (byte)LogConfigCommand.CMD_DELETE_BLOCK)
{
HandleDeleteLoggingCommand(id, block, errorStatus);
}
else if (cmd == (byte)LogConfigCommand.CMD_RESET_LOGGING)
{
HandleResetLoggingCommand();
}
}
if (message.Channel == (byte)LogChannel.CHAN_LOGDATA)
{
var id = message.Data[0];
var block = FindBlock(id);
HandleReceivedLogData(id, block, message.Data.Skip(1).ToArray());
}
}
private void CheckExceptedAnswer(CrtpMessage message)
{
WaitForMessageRequest foundRequest = null;
lock (_lock)
{
foreach (var expectation in _registeredMessageExceptions)
{
if (expectation.IsSatisfiedBy(message))
{
foundRequest = expectation;
break;
}
}
if (foundRequest != null)
{
// stop retry timer.
foundRequest.Dispose();
_registeredMessageExceptions.Remove(foundRequest);
_log.Debug($"received expected message for port {foundRequest.Request.Port} / channel: {foundRequest.Request.Channel}");
}
}
}
public ParameterRequest(ushort forParamId, CrtpMessage requestMessage, byte[] verificationBytes)
{
ForParamId = forParamId;
RequestMessage = requestMessage;
VerifciationBytes = verificationBytes;
}
public bool IsSatisfiedBy(CrtpMessage message)
{
return(_checkFunction(message));
}
public void SendMessageExcpectAnswer(CrtpMessage message, byte[] startResponseContent, TimeSpan timeout)
{
SendMessageExcpectAnswer(message, CheckMatch(message, startResponseContent), timeout);
}
public void SendMessageExcpectAnswer(CrtpMessage message, Func <CrtpMessage, bool> isExpectedResponse, TimeSpan timeout)
{
_atLeastOnceCommunicatorStrategy.RegisterExpectation(message, isExpectedResponse, timeout);
SendMessage(message);
}
public void SendMessageExcpectAnswer(CrtpMessage message, byte[] startResponseContent)
{
SendMessageExcpectAnswer(message, startResponseContent, TimeSpan.FromMilliseconds(250));
}
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");
}
