public ARC.MessagingService.Navigation2DV1.ScanPoint[] ParseRawSensorPacket(byte[] data)
{
ARC.MessagingService.Navigation2DV1.ScanPoint [] scanPoints = new ARC.MessagingService.Navigation2DV1.ScanPoint[360];
if (data.Length < PACKET_SIZE)
{
throw new Exception(string.Format("Packet length ({0}) is less than required packet size", data.Length));
}
else if (data.Length > PACKET_SIZE)
{
throw new Exception(string.Format("Packet length ({0}) is greater than required packet size", data.Length));
}
if (data[0] != 0xfa && data[1] != 0xa0)
{
throw new Exception(string.Format("First data is not a packet header (expecting 0xfa, 0xa0 and received {0}, {1})", data[0], data[1]));
}
for (int pos = 0; pos < data.Length; pos += 42)
{
// CRC Check
if (data[pos] == 0xFA && data[pos + 1] != (0xA0 + pos / 42))
{
OnWarning?.Invoke(this, "Packet failed CRC check");
return(new ARC.MessagingService.Navigation2DV1.ScanPoint[] { });
}
int rpm = (int)(int)BitConverter.ToUInt16(new byte[] { data[pos + 2], data[pos + 3] }, 0);
int degree = (data[pos + 1] - 0xa0) * 6;
if (degree < 0)
{
OnWarning?.Invoke(this, $"Invalid packet (degree reported {degree})");
return(new ARC.MessagingService.Navigation2DV1.ScanPoint[] { });
}
scanPoints[degree] = new ARC.MessagingService.Navigation2DV1.ScanPoint(
(int)BitConverter.ToUInt16(new byte[] { data[pos + 6], data[pos + 7] }, 0) / 10,
255,
degree);
scanPoints[degree + 1] = new ARC.MessagingService.Navigation2DV1.ScanPoint(
(int)BitConverter.ToUInt16(new byte[] { data[pos + 12], data[pos + 13] }, 0) / 10,
255,
degree + 1);
scanPoints[degree + 2] = new ARC.MessagingService.Navigation2DV1.ScanPoint(
(int)BitConverter.ToUInt16(new byte[] { data[pos + 18], data[pos + 19] }, 0) / 10,
255,
degree + 2);
scanPoints[degree + 3] = new ARC.MessagingService.Navigation2DV1.ScanPoint(
(int)BitConverter.ToUInt16(new byte[] { data[pos + 24], data[pos + 25] }, 0) / 10,
255,
degree + 3);
scanPoints[degree + 4] = new ARC.MessagingService.Navigation2DV1.ScanPoint(
(int)BitConverter.ToUInt16(new byte[] { data[pos + 30], data[pos + 31] }, 0) / 10,
255,
degree + 4);
scanPoints[degree + 5] = new ARC.MessagingService.Navigation2DV1.ScanPoint(
(int)BitConverter.ToUInt16(new byte[] { data[pos + 36], data[pos + 37] }, 0) / 10,
255,
degree + 5);
}
return(scanPoints);
}
private void _lidarService_OnLidarDataReady1(object sender, byte[] lidarData)
{
if (IsClosing)
{
return;
}
var scanPoints = _lidarParser.ParseRawSensorPacket(lidarData);
if (scanPoints.Length != 360)
{
if (_debugging)
{
Invokers.SetAppendText(tbLog, true, $"Expecting 360 degrees, received {scanPoints.Length} degrees");
}
return;
}
ARC.MessagingService.Navigation2DV1.Messenger.UpdateScan(scanPoints);
List <PointF> points = new List <PointF>();
var sensorMin = new ARC.MessagingService.Navigation2DV1.ScanPoint(999999, 0, 0);
var sensorMax = new ARC.MessagingService.Navigation2DV1.ScanPoint(0, 0, 0);
int[] distanceArray = new int[scanPoints.Length];
int degreesOffset = Convert.ToInt32(_cf.STORAGE[ConfigTitles.OFFSET_DEGREES]);
for (int x = 0; x < scanPoints.Length; x++)
{
var sensorValue = scanPoints[x];
sensorValue.Degree = sensorValue.Degree + degreesOffset % 360;
if (sensorValue.Distance > 0 && sensorValue.Distance < sensorMin.Distance)
{
sensorMin = sensorValue;
}
if (sensorValue.Distance > sensorMax.Distance)
{
sensorMax = sensorValue;
}
if (sensorValue.Distance > 0)
{
points.Add(new PointF(
(float)((IMAGE_WIDTH / 2) + EZ_B.Functions.DegX2(sensorValue.Degree, sensorValue.Distance / 4)),
(float)((IMAGE_HEIGHT / 2) + EZ_B.Functions.DegY2(sensorValue.Degree, sensorValue.Distance / 4))));
}
distanceArray[x] = sensorValue.Distance;
}
ARC.Scripting.VariableManager.SetVariable(_cf.STORAGE[ConfigTitles.NEAREST_DISTANCE_VAR].ToString(), sensorMin.Distance);
ARC.Scripting.VariableManager.SetVariable(_cf.STORAGE[ConfigTitles.NEAREST_DEGREE_VAR].ToString(), sensorMin.Degree);
ARC.Scripting.VariableManager.SetVariable(_cf.STORAGE[ConfigTitles.FURTHEST_DISTANCE_VAR].ToString(), sensorMax.Distance);
ARC.Scripting.VariableManager.SetVariable(_cf.STORAGE[ConfigTitles.FURTHEST_DEGREE_VAR].ToString(), sensorMax.Degree);
// Update image twice a second
if (_stopWatch.ElapsedMilliseconds > 500)
{
_stopWatch.Restart();
using (Bitmap bmRealtime = new Bitmap(IMAGE_WIDTH, IMAGE_HEIGHT)) {
using (Graphics gRealtime = Graphics.FromImage(bmRealtime)) {
gRealtime.Clear(Color.Black);
Pen p = new Pen(Color.Red, 7);
gRealtime.FillPolygon(Brushes.Silver, points.ToArray());
gRealtime.DrawLines(p, points.ToArray());
List <Point> triangle = new List <Point>();
triangle.Add(new Point((IMAGE_WIDTH / 2) - 5, (IMAGE_HEIGHT / 2) + 5));
triangle.Add(new Point((IMAGE_WIDTH / 2) + 5, (IMAGE_HEIGHT / 2) + 5));
triangle.Add(new Point((IMAGE_WIDTH / 2), (IMAGE_HEIGHT / 2) - 10));
gRealtime.FillPolygon(Brushes.Green, triangle.ToArray());
gRealtime.DrawPolygon(Pens.Black, triangle.ToArray());
}
this.Invoke(new Action(() => {
using (Graphics gRealTime = Graphics.FromImage(pbRealtime.Image))
gRealTime.DrawImage(bmRealtime, 0, 0, pbRealtime.Image.Width, pbRealtime.Image.Height);
pbRealtime.Refresh();
lblMinDistance.Text = sensorMin.Distance.ToString();
lblMinDegree.Text = sensorMin.Degree.ToString();
lblMaxDistance.Text = sensorMax.Distance.ToString();
lblMaxDegree.Text = sensorMax.Degree.ToString();
}));
}
}
}
