Revision 0:68470e808ae2, committed 2018-10-12

Comitter:

tanabe2000

Date:

Fri Oct 12 04:39:28 2018 +0000

Commit message:

kinect?

Changed in this revision

diff -r 000000000000 -r 68470e808ae2 FEP.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FEP.cpp	Fri Oct 12 04:39:28 2018 +0000
@@ -0,0 +1,72 @@
+#include "FEP.h"
+
+FEP::FEP(PinName tx, PinName rx, uint8_t addr, int baud) :
+    RawSerial(tx, rx, baud)
+{
+    _address = addr;
+}
+
+void FEP::startReceive(uint16_t dataSize)
+{
+    bufferSize = dataSize + 15;
+    bufferPoint = 0;
+    receivedBytes = 0;
+    buffer = new uint8_t[bufferSize];
+    data  = new uint8_t[dataSize];
+    attach(callback(this, &FEP::receiveByte));
+}
+
+void FEP::getData(uint8_t data_[])
+{
+    for(int i = 0; i < bufferSize - 15; i++) {
+        data_[i] = data[i];
+    }
+}
+
+void FEP::sendData(uint8_t data_[], uint8_t length)
+{
+    uint8_t checksum = 0x00;
+
+    printf("@TBN%03d%03d", _address, length + 1);
+    for (int j = 0; j < length; j++) {
+        checksum += data_[j];
+        putc(data_[j]);
+    }
+    putc(checksum);
+    putc('\r');
+    putc('\n');
+}
+
+void FEP::receiveByte()
+{
+    buffer[bufferPoint % bufferSize] = getc();
+
+    if(bufferPoint != 0xff) {
+        ++bufferPoint;
+    } else {
+        bufferPoint = (255%bufferSize)+1;
+    }
+
+    ++receivedBytes;
+
+    if(receivedBytes >= bufferSize) checkData();
+}
+
+void FEP::checkData()
+{
+    for(int i = 0; i < bufferSize; i++) {
+        if(buffer[i % bufferSize] == FEP_HEADER0 && buffer[(i + 1) % bufferSize] == FEP_HEADER1 && buffer[(i + 2) % bufferSize] == FEP_HEADER2) {
+            uint8_t checksum = 0x00;
+            for(int j = 9; j < bufferSize - 6; j++) {
+                checksum += buffer[(i + j)% bufferSize];
+            }
+            if(checksum == buffer[(i + bufferSize - 6)% bufferSize]) {
+                for(int j = 9; j < bufferSize - 6; j++) {
+                    data[j - 9] = buffer[(i + j) % bufferSize];
+                }
+                receivedBytes = 0;
+                return;
+            }
+        }
+    }
+}

diff -r 000000000000 -r 68470e808ae2 FEP.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FEP.h	Fri Oct 12 04:39:28 2018 +0000
@@ -0,0 +1,34 @@
+#ifndef FEP_H
+#define FEP_H
+
+#include "mbed.h"
+
+#define FEP_BUFFER_SIZE 22
+#define FEP_HEADER0 'R'
+#define FEP_HEADER1 'B'
+#define FEP_HEADER2 'N'
+
+class FEP : public RawSerial
+{
+public :
+    FEP(PinName tx, PinName rx, uint8_t addr, int baud=115200);
+
+    void startReceive(uint16_t dataSize);
+    void getData(uint8_t data_[]);
+    void sendData(uint8_t data_[], uint8_t length);
+
+private :
+    void receiveByte();
+    void checkData();
+
+    uint8_t *buffer;
+    uint8_t bufferPoint;
+    uint8_t receivedBytes;
+
+    uint16_t bufferSize;
+    uint8_t* data;
+
+    uint8_t _address;
+};
+
+#endif
\ No newline at end of file