diff --git a/Package.resolved b/Package.resolved
index da252e9..c14dc95 100644
--- a/Package.resolved
+++ b/Package.resolved
@@ -6,7 +6,7 @@
         "repositoryURL": "https://vato.ddns.net/gitlab/vato007/SwiftSerial.git",
         "state": {
           "branch": "dtr_support",
-          "revision": "9799f402e77e5d69b0811b6c714b172d9ccdd768",
+          "revision": "34e59a7d8766f7097eb68779ec039e77a1eec78a",
           "version": null
         }
       }
diff --git a/Sources/SwiftRPLidar/SwiftRPLidar.swift b/Sources/SwiftRPLidar/SwiftRPLidar.swift
index 1686f6d..9c8ac86 100644
--- a/Sources/SwiftRPLidar/SwiftRPLidar.swift
+++ b/Sources/SwiftRPLidar/SwiftRPLidar.swift
@@ -70,6 +70,7 @@ class RPLidar{
     
     init(serialPort: SerialPort) throws {
         self.serialPort = serialPort
+
     }
     
     deinit {
@@ -145,8 +146,8 @@ class RPLidar{
         return (status, errorCode)
     }
     
-    public func clearInput(){
-        
+    public func clearInput() throws{
+        try serialPort?.readData(ofLength: (serialPort?.inWaiting)!)
     }
     
     public func stop() throws{
@@ -170,22 +171,44 @@ class RPLidar{
         
         try sendCommand(Constants.SCAN.asData())
         let (dataSize, isSingle, dataType) = try readDescriptor()!
-        if (dataSize != 5){
+        if dataSize != 5 {
             
         }
-        if isSingle{
+        if isSingle {
             
         }
-        if dataType != Constants.SCAN_TYPE{
+        if dataType != Constants.SCAN_TYPE {
             
         }
+        
+        // Need to check in waiting or something...
         for _ in 0..<500{
             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))
+                }
+            }
+            // TODO: Support cancelling of measurements. Would it already work though?
+            try onMeasure(processScan(raw: raw))
         }
     }
     
-    public func iterScans(maxBufferMeasurements: Int = 500, minLength: Int = 5, _ onScan: ScanHandler){
+    public func iterScans(maxBufferMeasurements: Int = 500, minLength: Int = 5, _ onScan: ScanHandler) throws {
+        var scan: [LidarScan] = []
+        try iterMeasurements{ measurement in
+            if measurement.newScan {
+                if scan.count > minLength {
+                    onScan(scan)
+                }
+                scan = []
+            }
+            if measurement.quality > 0 && measurement.distance > 0 {
+                scan.append(measurement)
+            }
+        }
         
     }
     
@@ -220,10 +243,10 @@ class RPLidar{
             return nil
         }
         if (descriptor.count != Constants.DESCRIPTOR_LEN){
-            
+            return
         }
         else if (descriptor[0...1] != Data([Constants.SYNC, Constants.SYNC2])){
-            
+            return
         }
         let isSingle = descriptor[descriptor.count - 2] == 0
         return (descriptor[2], isSingle, descriptor[descriptor.count - 1])
@@ -234,7 +257,7 @@ class RPLidar{
             return Data()
         }
         if(data.count != dataSize){
-            
+            return
         }
         return data
     }