From 88efccb65abb29dc6b08d1ee8c140c533fb64dba Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bruno=20Ryb=C3=A1rsky?= Date: Thu, 1 May 2025 23:57:21 +0200 Subject: [PATCH] update --- .vscode/settings.json | 10 +- main/cameraTask.c | 0 main/cameraTask.h | 8 + main/main.c | 353 ++++++++++++++++++++++++++---------------- 4 files changed, 237 insertions(+), 134 deletions(-) create mode 100644 main/cameraTask.c create mode 100644 main/cameraTask.h diff --git a/.vscode/settings.json b/.vscode/settings.json index eb21f20..843e0ef 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -10,5 +10,13 @@ "idf.customExtraVars": { "IDF_TARGET": "esp32s3" }, - "idf.flashType": "UART" + "idf.flashType": "UART", + "idf.flashBaudRate": "115200", + "idf.monitorBaudRate": "115200", + "files.associations": { + "**/debian/*.install": "plain", + "condition_variable": "c", + "ctime": "c", + "mutex": "c" + }, } diff --git a/main/cameraTask.c b/main/cameraTask.c new file mode 100644 index 0000000..e69de29 diff --git a/main/cameraTask.h b/main/cameraTask.h new file mode 100644 index 0000000..01cbacf --- /dev/null +++ b/main/cameraTask.h @@ -0,0 +1,8 @@ +#include +#ifndef CAMERA_TASK +#define CAMERA_TASK + +void camera_task(void *pvParameters); +esp_err_t camera_init(); // already exists + +#endif \ No newline at end of file diff --git a/main/main.c b/main/main.c index d4e7a3d..c0ef076 100644 --- a/main/main.c +++ b/main/main.c @@ -21,9 +21,11 @@ #define HREF_GPIO_NUM 7 #define PCLK_GPIO_NUM 13 +// #define ENABLE_LEPTON + #define WIFI_IF WIFI_IF_AP -#define MAX_PAYLOAD_SIZE 1400 +#define MAX_PAYLOAD_SIZE 1472 #include "esp_event.h" #include "esp_system.h" @@ -72,10 +74,14 @@ static camera_config_t camera_config = { .pin_pwdn = PWDN_GPIO_NUM, .pin_reset = RESET_GPIO_NUM, .pin_xclk = XCLK_GPIO_NUM, - // .pin_sccb_sda = SIOD_GPIO_NUM, - // .pin_sccb_scl = SIOC_GPIO_NUM, + +#ifndef LEPTON_ENABLE + .pin_sccb_sda = SIOD_GPIO_NUM, + .pin_sccb_scl = SIOC_GPIO_NUM, +#else .pin_sccb_sda = -1, .pin_sccb_scl = -1, +#endif .sccb_i2c_port = I2C_MASTER_NUM, .pin_d7 = Y9_GPIO_NUM, @@ -107,16 +113,6 @@ static camera_config_t camera_config = { esp_err_t esp_wifi_80211_tx(wifi_interface_t ifx, const void *buffer, int len, bool en_sys_seq); -uint16_t calculate_checksum(uint8_t *data, size_t len) -{ - uint16_t checksum = 0; - for (size_t i = 0; i < len; i++) - { - checksum ^= data[i]; // Simple XOR-based checksum - } - return checksum; -} - void send_data_frame(uint8_t type, uint8_t *data, size_t len) { uint8_t mac_addr[6]; @@ -126,54 +122,61 @@ void send_data_frame(uint8_t type, uint8_t *data, size_t len) return; } + // 802.11 header template uint8_t header[] = { 0x08, 0x02, 0x00, 0x00, - 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x10, 0x00}; - memcpy(&header[10], mac_addr, 6); - memcpy(&header[16], mac_addr, 6); + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, // Destination: broadcast + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // Source: MAC (to be set) + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // BSSID: MAC (to be set) + 0x10, 0x00 // Sequence control + }; + memcpy(&header[10], mac_addr, 6); // Source + memcpy(&header[16], mac_addr, 6); // BSSID - size_t max_chunk_size = MAX_PAYLOAD_SIZE - (11 + 2); + size_t max_chunk_size = MAX_PAYLOAD_SIZE - (11 + 2 + 1); // Payload minus custom header and checksum size_t total_chunks = (len + max_chunk_size - 1) / max_chunk_size; + ESP_LOGI(TAG, "Got data with %d bytes", len); + + size_t offset = 0; for (uint16_t seq = 0; seq < total_chunks; seq++) { - size_t offset = seq * max_chunk_size; - size_t chunk_size = (len - offset > max_chunk_size) ? max_chunk_size : (len - offset); + size_t chunk_size = len - offset; + if (chunk_size > max_chunk_size) + { + chunk_size = max_chunk_size; + } + size_t full_len = sizeof(header) + 11 + chunk_size; - uint8_t *raw_frame = malloc(11 + chunk_size + 2); - uint8_t *full_packet = malloc(sizeof(header) + 11 + chunk_size + 2); - if (!raw_frame || !full_packet) + uint8_t *full_packet = malloc(full_len + 2); + if (!full_packet) { ESP_LOGE(TAG, "Memory allocation failed!"); - free(raw_frame); - free(full_packet); return; } - raw_frame[0] = type; - raw_frame[1] = seq >> 8; - raw_frame[2] = seq & 0xFF; - raw_frame[3] = total_chunks >> 8; - raw_frame[4] = total_chunks & 0xFF; - raw_frame[5] = 'P'; - raw_frame[6] = 'l'; - raw_frame[7] = 'e'; - raw_frame[8] = 'c'; - raw_frame[9] = 'y'; - raw_frame[10] = 'C'; - - memcpy(raw_frame + 11, data + offset, chunk_size); - uint16_t checksum = calculate_checksum(raw_frame, 11 + chunk_size); - raw_frame[11 + chunk_size] = checksum >> 8; - raw_frame[12 + chunk_size] = checksum & 0xFF; - memcpy(full_packet, header, sizeof(header)); - memcpy(full_packet + sizeof(header), raw_frame, 11 + chunk_size + 2); + uint8_t *payload = full_packet + sizeof(header); - esp_err_t ret = tx_with_retry(WIFI_IF, full_packet, sizeof(header) + 11 + chunk_size + 2, true); + // Custom header + payload[0] = type; + payload[1] = seq >> 8; + payload[2] = seq & 0xFF; + payload[3] = total_chunks >> 8; + payload[4] = total_chunks & 0xFF; + payload[5] = 'P'; + payload[6] = 'l'; + payload[7] = 'e'; + payload[8] = 'c'; + payload[9] = 'y'; + payload[10] = 'C'; + + memcpy(payload + 11, data + offset, chunk_size); + + ESP_LOGI(TAG, "TxTarget: %d bytes", full_len); + full_packet[full_len] = 0; + esp_err_t ret = tx_with_retry(WIFI_IF, full_packet, full_len + 2, true); + //ESP_LOG_BUFFER_HEXDUMP(TAG, full_packet, full_len, ESP_LOG_INFO); if (ret == ESP_OK) { @@ -184,9 +187,9 @@ void send_data_frame(uint8_t type, uint8_t *data, size_t len) ESP_LOGE(TAG, "Failed to send chunk %d, %s", seq, esp_err_to_name(ret)); } - free(raw_frame); free(full_packet); - vTaskDelay(50 / portTICK_PERIOD_MS); + vTaskDelay(60 / portTICK_PERIOD_MS); + offset += chunk_size; } } @@ -206,56 +209,111 @@ esp_err_t camera_init() typedef struct { bool isVisible; - size_t len; - size_t width; - size_t height; + uint32_t len; + uint16_t width; + uint16_t height; pixformat_t format; struct timeval timestamp; } PreData; +uint8_t *create_frame(const uint8_t *data, size_t *outSize, PreData pre) +{ + + const size_t headerSize = 19; + const size_t totalSize = pre.len + headerSize; + + ESP_LOGI(TAG, "DataWithFrame: %d\n", totalSize); + + uint8_t *dataOut = malloc(totalSize); + if (!dataOut) + { + ESP_LOGE(TAG, "Failed to allocate memory for frame"); + return NULL; + } + + size_t bytePos = 0; + + int64_t time_us = (int64_t)pre.timestamp.tv_sec * 1000000L + (int64_t)pre.timestamp.tv_usec; + + dataOut[bytePos++] = pre.isVisible; + dataOut[bytePos++] = (pre.len >> 24) & 0xFF; + dataOut[bytePos++] = (pre.len >> 16) & 0xFF; + dataOut[bytePos++] = (pre.len >> 8) & 0xFF; + dataOut[bytePos++] = (pre.len) & 0xFF; + dataOut[bytePos++] = (pre.width >> 8) & 0xFF; + dataOut[bytePos++] = (pre.width) & 0xFF; + dataOut[bytePos++] = (pre.height >> 8) & 0xFF; + dataOut[bytePos++] = (pre.height) & 0xFF; + dataOut[bytePos++] = (pre.format) & 0xFF; + dataOut[bytePos++] = (time_us >> 56) & 0xFF; + dataOut[bytePos++] = (time_us >> 48) & 0xFF; + dataOut[bytePos++] = (time_us >> 40) & 0xFF; + dataOut[bytePos++] = (time_us >> 32) & 0xFF; + dataOut[bytePos++] = (time_us >> 24) & 0xFF; + dataOut[bytePos++] = (time_us >> 16) & 0xFF; + dataOut[bytePos++] = (time_us >> 8) & 0xFF; + dataOut[bytePos++] = (time_us) & 0xFF; + + ESP_LOGI(TAG, "Vis: %d, len: %ld, width:%d, height:%d, format:%d, timestamp: %lld", pre.isVisible, pre.len, pre.width, pre.height, pre.format, time_us); + // Copy image data after PreData + memcpy(dataOut + bytePos, data, pre.len); + + *outSize = totalSize; + return dataOut; +} + esp_err_t camera_capture() { ESP_LOGI(TAG, "Pregrab"); camera_fb_t *fb = esp_camera_fb_get(); ESP_LOGI(TAG, "Postgrab"); + if (!fb) { ESP_LOGE(TAG, "Camera Capture Failed"); return ESP_FAIL; } - const size_t headerSize = sizeof(PreData); - const size_t totalSize = fb->len + headerSize; - uint8_t *dataOut = malloc(totalSize); + // Fill PreData at the start + PreData pre; + pre.isVisible = true; + pre.len = fb->len; + pre.width = fb->width; + pre.height = fb->height; + pre.format = fb->format; + pre.timestamp = fb->timestamp; + + size_t totalSize; + uint8_t *dataOut = create_frame(fb->buf, &totalSize, pre); if (!dataOut) { - ESP_LOGE(TAG, "Failed to allocate memory"); esp_camera_fb_return(fb); return ESP_ERR_NO_MEM; } - // Fill PreData at the start - PreData *pre = (PreData *)dataOut; - pre->isVisible = true; - pre->len = fb->len; - pre->width = fb->width; - pre->height = fb->height; - pre->format = fb->format; - pre->timestamp = fb->timestamp; - - // Copy image data after PreData - memcpy(dataOut + headerSize, fb->buf, fb->len); - - // Now chunk and send + esp_camera_fb_return(fb); send_data_frame(0x10, dataOut, totalSize); free(dataOut); - esp_camera_fb_return(fb); return ESP_OK; } void app_main(void) { + const uart_port_t uart_num = UART_NUM_0; + uart_config_t uart_config = { + .baud_rate = 115200, + .data_bits = UART_DATA_8_BITS, + .parity = UART_PARITY_DISABLE, + .stop_bits = UART_STOP_BITS_1, + .flow_ctrl = UART_HW_FLOWCTRL_DISABLE + }; + uart_param_config(uart_num, &uart_config); + uart_driver_install(uart_num, BUF_SIZE * 2, 0, 0, NULL, 0); + + uint8_t* buf = malloc(BUF_SIZE); + char* line = malloc(BUF_SIZE); + int line_len = 0; nvs_flash_init(); ESP_ERROR_CHECK(esp_netif_init()); @@ -263,6 +321,9 @@ void app_main(void) esp_netif_create_default_wifi_sta(); wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT(); + cfg.static_tx_buf_num = 32; // default is 4 or 8 + cfg.dynamic_tx_buf_num = 32; // boost this too + cfg.ampdu_tx_enable = 1; ESP_ERROR_CHECK(esp_wifi_init(&cfg)); ESP_ERROR_CHECK(esp_wifi_set_storage(WIFI_STORAGE_RAM)); ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_AP)); @@ -288,76 +349,102 @@ void app_main(void) camera_init(); - vTaskDelay(pdMS_TO_TICKS(300)); // Let the camera wake up fully +#ifdef LEPTON_ENABLE + gpio_config_t usb_phy_conf = { + .pin_bit_mask = (1ULL << 19) | (1ULL << 20), + .mode = GPIO_MODE_OUTPUT, + .pull_up_en = GPIO_PULLUP_ENABLE, + .pull_down_en = GPIO_PULLDOWN_DISABLE, + .intr_type = GPIO_INTR_DISABLE, + }; + gpio_config(&usb_phy_conf); + + printf("[MAIN] Start task\n"); + + LEP_CONFIG.agc_set_enabled = true; + LEP_CONFIG.emissivity = 100; + LEP_CONFIG.gain_mode = LEP_GAIN_AUTO; + // initialize spi, i2c and gpio for lepton + if (!lepton_io_init()) + { + printf("[MAIN] Error: I/O init failed"); + while (1) + { + vTaskDelay(pdMS_TO_TICKS(100)); + } + } + + // allocate lepton buffers + if (!lepton_buffer_init()) + { + printf("[MAIN] Error: Memory init failed"); + while (1) + { + vTaskDelay(pdMS_TO_TICKS(100)); + } + } + + // wait for lepton to initialize + vTaskDelay(pdMS_TO_TICKS(1000)); + ; + + // Attempt to initialize the VoSPI interface + if (vospi_init() != ESP_OK) + { + printf("[MAIN] Error: Lepton VoSPI initialization failed\n"); + while (1) + { + vTaskDelay(pdMS_TO_TICKS(100)); + } + } + + // initialize lepton + if (!lepton_init()) + { + printf("[MAIN] Error: Lepton CCI initialization failed\n"); + while (1) + { + vTaskDelay(pdMS_TO_TICKS(100)); + } + } + + // disable data from lepton + gpio_set_level(LEP_CSN_PIN, 1); + + printf("LEPTON INIT DONE"); + +#endif while (1) { - gpio_config_t usb_phy_conf = { - .pin_bit_mask = (1ULL << 19) | (1ULL << 20), - .mode = GPIO_MODE_OUTPUT, - .pull_up_en = GPIO_PULLUP_ENABLE, - .pull_down_en = GPIO_PULLDOWN_DISABLE, - .intr_type = GPIO_INTR_DISABLE, - }; - gpio_config(&usb_phy_conf); - - printf("[MAIN] Start task\n"); - - LEP_CONFIG.agc_set_enabled = true; - LEP_CONFIG.emissivity = 100; - LEP_CONFIG.gain_mode = LEP_GAIN_AUTO; - - // initialize spi, i2c and gpio for lepton - if (!lepton_io_init()) - { - printf("[MAIN] Error: I/O init failed"); - while (1) - { - vTaskDelay(pdMS_TO_TICKS(100)); - } - } - - // allocate lepton buffers - if (!lepton_buffer_init()) - { - printf("[MAIN] Error: Memory init failed"); - while (1) - { - vTaskDelay(pdMS_TO_TICKS(100)); - } - } - - // wait for lepton to initialize - vTaskDelay(pdMS_TO_TICKS(1000));; - - // Attempt to initialize the VoSPI interface - if (vospi_init() != ESP_OK) - { - printf("[MAIN] Error: Lepton VoSPI initialization failed\n"); - while (1) - { - vTaskDelay(pdMS_TO_TICKS(100)); - } - } - - // initialize lepton - if (!lepton_init()) - { - printf("[MAIN] Error: Lepton CCI initialization failed\n"); - while (1) - { - vTaskDelay(pdMS_TO_TICKS(100)); - } - } - - // disable data from lepton - gpio_set_level(LEP_CSN_PIN, 1); - - printf("LEPTON INIT DONE"); - int64_t start = esp_timer_get_time(); camera_capture(); +#ifdef LEPTON_ENABLE + uint8_t *picture = take_picture(); + if (picture) + { + + struct timeval tv_now; + gettimeofday(&tv_now, NULL); + + PreData pre; + pre.isVisible = false; + pre.len = LEP_NUM_PIXELS; + pre.width = LEP_WIDTH; + pre.height = LEP_HEIGHT; + pre.format = 0; + pre.timestamp = tv_now; + size_t totalSize; + uint8_t *dataOut = create_frame(picture, &totalSize, pre); + send_data_frame(0x10, dataOut, totalSize); + free(picture); + } + else + { + printf("Failed to lepton take picture\n"); + } +#endif int64_t end = esp_timer_get_time(); int64_t duration = end - start;