public SickRXEventArgs(SickScan s)
{
this.s = s;
}
public SickRXEventArgs(SickScan s)
{
this.s = s;
}
private int ProcessLidarPacket(Byte[] rx)
{
SickScan scan = new SickScan();
scan.points = new List<SickPoint>();
MemoryStream ms = new MemoryStream(rx);
int id = ms.ReadByte();
//ghetto:
//check for 180s
bool is180 = false;
if (id == 5) is180 = true;
int mode = ms.ReadByte();//Remove id/mode
int secs = (ms.ReadByte() << 8) + ms.ReadByte();
int ticks = (ms.ReadByte() << 24) + (ms.ReadByte() <<16)+ (ms.ReadByte() <<8) + (ms.ReadByte());
if (ms.ReadByte() != STX) //STX
return -1;
if (ms.ReadByte() != LIDAR_ADDR) //Address
return -2;
int length = ms.ReadByte() + (ms.ReadByte() * 256); //message len excluding CRC
int cmd = ms.ReadByte(); //command
ms.ReadByte(); //boolstuff 2 bytes
ms.ReadByte();
if (is180)
{
for (double i = -90; i <= 90; i += 0.5)
{
int r = (ms.ReadByte() + (ms.ReadByte() * 256));
double theta = ((i) * (Math.PI / 180)); //radians
double range = (double)r * .01; //range in meters
double rawY = -1 * Math.Sin(theta) * range;
double rawX = Math.Cos(theta) * range;
scan.points.Add(new SickPoint(new RThetaCoordinate((float)range, (float)theta)));
}
}
else
{
for (double i = -45; i <= 45; i += 0.5)
{
int r = (ms.ReadByte() + (ms.ReadByte() * 256));
double theta = ((i) * (Math.PI / 180)); //radians
double range = (double)r * .01; //range in meters
double rawY = -1 * Math.Sin(theta) * range;
double rawX = Math.Cos(theta) * range;
scan.points.Add(new SickPoint(new RThetaCoordinate((float)range, (float)theta)));
}
}
scan.packetNum = totalPackets;
scan.scannerID = id;
scan.timestamp = (double)secs + (double)ticks / 10000.0;
if (ScanReceived != null)
ScanReceived(this,new SickRXEventArgs (scan));
return 0;
}
private int ProcessLidarPacket(Byte[] rx)
{
SickScan scan = new SickScan();
scan.points = new List <SickPoint>();
MemoryStream ms = new MemoryStream(rx);
int id = ms.ReadByte();
//ghetto:
//check for 180s
bool is180 = false;
if (id == 5)
{
is180 = true;
}
int mode = ms.ReadByte(); //Remove id/mode
int secs = (ms.ReadByte() << 8) + ms.ReadByte();
int ticks = (ms.ReadByte() << 24) + (ms.ReadByte() << 16) + (ms.ReadByte() << 8) + (ms.ReadByte());
if (ms.ReadByte() != STX) //STX
{
return(-1);
}
if (ms.ReadByte() != LIDAR_ADDR) //Address
{
return(-2);
}
int length = ms.ReadByte() + (ms.ReadByte() * 256); //message len excluding CRC
int cmd = ms.ReadByte(); //command
ms.ReadByte(); //boolstuff 2 bytes
ms.ReadByte();
if (is180)
{
for (double i = -90; i <= 90; i += 0.5)
{
int r = (ms.ReadByte() + (ms.ReadByte() * 256));
double theta = ((i) * (Math.PI / 180)); //radians
double range = (double)r * .01; //range in meters
double rawY = -1 * Math.Sin(theta) * range;
double rawX = Math.Cos(theta) * range;
scan.points.Add(new SickPoint(new RThetaCoordinate((float)range, (float)theta)));
}
}
else
{
for (double i = -45; i <= 45; i += 0.5)
{
int r = (ms.ReadByte() + (ms.ReadByte() * 256));
double theta = ((i) * (Math.PI / 180)); //radians
double range = (double)r * .01; //range in meters
double rawY = -1 * Math.Sin(theta) * range;
double rawX = Math.Cos(theta) * range;
scan.points.Add(new SickPoint(new RThetaCoordinate((float)range, (float)theta)));
}
}
scan.packetNum = totalPackets;
scan.scannerID = id;
scan.timestamp = (double)secs + (double)ticks / 10000.0;
if (ScanReceived != null)
{
ScanReceived(this, new SickRXEventArgs(scan));
}
return(0);
}
