private int GetUltrasoundDistanceFromLidarData(int mountingAngle, float sensorOffset)
{
const int closeRangeHalfBeam = 15;
const float closeRangeMaxDistance = 1.00f;
const int longRangeHalfBeam = 5;
float closeRangeDistance = _distances.GetSmallestDistanceInRange(mountingAngle - closeRangeHalfBeam, mountingAngle + closeRangeHalfBeam);
float longRangeDistance = _distances.GetSmallestDistanceInRange(mountingAngle - longRangeHalfBeam, mountingAngle + longRangeHalfBeam);
if (float.IsNaN(closeRangeDistance))
{
closeRangeDistance = TimeOutDistance;
}
if (float.IsNaN(longRangeDistance))
{
longRangeDistance = TimeOutDistance;
}
float shortestDistanceInCm = (closeRangeDistance > closeRangeMaxDistance) ? longRangeDistance : Math.Min(closeRangeDistance, longRangeDistance) - sensorOffset;
return((int)(Math.Min(shortestDistanceInCm, MaximumDistance) * 100));
}