public void RemoveSelectedVerticalAngleFromActive()
{
if (Lidar.Config.ActiveVerticalAngles.Count > 1 && SelectedActiveVerticalAngle != Lidar.Config.DefaultVerticalAngle)
{
VerticalAngle angleToRemove = SelectedActiveVerticalAngle;
SelectedActiveVerticalAngle = Lidar.Config.DefaultVerticalAngle;
Lidar.Config.ActiveVerticalAngles.Remove(angleToRemove);
}
}
internal static float GetDistance(byte[] dataBlock, VerticalAngle vertical)
{
int channelNumber = (int)vertical;
byte distanceByte1 = dataBlock[4 + 3 * channelNumber];
byte distanceByte2 = dataBlock[5 + 3 * channelNumber];
float distanceInMillimeters = ReverseAndCombineBytes(distanceByte1, distanceByte2) * 2.0f;
return(distanceInMillimeters / 1000);
}
public float GetDistance(float fromAngle, float toAngle, VerticalAngle verticalAngle, CalculationType calculationType)
{
if (Distances == null || (!Distances.ContainsKey(verticalAngle)))
{
return(float.NaN);
}
List <float> distancesInRange = GetDistancesInRange(fromAngle, toAngle, verticalAngle);
return(PerformCalculation(distancesInRange, calculationType));
}
public List <float> GetDistancesInRange(float fromAngle, float toAngle, VerticalAngle verticalAngle)
{
if (Distances == null || (!Distances.ContainsKey(verticalAngle)))
{
return new List <float>()
{
float.NaN
}
}
;
List <float> distancesInRange = new List <float>();
List <HorizontalPoint> horizontalPointsInRange = GetHorizontalPointsInRange(fromAngle, toAngle, verticalAngle);
foreach (HorizontalPoint point in horizontalPointsInRange)
{
distancesInRange.Add(point.Distance);
}
return(distancesInRange);
}
public void GetDistance_ValidData_ReturnExpectedValue(VerticalAngle angle)
{
float distance = LidarPacketInterpreter.GetDistance(FakeLidarData.FakeDataBlock1, angle);
Assert.Equal(FakeLidarData.ExpectedValues[angle], distance, 3);
}
public List <HorizontalPoint> GetHorizontalPointsInRange(float fromAngle, float toAngle, VerticalAngle verticalAngle)
{
if (Distances == null || (!Distances.ContainsKey(verticalAngle)))
{
return new List <HorizontalPoint>()
{
new HorizontalPoint(0, float.NaN)
}
}
;
List <HorizontalPoint> pointsInRange = new List <HorizontalPoint>();
bool angleSpansZero = fromAngle > toAngle;
int startIndex = Distances[verticalAngle].FindIndex(point => point.Angle > fromAngle);
if (angleSpansZero)
{
if (startIndex != -1)
{
for (int i = startIndex; i < Distances[verticalAngle].Count; i++)
{
pointsInRange.Add(Distances[verticalAngle][i]);
}
}
int endIndex = Distances[verticalAngle].FindIndex(point2 => point2.Angle > toAngle);
for (int i = 0; i < endIndex; i++)
{
pointsInRange.Add(Distances[verticalAngle][i]);
}
}
else
{
if (startIndex == -1)
{
return new List <HorizontalPoint>()
{
new HorizontalPoint(0, float.NaN)
}
}
;
int i = startIndex;
HorizontalPoint point = Distances[verticalAngle][i];
while (point.Angle < toAngle && i < Distances[verticalAngle].Count)
{
point = Distances[verticalAngle][i];
pointsInRange.Add(point);
++i;
}
}
return(pointsInRange);
}
