From 9c4cdab4a77b5f4b389ad78f55d2daa9a086b9ea Mon Sep 17 00:00:00 2001
From: Jonas Arnold <jonas.arnold@live.de>
Date: Fri, 9 Dec 2022 16:24:03 +0100
Subject: [PATCH] implemented mqtt parsing for robo commands

---
 ADIS_ESP32_Eclipse/main/challenge_com.c | 50 +++++++++++++++++++++++--
 ADIS_ESP32_Eclipse/main/robo_wrapper.c  | 12 +++---
 2 files changed, 53 insertions(+), 9 deletions(-)

diff --git a/ADIS_ESP32_Eclipse/main/challenge_com.c b/ADIS_ESP32_Eclipse/main/challenge_com.c
index 89003fc..37e6636 100644
--- a/ADIS_ESP32_Eclipse/main/challenge_com.c
+++ b/ADIS_ESP32_Eclipse/main/challenge_com.c
@@ -16,6 +16,7 @@
 #include "mqtt_client.h"
 #include "myMqtt.h"
 #include "splitflap_wrapper.h"
+#include "robo_wrapper.h"
 
 #define TAG "CHALLENGE_COM" /* tag for logging with ESP_LOG */
 
@@ -25,6 +26,7 @@ const char MQTT_TOPIC_SF_INITALL[] = "/splitFlap/cmd/init/";
 const char MQTT_TOPIC_SF_CONFIG_SETUP[] = "/splitFlap/config/setup/";
 const char MQTT_TOPIC_ROBO_MODE[] = "/mobile/cmd/mode/";
 const char MQTT_TOPIC_ROBO_NAV_TURN[] = "/mobile/cmd/nav/turn";
+const char MQTT_TOPIC_ROBO_NAV_MOVE[] = "/mobile/cmd/nav/move";
 const char MQTT_TOPIC_ROBO_NAV_STOP[] = "/mobile/cmd/nav/stop";
 
 void Challenge_Com_ParseMqtt(void *handler_args, esp_event_base_t base, int32_t event_id, void *event_data){
@@ -104,15 +106,57 @@ void Challenge_Com_ParseMqtt(void *handler_args, esp_event_base_t base, int32_t
     else{
     	// CMD: ROBO MOBILE MODE
     	if(McuUtility_strcmp(topic, MQTT_TOPIC_ROBO_MODE)==0){
-
+    		handled = true;
+			// parse json
+			bool modeAutomatic = false;
+			struct json_attr_t json_attrs[] = {
+				{"automatic",    t_boolean, .addr.boolean = &modeAutomatic},
+				{NULL},
+			};
+			int err = json_read_object(data, json_attrs, NULL);
+			if(err != 0){
+				ESP_LOGE(TAG, "Parsing JSON data of ROBO MOBILE MODE message failed. Event data was = %s", data);
+				ESP_LOGE(TAG, "Parse error was: %s", json_error_string(err));
+			} else{  // successfully parsed
+				Robo_Wrapper_SetMode(modeAutomatic);
+			}
     	}
     	// CMD: ROBO MOBILE NAV TURN
     	else if(McuUtility_strcmp(topic, MQTT_TOPIC_ROBO_NAV_TURN)==0){
-
+    		handled = true;
+    		// data pointer for xatio
+			const unsigned char *dataPtr = (unsigned char*)data;
+			// parse value
+    		int32_t angle = 0; // default value 0
+    		McuUtility_xatoi(&dataPtr, &angle);
+			if(angle == 0){
+				ESP_LOGE(TAG, "Parsing data of ROBO MOBILE NAV TURN message failed. Event data was = %s", data);
+			} else if (angle > 32767 || angle < -32768){ // check range
+				ESP_LOGE(TAG, "Data of ROBO MOBILE NAV TURN was out of range. Event data was = %s", data);
+			} else{  // successfully parsed
+				Robo_Wrapper_Nav_Turn((int16_t)angle);
+			}
     	}
+    	// CMD: ROBO MOBILE NAV MOVE
+		else if(McuUtility_strcmp(topic, MQTT_TOPIC_ROBO_NAV_MOVE)==0){
+    		handled = true;
+    		// data pointer for xatio
+			const unsigned char *dataPtr = (unsigned char*)data;
+			// parse value
+    		int32_t speed = 40000; // default value => to identify
+    		McuUtility_xatoi(&dataPtr, &speed);
+			if(speed == 40000){
+				ESP_LOGE(TAG, "Parsing data of ROBO MOBILE NAV MOVE message failed. Event data was = %s", data);
+			} else if (speed > 32767 || speed < -32768){ // check range
+				ESP_LOGE(TAG, "Data of ROBO MOBILE NAV MOVE was out of range. Event data was = %s", data);
+			} else{  // successfully parsed
+				Robo_Wrapper_Nav_Move((int16_t)speed);
+			}
+		}
     	// CMD: ROBO MOBILE NAV STOP
     	else if(McuUtility_strcmp(topic, MQTT_TOPIC_ROBO_NAV_STOP)==0){
-
+        	handled = true;
+        	Robo_Wrapper_Nav_Stop();
     	}
     }
 
diff --git a/ADIS_ESP32_Eclipse/main/robo_wrapper.c b/ADIS_ESP32_Eclipse/main/robo_wrapper.c
index 819af40..bcf228d 100644
--- a/ADIS_ESP32_Eclipse/main/robo_wrapper.c
+++ b/ADIS_ESP32_Eclipse/main/robo_wrapper.c
@@ -5,24 +5,24 @@
  *      Author: jonas
  */
 
+#include "robo_wrapper.h"
+
 /* Sets the robot to automatic / manual mode */
 bool Robo_Wrapper_SetMode(bool automatic){
-
+	return true;
 }
 
 /* Lets the robot turn to a specific angle and then proceed straight with its previous movement */
 bool Robo_Wrapper_Nav_Turn(int16_t angle){
-
+	return true;
 }
 
-
-
 /* Sets the movementspeed of the robot */
 bool Robo_Wrapper_Nav_Move(int16_t speed){
-
+	return true;
 }
 
 /* Stops the movement of the robot */
 bool Robo_Wrapper_Nav_Stop(void){
-
+	return true;
 }