private void HandlePacket(byte[] packet, IPEndPoint sender)
{
try
{
using (MemoryStream stream = new MemoryStream(packet))
{
PacketBuffer buffer = new PacketBuffer(stream);
if (packet.Length < 3 || buffer.ReadByte() != 'F' || buffer.ReadByte() != 'L' || buffer.ReadByte() != 'Y')
{
Log.Debug("Hello: Invalid magic value!");
return;
}
if (buffer.ReadByte() != (byte)HelloPacketType.Answer)
return;
DroneEntry entry = new DroneEntry();
entry.Address = sender.Address;
entry.LastFound = DateTime.Now;
entry.Name = buffer.ReadString();
entry.Model = buffer.ReadString();
entry.SerialCode = buffer.ReadString();
entry.FirmwareVersion = buffer.ReadByte();
AddDrone(entry);
}
}
catch (Exception e)
{
Log.Error(e.ToString());
}
}
public DebugData(PacketBuffer buffer)
{
Profiler = new DebugProfiler(buffer);
PitchOutput = buffer.ReadFloat();
RollOutput = buffer.ReadFloat();
YawOutput = buffer.ReadFloat();
}
public DebugProfiler(PacketBuffer buffer)
{
uint count = buffer.ReadUInt();
Entries = new Entry[count];
for (int i = 0; i < count; i++)
Entries[i] = new Entry(buffer);
}
public void Write(PacketBuffer packet)
{
if (packet == null)
throw new ArgumentNullException(nameof(packet));
packet.Write(Pitch);
packet.Write(Roll);
packet.Write(RotationalSpeed);
packet.Write(Thrust);
}
public void Write(PacketBuffer packet)
{
if (packet == null)
throw new ArgumentNullException(nameof(packet));
packet.Write(Values.FrontLeft);
packet.Write(Values.FrontRight);
packet.Write(Values.BackLeft);
packet.Write(Values.BackRight);
}
public GyroData(PacketBuffer buffer)
{
this.Roll = buffer.ReadFloat();
this.Pitch = buffer.ReadFloat();
this.Yaw = buffer.ReadFloat();
this.GyroX = buffer.ReadFloat();
this.GyroY = buffer.ReadFloat();
this.GyroZ = buffer.ReadFloat();
this.AccelerationX = buffer.ReadFloat();
this.AccelerationY = buffer.ReadFloat();
this.AccelerationZ = buffer.ReadFloat();
this.MagnetX = buffer.ReadFloat();
this.MagnetY = buffer.ReadFloat();
this.MagnetZ = buffer.ReadFloat();
this.Temperature = buffer.ReadFloat();
}
public GyroData(PacketBuffer buffer)
{
this.Roll = buffer.ReadFloat();
this.Pitch = buffer.ReadFloat();
this.Yaw = buffer.ReadFloat();
this.GyroX = buffer.ReadFloat();
this.GyroY = buffer.ReadFloat();
this.GyroZ = buffer.ReadFloat();
this.AccelerationX = buffer.ReadFloat();
this.AccelerationY = buffer.ReadFloat();
this.AccelerationZ = buffer.ReadFloat();
this.MagnetX = buffer.ReadFloat();
this.MagnetY = buffer.ReadFloat();
this.MagnetZ = buffer.ReadFloat();
this.Temperature = buffer.ReadFloat();
}
public DroneInfo(PacketBuffer buffer)
{
Name = buffer.ReadString();
ModelName = buffer.ReadString();
SerialCode = buffer.ReadString();
BuildName = buffer.ReadString().Trim().Replace(' ', '_');
BuildVersion = buffer.ReadByte();
HighestRevision = buffer.ReadInt();
ResetReason = (ResetReason)buffer.ReadByte();
ResetException = (ResetException)buffer.ReadByte();
if (ResetReason != ResetReason.Exception)
{
ResetException = ResetException.None;
}
StopReason = (StopReason)buffer.ReadByte();
GyroSensor = buffer.ReadString();
Magnetometer = buffer.ReadString();
}
private void HandlePacket(byte[] packet, IPEndPoint sender)
{
try
{
using (MemoryStream stream = new MemoryStream(packet))
{
PacketBuffer buffer = new PacketBuffer(stream);
if (packet.Length < 3 || buffer.ReadByte() != 'F' || buffer.ReadByte() != 'L' || buffer.ReadByte() != 'Y')
{
Log.Debug("DroneList: Invalid magic value!");
return;
}
if (buffer.ReadByte() != (byte)HelloPacketType.Answer)
{
return;
}
DroneEntry entry = new DroneEntry();
entry.Address = sender.Address;
entry.LastFound = DateTime.Now;
entry.Name = buffer.ReadString();
entry.Model = buffer.ReadString();
entry.SerialCode = buffer.ReadString();
entry.FirmwareVersion = buffer.ReadByte();
AddDrone(entry);
}
}
catch (Exception e)
{
Log.Error("Error while searching for drones:");
Log.Error(e);
}
}
private void HandleDataPacket(byte[] packet)
{
using (MemoryStream stream = new MemoryStream(packet))
{
PacketBuffer buffer = new PacketBuffer(stream);
if (buffer.Size < 3 || buffer.ReadByte() != 'F' || buffer.ReadByte() != 'L' || buffer.ReadByte() != 'Y')
{
return;
}
int revision = buffer.ReadInt();
DataPacketType type = (DataPacketType)buffer.ReadByte();
lastDataTime = Environment.TickCount;
switch (type)
{
case DataPacketType.Drone:
if (!CheckRevision(lastDataDroneRevision, revision))
{
return;
}
DroneState state = (DroneState)buffer.ReadByte();
QuadMotorValues motorValues = new QuadMotorValues(buffer);
GyroData gyro = new GyroData(buffer);
float batteryVoltage = buffer.ReadFloat();
int wifiRssi = buffer.ReadInt();
Data = new DroneData(state, motorValues, gyro, batteryVoltage, wifiRssi);
lastDataDroneRevision = revision;
break;
case DataPacketType.Log:
if (!CheckRevision(lastDataLogRevision, revision))
{
return;
}
int lines = buffer.ReadInt();
for (int i = 0; i < lines; i++)
{
string msg = buffer.ReadString();
if (OnLogMessage == null)
{
Log.Info("[Drone] " + msg);
}
else
{
OnLogMessage(msg + Environment.NewLine);
}
}
lastDataLogRevision = revision;
break;
case DataPacketType.Debug:
if (!CheckRevision(lastDataDebugRevision, revision))
{
return;
}
DebugData = new DebugData(buffer);
lastDataDebugRevision = revision;
break;
}
}
}
public void Write(PacketBuffer packetBuffer)
{
int size = Marshal.SizeOf(typeof(DroneSettings));
byte[] buffer = new byte[size];
GCHandle handle = GCHandle.Alloc(buffer, GCHandleType.Pinned);
Marshal.StructureToPtr(this, handle.AddrOfPinnedObject(), false);
handle.Free();
packetBuffer.Write(buffer, 0, buffer.Length);
}
public static DroneSettings Read(PacketBuffer packetBuffer)
{
int size = Marshal.SizeOf(typeof(DroneSettings));
byte[] buffer = new byte[size];
packetBuffer.Read(buffer, 0, buffer.Length);
GCHandle handle = GCHandle.Alloc(buffer, GCHandleType.Pinned);
DroneSettings settings = (DroneSettings)Marshal.PtrToStructure(handle.AddrOfPinnedObject(), typeof(DroneSettings));
handle.Free();
return settings;
}
private void HandleDataPacket(byte[] packet)
{
using (MemoryStream stream = new MemoryStream(packet))
{
PacketBuffer buffer = new PacketBuffer(stream);
if (buffer.Size < 3 || buffer.ReadByte() != 'F' || buffer.ReadByte() != 'L' || buffer.ReadByte() != 'Y')
return;
int revision = buffer.ReadInt();
DataPacketType type = (DataPacketType)buffer.ReadByte();
lastDataTime = Environment.TickCount;
switch (type)
{
case DataPacketType.Drone:
if (!CheckRevision(lastDataDroneRevision, revision))
return;
DroneState state = (DroneState)buffer.ReadByte();
QuadMotorValues motorValues = new QuadMotorValues(buffer);
GyroData gyro = new GyroData(buffer);
float batteryVoltage = buffer.ReadFloat();
int wifiRssi = buffer.ReadInt();
Data = new DroneData(state, motorValues, gyro, batteryVoltage, wifiRssi);
lastDataDroneRevision = revision;
break;
case DataPacketType.Log:
if (!CheckRevision(lastDataLogRevision, revision))
return;
int lines = buffer.ReadInt();
for (int i = 0; i < lines; i++)
{
string msg = buffer.ReadString();
if (OnLogMessage == null)
Log.Info("[Drone] " + msg);
else
OnLogMessage(msg + Environment.NewLine);
}
lastDataLogRevision = revision;
break;
case DataPacketType.Debug:
if (!CheckRevision(lastDataDebugRevision, revision))
return;
DebugData = new DebugData(buffer);
lastDataDebugRevision = revision;
break;
}
}
}
private void HandlePacket(byte[] packet)
{
// jedes Drohnen Paket ist mindestens HeaderSize Bytes lang und fangen mit "FLY" an
using (MemoryStream stream = new MemoryStream(packet))
{
PacketBuffer buffer = new PacketBuffer(stream);
if (packet.Length < HeaderSize || buffer.ReadByte() != 'F' || buffer.ReadByte() != 'L' || buffer.ReadByte() != 'Y')
return;
int revision = buffer.ReadInt();
bool isGuaranteed = buffer.ReadByte() > 0;
PacketType type = (PacketType)buffer.ReadByte();
if (Config.VerbosePacketReceive
&& type != PacketType.Ack
&& (type != PacketType.Ping || Config.LogPingPacket))
Log.Verbose("[{0}] Received: [{1}] {2}, size: {3} bytes", Address.ToString(), revision, type, packet.Length);
switch (type)
{
case PacketType.Ping:
if (packet.Length < HeaderSize + sizeof(long))
throw new InvalidDataException("Packet is not long enough.");
bool wasNotConnected = !CheckConnection();
lastPing = Environment.TickCount;
long time = buffer.ReadLong(); // time ist der Wert von stopwatch zum Zeitpunkt des Absenden des Pakets
Ping = (int)(stopwatch.ElapsedMilliseconds - time);
if (wasNotConnected)
OnConnected?.Invoke(this, EventArgs.Empty);
RemovePacketToAcknowlegde(revision);
break;
case PacketType.Ack:
IPacket acknowlegdedPacket;
if (!packetsToAcknowledge.TryGetValue(revision, out acknowlegdedPacket))
{
if (Config.VerbosePacketReceive)
Log.Verbose("[{0}] Unknown acknowlegde: [{1}]", Address.ToString(), revision);
break;
}
if (Config.VerbosePacketReceive)
Log.Verbose("[{0}] Acknowlegde: [{1}] {2}", Address.ToString(), revision, acknowlegdedPacket.Type);
RemovePacketToAcknowlegde(revision);
break;
case PacketType.Info:
Info = new DroneInfo(buffer);
Settings = DroneSettings.Read(buffer);
RemovePacketToAcknowlegde(revision);
break;
default:
throw new InvalidDataException("Invalid packet type to get sent by cluster.");
}
}
}
public Drone(IPAddress address, Config config)
{
if (address == null)
throw new ArgumentNullException(nameof(address));
this.Config = config;
this.Address = address;
controlSocket = new UdpClient();
controlSocket.Connect(address, Config.ProtocolControlPort);
dataSocket = new UdpClient(Config.ProtocolDataPort);
packetBuffer = new PacketBuffer(packetStream);
controlSocket.BeginReceive(ReceivePacket, null);
dataSocket.BeginReceive(ReceiveDataPacket, null);
// Ping senden und ein ResetRevision Paket senden damit die Revision wieder zurück gesetzt wird
SendPing();
OnConnected += (sender, args) =>
{
Log.Info("Connected to {0}", Address);
currentRevision = 1;
lastDataDroneRevision = 0;
lastDataLogRevision = 0;
lastDataDebugRevision = 0;
lastPing = Environment.TickCount;
lastDataTime = Environment.TickCount;
// alle Pending Packets leeren, damit die Drone nach Reconnect nicht überfordert wird
lock (packetsToAcknowledge)
{
packetsToAcknowledge.Clear();
packetSendTime.Clear();
packetAcknowlegdeEvents.Clear();
}
SendGetInfo();
SendPacket(new PacketResetRevision(), true);
SendPacket(new PacketCalibrateGyro(), true);
SendPacket(new PacketSubscribeDataFeed(), true);
};
}
public Entry(PacketBuffer buffer)
{
Name = buffer.ReadString();
TimeMicros = buffer.ReadUInt();
TimeMaxMicros = buffer.ReadUInt();
}
private void HandlePacket(byte[] packet)
{
// jedes Drohnen Paket ist mindestens HeaderSize Bytes lang und fangen mit "FLY" an
using (MemoryStream stream = new MemoryStream(packet))
{
PacketBuffer buffer = new PacketBuffer(stream);
if (packet.Length < HeaderSize || buffer.ReadByte() != 'F' || buffer.ReadByte() != 'L' || buffer.ReadByte() != 'Y')
{
return;
}
int revision = buffer.ReadInt();
bool isGuaranteed = buffer.ReadByte() > 0;
PacketType type = (PacketType)buffer.ReadByte();
if (Config.VerbosePacketReceive &&
type != PacketType.Ack &&
(Config.LogPingPacket || type != PacketType.Ping) &&
(Config.LogNoisyPackets || !type.IsNosiy()))
{
Log.Verbose("[{0}] Received: [{1}] {2}, size: {3} bytes", Address.ToString(), revision, type, packet.Length);
}
switch (type)
{
case PacketType.Ping:
bool wasNotConnected = !CheckConnection();
lastPing = Environment.TickCount;
long time = 0;
if (packet.Length >= HeaderSize + sizeof(long))
{
time = buffer.ReadLong(); // time ist der Wert von stopwatch zum Zeitpunkt des Absenden des Pakets
}
else
{
Log.Error("Invalid ping packet received with length: {0}", packet.Length);
}
int ping = (int)(stopwatch.ElapsedMilliseconds - time);
if (ping < 0)
{
Log.Warning("Invalid ping value received: {0}", ping);
ping = 0;
}
Ping = ping;
if (wasNotConnected)
{
OnConnected?.Invoke(this, EventArgs.Empty);
}
RemovePacketToAcknowledge(revision);
break;
case PacketType.Ack:
IPacket acknowlegdedPacket;
if (!packetsToAcknowledge.TryGetValue(revision, out acknowlegdedPacket))
{
if (Config.VerbosePacketReceive)
{
Log.Verbose("[{0}] Unknown acknowledge: [{1}]", Address.ToString(), revision);
}
break;
}
if (Config.VerbosePacketReceive)
{
Log.Verbose("[{0}] Acknowledge: [{1}] {2}", Address.ToString(), revision, acknowlegdedPacket.Type);
}
RemovePacketToAcknowledge(revision);
break;
case PacketType.Info:
Info = new DroneInfo(buffer);
Settings = DroneSettings.Read(buffer);
if (firstInfo)
{
Log.Info("Received drone info for first time...");
Log.WriteProperties(LogLevel.Info, Info);
firstInfo = false;
}
RemovePacketToAcknowledge(revision);
break;
default:
throw new InvalidDataException("Invalid packet type received.");
}
}
}
private void HandlePacket(byte[] packet)
{
// jedes Drohnen Paket ist mindestens HeaderSize Bytes lang und fangen mit "FLY" an
using (MemoryStream stream = new MemoryStream(packet))
{
PacketBuffer buffer = new PacketBuffer(stream);
if (packet.Length < HeaderSize || buffer.ReadByte() != 'F' || buffer.ReadByte() != 'L' || buffer.ReadByte() != 'Y')
{
return;
}
int revision = buffer.ReadInt();
bool isGuaranteed = buffer.ReadByte() > 0;
PacketType type = (PacketType)buffer.ReadByte();
if (Config.VerbosePacketReceive &&
type != PacketType.Ack &&
(type != PacketType.Ping || Config.LogPingPacket))
{
Log.Verbose("[{0}] Received: [{1}] {2}, size: {3} bytes", Address.ToString(), revision, type, packet.Length);
}
switch (type)
{
case PacketType.Ping:
if (packet.Length < HeaderSize + sizeof(long))
{
throw new InvalidDataException("Packet is not long enough.");
}
bool wasNotConnected = !CheckConnection();
lastPing = Environment.TickCount;
long time = buffer.ReadLong(); // time ist der Wert von stopwatch zum Zeitpunkt des Absenden des Pakets
Ping = (int)(stopwatch.ElapsedMilliseconds - time);
if (wasNotConnected)
{
OnConnected?.Invoke(this, EventArgs.Empty);
}
RemovePacketToAcknowlegde(revision);
break;
case PacketType.Ack:
IPacket acknowlegdedPacket;
if (!packetsToAcknowledge.TryGetValue(revision, out acknowlegdedPacket))
{
if (Config.VerbosePacketReceive)
{
Log.Verbose("[{0}] Unknown acknowlegde: [{1}]", Address.ToString(), revision);
}
break;
}
if (Config.VerbosePacketReceive)
{
Log.Verbose("[{0}] Acknowlegde: [{1}] {2}", Address.ToString(), revision, acknowlegdedPacket.Type);
}
RemovePacketToAcknowlegde(revision);
break;
case PacketType.Info:
Info = new DroneInfo(buffer);
Settings = DroneSettings.Read(buffer);
RemovePacketToAcknowlegde(revision);
break;
default:
throw new InvalidDataException("Invalid packet type to get sent by cluster.");
}
}
}
public Entry(PacketBuffer buffer)
{
Name = buffer.ReadString();
TimeMicros = buffer.ReadUInt();
TimeMaxMicros = buffer.ReadUInt();
}
private void HandleDataPacket(byte[] packet)
{
using (MemoryStream stream = new MemoryStream(packet))
{
PacketBuffer buffer = new PacketBuffer(stream);
if (buffer.Size < 3 || buffer.ReadByte() != 'F' || buffer.ReadByte() != 'L' || buffer.ReadByte() != 'Y')
{
return;
}
int revision = buffer.ReadInt();
DataPacketType type = (DataPacketType)buffer.ReadByte();
lastDataTime = Environment.TickCount;
switch (type)
{
case DataPacketType.Drone:
if (!CheckRevision(lastDataDroneRevision, revision))
{
return;
}
DroneState state = (DroneState)buffer.ReadByte();
QuadMotorValues motorValues = new QuadMotorValues(buffer);
SensorData sensor = new SensorData(buffer);
float batteryVoltage = buffer.ReadFloat();
int wifiRssi = buffer.ReadInt();
Data = new DroneData(state, motorValues, sensor, batteryVoltage, wifiRssi);
lastDataDroneRevision = revision;
break;
case DataPacketType.Log:
if (!CheckRevision(lastDataLogRevision, revision))
{
return;
}
int lines = buffer.ReadInt();
for (int i = 0; i < lines; i++)
{
string msg = buffer.ReadString();
DroneLog.AddLine(msg);
}
lastDataLogRevision = revision;
break;
case DataPacketType.DebugOutput:
if (!CheckRevision(lastDataOutputRevision, revision))
{
return;
}
DebugOutputData = new OutputData(buffer);
lastDataOutputRevision = revision;
NotifyDebugDataChanged();
break;
case DataPacketType.DebugProfiler:
if (!CheckRevision(lastDataProfilerRevision, revision))
{
return;
}
DebugProfilerData = new ProfilerData(buffer);
lastDataProfilerRevision = revision;
NotifyDebugDataChanged();
break;
}
}
}
