fix cmd detection from esp32

ADIS_Sumo/Sumo/Remote.c

@@ -13,6 +13,7 @@
 #include "Buzzer.h"
 #include "McuESP32.h"
 #include "McuLog.h"
+#include <stdlib.h>
 
 /*! \todo ADIS: This is a (mostly empty) template where the remote commands from the ESP32 could be handled. This module should be extended to send data back to the ESP32 */
 #define ESP_MSG_PREFIX "@robot:cmd "
@@ -37,6 +38,7 @@ static void RemoteTask(void *pv) {
   unsigned char espMsg[McuShell_DEFAULT_SHELL_BUFFER_SIZE] = "";
   uint8_t saveChar = 0;
   uint8_t writeIndex = 0;
+  uint8_t readIndex = 0;
   (void)pv; /* not used */
 #if 0 /* example making a beep */
   BUZ_Beep(200, 500);
@@ -66,34 +68,40 @@ static void RemoteTask(void *pv) {
     else if(res == pdPASS) {
     	/* something was received */
     	/* handle remote stream from ESP */
-//    	uint8_t mCh = (uint8_t)ch;
-//    	uint8_t mCh[10] = "Char: ";
-//    	unsigned char newLine = '\n';
-//    	McuUtility_strcat(mCh, 10, &ch);
-//    	McuUtility_strcat(mCh, 10, &newLine);
-//    	McuShell_SendStr(mCh, McuShell_GetStdio()->stdOut);
         if(ch == '@'){
         	saveChar = 1;
-        	writeIndex = 0;
-        	BUZ_Beep(200, 500);
         }
         if(saveChar == 1){
         	espMsg[writeIndex++] = ch;
+        	if(writeIndex >= McuShell_DEFAULT_SHELL_BUFFER_SIZE){
+        		writeIndex = 0;
+        	}
         }
         if(ch == '!' && saveChar == 1){
         	saveChar = 0;
-        	McuShell_SendStr(espMsg, McuShell_GetStdio()->stdOut);
-        	if(McuUtility_strcmp((char*)espMsg, ESP_MSG_PREFIX)==0){ /* check prefix */
+        	unsigned char str[McuShell_DEFAULT_SHELL_BUFFER_SIZE] = {'\0'};
+        	if(str != NULL){
+        		uint8_t i;
+        		for(i = 0; i < McuShell_DEFAULT_SHELL_BUFFER_SIZE; i++){
+        			str[i] = espMsg[readIndex++];
+        			if(readIndex >= McuShell_DEFAULT_SHELL_BUFFER_SIZE){
+        				readIndex = 0;
+        			}
+        			if(str[i] == '!'){
+        				break;
+        			}
+        		}
+        	}
+        	McuShell_SendStr((const uint8_t *)str, McuShell_GetStdio()->stdOut);
+        	if(McuUtility_strncmp((char *)str,"@robot:cmd ",sizeof("@robot:cmd ")-1) == 0){
         		unsigned char *p;
-          		p = (unsigned char*)espMsg + sizeof(ESP_MSG_PREFIX) - 1;
+        		p = (unsigned char*)str + sizeof(ESP_MSG_PREFIX) - 1;
 				McuUtility_strCutTail((uint8_t*)p, (uint8_t*)ESP_MSG_POSTFIX);
 				McuShell_SendStr((uint8_t*)p, McuShell_GetStdio()->stdOut);
-          		SHELL_ParseCommandWithIO((unsigned char*)espMsg, McuESP32_GetTxToESPStdio());
+				SHELL_ParseCommandWithIO((unsigned char*)p, McuESP32_GetTxToESPStdio());
+				free(p);
         	}
-        }else if(writeIndex == McuShell_DEFAULT_SHELL_BUFFER_SIZE && saveChar == 1){
-        	saveChar = 0;
-        	McuShell_SendStr(espMsg, McuShell_GetStdio()->stdOut);
-        	McuShell_SendStr((uint8_t*)" Error in cmd.\r\n", McuShell_GetStdio()->stdOut);
+        	free(str);
         }
     }
   }