Diff: R1370.cpp

Revision:

3:2f00371553c1

Parent:

2:08ff7c46665f

Child:

4:ffc2a55269f9

--- a/R1370.cpp	Fri Aug 24 05:39:24 2018 +0000
+++ b/R1370.cpp	Wed Dec 19 07:44:20 2018 +0000
@@ -3,43 +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];
-    }
-}
-
-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());
-            }
-        }
-    }
+        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()
@@ -87,4 +101,14 @@
 int16_t R1370::getAccZ()
 {
     return acc[2];
+}
+
+unsigned char R1370::upbit()
+{
+    return upbit_;
+}
+
+unsigned char R1370::downbit()
+{
+    return downbit_;
 }
\ No newline at end of file