public void SendMessage(SBP_Enums.MessageTypes messageType, int senderID, object data)
{
if (data is IPayload)
{
if (messageType == SBP_Enums.MessageTypes.Unknown)
{
throw new Exception("Parameter messageType can't be unknown");
}
SBPSendMessage messageToSend = new SBPSendMessage(messageType, senderID, (IPayload)data);
List <Byte> bytes = new List <byte>();
bytes.Add(PREAMBLE);
bytes.AddRange(BitConverter.GetBytes(messageToSend.MessageType));
bytes.AddRange(BitConverter.GetBytes(messageToSend.SenderID));
bytes.Add(messageToSend.Length);
bytes.AddRange(messageToSend.Payload.Data);
bytes.AddRange(BitConverter.GetBytes(messageToSend.GetChecksum()));
lock (_syncobject)
_sendMessageQueue.Enqueue(bytes.ToArray());
}
else
{
throw new Exception("Parameter data has no IPayload interface");
}
}
protected override bool InvokeThreadExecute()
{
SBPRawMessageEventArgs sendMessage = null;
if (_messageQueue.Count > 0)
{
byte[] message = new byte[0];
lock (_syncobject)
message = _messageQueue.Dequeue();
int messageType = BitConverter.ToUInt16(new byte[] { message[1], message[2] }, 0);
SBP_Enums.MessageTypes messageTypeEnum = SBP_Enums.MessageTypes.Unknown;
if (Enum.IsDefined(typeof(SBP_Enums.MessageTypes), (int)messageType))
{
messageTypeEnum = (SBP_Enums.MessageTypes)(int) messageType;
}
sendMessage = new SBPRawMessageEventArgs(messageTypeEnum, message);
}
if (sendMessage != null)
{
OnReceivedRawMessage(sendMessage);
return(true);
}
else
{
return(false);
}
}
public SBPSendMessage(SBP_Enums.MessageTypes messageType, int senderID, IPayload payload)
{
_messageType = messageType;
_senderID = senderID;
_payload = payload;
}
protected override void ProcessReading(bool restart)
{
if (restart)
{
_preambleFound = false;
_message = new SBPReceiveMessage();
}
if (!_preambleFound)
{
while (_receivedBytes.Count > 0)
{
if (_receivedBytes[0] == PREAMBLE)
{
_preambleFound = true;
_receivedBytes.RemoveAt(0);
break;
}
else
{
_receivedBytes.RemoveAt(0);
}
}
}
if (_preambleFound)
{
if (!_message.MessageType.HasValue && _receivedBytes.Count > 1)
{
_message.MessageType = BitConverter.ToUInt16(_receivedBytes.GetRange(0, 2).ToArray(), 0);
_receivedBytes.RemoveRange(0, 2);
}
if (!_message.SenderID.HasValue && _message.MessageType.HasValue && _receivedBytes.Count > 1)
{
_message.SenderID = BitConverter.ToUInt16(_receivedBytes.GetRange(0, 2).ToArray(), 0);
_receivedBytes.RemoveRange(0, 2);
}
if (!_message.Length.HasValue && _message.SenderID.HasValue && _receivedBytes.Count > 0)
{
_message.Length = _receivedBytes[0];
_receivedBytes.RemoveAt(0);
}
if (_message.Length.HasValue)
{
if (_receivedBytes.Count > _message.Length.Value + 2)
{
_message.Payload.AddRange(_receivedBytes.GetRange(0, _message.Length.Value));
_receivedBytes.RemoveRange(0, _message.Length.Value);
_message.ReceicevedChecksum = BitConverter.ToUInt16(_receivedBytes.GetRange(0, 2).ToArray(), 0);
_receivedBytes.RemoveRange(0, 2);
if (_message.ValidateCheckSum())
{
SBP_Enums.MessageTypes messageTypeEnum = SBP_Enums.MessageTypes.Unknown;
if (Enum.IsDefined(typeof(SBP_Enums.MessageTypes), (int)_message.MessageType))
{
messageTypeEnum = (SBP_Enums.MessageTypes)(int) _message.MessageType;
}
if (MESSAGE_STRUCTS.ContainsKey(messageTypeEnum))
{
object messageData = Activator.CreateInstance(MESSAGE_STRUCTS[messageTypeEnum], _message.Payload.ToArray());
lock (_syncobject)
_messageQueue.Enqueue(new SBPMessageEventArgs((int)_message.SenderID.Value, messageTypeEnum, messageData));
}
}
else
{
lock (_syncobject)
_readExceptionQueue.Enqueue(new SBPReadExceptionEventArgs(new Exception("CRC not valid")));
}
_message = new SBPReceiveMessage();
_preambleFound = false;
}
}
}
}
public SBPMessageEventArgs(int senderId, SBP_Enums.MessageTypes messageType, object data)
{
_senderId = senderId;
_messageType = messageType;
_data = data;
}
public SBPRawMessageEventArgs(SBP_Enums.MessageTypes messageType, byte[] message)
{
_message = message;
_messageType = messageType;
}