re-added changes in Remote.c

ADIS_Sumo/Sumo/Remote.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2022, Erich Styger
+ * Copyright (c) 2019-2021, Erich Styger
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -12,8 +12,12 @@
 #include "Shell.h"
 #include "Buzzer.h"
 #include "McuESP32.h"
+#include "McuLog.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 "
+#define ESP_MSG_POSTFIX "!\0"
+
 
 static QueueHandle_t REMOTE_RxFromESP_Queue;
 
@@ -30,7 +34,9 @@ void REMOTE_GatewayRxFromESP(unsigned char ch) {
 static void RemoteTask(void *pv) {
   unsigned char ch;
   BaseType_t res;
-
+  unsigned char espMsg[McuShell_DEFAULT_SHELL_BUFFER_SIZE] = "";
+  uint8_t saveChar = 0;
+  uint8_t writeIndex = 0;
   (void)pv; /* not used */
 #if 0 /* example making a beep */
   BUZ_Beep(200, 500);
@@ -52,20 +58,52 @@ static void RemoteTask(void *pv) {
   }
 #endif
   for(;;) {
-    res = xQueueReceive(REMOTE_RxFromESP_Queue, &ch, portMAX_DELAY); /* make sure we empty the queue */
+    res = xQueueReceive(REMOTE_RxFromESP_Queue, &ch, pdMS_TO_TICKS(10)); /* wait max 10 ms */
-    /*! \todo ADIS: implement handling remote stream from ESP **/
+    if (res==errQUEUE_EMPTY) {
+	    ch = '\0'; /* nothing received */
+
+    }
+    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(ch == '!' && saveChar == 1){
+        	saveChar = 0;
+        	McuShell_SendStr(espMsg, McuShell_GetStdio()->stdOut);
+        	if(McuUtility_strcmp((char*)espMsg, ESP_MSG_PREFIX)==0){ /* check prefix */
+        		unsigned char *p;
+          		p = (unsigned char*)espMsg + 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());
+        	}
+        }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);
+        }
+    }
   }
 }
 
 uint8_t REMOTE_ParseCommand(const unsigned char *cmd, bool *handled, const McuShell_StdIOType *io) {
-  (void)cmd;
-  (void)handled;
-  (void)io;
   return ERR_OK;
 }
 
 void REMOTE_Deinit(void) {
-  /* nothing needed */
 }
 
 void REMOTE_Init(void) {