Revision 7:81f9d1372c45, committed 2019-06-24

Comitter:

highfieldsnj

Date:

Mon Jun 24 08:59:14 2019 +0000

Parent:

6:3651a37efd0e

Commit message:

fix

Changed in this revision

--- 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

--- a/TFmini.h	Sat Oct 06 05:01:42 2018 +0000
+++ b/TFmini.h	Mon Jun 24 08:59:14 2019 +0000
@@ -6,34 +6,32 @@
 #define TF_SERIAL_BUFFER_SIZE 9
 #define TF_HEADER_FIRST_BYTE 0x59
 #define TF_HEADER_SECOND_BYTE 0x59
-#define TF_RECEIVE_FREQ 0.01
 #define TF_DEFAULT_BAUD 115200
 
-class TFmini : public RawSerial {
-    public :
-        TFmini(PinName serialTX, PinName serialRX);
+class TFmini : public RawSerial
+{
+public :
+    TFmini(PinName serialTX, PinName serialRX);
 
-        int getDistance();
-        int getStrength();
-
-    private :
-        void readData();
-        void assemble();
-
-        int incrementCounter(int counter);
+    int getDistance();
+    int getStrength();
 
-        int readCounter;
-        int takeCounter;
-        Ticker assembleTicker;
-        
-        int distance;
-        int strength;
+private :    
+    void receiveByte();
+    void checkData();
+    void assemble();
+    
+    int distance;
+    int strength;
+    uint8_t reserved;
+    uint8_t rawQual;
 
-        uint8_t *buffer;
-        uint8_t *data;
-
-        bool headerCheck;
-        int headerPoint;
+    uint8_t buffer[TF_SERIAL_BUFFER_SIZE];
+    uint8_t data[TF_SERIAL_BUFFER_SIZE - 3];
+    //uint8_t mask;
+    uint8_t bufferPoint;
+    uint8_t receivedBytes;
+    
 };
 
 #endif