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); }