private void OnSettingsArrived(SettingsPacket settings)
{
if (SettingsArrived != null)
{
SettingsArrived(settings);
}
}
void _comLink_SettingsArrived(SettingsPacket settings)
{
Dispatcher.BeginInvoke((Action)(() =>
{
_rollPid.UpdateValue(settings.PidRoll);
_pitchPid.UpdateValue(settings.PidPitch);
_yawPid.UpdateValue(settings.PidYaw);
_windupGuard.Text = settings.WindupGuard.ToString();
_userControlGain.Text = settings.Gain.ToString();
}));
}
private void ProcessSerialPortData()
{
byte[] buffer = new byte[1024];
if (_serialPort.BytesToRead == 0)
{
Thread.Sleep(100);
}
while (true)
{
if (_serialPort.BytesToRead == 0)
{
break;
}
int bytesRead = _serialPort.Read(buffer, 0, Math.Min(buffer.Length, _serialPort.BytesToRead));
if (!_continue || bytesRead == 0)
{
break;
}
_queue.Write(buffer, 0, bytesRead);
bool stop = false;
while (_queue.Length > _header.Length && !stop)
{
if (_packetType == null)
{
byte[] otherData = _queue.DequeueUntil(_header);
if (otherData != null && otherData.Length > 0)
{
OnDataArrived(otherData);
}
if (!_queue.StartsWith(_header))
{
break;
}
_queue.Dequeue(_header.Length);
_packetType = _queue.Dequeue();
}
switch (_packetType.Value)
{
case PT_TELEMETRY:
if (_queue.Length >= SIZE_TELEMETRY)
{
byte[] telemetryBytes = _queue.Dequeue(SIZE_TELEMETRY);
_packetType = null;
int i = 0;
Telemetry telemetry = new Telemetry
{
Yaw = BinaryHelper.ReadFloat(telemetryBytes, ref i),
Pitch = BinaryHelper.ReadFloat(telemetryBytes, ref i),
Roll = BinaryHelper.ReadFloat(telemetryBytes, ref i),
MotorLeft = BinaryHelper.ReadFloat(telemetryBytes, ref i),
MotorRight = BinaryHelper.ReadFloat(telemetryBytes, ref i),
MotorFront = BinaryHelper.ReadFloat(telemetryBytes, ref i),
MotorBack = BinaryHelper.ReadFloat(telemetryBytes, ref i),
BatteryLevel = BinaryHelper.ReadFloat(telemetryBytes, ref i),
UserThrottle = BinaryHelper.ReadFloat(telemetryBytes, ref i),
UserPitch = BinaryHelper.ReadFloat(telemetryBytes, ref i),
UserRoll = BinaryHelper.ReadFloat(telemetryBytes, ref i),
UserYaw = BinaryHelper.ReadFloat(telemetryBytes, ref i),
LoopCount = BinaryHelper.ReadLong(telemetryBytes, ref i)
};
WriteToLog(telemetry.Pitch + "," + telemetry.UserThrottle + "," + telemetry.MotorFront + "," + telemetry.MotorBack);
OnTelemetryArrived(telemetry);
}
else
{
stop = true;
}
break;
case PT_SETTINGS:
if (_queue.Length >= SIZE_SETTINGS)
{
byte[] pidBytes = _queue.Dequeue(SIZE_SETTINGS);
_packetType = null;
int i = 0;
SettingsPacket settings = new SettingsPacket
{
PidRoll = new PidObj
{
P = BinaryHelper.ReadFloat(pidBytes, ref i),
I = BinaryHelper.ReadFloat(pidBytes, ref i),
D = BinaryHelper.ReadFloat(pidBytes, ref i)
},
PidPitch = new PidObj
{
P = BinaryHelper.ReadFloat(pidBytes, ref i),
I = BinaryHelper.ReadFloat(pidBytes, ref i),
D = BinaryHelper.ReadFloat(pidBytes, ref i)
},
PidYaw = new PidObj
{
P = BinaryHelper.ReadFloat(pidBytes, ref i),
I = BinaryHelper.ReadFloat(pidBytes, ref i),
D = BinaryHelper.ReadFloat(pidBytes, ref i)
},
Gain = BinaryHelper.ReadFloat(pidBytes, ref i),
WindupGuard = BinaryHelper.ReadFloat(pidBytes, ref i)
};
OnSettingsArrived(settings);
}
else
{
stop = true;
}
break;
case PT_RECV:
if (_queue.Length >= SIZE_RECV)
{
byte[] recvBytes = _queue.Dequeue(SIZE_RECV);
_packetType = null;
int i = 0;
RecieverPacket recvData = new RecieverPacket
{
Time = BinaryHelper.ReadUnsignedLong(recvBytes, ref i),
HasSignal = BinaryHelper.ReadByte(recvBytes, ref i) == 0 ? false : true,
};
recvData.Channel[0] = BinaryHelper.ReadFloat(recvBytes, ref i);
recvData.Channel[1] = BinaryHelper.ReadFloat(recvBytes, ref i);
recvData.Channel[2] = BinaryHelper.ReadFloat(recvBytes, ref i);
recvData.Channel[3] = BinaryHelper.ReadFloat(recvBytes, ref i);
recvData.Channel[4] = BinaryHelper.ReadFloat(recvBytes, ref i);
recvData.Channel[5] = BinaryHelper.ReadFloat(recvBytes, ref i);
WriteToLog(recvData.Time + "," + recvData.HasSignal + "," + recvData.Channel[0] + "," + recvData.Channel[1] + "," + recvData.Channel[2] + "," + recvData.Channel[3] + "," + recvData.Channel[4] + "," + recvData.Channel[5]);
OnReceiverArrived(recvData);
}
else
{
stop = true;
}
break;
case PT_IMU:
if (_queue.Length >= SIZE_IMU)
{
byte[] recvBytes = _queue.Dequeue(SIZE_IMU);
_packetType = null;
int i = 0;
ImuPacket imuData = new ImuPacket
{
Time = BinaryHelper.ReadUnsignedLong(recvBytes, ref i),
AccelRoll = BinaryHelper.ReadFloat(recvBytes, ref i),
AccelPitch = BinaryHelper.ReadFloat(recvBytes, ref i),
CompassYaw = BinaryHelper.ReadFloat(recvBytes, ref i),
Gx = BinaryHelper.ReadFloat(recvBytes, ref i),
Gy = BinaryHelper.ReadFloat(recvBytes, ref i),
Gz = BinaryHelper.ReadFloat(recvBytes, ref i),
Roll = BinaryHelper.ReadFloat(recvBytes, ref i),
Pitch = BinaryHelper.ReadFloat(recvBytes, ref i),
Yaw = BinaryHelper.ReadFloat(recvBytes, ref i)
};
WriteToLog((imuData.Time / 1000000.0).ToString("0.00") + "," + imuData.Gx + "," + imuData.Gy + "," + imuData.Gz + "," + imuData.AccelRoll + "," + imuData.AccelPitch + "," + imuData.CompassYaw + "," + imuData.Roll + "," + imuData.Pitch + "," + imuData.Yaw);
OnImuArrived(imuData);
}
else
{
stop = true;
}
break;
default:
_packetType = null;
break;
}
}
}
}
private void OnSettingsArrived(SettingsPacket settings)
{
if (SettingsArrived != null)
{
SettingsArrived(settings);
}
}
void _comLink_SettingsArrived(SettingsPacket settings)
{
Dispatcher.BeginInvoke((Action)(() =>
{
_rollPid.UpdateValue(settings.PidRoll);
_pitchPid.UpdateValue(settings.PidPitch);
_yawPid.UpdateValue(settings.PidYaw);
_windupGuard.Text = settings.WindupGuard.ToString();
_userControlGain.Text = settings.Gain.ToString();
}));
}
private void ProcessSerialPortData()
{
byte[] buffer = new byte[1024];
if (_serialPort.BytesToRead == 0)
{
Thread.Sleep(100);
}
while (true)
{
if (_serialPort.BytesToRead == 0)
{
break;
}
int bytesRead = _serialPort.Read(buffer, 0, Math.Min(buffer.Length, _serialPort.BytesToRead));
if (!_continue || bytesRead == 0)
{
break;
}
_queue.Write(buffer, 0, bytesRead);
bool stop = false;
while (_queue.Length > _header.Length && !stop)
{
if (_packetType == null)
{
byte[] otherData = _queue.DequeueUntil(_header);
if (otherData != null && otherData.Length > 0)
{
OnDataArrived(otherData);
}
if (!_queue.StartsWith(_header))
{
break;
}
_queue.Dequeue(_header.Length);
_packetType = _queue.Dequeue();
}
switch (_packetType.Value)
{
case PT_TELEMETRY:
if (_queue.Length >= SIZE_TELEMETRY)
{
byte[] telemetryBytes = _queue.Dequeue(SIZE_TELEMETRY);
_packetType = null;
int i = 0;
Telemetry telemetry = new Telemetry
{
Yaw = BinaryHelper.ReadFloat(telemetryBytes, ref i),
Pitch = BinaryHelper.ReadFloat(telemetryBytes, ref i),
Roll = BinaryHelper.ReadFloat(telemetryBytes, ref i),
MotorLeft = BinaryHelper.ReadFloat(telemetryBytes, ref i),
MotorRight = BinaryHelper.ReadFloat(telemetryBytes, ref i),
MotorFront = BinaryHelper.ReadFloat(telemetryBytes, ref i),
MotorBack = BinaryHelper.ReadFloat(telemetryBytes, ref i),
BatteryLevel = BinaryHelper.ReadFloat(telemetryBytes, ref i),
UserThrottle = BinaryHelper.ReadFloat(telemetryBytes, ref i),
UserPitch = BinaryHelper.ReadFloat(telemetryBytes, ref i),
UserRoll = BinaryHelper.ReadFloat(telemetryBytes, ref i),
UserYaw = BinaryHelper.ReadFloat(telemetryBytes, ref i),
LoopCount = BinaryHelper.ReadLong(telemetryBytes, ref i)
};
WriteToLog(telemetry.Pitch + "," + telemetry.UserThrottle + "," + telemetry.MotorFront + "," + telemetry.MotorBack);
OnTelemetryArrived(telemetry);
}
else
{
stop = true;
}
break;
case PT_SETTINGS:
if (_queue.Length >= SIZE_SETTINGS)
{
byte[] pidBytes = _queue.Dequeue(SIZE_SETTINGS);
_packetType = null;
int i = 0;
SettingsPacket settings = new SettingsPacket
{
PidRoll = new PidObj
{
P = BinaryHelper.ReadFloat(pidBytes, ref i),
I = BinaryHelper.ReadFloat(pidBytes, ref i),
D = BinaryHelper.ReadFloat(pidBytes, ref i)
},
PidPitch = new PidObj
{
P = BinaryHelper.ReadFloat(pidBytes, ref i),
I = BinaryHelper.ReadFloat(pidBytes, ref i),
D = BinaryHelper.ReadFloat(pidBytes, ref i)
},
PidYaw = new PidObj
{
P = BinaryHelper.ReadFloat(pidBytes, ref i),
I = BinaryHelper.ReadFloat(pidBytes, ref i),
D = BinaryHelper.ReadFloat(pidBytes, ref i)
},
Gain = BinaryHelper.ReadFloat(pidBytes, ref i),
WindupGuard = BinaryHelper.ReadFloat(pidBytes, ref i)
};
OnSettingsArrived(settings);
}
else
{
stop = true;
}
break;
case PT_RECV:
if (_queue.Length >= SIZE_RECV)
{
byte[] recvBytes = _queue.Dequeue(SIZE_RECV);
_packetType = null;
int i = 0;
RecieverPacket recvData = new RecieverPacket
{
Time = BinaryHelper.ReadUnsignedLong(recvBytes, ref i),
HasSignal = BinaryHelper.ReadByte(recvBytes, ref i) == 0 ? false : true,
};
recvData.Channel[0] = BinaryHelper.ReadFloat(recvBytes, ref i);
recvData.Channel[1] = BinaryHelper.ReadFloat(recvBytes, ref i);
recvData.Channel[2] = BinaryHelper.ReadFloat(recvBytes, ref i);
recvData.Channel[3] = BinaryHelper.ReadFloat(recvBytes, ref i);
recvData.Channel[4] = BinaryHelper.ReadFloat(recvBytes, ref i);
recvData.Channel[5] = BinaryHelper.ReadFloat(recvBytes, ref i);
WriteToLog(recvData.Time + "," + recvData.HasSignal + "," + recvData.Channel[0] + "," + recvData.Channel[1] + "," + recvData.Channel[2] + "," + recvData.Channel[3] + "," + recvData.Channel[4] + "," + recvData.Channel[5]);
OnReceiverArrived(recvData);
}
else
{
stop = true;
}
break;
case PT_IMU:
if (_queue.Length >= SIZE_IMU)
{
byte[] recvBytes = _queue.Dequeue(SIZE_IMU);
_packetType = null;
int i = 0;
ImuPacket imuData = new ImuPacket
{
Time = BinaryHelper.ReadUnsignedLong(recvBytes, ref i),
AccelRoll = BinaryHelper.ReadFloat(recvBytes, ref i),
AccelPitch = BinaryHelper.ReadFloat(recvBytes, ref i),
CompassYaw = BinaryHelper.ReadFloat(recvBytes, ref i),
Gx = BinaryHelper.ReadFloat(recvBytes, ref i),
Gy = BinaryHelper.ReadFloat(recvBytes, ref i),
Gz = BinaryHelper.ReadFloat(recvBytes, ref i),
Roll = BinaryHelper.ReadFloat(recvBytes, ref i),
Pitch = BinaryHelper.ReadFloat(recvBytes, ref i),
Yaw = BinaryHelper.ReadFloat(recvBytes, ref i)
};
WriteToLog((imuData.Time / 1000000.0).ToString("0.00") + "," + imuData.Gx + "," + imuData.Gy + "," + imuData.Gz + "," + imuData.AccelRoll + "," + imuData.AccelPitch + "," + imuData.CompassYaw + "," + imuData.Roll + "," + imuData.Pitch + "," + imuData.Yaw);
OnImuArrived(imuData);
}
else
{
stop = true;
}
break;
default:
_packetType = null;
break;
}
}
}
}
