From ddba2120e110a1fac57b4bb2b8f5a0d1f198a49c Mon Sep 17 00:00:00 2001 From: FReenen Date: Thu, 15 Aug 2024 17:43:48 +0200 Subject: [PATCH] tx works --- rx_esp32/src/adc.c | 6 +- rx_esp32/src/adc.h | 2 +- rx_esp32/src/main.c | 132 +++++++++++++++++++++++++++++--------------- rx_esp32/src/ws.c | 53 +++++++++++++++++- rx_esp32/src/ws.h | 2 + 5 files changed, 146 insertions(+), 49 deletions(-) diff --git a/rx_esp32/src/adc.c b/rx_esp32/src/adc.c index 55abfe8..38efb23 100644 --- a/rx_esp32/src/adc.c +++ b/rx_esp32/src/adc.c @@ -70,8 +70,10 @@ bool adc_read(ADC_Data_t* out_data) ); if (ret == ESP_OK) { - for (int i=0; i<6; i++) { - out_data->value[i] = data[i].type2.data; + for (uint8_t i=0; i<3; i++) { + for (uint8_t ii=0; ii<2; ii++) { + out_data->value[i][ii] = data[i*2 + ii].type2.data; + } } } return (ret == ESP_OK); diff --git a/rx_esp32/src/adc.h b/rx_esp32/src/adc.h index 5bf3216..5087c40 100644 --- a/rx_esp32/src/adc.h +++ b/rx_esp32/src/adc.h @@ -5,7 +5,7 @@ #include typedef struct { - uint16_t value[6]; + uint16_t value[3][2]; } ADC_Data_t; void adc_init(); diff --git a/rx_esp32/src/main.c b/rx_esp32/src/main.c index cd57db0..14515ce 100644 --- a/rx_esp32/src/main.c +++ b/rx_esp32/src/main.c @@ -19,7 +19,7 @@ #include "ws.h" #if TARGET == TARGET_TX #include "led.h" -// #include "adc.h" +#include "adc.h" #endif bool volatile running = true; @@ -116,9 +116,8 @@ void app_main() { servo_init(); wifiInit(); -#if TARGET == TARGET_TX - // adc_init(); -#endif + bool adc_inited = false; + uint8_t adc_counter = 0; running = true; @@ -189,49 +188,94 @@ void app_main() { #if TARGET == TARGET_TX case STATE_WIFI_CONNECTED: - switch (WSStates[WS_conn_proc]) + if (!adc_inited) { - case STATE_WS_CONNECTING: - ws_client[WS_conn_proc] = ws_connect(WS_URL, BoatId + WS_conn_proc, WS_DEV_CODE_CLIENT); - WSStates[WS_conn_proc] = STATE_WS_WAIT_CONNECTION; - LOG_I("main: websocket %d: connecting", WS_conn_proc); - break; - case STATE_WS_WAIT_CONNECTION: - if (ws_client[WS_conn_proc]->connected) - { - cli_ws_client[WS_conn_proc] = CLI_init((CLI_charOutFn)&charOut_ws[WS_conn_proc], cmdList); - cli_ws_client[WS_conn_proc].echo = false; - MainState = STATE_IDEL; - LOG_I("main: websocket %d: connected", WS_conn_proc); - } - break; - - case STATE_IDEL: - case STATE_DRIVING: - if (!ws_client[WS_conn_proc]->connected) - { - CLI_deinit(&cli_ws_client[WS_conn_proc]); - MainState = STATE_WS_WAIT_CONNECTION; - } - - while ((charIn = ws_getchar(ws_client[WS_conn_proc])) != 255) - { - CLI_charIn(&cli_ws_client[WS_conn_proc], charIn); - } - ws_sendData(ws_client[WS_conn_proc]); - break; + adc_init(); + adc_inited = true; + } + ADC_Data_t adc_data; + bool adc_data_valid = false; + if (adc_read(&adc_data)) + { + adc_counter++; + if (adc_counter >= 20) + { + // LOG_D("ADC_DATA: %d, %d, %d, %d, %d, %d", adc_data.value[0], adc_data.value[1], adc_data.value[2], adc_data.value[3], adc_data.value[4], adc_data.value[5]); + adc_data_valid = true; + adc_counter = 0; + } } - // WS_conn_proc++; - // if (WS_conn_proc >= 3) - // { - // WS_conn_proc = 0; - // } - // ADC_Data_t data; - // if (adc_read(&data)) - // { - // LOG_D("ADC_DATA: %d, %d, %d, %d, %d, %d", data.value[0], data.value[1], data.value[2], data.value[3], data.value[4], data.value[5]); - // } + for (WS_conn_proc = 0; WS_conn_proc <3; WS_conn_proc++) + { + switch (WSStates[WS_conn_proc]) + { + case STATE_WS_CONNECTING: + LOG_I("main: websocket %d: connecting", WS_conn_proc); + ws_client[WS_conn_proc] = ws_connect(WS_URL, BoatId + WS_conn_proc, WS_DEV_CODE_CLIENT); + WSStates[WS_conn_proc] = STATE_WS_WAIT_CONNECTION; + break; + case STATE_WS_WAIT_CONNECTION: + if (ws_client[WS_conn_proc]->connected) + { + LOG_I("main: websocket %d: connected", WS_conn_proc); + WSStates[WS_conn_proc] = STATE_IDEL; + } + break; + + case STATE_IDEL: + if (adc_data_valid) + { + char cmd[100]; + snprintf(&cmd[0], 100, "%04u;ctrl;%04u", + ws_client[WS_conn_proc]->id, + ws_client[WS_conn_proc]->id + ); + ws_sendString(ws_client[WS_conn_proc], &cmd[0]); + } + case STATE_DRIVING: + if (!ws_client[WS_conn_proc]->connected) + { + WSStates[WS_conn_proc] = STATE_WS_WAIT_CONNECTION; + } + + char ws_resp[10]; + uint16_t ws_resp_len; + while ((ws_resp_len = ws_getstr(ws_client[WS_conn_proc], 10, &ws_resp[0])) > 0) + { + LOG_D("main: websocket %d: resv: %.*s", WS_conn_proc, ws_resp_len, ws_resp); + if (ws_resp_len == 4) + { + ws_resp[4] = 0; + if (strcmp(&ws_resp[0], "FAIL") == 0) + { + WSStates[WS_conn_proc] = STATE_IDEL; + } + } + else if (ws_resp_len == 2) + { + ws_resp[2] = 0; + if (strcmp(&ws_resp[0], "OK") == 0) + { + WSStates[WS_conn_proc] = STATE_DRIVING; + } + } + } + + if ((WSStates[WS_conn_proc] == STATE_DRIVING) && adc_data_valid) + { + char ctrl_cmd[100]; + snprintf(&ctrl_cmd[0], 100, "%04u;d;%u,%u", + ws_client[WS_conn_proc]->id, + adc_data.value[WS_conn_proc][0] >> 4, + adc_data.value[WS_conn_proc][1] >> 4 + ); + ws_sendString(ws_client[WS_conn_proc], &ctrl_cmd[0]); + } + + break; + } + } break; #elif TARGET == TARGET_RX case STATE_WS_CONNECTING: diff --git a/rx_esp32/src/ws.c b/rx_esp32/src/ws.c index 367b02b..99c3dc6 100644 --- a/rx_esp32/src/ws.c +++ b/rx_esp32/src/ws.c @@ -16,12 +16,12 @@ static void ws_event_handler(void* handler_args, esp_event_base_t base, int32_t { case WEBSOCKET_EVENT_CONNECTED: LOG_I("ws_event_handler: connected"); - client->connected = true; char str[100]; snprintf(&str[0], 100, "%04u;%04u;" BOAT_NAME "\n", client->id, client->dev_code); esp_websocket_client_send_text(client->handle, &str[0], strlen(str), 1000); + client->connected = true; break; case WEBSOCKET_EVENT_DISCONNECTED: LOG_W("ws_event_handler: disconnected"); @@ -105,6 +105,9 @@ static void ws_event_handler(void* handler_args, esp_event_base_t base, int32_t LOG_E("ws_event_handler: captured as transport's socket errno %d", data->error_handle.esp_transport_sock_errno); } break; + case WEBSOCKET_EVENT_BEFORE_CONNECT: + LOG_D("ws_event_handler: event before connect fired"); + break; default: LOG_D("ws_event_handler: a unkown event happened: %d", event_id); break; @@ -138,7 +141,7 @@ int ws_getchar(ws_client_t client) if (client->rxBuffer_rp != client->rxBuffer_wp) { out = client->rxBuffer[client->rxBuffer_rp]; - client->rxBuffer_rp++; + client->rxBuffer_rp++; if (client->rxBuffer_rp >= WS_RX_BUFFER_LEN) { client->rxBuffer_rp = 0; @@ -147,6 +150,47 @@ int ws_getchar(ws_client_t client) return out; } +int ws_getstr(ws_client_t client, uint16_t max_size, char *str) +{ + uint16_t out = 0; + if (client->rxBuffer_rp != client->rxBuffer_wp) + { + out = WS_RX_BUFFER_LEN + client->rxBuffer_wp - client->rxBuffer_rp; + if (out > WS_RX_BUFFER_LEN) + { + out -= WS_RX_BUFFER_LEN; + } + if (out > max_size) + { + out = max_size; + } + + if (client->rxBuffer_rp < client->rxBuffer_wp) + { + memcpy(str, &client->rxBuffer[client->rxBuffer_rp], out); + } + else + { + uint16_t sizeUntilEnd = WS_RX_BUFFER_LEN - client->rxBuffer_rp; + if (sizeUntilEnd >= out) + { + memcpy(str, &client->rxBuffer[client->rxBuffer_rp], out); + } + else + { + memcpy(str, &client->rxBuffer[client->rxBuffer_rp], sizeUntilEnd); + memcpy(str + sizeUntilEnd, &client->rxBuffer[0], out - sizeUntilEnd); + } + } + client->rxBuffer_rp += out; + if (client->rxBuffer_rp >= WS_RX_BUFFER_LEN) + { + client->rxBuffer_rp -= WS_RX_BUFFER_LEN; + } + } + return out; +} + void ws_putchar(ws_client_t client, char c) { if (client->txBuffer_wp == WS_TX_BUFFER_LEN) @@ -173,3 +217,8 @@ void ws_sendData(ws_client_t client) client->txBuffer_wp = 0; } } + +void ws_sendString(ws_client_t client, const char *str) +{ + esp_websocket_client_send_text(client->handle, str, strlen(str), 1000 * portTICK_PERIOD_MS); +} diff --git a/rx_esp32/src/ws.h b/rx_esp32/src/ws.h index 5b6d8a3..e1f38bb 100644 --- a/rx_esp32/src/ws.h +++ b/rx_esp32/src/ws.h @@ -25,7 +25,9 @@ typedef ws_client_data_t* ws_client_t; ws_client_t ws_connect(char *url, uint16_t id, uint16_t dev_code); int ws_getchar(ws_client_t client); +int ws_getstr(ws_client_t client, uint16_t max_len, char *str); void ws_putchar(ws_client_t client, char c); void ws_sendData(ws_client_t client); +void ws_sendString(ws_client_t client, const char *str); #endif