Diff: OmniPosition.cpp

Revision:

1:0229fc98a26f

Parent:

0:58910ef3f2b0

Child:

2:edd33d3ad0fd

--- a/OmniPosition.cpp	Fri Jun 29 07:33:01 2018 +0000
+++ b/OmniPosition.cpp	Sat Jun 30 14:23:07 2018 +0900
@@ -1,5 +1,42 @@
 #include "OmniPosition.h"
 
-OmniPosition::OmniPosition()
+OmniPosition::OmniPosition(PinName serialTX, PinName serialRX) :
+    RawSerial(serialTX, serialRX, DEFAULT_BAUD),
+    readCounter(SERIAL_BUFFER_SIZE, 0),
+    takeCounter(SERIAL_BUFFER_SIZE, 0)
+{
+    buffer = new char[SERIAL_BUFFER_SIZE];
+    attach(callback(this, &OmniPosition::readData));
+    ticker.attach(callback(this, &OmniPosition::assemble), RECEIVE_FREQ);
+}
+
+void OmniPosition::readData()
+{
+    buffer[(int)readCounter] = getc();
+    ++readCounter;
+}
+
+void OmniPosition::assemble()
 {
-}
\ No newline at end of file
+    //Find header
+    headerCheck = false;
+    headerPoint = 0xff;
+
+    for(int i = 0; i < SERIAL_BUFFER_SIZE; i++) {
+        if(buffer[i] == HEADER_FIRST_BYTE) {
+            takeCounter = i;
+            ++takeCounter;
+            if(buffer[(int)takeCounter] == HEADER_SECOND_BYTE) {
+                headerCheck = true;
+                headerPoint = i;
+            }
+        }
+    }
+    if(headerPoint == 0xff) {
+        return;
+    }
+
+    //assemble
+    takeCounter = headerPoint;  //firstheader
+    ++takeCounter;  //secondheader
+}