vato007/swiftrplidar
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Get lidar to a working state, add errors, example.

Browse Source
This commit is contained in:
Piv
2020-09-19 19:50:56 +09:30
parent 761eb0bc1a
commit ef84c19f92
5 changed files with 120 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files

143
Sources/SwiftRPLidar/SwiftRPLidar.swift
View File

@@ -30,18 +30,26 @@ public enum HEALTH_STATUSES: UInt8 {
case GOOD = 0, WARNING, ERROR
}
public enum RPLidarError: Error {
case FAILED_TO_START,
INCORRECT_INFO_FORMAT,
INCORRECT_HEALTH_FORMAT,
SCAN_ERROR(message: String),
INCORRECT_DESCRIPTOR_FORMAT
}
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 newScan == inversedNewScan {
throw RPLidarError.SCAN_ERROR(message: "New scan flags mismatch")
}
if ((raw[1] & 0b1) != 1) {
if (raw[1] & 0b1) != 1 {
throw RPLidarError.SCAN_ERROR(message: "Check bit not equal to 1")
}
let angle = Float(raw[1] >> 1) + Float(raw[2] << 7) / 64
let distance = Float(raw[3]) + Float(raw[4] << 8) / 4
let angle = (Float(raw[1] >> 1) + Float(raw[2] << 7)) / 64
let distance = (Float(raw[3]) + Float(raw[4] << 8)) / 4
return LidarScan(newScan: newScan == 1, quality: quality, angle: angle, distance: distance)
}
@@ -53,44 +61,46 @@ public struct LidarScan{
}
public typealias MeasurementHandler = (_ scan: LidarScan) -> Bool
public typealias ScanHandler = (_ scans: [LidarScan]) -> Bool
public class SwiftRPLidar {
private var motor: Bool = false
private var serialPort: LidarSerial? = nil
private var serialPort: LidarSerial
private var motorRunning = false
private let measurementDelayus: UInt32
public init(onPort serialPort: LidarSerial) throws {
public init(onPort serialPort: LidarSerial, measurementDelayus: UInt32 = 500) throws {
self.serialPort = serialPort
self.measurementDelayus = measurementDelayus
try connect()
try startMotor()
}
deinit {
if(serialPort != nil){
serialPort?.closePort()
disconnect()
do {
try stop()
try stopMotor()
} catch {
print("Failed to stop lidar/motor.")
}
}
public func connect() throws {
disconnect()
try serialPort!.openPort()
serialPort?.setBaudrate(baudrate: 115200)
try serialPort.openPort()
serialPort.setBaudrate(baudrate: 115200)
}
public func disconnect(){
if(serialPort != nil){
// Need to close, SwiftSerial is blocking.
serialPort!.closePort()
}
// Need to close, SwiftSerial is blocking.
serialPort.closePort()
}
public func startMotor() throws{
// A1
serialPort?.dtr = true
serialPort.dtr = true
// A2
try self.setPwm(Constants.DEFAULT_MOTOR_PWM)
self.motorRunning = true
@@ -98,7 +108,7 @@ public class SwiftRPLidar {
public func stopMotor() throws{
try setPwm(0)
serialPort?.dtr = false
serialPort.dtr = false
motorRunning = false
}
@@ -107,35 +117,26 @@ public class SwiftRPLidar {
try self.sendPayloadCommand(Constants.SET_PWM_BYTE, payload: pwm.asData())
}
public func getInfo() throws -> (UInt8, UInt8, UInt8, String){
// Returns (model, firmware, hardware, serialNumber)
public func getInfo() throws -> (UInt8, (UInt8, UInt8), UInt8, String){
try sendCommand(Data([Constants.GET_HEALTH]))
let (dataSize, isSingle, dataType) = try readDescriptor()!
if (dataSize != Constants.INFO_LEN){
}
if(!isSingle){
}
if(dataType != Constants.INFO_TYPE){
if dataSize != Constants.INFO_LEN || !isSingle || dataType != Constants.INFO_TYPE {
throw RPLidarError.INCORRECT_INFO_FORMAT
}
let raw = try readResponse(dataSize)
let serialNumber = String(bytes: raw[4...], encoding: .ascii)!
return (raw[0], raw[2], raw[3], serialNumber)
return (raw[0], (raw[2], raw[1]), raw[3], serialNumber)
}
public func getHealth() throws -> (HEALTH_STATUSES, UInt8){
try sendCommand(Constants.GET_HEALTH.asData())
let (dataSize, isSingle, dataType) = try readDescriptor()!
if (dataSize != Constants.HEALTH_LEN){
guard let (dataSize, isSingle, dataType) = try readDescriptor() else {
throw RPLidarError.INCORRECT_HEALTH_FORMAT
}
if (!isSingle){
}
if(dataType != Constants.HEALTH_TYPE){
if dataSize != Constants.HEALTH_LEN || !isSingle || dataType != Constants.HEALTH_TYPE{
throw RPLidarError.INCORRECT_HEALTH_FORMAT
}
let raw = try readResponse(dataSize)
let status = HEALTH_STATUSES(rawValue: raw[0])!
@@ -144,7 +145,7 @@ public class SwiftRPLidar {
}
public func clearInput() throws{
_ = try serialPort?.readData(ofLength: (serialPort?.inWaiting)!)
_ = try serialPort.readData(ofLength: serialPort.inWaiting)
}
public func stop() throws{
@@ -159,44 +160,45 @@ public class SwiftRPLidar {
try startMotor()
let (status, _) = try getHealth()
if status == .ERROR{
// Throw Exception
throw RPLidarError.INCORRECT_HEALTH_FORMAT
}
else if status == .WARNING {
print("Warning given when checking health.")
}
try sendCommand(Constants.SCAN.asData())
let (dataSize, isSingle, dataType) = try readDescriptor()!
if dataSize != 5 {
}
if isSingle {
}
if dataType != Constants.SCAN_TYPE {
if dataSize != 5 || isSingle || dataType != Constants.SCAN_TYPE {
throw RPLidarError.INCORRECT_DESCRIPTOR_FORMAT
}
var read = true
// Need to check in waiting or something...
while read {
let raw = try readResponse(Int(dataSize))
if maxBufferMeasurements > 0 {
let dataInWaiting = serialPort?.inWaiting
if dataInWaiting! > maxBufferMeasurements {
print("Too many measurements in the input buffer. Clearing Buffer")
_ = try serialPort?.readData(ofLength: dataInWaiting! / Int(dataSize) * Int(dataSize))
do {
let raw = try readResponse(Int(dataSize))
if maxBufferMeasurements > 0 {
let dataInWaiting = serialPort.inWaiting
if dataInWaiting > maxBufferMeasurements {
print("Too many measurements in the input buffer. Clearing Buffer")
_ = try serialPort.readData(ofLength: dataInWaiting / Int(dataSize) * Int(dataSize))
}
}
read = try onMeasure(processScan(raw: raw))
// TODO: Figure out why this delay is needed.
// If some delay is not present, then there is a high chance that the scan will be incorrect, and processScan will error.
usleep(measurementDelayus)
} catch RPLidarError.INCORRECT_DESCRIPTOR_FORMAT {
print("Incorrect Descriptor, skipping scan")
} catch RPLidarError.SCAN_ERROR(let message) {
print("Scan processing failed, skipping scan: \(message)")
}
// TODO: Support cancelling of measurements. Would it already work though?
read = try onMeasure(processScan(raw: raw))
}
}
public func iterScans(maxBufferMeasurements: Int = 500, minLength: Int = 5, _ onScan: ScanHandler) throws {
var scan: [LidarScan] = []
var read = true
try iterMeasurements{ measurement in
try iterMeasurements(maxBufferMeasurements: maxBufferMeasurements){ measurement in
if measurement.newScan {
if scan.count > minLength {
read = onScan(scan)
@@ -208,7 +210,6 @@ public class SwiftRPLidar {
}
return read
}
}
private func sendPayloadCommand(_ cmd: Int, payload: Data) throws{
@@ -216,7 +217,7 @@ public class SwiftRPLidar {
var req = Constants.SYNC.asData() + cmd.asData() + size.asData() + payload
let checksum = calcChecksum(req)
req += checksum.asData()
_ = try serialPort?.writeData(req)
_ = try serialPort.writeData(req)
}
private func sendPayloadCommand(_ cmd: UInt8, payload: Data) throws {
@@ -225,7 +226,7 @@ public class SwiftRPLidar {
private func calcChecksum(_ data: Data) -> Int{
var checksum = 0;
data.forEach{ body in
data.forEach { body in
checksum ^= Int(body)
}
return checksum
@@ -234,13 +235,11 @@ public class SwiftRPLidar {
private func sendCommand(_ command: Data) throws{
var req = Constants.SYNC.asData()
req.append(command)
_ = try serialPort?.writeData(req)
_ = try serialPort.writeData(req)
}
private func readDescriptor() throws -> (UInt8, Bool, UInt8)?{
guard let descriptor = try serialPort?.readData(ofLength: Constants.DESCRIPTOR_LEN) else {
return nil
}
let descriptor = try serialPort.readData(ofLength: Constants.DESCRIPTOR_LEN)
if (descriptor.count != Constants.DESCRIPTOR_LEN){
return nil
}
@@ -252,11 +251,9 @@ public class SwiftRPLidar {
}
private func readResponse(_ dataSize: Int) throws -> Data {
guard let data = try serialPort?.readData(ofLength: dataSize) else{
return Data()
}
let data = try serialPort.readData(ofLength: dataSize)
if(data.count != dataSize){
return Data()
throw RPLidarError.INCORRECT_DESCRIPTOR_FORMAT
}
return data
}
@@ -264,10 +261,10 @@ public class SwiftRPLidar {
private func readResponse(_ dataSize: UInt8) throws -> Data {
return try readResponse(Int(dataSize))
}
}
// MARK: Convenience packing extensions.
extension Int{
func asData() -> Data {
var int = self