Diff: R1370.cpp

Revision:

0:243364135087

Child:

1:ec87437c2029

Child:

3:08ff7c46665f

diff -r 000000000000 -r 243364135087 R1370.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/R1370.cpp	Fri Aug 24 04:38:13 2018 +0000
@@ -0,0 +1,91 @@
+#include "R1370.h"
+
+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());
+}
+
+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());
+            }
+            assemble();
+        }
+    }
+}
+
+float R1370::getAngle()
+{
+    return (float)(angle / 100.0);
+}
+
+float R1370::getRate()
+{
+    return (float)(rate / 100.0);
+}
+
+int16_t R1370::getAcc(char l)
+{
+    if(l == 'x' || l == 'X') {
+        return acc[0];
+    } else if(l == 'y' || l == 'Y') {
+        return acc[1];
+    } else if(l == 'z' || l == 'Z') {
+        return acc[2];
+    } else {
+        return 0;
+    }
+}
+
+int16_t R1370::getAcc(int i)
+{
+    if(i >= 0 && i < 3) {
+        return acc[i];
+    } else {
+        return 0;
+    }
+}
+
+int16_t R1370::getAccX()
+{
+    return acc[0];
+}
+
+int16_t R1370::getAccY()
+{
+    return acc[1];
+}
+
+int16_t R1370::getAccZ()
+{
+    return acc[2];
+}
\ No newline at end of file