Revision 4:ee51008e03e2, committed 2018-12-18

Comitter:

UCHITAKE

Date:

Tue Dec 18 18:51:58 2018 +0900

Parent:

1:ec87437c2029

Commit message:

ring buffering

Changed in this revision

diff -r ec87437c2029 -r ee51008e03e2 R1370.cpp
--- a/R1370.cpp	Mon Aug 27 04:45:30 2018 +0000
+++ b/R1370.cpp	Tue Dec 18 18:51:58 2018 +0900
@@ -3,46 +3,57 @@
 R1370::R1370(PinName tx, PinName rx) : RawSerial(tx, rx, 115200)
 {
     attach(callback(this, &R1370::receiveByte));
-    thread.start(callback(this, &R1370::assembleLoop));
 }
 
 void R1370::receiveByte()
 {
-    buf.push_back(getc());
+    buffer[bufferPoint % R1370_BUFFER_SIZE] = getc();
+ 
+    if(bufferPoint != 0xff) {
+        ++bufferPoint;
+    } else {
+        bufferPoint = (255%R1370_BUFFER_SIZE)+1;
+    }
+ 
+    ++receivedBytes;
+ 
+    if(receivedBytes >= R1370_BUFFER_SIZE) checkData();
+}
+
+void R1370::checkData()
+{
+    for(int i = 0; i < R1370_BUFFER_SIZE; i++) {
+        if(buffer[i % R1370_BUFFER_SIZE] == R1370_HEADER0 && buffer[(i + 1) % R1370_BUFFER_SIZE] == R1370_HEADER1) {
+
+            uint8_t checksum = 0x00;
+            for(int j = 0; j < R1370_BUFFER_SIZE - 3; j++) {
+                checksum += buffer[(i + 2 + j)% R1370_BUFFER_SIZE];
+
+            }
+            if(checksum == buffer[(i + R1370_BUFFER_SIZE - 1)% R1370_BUFFER_SIZE]) {
+                for(int j = 0; j < R1370_BUFFER_SIZE - 3; j++) {
+                    data[j] = buffer[(i + 2 + j) % R1370_BUFFER_SIZE];
+                }
+                receivedBytes = 0;
+
+                assemble();
+                return;
+            }
+        }
+    }
 }
 
 void R1370::assemble()
 {
-    char checksum = 0;
-    for(int i = 2; i <= 13; i++) {
-        checksum += buf[i];
-    }
-    if(checksum == buf[14]) {
-        index = buf[2];
-        angle = (buf[3] & 0xFF) | ((buf[4] << 8) & 0xFF00);
-        rate = (buf[5] & 0xFF) | ((buf[6] << 8) & 0xFF00);
-        acc[0] = (buf[7] & 0xFF) | ((buf[8] << 8) & 0xFF00);
-        acc[1] = (buf[9] & 0xFF) | ((buf[10] << 8) & 0xFF00);
-        acc[2] = (buf[11] & 0xFF) | ((buf[12] << 8) & 0xFF00);
-        reserved = buf[13];
-        upbit_ = buf[3];
-        downbit_ = buf[4];
-    }
-}
-
-void R1370::assembleLoop()
-{
-    while(true) {
-        if(buf.size() > R1370_BUFFER_SIZE) {
-            if(buf[0] == R1370_HEADER0 && buf[1] == R1370_HEADER1) {
-                assemble();
-                buf.erase(buf.begin(), buf.begin() + (R1370_BUFFER_SIZE - 1));
-            } else {
-                buf.erase(buf.begin());
-            }
-            assemble();
-        }
-    }
+        index = data[0];
+        angle = (data[1] & 0xFF) | ((data[2] << 8) & 0xFF00);
+        rate = (data[3] & 0xFF) | ((data[4] << 8) & 0xFF00);
+        acc[0] = (data[5] & 0xFF) | ((data[6] << 8) & 0xFF00);
+        acc[1] = (data[7] & 0xFF) | ((data[8] << 8) & 0xFF00);
+        acc[2] = (data[9] & 0xFF) | ((data[10] << 8) & 0xFF00);
+        reserved = data[11];
+        upbit_ = data[1];
+        downbit_ = data[2];
 }
 
 float R1370::getAngle()
@@ -100,4 +111,4 @@
 unsigned char R1370::downbit()
 {
     return downbit_;
-}
\ No newline at end of file
+}

diff -r ec87437c2029 -r ee51008e03e2 R1370.h
--- a/R1370.h	Mon Aug 27 04:45:30 2018 +0000
+++ b/R1370.h	Tue Dec 18 18:51:58 2018 +0900
@@ -27,19 +27,21 @@
     
 private :
     void receiveByte();
+    void checkData();
     void assembleLoop();
     void assemble();
     
-    Thread thread;
-    std::vector<unsigned char> buf;
-    
     uint8_t index;
     int16_t angle;
     int16_t rate;
     int16_t acc[3];
     uint8_t reserved;
+    uint8_t buffer[R1370_BUFFER_SIZE];
+    uint8_t data[R1370_BUFFER_SIZE-3];
+    uint8_t bufferPoint;
+    uint8_t receivedBytes;
     
     unsigned char upbit_, downbit_;
 };
 
-#endif
\ No newline at end of file
+#endif