You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
237 lines
9.0 KiB
237 lines
9.0 KiB
/*
|
|
* challenge_com.c
|
|
*
|
|
* Created on: 05.12.2022
|
|
* Author: jonas
|
|
*/
|
|
|
|
#include <stdbool.h>
|
|
|
|
#include "challenge_com.h"
|
|
#include "challenge_app.h"
|
|
#include "platform.h"
|
|
#include "esp_log.h"
|
|
#include "McuUtility.h"
|
|
#include "mjson.h"
|
|
#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 */
|
|
|
|
/* TOPICS */
|
|
const char MQTT_TOPIC_SF_DISPLAY[] = "/splitFlap/cmd/display/";
|
|
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/";
|
|
const char MQTT_TOPIC_ROBO_CALIB_CMD[] = "/mobile/cmd/line_sens/calib";
|
|
const char MQTT_TOPIC_ROBO_GET_CALIB[] = "/mobile/cmd/line_sens/get_calib";
|
|
const char MQTT_TOPIC_ROBO_CALIB_STATE[] = "/mobile/state/line_sens/calib_data";
|
|
const char MQTT_TOPIC_ROBO_REQ_BATTERY[] = "/mobile/cmd/battery/get_volt";
|
|
const char MQTT_TOPIC_ROBO_RESP_BATTERY[] = "/mobile/state/battery/voltage";
|
|
|
|
void Challenge_Com_ParseMqtt(void *handler_args, esp_event_base_t base, int32_t event_id, void *event_data){
|
|
/* INFO
|
|
* TOPIC: event->topic ==> do not directly use, string is not limited
|
|
* TOPIC LENGTH: event->topic_len
|
|
* DATA: event->data ==> do not directly use, string is not limited
|
|
* DATA LENGTH: event->data_len
|
|
* CLIENT: event->client
|
|
*/
|
|
|
|
esp_mqtt_event_handle_t event = event_data;
|
|
// cut strings to length
|
|
char topic[event->topic_len+1]; char data[event->data_len+1];
|
|
McuUtility_strcpy((unsigned char*)topic, sizeof(topic), (unsigned char*)event->topic);
|
|
McuUtility_strcpy((unsigned char*)data, sizeof(data), (unsigned char*)event->data);
|
|
|
|
bool handled = false;
|
|
|
|
// check if event id is MQTT_EVENT_DATA => break if not
|
|
if((esp_mqtt_event_id_t)event_id != MQTT_EVENT_DATA) {
|
|
ESP_LOGE(TAG, "Wrong data received. Can only handle MQTT_EVENT_DATA (event id %i). But received event id: %i", MQTT_EVENT_DATA, (esp_mqtt_event_id_t)event_id);
|
|
return;
|
|
}
|
|
|
|
/* check topic and call related command if allowed by robot mode */
|
|
/* stationary robot allowed commands */
|
|
if(Challenge_App_GetRobotMode() == true) {
|
|
// CMD: CONFIG SETUP
|
|
if(McuUtility_strcmp(topic, MQTT_TOPIC_SF_CONFIG_SETUP)==0){
|
|
handled = true;
|
|
// parse json
|
|
int setupId = 0;
|
|
int hwId = 0;
|
|
struct json_attr_t json_attrs[] = {
|
|
{"setupId", t_integer, .addr.integer = &setupId},
|
|
{"hardwareId", t_integer, .addr.integer = &hwId,},
|
|
{NULL},
|
|
};
|
|
int err = json_read_object(data, json_attrs, NULL);
|
|
if(err != 0){
|
|
ESP_LOGE(TAG, "Parsing JSON data of CONFIG SETUP message failed. Event data was = %s", data);
|
|
ESP_LOGE(TAG, "Parse error was: %s", json_error_string(err));
|
|
} else{ // successfully parsed
|
|
// check values
|
|
if(setupId >= 0 && setupId <= 10 && hwId >= 0 && hwId <= 50){
|
|
SplitFlap_Wrapper_SetHardwareIdentifier(setupId, hwId);
|
|
} else{
|
|
ESP_LOGE(TAG, "SetupID or HardwareID out of range. Event data was = %s", data);
|
|
}
|
|
}
|
|
}
|
|
// CMD: INIT ALL
|
|
else if(McuUtility_strcmp(topic, MQTT_TOPIC_SF_INITALL)==0){
|
|
handled = true;
|
|
SplitFlap_Wrapper_MoveAllToZeroPosition();
|
|
}
|
|
// CMD: DISPLAY
|
|
else if(McuUtility_strcmp(topic, MQTT_TOPIC_SF_DISPLAY)==0){
|
|
handled = true;
|
|
// parse json
|
|
char message[10] = "TEST";
|
|
struct json_attr_t json_attrs[] = {
|
|
{"message", t_string, .addr.string = message, .len = sizeof(message)},
|
|
{NULL},
|
|
};
|
|
int err = json_read_object(data, json_attrs, NULL);
|
|
if(err != 0){
|
|
ESP_LOGE(TAG, "Parsing JSON data of DISPLAY message failed. Event data was = %s", data);
|
|
ESP_LOGE(TAG, "Parse error was: %s", json_error_string(err));
|
|
} else{ // successfully parsed
|
|
SplitFlap_Wrapper_Display((unsigned char*)message);
|
|
}
|
|
}
|
|
}
|
|
/* mobile robot allowed commands */
|
|
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();
|
|
}
|
|
// CMD: ROBO_CALIB_CMD
|
|
else if(McuUtility_strcmp(topic, MQTT_TOPIC_ROBO_CALIB_CMD)==0){
|
|
handled = true;
|
|
// parse json
|
|
bool start = false;
|
|
struct json_attr_t json_attrs[] = {
|
|
{"start", t_boolean, .addr.boolean = &start},
|
|
{NULL},
|
|
};
|
|
int err = json_read_object(data, json_attrs, NULL);
|
|
if(err != 0){
|
|
ESP_LOGE(TAG, "Parsing JSON data of ROBO_CALIB_CMD message failed. Event data was = %s", data);
|
|
ESP_LOGE(TAG, "Parse error was: %s", json_error_string(err));
|
|
} else{ // successfully parsed
|
|
Robo_Wrapper_Calibration(start);
|
|
}
|
|
}
|
|
// CMD: ROBO_GET_CALIB
|
|
else if(McuUtility_strcmp(topic, MQTT_TOPIC_ROBO_GET_CALIB)==0){
|
|
handled = true;
|
|
LineSensorCalibData_t calibData;
|
|
Robo_Wrapper_GetCalibrationValues(&calibData);
|
|
ESP_LOGI(TAG, "Read calibration data: state=%s", calibData.state);
|
|
ESP_LOGI(TAG, "Read calibration data: values=%s", calibData.values);
|
|
|
|
// build json and respond
|
|
unsigned char payload[100] = "{\"state\": \"";
|
|
McuUtility_strcat(payload, sizeof(payload), calibData.state);
|
|
McuUtility_strcat(payload, sizeof(payload), (unsigned char*)"\", \"data\": \"");
|
|
McuUtility_strcat(payload, sizeof(payload), calibData.values);
|
|
McuUtility_strcat(payload, sizeof(payload), (unsigned char*)"\"}");
|
|
|
|
MyMqtt_Publish(MQTT_TOPIC_ROBO_CALIB_STATE, (char*)payload);
|
|
}
|
|
// CMD: ROBO_REQ_BATTERY
|
|
else if(McuUtility_strcmp(topic, MQTT_TOPIC_ROBO_REQ_BATTERY)==0){
|
|
handled = true;
|
|
unsigned char voltage[20] = "";
|
|
Robo_Wrapper_GetBatteryVoltage(voltage);
|
|
ESP_LOGI(TAG, "Read battery voltage=%s", voltage);
|
|
|
|
// build json and respond
|
|
unsigned char payload[100] = "{\"voltage\": \"";
|
|
McuUtility_strcat(payload, sizeof(payload), voltage);
|
|
McuUtility_strcat(payload, sizeof(payload), (unsigned char*)"\"}");
|
|
|
|
MyMqtt_Publish(MQTT_TOPIC_ROBO_RESP_BATTERY, (char*)payload);
|
|
}
|
|
}
|
|
|
|
/* both robot modes allowed commands */
|
|
|
|
|
|
|
|
if(handled == false){
|
|
ESP_LOGE(TAG, "Received data could not be handled. Topic was %s", topic);
|
|
}
|
|
return;
|
|
}
|
|
|
|
void Challenge_Com_SubscribeToAllTopics(void){
|
|
MyMqtt_Subscribe(MQTT_TOPIC_SF_INITALL);
|
|
MyMqtt_Subscribe(MQTT_TOPIC_SF_DISPLAY);
|
|
MyMqtt_Subscribe(MQTT_TOPIC_SF_CONFIG_SETUP);
|
|
MyMqtt_Subscribe(MQTT_TOPIC_ROBO_MODE);
|
|
MyMqtt_Subscribe(MQTT_TOPIC_ROBO_NAV_MOVE);
|
|
MyMqtt_Subscribe(MQTT_TOPIC_ROBO_NAV_TURN);
|
|
MyMqtt_Subscribe(MQTT_TOPIC_ROBO_NAV_STOP);
|
|
MyMqtt_Subscribe(MQTT_TOPIC_ROBO_CALIB_CMD);
|
|
MyMqtt_Subscribe(MQTT_TOPIC_ROBO_GET_CALIB);
|
|
MyMqtt_Subscribe(MQTT_TOPIC_ROBO_REQ_BATTERY);
|
|
}
|
|
|