void _comLink_TelemetryArrived(Telemetry telemetry)
{
if (DateTime.Now - _lastUpdate < TimeSpan.FromMilliseconds(100))
{
return;
}
Dispatcher.BeginInvoke((Action)(() =>
{
if (!IsEnabled)
{
return;
}
_hud.SuspendRedraw();
_hud.Roll = telemetry.Roll;
_hud.Pitch = telemetry.Pitch;
_hud.Yaw = telemetry.Yaw;
_hud.QuadcopterFrontLevel = telemetry.MotorFront;
_hud.QuadcopterBackLevel = telemetry.MotorBack;
_hud.QuadcopterLeftLevel = telemetry.MotorLeft;
_hud.QuadcopterRightLevel = telemetry.MotorRight;
_hud.BatteryLevel = telemetry.BatteryLevel;
_hud.UserInputElevation = telemetry.UserThrottle;
_hud.UserInputPitch = telemetry.UserPitch;
_hud.UserInputRoll = telemetry.UserRoll;
_hud.UserInputYaw = telemetry.UserYaw;
_hud.ResumeRedraw();
_lastUpdate = DateTime.Now;
}));
}
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 OnTelemetryArrived(Telemetry telemetry)
{
if (TelemetryArrived != null)
{
TelemetryArrived(telemetry);
}
}
void _comLink_TelemetryArrived(Telemetry telemetry)
{
if ( DateTime.Now - _telemetryLastUpdate < TimeSpan.FromMilliseconds(100))
{
return;
}
Dispatcher.BeginInvoke((Action)(() =>
{
if (!IsEnabled)
{
return;
}
_motorLeft.Text = telemetry.MotorLeft.ToString("##0.00");
_motorRight.Text = telemetry.MotorRight.ToString("##0.00");
_motorFront.Text = telemetry.MotorFront.ToString("##0.00");
_motorBack.Text = telemetry.MotorBack.ToString("##0.00");
_userThrottle.Text = telemetry.UserThrottle.ToString("##0.00");
_userRoll.Text = telemetry.UserRoll.ToString("##0.00");
_userPitch.Text = telemetry.UserPitch.ToString("##0.00");
_userYaw.Text = telemetry.UserYaw.ToString("##0.00");
_telemYaw.Text = telemetry.Yaw.ToString("##0.00");
_telemPitch.Text = telemetry.Pitch.ToString("##0.00");
_telemRoll.Text = telemetry.Roll.ToString("##0.00");
_battery.Text = telemetry.BatteryLevel.ToString("##0.00");
if (DateTime.Now - _lastLoopCountTime > TimeSpan.FromMilliseconds(1000))
{
double loopSpeed = (telemetry.LoopCount - _lastLoopCount) / (DateTime.Now - _lastLoopCountTime).TotalSeconds;
_lastLoopCount = telemetry.LoopCount;
_lastLoopCountTime = DateTime.Now;
_loopSpeed.Text = loopSpeed.ToString("##0.00") + "Hz";
}
_telemetryLastUpdate = DateTime.Now;
}));
}
private void _comLink_TelemetryArrived(Telemetry telemetry)
{
UpdateGraph(x =>
{
_graph.Graph.Series[SERIES_TELEM_ROLL].DataPoints.Add(new DataPoint { XValue = x, YValue = telemetry.Roll });
_graph.Graph.Series[SERIES_TELEM_PITCH].DataPoints.Add(new DataPoint { XValue = x, YValue = telemetry.Pitch });
_graph.Graph.Series[SERIES_TELEM_YAW].DataPoints.Add(new DataPoint { XValue = x, YValue = telemetry.Yaw });
_graph.Graph.Series[SERIES_TELEM_MOTORFRONT].DataPoints.Add(new DataPoint { XValue = x, YValue = telemetry.MotorFront });
_graph.Graph.Series[SERIES_TELEM_MOTORBACK].DataPoints.Add(new DataPoint { XValue = x, YValue = telemetry.MotorBack });
_graph.Graph.Series[SERIES_TELEM_MOTORLEFT].DataPoints.Add(new DataPoint { XValue = x, YValue = telemetry.MotorLeft });
_graph.Graph.Series[SERIES_TELEM_MOTORRIGHT].DataPoints.Add(new DataPoint { XValue = x, YValue = telemetry.MotorRight });
_graph.Graph.Series[SERIES_TELEM_USERROLL].DataPoints.Add(new DataPoint { XValue = x, YValue = telemetry.UserRoll });
_graph.Graph.Series[SERIES_TELEM_USERPITCH].DataPoints.Add(new DataPoint { XValue = x, YValue = telemetry.UserPitch });
_graph.Graph.Series[SERIES_TELEM_USERYAW].DataPoints.Add(new DataPoint { XValue = x, YValue = telemetry.UserYaw });
_graph.Graph.Series[SERIES_TELEM_USERTHROTTLE].DataPoints.Add(new DataPoint { XValue = x, YValue = telemetry.UserThrottle });
});
}
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;
}
}
}
}
