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