public void Should_To_Point(byte paramByte1, byte paramByte2, byte paramByte3, byte paramByte4, byte paramByte5, float angle, float distance, Int32 quality, bool startFlag)
{
ScanDataResponse scanDataResponse = ScanDataResponseHelper.ToScanDataResponse(new [] { paramByte1, paramByte2, paramByte3, paramByte4, paramByte5 });
Point result = ScanDataResponseHelper.ToPoint(scanDataResponse);
Assert.Equal(angle, result.Angle);
Assert.Equal(distance, result.Distance);
Assert.Equal(quality, result.Quality);
Assert.Equal(startFlag, result.StartFlag);
}
public static Point ToPoint(ScanDataResponse scanDataResponse)
{
Point point = new Point();
point.Distance = scanDataResponse.DistanceQ2 / 4.0f;
point.Angle = (scanDataResponse.AngleQ6AndCheckbit >> Constants.RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.0f;
point.Quality = (byte)(scanDataResponse.SyncAndQuality >> Constants.RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);
int startFlag = (scanDataResponse.SyncAndQuality & Constants.RPLIDAR_RESP_MEASUREMENT_SYNCBIT);
point.StartFlag = (startFlag == 1);
Log.Information("ToPoint -- point: {@Point}", point);
return(point);
}
public static ScanDataResponse ToScanDataResponse(byte[] bytes)
{
if (bytes.Length < Constants.ScanDataResponseLength)
{
throw new Exception("RESULT_INVALID_ANS_TYPE");
}
Log.ForContext <ScanDataResponseHelper>();
string hexString = ByteHelper.ToHexString(bytes);
Log.Information("ToScanDataResponse -- bytes : {@HexString}", hexString);
ScanDataResponse scanDataResponse = new ScanDataResponse();
scanDataResponse.SyncAndQuality = bytes[0];
scanDataResponse.AngleQ6AndCheckbit = BitConverter.ToUInt16(bytes, 1);
scanDataResponse.DistanceQ2 = BitConverter.ToUInt16(bytes, 3);
Log.Information("ToScanDataResponse -- scanDataResponse : {@ScanDataResponse}", scanDataResponse);
return(scanDataResponse);
}
