diff --git a/Sources/SwiftRPLidar/SwiftRPLidar.swift b/Sources/SwiftRPLidar/SwiftRPLidar.swift index e95614d..0e8c5da 100644 --- a/Sources/SwiftRPLidar/SwiftRPLidar.swift +++ b/Sources/SwiftRPLidar/SwiftRPLidar.swift @@ -35,11 +35,23 @@ struct SwiftRPLidar { var text = "Hello, World!" } -func processScan(raw: Data){ - +func processScan(raw: Data) throws -> LidarScan { + let newScan = raw[0] & 0b1 + let inversedNewScan = (raw[0] >> 1) & 0b1 + let quality = raw[0] >> 2 + if (newScan == inversedNewScan){ + + } + if ((raw[1] & 0b1) != 1) { + + } + let angle = (raw[1] >> 1) + (raw[2] << 7) / 64 + let distance = raw[3] + (raw[4] << 8) / 4 + return LidarScan(newScan: newScan, quality: quality, angle: angle, distance: distance) } struct LidarScan{ + var newScan: Bool var quality: Float var angle: Float var distance: Float