Diff: FEP.cpp

Revision:

0:b0bd3bee5ab4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FEP.cpp	Fri Oct 08 07:23:21 2021 +0000
@@ -0,0 +1,86 @@
+#include "FEP.h"
+
+FEP::FEP(PinName tx, PinName rx, uint8_t addr, int baud) :
+    RawSerial(tx, rx, baud),
+    timeout(TIMEOUT_COUNT),
+    status(false)
+{
+    _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));
+    timeoutTicker.attach(callback(this, &FEP::timeoutLoop), 0.1);
+}
+
+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();
+    timeout = 0;
+
+    if(bufferPoint != 0xff) {
+        ++bufferPoint;
+    } else {
+        bufferPoint = (255%bufferSize)+1;
+    }
+
+    ++receivedBytes;
+
+    if(receivedBytes >= bufferSize) checkData();
+}
+
+void FEP::timeoutLoop()
+{
+    if(timeout >= TIMEOUT_COUNT) {
+        status = false;
+        return;
+    }
+    status = true;
+    timeout++;   
+}
+
+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;
+            }
+        }
+    }
+}
\ No newline at end of file