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 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.");
}
}
}
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.");
}
}
}
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;
}
}
}
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.");
}
}
}
