private async Task <byte[]> SendCommand(IBleGattServerConnection connection, RileyLinkCommandType cmd, byte[] cmdData = null, int timeout = 2000)
{
ResetTimer();
byte[] data;
if (cmdData == null)
{
data = new byte[] { 1, (byte)cmd };
}
else
{
data = new byte[cmdData.Length + 2];
data[0] = (byte)(cmdData.Length + 1);
data[1] = (byte)cmd;
Buffer.BlockCopy(cmdData, 0, data, 2, cmdData.Length);
}
NotificationQueued.Reset();
Console.WriteLine("writing data");
await connection.WriteCharacteristicValue(RileyLinkServiceUUID, RileyLinkDataCharacteristicUUID, data);
if (!NotificationQueued.Wait(timeout))
{
throw new Exception("timed out expecting a response from rileylink");
}
NotificationQueued.Reset();
byte[] result = null;
if (ResponseQueue.Count > 0)
{
result = ResponseQueue.Dequeue();
}
if (result == null || result.Length == 0)
{
throw new Exception("RL returned no result");
}
else if (result[0] == (byte)RileyLinkResponseType.OK)
{
if (result.Length > 1)
{
var response = new byte[result.Length - 1];
Buffer.BlockCopy(result, 1, response, 0, response.Length);
return(response);
}
else
{
return(null);
}
}
else if (result[0] == (byte)RileyLinkResponseType.Interrupted)
{
return(await SendCommand(connection, cmd, cmdData, timeout));
}
else
{
throw new Exception($"RL returned error code {result[0]}");
}
}
private async Task <byte[]> SendCommand(RileyLinkCommandType cmd, byte[] cmdData = null, int timeout = 5000)
{
try
{
byte[] data;
if (cmdData == null)
{
data = new byte[] { 1, (byte)cmd };
}
else
{
data = new byte[cmdData.Length + 2];
data[0] = (byte)(cmdData.Length + 1);
data[1] = (byte)cmd;
Buffer.BlockCopy(cmdData, 0, data, 2, cmdData.Length);
}
var result = await WriteAndRead(data, timeout);
if (result == null || result.Length == 0)
{
throw new PacketRadioException("RL returned no result");
}
else if (result[0] == (byte)RileyLinkResponseType.OK ||
result[0] == (byte)RileyLinkResponseType.Interrupted)
{
if (result.Length > 1)
{
var response = new byte[result.Length - 1];
Buffer.BlockCopy(result, 1, response, 0, response.Length);
return(response);
}
else
{
return(null);
}
}
else if (result[0] == (byte)RileyLinkResponseType.Timeout)
{
throw new OmniCoreTimeoutException();
}
else
{
throw new PacketRadioException($"RL returned error code {result[0]}");
}
}
catch (OmniCoreException) { throw; }
catch (Exception e)
{
throw new PacketRadioException("Error while sending a command via BLE", e);
}
}
private async Task <Bytes> SendCommand(RileyLinkCommandType cmd, Bytes cmdData, int timeout = 2000)
{
return(new Bytes(await SendCommand(cmd, cmdData.ToArray(), timeout)));
}
