add handle udp message, not tested yet

ADIS_ESP32_Eclipse/main/udp_server.c

@@ -12,6 +12,7 @@
 #include "udp_server.h"
 #include <string.h>
 #include <sys/param.h>
+#include <stdlib.h>
 #include "freertos/FreeRTOS.h"
 #include "freertos/task.h"
 #include "esp_system.h"
@@ -31,6 +32,7 @@
 #include "McuLog.h"
 
 #define CONFIG_EXAMPLE_IPV4
+#define TAG "UDP_SERVER"
 
 static TaskHandle_t taskHandle = NULL; /* udp server task handle */
 
@@ -38,6 +40,41 @@ static int SendToSocket(int sock, const char *msg, const struct sockaddr *to, so
   return sendto(sock, msg, McuUtility_strlen((char*)msg), 0, to, tolen);
 }
 
+void HandleIncomingUdpMessage(const char *rxMsg, int sock, struct sockaddr *source_addr_p, socklen_t source_addr_len) {
+	unsigned char response[128];
+	int err;
+
+	ESP_LOGI(TAG, "handling incoming message %s", rxMsg);
+	McuUtility_strcpy(response, sizeof(response), (unsigned char*)"OK"); /* default response */
+	/* check framing */
+	if (McuUtility_strncmp(rxMsg, "@esp:", sizeof("@esp:")-1)==0) { /* check prefix */
+		size_t strLen = McuUtility_strlen(rxMsg);
+		if (rxMsg[strLen-1]=='!') {
+			/* send to ESP32 shell */
+			rxMsg += sizeof("@esp:")-1;
+			unsigned char *cmd = (unsigned char*)calloc(strLen, sizeof(unsigned char));
+			if(cmd != NULL){
+				size_t cmdLen = strLen-(sizeof("@esp:")-1)-1;
+				McuUtility_strcpy(cmd, cmdLen, (const unsigned char *)rxMsg);
+				SHELL_SendToESPAndGetResponse((unsigned char*)cmd, response, sizeof(response));
+			}else{
+				McuUtility_strcpy(response, sizeof(response), (unsigned char*)"Failed to parse cmd!");
+			}
+
+		} else {
+			McuUtility_strcpy(response, sizeof(response), (unsigned char*)"!missing!");
+		}
+	}
+	/* send back response */
+	ESP_LOGI(TAG, "Sending back response");
+	err = SendToSocket(sock, (const char*)response, source_addr_p, source_addr_len);
+	if (err < 0) {
+		ESP_LOGE(TAG, "Error occurred during sending: errno %d", errno);
+	}
+}
+
+
+
 static void udp_server_task(void *pvParameters) {
   char rx_buffer[128];
   char addr_str[128];
@@ -100,20 +137,20 @@ static void udp_server_task(void *pvParameters) {
         McuLog_info("Received %d bytes from %s:\n%s", len, addr_str, rx_buffer);
 
         /* \TODO Need to handle messages and send them to the robot */
-
+        HandleIncomingUdpMessage(rx_buffer, sock, (struct sockaddr *)&source_addr, socklen);
         /* send back response */
-        unsigned char test_response[128];
-        int err;
-
-        McuLog_info("Sending back response");
-        McuUtility_strcpy(test_response, sizeof(test_response), (unsigned char*)"OK"); /* default response */
-        if (McuUtility_strncmp(rx_buffer, "test", sizeof("test")-1)==0) { /* hard-coded command */
-          McuUtility_strcpy(test_response, sizeof(test_response), (unsigned char*)"test_response");
-        }
-        err = SendToSocket(sock, (const char*)test_response, (struct sockaddr *)&source_addr, sizeof(source_addr));
-        if (err < 0) {
-          McuLog_error("Error occurred during sending: errno %d", errno);
-        }
+//        unsigned char test_response[128];
+//        int err;
+//
+//        McuLog_info("Sending back response");
+//        McuUtility_strcpy(test_response, sizeof(test_response), (unsigned char*)"OK"); /* default response */
+//        if (McuUtility_strncmp(rx_buffer, "test", sizeof("test")-1)==0) { /* hard-coded command */
+//          McuUtility_strcpy(test_response, sizeof(test_response), (unsigned char*)"test_response");
+//        }
+//        err = SendToSocket(sock, (const char*)test_response, (struct sockaddr *)&source_addr, sizeof(source_addr));
+//        if (err < 0) {
+//          McuLog_error("Error occurred during sending: errno %d", errno);
+//        }
       } /* if */
     } /* while */
     if (sock != -1) {