Diff: TFmini.cpp

Revision:

7:81f9d1372c45

Parent:

5:eeb0e5bdd8b9

--- a/TFmini.cpp	Sat Oct 06 05:01:42 2018 +0000
+++ b/TFmini.cpp	Mon Jun 24 08:59:14 2019 +0000
@@ -4,40 +4,51 @@
     RawSerial(serialTX, serialRX, TF_DEFAULT_BAUD)
 {
     attach(callback(this, &TFmini::receiveByte));
-    thread.start(callback(this, &TFmini::assembleLoop));
 }
 
 void TFmini::receiveByte()
 {
-    buf.push_back(getc());
+    buffer[bufferPoint % TF_SERIAL_BUFFER_SIZE] = getc();
+ 
+    if(bufferPoint != 0xff) {
+        ++bufferPoint;
+    } else {
+        bufferPoint = (255 % TF_SERIAL_BUFFER_SIZE)+1;
+    }
+ 
+    ++receivedBytes;
+ 
+    if(receivedBytes >= TF_SERIAL_BUFFER_SIZE){
+        checkData();
+    }
+}
+void TFmini::checkData()
+{
+    for(int i = 0; i < TF_SERIAL_BUFFER_SIZE; i++) {
+        if(buffer[i % TF_SERIAL_BUFFER_SIZE] == TF_HEADER_FIRST_BYTE && buffer[(i + 1) % TF_SERIAL_BUFFER_SIZE] == TF_HEADER_SECOND_BYTE) {
+            uint8_t checksum = 0x00;
+            for(int j = 0; j < TF_SERIAL_BUFFER_SIZE - 1; j++) {
+                checksum += buffer[(i + j)% TF_SERIAL_BUFFER_SIZE];
+            }
+            if(checksum == buffer[(i + TF_SERIAL_BUFFER_SIZE - 1)% TF_SERIAL_BUFFER_SIZE]) {
+                for(int j = 0; j < TF_SERIAL_BUFFER_SIZE - 3; j++) {
+                    data[j] = buffer[(i + 2 + j) % TF_SERIAL_BUFFER_SIZE];
+                }
+                receivedBytes = 0;
+ 
+                assemble();
+                return;
+            }
+        }
+    }
 }
 
 void TFmini::assemble()
 {
-    uint16_t checksum = 0x0000;
-    for(int i = 0; i < TF_SERIAL_BUFFER_SIZE - 1; i++) {
-        checksum+= buf[i];
-    }
-    uint8_t checksum_L = (checksum) & 0x00ff;
-    if(checksum_L == buf[8]) {
-        distance = ((buf[3]<<8)|buf[2]);    
-        if(distance > 1200) distance = 1200;
-        strength = ((buf[5]<<8)|buf[4]);
-    }
-}
-
-void TFmini::assembleLoop()
-{
-    while(true) {
-        if(buf.size() > TF_SERIAL_BUFFER_SIZE) {
-            if(buf[0] == TF_HEADER_FIRST_BYTE && buf[1] == TF_HEADER_SECOND_BYTE) {
-                assemble();
-                buf.erase(buf.begin(), buf.begin() + (TF_SERIAL_BUFFER_SIZE - 1));
-            } else {
-                buf.erase(buf.begin());
-            }
-        }
-    }
+    distance = (data[0] & 0xFF) | ((data[1] << 8) & 0xFF00);
+    strength = (data[2] & 0xFF) | ((data[3] << 8) & 0xFF00);
+    reserved = data[4];
+    rawQual = data[5];
 }
 
 int TFmini::getDistance()
@@ -49,8 +60,3 @@
 {
     return strength;
 }
-
-int TFmini::getSize()
-{
-    return buf.size();
-}
\ No newline at end of file