diff --git a/ADIS_Sumo/Sumo/Remote.c b/ADIS_Sumo/Sumo/Remote.c index f2a2f72..e72ada7 100644 --- a/ADIS_Sumo/Sumo/Remote.c +++ b/ADIS_Sumo/Sumo/Remote.c @@ -13,6 +13,7 @@ #include "Buzzer.h" #include "McuESP32.h" #include "McuLog.h" +#include /*! \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; - 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()); + 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*)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); } } }