private void ProcessReceivedData(TiMDataMessage TimData)
{
lock (LidarPoints)
{
LidarPoints.Clear();
LidarPolarPointsList.Clear();
double angleLidar = Toolbox.DegToRad(Location.Theta);
for (int i = 0; i < TimData.AngleData.Count; i++)
{
if (TimData.AngleData[i] >= AngleMin && TimData.AngleData[i] <= AngleMax) // Filtre d'angle
{
double distance = TimData.DistanceData[i] / 1000.0;
double angle = Toolbox.DegToRad(TimData.AngleData[i]);
//On trouve les coordonnées du point brut en cartésien dans le ref du Lidar
double xRefRobot = Location.X + distance * Math.Cos(angle + angleLidar);
double yRefRobot = Location.Y + distance * Math.Sin(angle + angleLidar);
//On trouve les coordonnées du point en polaire dans le ref du robot
double distanceCentreRobot =
Math.Sqrt(Math.Pow(xRefRobot, 2) +
Math.Pow(yRefRobot, 2));
double angleAxeRobot;
if (yRefRobot != 0 && xRefRobot != 0)
{
angleAxeRobot = Math.Atan2(yRefRobot, xRefRobot);
}
else
{
angleAxeRobot = angle;
}
PolarPointRssi pt = new PolarPointRssi(angleAxeRobot, distance, TimData.RssiData[i]);
//LidarPoint point = new LidarPoint(distanceCentreRobot, angleAxeRobot, TimData.RssiData[i]);
// Supression des points en dehors des limites hardware
//if (distance >= 0.01 && distance <= 10.01) // Filtre de distance
{
//LidarPoints.Add(point);
LidarPolarPointsList.Add(pt);
}
}
}
}
//OnLidarPointsReady();
OnLidarDecodedFrame(robotId, LidarPolarPointsList);
}
private TiMDataMessage ParseTimDataMessage(string[] argList)
{
TiMDataMessage tms = new TiMDataMessage();
int pos = 1;
if (argList[pos++] != "LMDscandata")
{
return(tms);
}
tms.VersionNumber = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);
tms.DeviceNumber = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);
tms.SerialNumber = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);
tms.DeviceStatus1 = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);
tms.DeviceStatus2 = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);
tms.TelegramCounter = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);
tms.ScanCounter = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);
tms.TimeSinceStartUp = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);
tms.TimeOfTransmission = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);
tms.InputStatus1 = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);
tms.InputStatus2 = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);
tms.OutputStatus1 = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);
tms.OutputStatus2 = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);
tms.ReservedByteA = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);
tms.ScanningFrequency = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);
tms.MeasurementFrequency = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);
tms.NumberOfEncoders = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);
tms.NumberOf16BitsChannels = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);
tms.MeasuredDataContents = argList[pos++];
tms.ScalingFactor = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);
tms.ScalingOffset = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);
tms.StartingAngle = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);
tms.AngularStepWidth = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);
tms.NumberOfData = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);
tms.DistanceData = new List <int>();
tms.AngleData = new List <double>();
tms.RssiData = new List <int>();
tms.RssiDataPercent = new List <double>();
double angleOffset = -900000; // Pour placer le zéro d'angle en face du LIDAR
for (int i = 0; i < tms.NumberOfData; i++)
{
double angle = (tms.StartingAngle + angleOffset + i * tms.AngularStepWidth) / 10000.0;
if (IsUpsideDown) // Inverse les angles si le Lidar est retourné
{
angle *= -1;
}
tms.AngleData.Add(angle);
tms.DistanceData.Add(int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber));
}
tms.RssiDataType = argList[pos++];
tms.ScalingFactor = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);
tms.ScalingOffset = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);
tms.OutputStatus1 = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);
tms.OutputStatus2 = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);
tms.NumberOfDataRssi = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);
for (int i = 0; i < tms.NumberOfDataRssi; i++)
{
tms.RssiData.Add(int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber));
double calculatePercentRssi = tms.RssiData[i] / 65536.0 * 100.0;//(((byte)(tms.RssiData[i])) / 255.0) * 100;
tms.RssiDataPercent.Add(calculatePercentRssi);
}
//double maxvalue = tms.RssiData.Max();
int outputOfPosition = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); //RSSI missing
int deviceName = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);; //RSSI missing : not really clear in the datasheet
tms.Comment = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);
if (tms.Comment == 1)
{
pos++; //int CommentLenght = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);
pos++; //int CommentTransmit = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);
}
pos++;//tms.TimeInformation = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);
tms.EventInformation = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);
return(tms);
}
