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