Diff: FEP.cpp

Revision:

3:12dcc46fb9dc

Parent:

2:aa9a344a42a8

Child:

4:8d754f144b96

--- a/FEP.cpp	Sat Oct 09 13:35:40 2021 +0000
+++ b/FEP.cpp	Mon Oct 11 13:22:16 2021 +0000
@@ -1,7 +1,7 @@
 #include "FEP.h"
 
-myFEP::myFEP(PinName tx, PinName rx, uint8_t addr_, int baud=115200) :
-    RawSerial(tx, rx)
+myFEP::myFEP(PinName tx, PinName rx, uint8_t addr_, int baud) :
+    RawSerial(tx, rx, baud)
 {
     addr     = addr_;
     bufindex = 0;
@@ -16,20 +16,20 @@
 
 void myFEP::ReceiveBytes()
 {
-    if (bufindex > 256) bufindex %= 256;
     buffer[bufindex] = getc(); // get data
 
-    if ( (bufindex > 0) && (!strcmp((char*)(buffer + bufindex - 1) , "\r\n")) ) { // <CR><LF>
+    if ((!strncmp((char*)(buffer + ((256 + bufindex - 1)%256) ), "\r\n", 2)) ) { // <CR><LF>
         CheckData(); // check message
     }
+    bufindex++;
 }
 
-void CheckData()
+void myFEP::CheckData()
 {
     uint8_t temp=0, length=0; // temp:where's array   length:length of array
     for (uint16_t i=0; i<256; i++) {
         temp = (256 + bufindex - i) % 256;
-        if ( !str((char*)(buffer + temp) , "RBN") ) { // check header
+        if ( !strncmp((char*)(buffer + temp) , "RBN", 3) ) { // check header
             length = ctoi(buffer[temp+6])*100 + ctoi(buffer[temp+7])*10 + ctoi(buffer[temp+8]); // calculate length of message
             for (int j = 9; j < length+9; j++) {
                 retdata[j-9] = buffer[temp+j]; // 
@@ -38,46 +38,44 @@
     }
 }
 
-void GetData(uint8_t *data)
+void myFEP::GetData(uint8_t *data)
 {
     strcpy((char*)data, (char*)retdata);
 }
 
-
-uint8_t SendData(uint8_t *data)
+int8_t myFEP::SendData(uint8_t *data)
 {
     return SendData(data, sizeof(data));
 }
 
-uint8_t SendData(uint8_t *data, uint8_t length)
+int8_t myFEP::SendData(uint8_t *data, uint8_t length)
 {
     if(length > 128) return 1;
     uint8_t sendindex; // index of 'data'
     printf("@TBN%03d%03d", addr, length); // send comand
-    for (sendindex=0; sendindex<length; sendindex++) { 
+    for (sendindex=0; sendindex<length; sendindex++) {
         putc(data[sendindex]); // send message
     }
     printf("\r\n"); // <cr><lf>
     return GetResponse(); // check response
 }
 
-uint8_t GetResponse()
+int8_t myFEP::GetResponse()
 {
-    char res[4]; // array of storing response
-    uint8_t resindex=0; // 
-    while (1) {
+    char res[256]; // array of storing response
+    for (uint8_t resindex=0; resindex<256; resindex++) {
         res[resindex] = getc();
-        if (resindex >= 3) {
-            if (!strcmp(temp, "P0\r\n"))  return 0;
-            else if (!strcmp(temp, "N0\r\n")) return 2;
-            else if (!strcmp(temp, "N1\r\n")) return 3;
-            else if (!strcmp(temp, "N3\r\n")) return 4;
+        if (!strncmp(res+resindex-1, "\r\n", 2)) {
+            if (!strncmp(res+resindex-3, "P0", 2))  return 0;
+            else if (!strncmp(res+resindex-3, "N0", 2)) return 2;
+            else if (!strncmp(res+resindex-3, "N1", 2)) return 3;
+            else if (!strncmp(res+resindex-3, "N3", 2)) return 4;
         }
-        if (++resindex > 3) return -1;
     }
+    return -1;
 }
 
-uint8_t FEP::ctoi(char c)
+int8_t myFEP::ctoi(char c)
 {
     if('0' <= c && c <= '9') return (c-'0');
     else return -1;