private static void HandleBinaryCommand(byte[] data, BlockingCollection <TelloCommand> queue)
{
TelloCommand command = serializer.Read(data);
log.Debug($"Cmd: {command.Id}");
queue.Add(command);
}
public void SendCommand(TelloCommand command)
{
lock (sendLock)
{
byte[] data = serializer.Write(command, seqId);
cmdClient.Send(data, data.Length);
}
seqId++;
}
public IEventCommand Read(TelloCommand command)
{
EndianBinaryReader reader = command.CreateDataReader();
byte disturb = reader.ReadByte();
byte strength = reader.ReadByte();
return(new WifiStatus(disturb, strength));
}
private void HandleCommandEvent(TelloCommand command)
{
ICommandReader reader =
commandReaders.SingleOrDefault(t => t.Id == command.Id);
IEventCommand evt = reader?.Read(command);
if (evt == null)
{
return;
}
CommandReceived?.Invoke(this, evt);
}
public byte[] Write(TelloCommand command, ushort seqId)
{
int size = command.Data.Length + HeaderSize;
MemoryStream outStream = new MemoryStream();
EndianBinaryWriter writer = EndianBinaryWriter.FromStream(outStream, true);
writer.Write(0xcc);
writer.Write((ushort)size << 3);
writer.Write(CrcCalculator.Crc8(outStream.ToArray()));
writer.Write(command.Type);
writer.Write((ushort)command.Id);
writer.Write(seqId);
if (command.Data != null)
{
writer.Write(command.Data);
}
writer.Write(CrcCalculator.Crc16(outStream.ToArray()));
return(outStream.ToArray());
}
public static TelloDatagram DeserializeNew(byte[] buffer, int offset, int count, out TelloErrorCode errorCode)
{
if (buffer == null)
{
throw new ArgumentNullException(nameof(buffer));
}
if (count <= 0)
{
throw new ArgumentOutOfRangeException(nameof(count), count, $"Argument {nameof(count)} must be positive.");
}
if (buffer.Length < offset + count)
{
throw new ArgumentException(
$"The length of '{nameof(buffer)}' can't be " +
$"less than the sum of '{nameof(offset)}' and '{nameof(count)}'.");
}
var magic = buffer[offset];
switch (magic)
{
case TelloCommand.Magic:
return(TelloCommand.DeserializeNew(buffer, offset, count, out errorCode));
default:
var datagram = TelloAsciiDatagram.DeserializeNew(buffer, offset, count, out errorCode);
if (errorCode == TelloErrorCode.NoError)
{
return(datagram);
}
if (errorCode == TelloErrorCode.InvalidAsciiCharacter)
{
errorCode = TelloErrorCode.UnsupportedMagic;
}
return(null);
}
}
private static void EventThreadMain(object state)
{
EventThreadState eventState = (EventThreadState)state;
BlockingCollection <TelloCommand> queue = eventState.Queue;
isActive = true;
while (isActive)
{
TelloCommand command = null;
try { command = queue.Take(); }
catch (InvalidOperationException) { Thread.Sleep(100); }
if (command == null)
{
continue;
}
ICommandReader reader = commandReaders.SingleOrDefault(t => t.Id == command.Id);
IEventCommand evt = reader?.Read(command);
if (evt == null)
{
continue;
}
}
}
