Diff: main.cpp

Branch:

RF24_library_test_tx

Revision:

18:2342aa54dc9b

Parent:

17:6ae73e8943ce

--- a/main.cpp	Sat Dec 01 17:40:15 2018 +0000
+++ b/main.cpp	Sat Dec 01 18:19:22 2018 +0000
@@ -15,24 +15,33 @@
 
 #define DATA_PROTOCOL_BEGIN '$'
 #define DATA_PROTOCOL_TOKEN ','
+#define DATA_PROTOCOL_TOKEN_ ","
 #define DATA_PROTOCOL_END   '*'
+#define DATA_PROTOCOL_END_  "*"
 
 nRF24L01P nrf(spi_MOSI, spi_MISO, spi_SCK, nrf_CSN, nrf_CE, spi_IRQ);    // mosi, miso, sck, csn, ce, irq
 
 Serial pc(USBTX, USBRX);
 PwmOut buzzer(D5);
 
+typedef struct {
+    int bot_id;
+    int cmd_id;
+    int lspeed;
+    int rspeed;
+} payload_t;
+
 void beepStart();
 void endBeep();
 void initNRF();
 void dumpRFInfo();
-void getPayload(int id, unsigned int count, int lspeed, int rspeed, char* txData);
+
 int readSerialUntil(char *buffer, unsigned int buf_len, char terminator);
 char* inspectData(char *buffer, int buf_len);
+payload_t* getPayload(char* raw_data);
 
 int main()
 {
-    int inByteCnt = 0;
     char buffer[BUFFER_SIZE];
     beepStart();
     pc.baud(115200);
@@ -47,8 +56,11 @@
         if(result>0) {
             char *data = inspectData(buffer, BUFFER_SIZE);
             if(*data == DATA_PROTOCOL_BEGIN) {
-                printf("Data: %s\r\n", data);
+                printf("%s\r\n", data);
+                payload_t *payload = getPayload(data);
+                printf("BotID:%d, CmdID:%d, LSpeed:%d, RSpeed:%d\r\n", payload->bot_id, payload->cmd_id, payload->lspeed, payload->rspeed);
                 free(data);
+                free(payload);
             }
         }
         /*
@@ -70,6 +82,19 @@
     }
 }
 
+payload_t* getPayload(char *data)
+{
+    int n = strlen(data);
+    char _data[n+1];
+    strcpy(_data, data);
+    payload_t *p = (payload_t*)malloc(sizeof(payload_t));
+    p->bot_id = atoi(strtok(data+1, DATA_PROTOCOL_TOKEN_));
+    p->cmd_id = atoi(strtok(NULL, DATA_PROTOCOL_TOKEN_));
+    p->lspeed = atoi(strtok(NULL, DATA_PROTOCOL_TOKEN_));
+    p->rspeed = atoi(strtok(NULL, DATA_PROTOCOL_END_));
+    return p;
+}
+
 char* inspectData(char *buffer, int buf_len)
 {
     int start, end;
@@ -82,11 +107,19 @@
         if(i==buf_len) return 0;
     }
     int data_len = end-start+1;
-
     char *str = (char*)malloc(sizeof(char)*(data_len+1));
     memset(str, NULL, data_len+1);
     for(int i=0; i<data_len; i++) {
         *(str+i) = buffer[start+i];
+        
+        if(buffer[start+i]==DATA_PROTOCOL_BEGIN || buffer[start+i]==DATA_PROTOCOL_TOKEN || buffer[start+i]==DATA_PROTOCOL_END
+            || (buffer[start+i]>='0'&&buffer[start+i]<='9') || buffer[start+i]=='+' || buffer[start+i]=='-') {
+            continue;
+        } else {
+            //pc.putc(buffer[start+i]);
+            free(str);
+            return 0;
+        }
     }
     return str;
 }