diff --git a/.vscode/settings.json b/.vscode/settings.json index 843e0ef..fe33b83 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -5,7 +5,7 @@ "idf.openOcdConfigs": [ "board/esp32s3-builtin.cfg" ], - "idf.port": "/dev/ttyUSB0", + "idf.port": "/dev/ttyUSB1", "idf.toolsPath": "/home/bruno/.espressif", "idf.customExtraVars": { "IDF_TARGET": "esp32s3" @@ -17,6 +17,7 @@ "**/debian/*.install": "plain", "condition_variable": "c", "ctime": "c", - "mutex": "c" + "mutex": "c", + "cstdint": "c" }, } diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index 39fd631..a3a0471 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -1,12 +1,12 @@ idf_component_register(SRCS "main.c" - "lipton/cci.cpp" -"lipton/i2c.cpp" - "lipton/lepton_system.cpp" - "lipton/lepton_utilities.cpp" - "lipton/vospi.cpp" - "lipton/cci.h" - "lipton/i2c.h" - "lipton/lepton_system.h" - "lipton/lepton_utilities.h" - "lipton/vospi.h" - INCLUDE_DIRS ".") \ No newline at end of file + "lipton/cci.c" + "lipton/cci.h" + "lipton/i2c.c" + "lipton/i2c.h" + "lipton/lepton_system.c" + "lipton/lepton_system.h" + "lipton/lepton_utilities.c" + "lipton/lepton_utilities.h" + "lipton/vospi.c" + "lipton/vospi.h" + INCLUDE_DIRS ".") diff --git a/main/lipton/cci.cpp b/main/lipton/cci.c similarity index 99% rename from main/lipton/cci.cpp rename to main/lipton/cci.c index 85ced4d..d354105 100644 --- a/main/lipton/cci.cpp +++ b/main/lipton/cci.c @@ -631,10 +631,10 @@ static int cci_write_register(uint16_t reg, uint16_t value) { // Write the register address and value uint8_t write_buf[4] = { - uint8_t (reg >> 8 & 0xff), - uint8_t (reg & 0xff), - uint8_t (value >> 8 & 0xff), - uint8_t (value & 0xff) + (uint8_t) (reg >> 8 & 0xff), + (uint8_t) (reg & 0xff), + (uint8_t) (value >> 8 & 0xff), + (uint8_t) (value & 0xff) }; if (i2c_master_write_slave(CCI_ADDRESS, write_buf, sizeof(write_buf)) != ESP_OK) { printf("[CCI] Error: failed to write CCI register %02x with value %02x\n", reg, value); diff --git a/main/lipton/i2c.cpp b/main/lipton/i2c.c similarity index 100% rename from main/lipton/i2c.cpp rename to main/lipton/i2c.c diff --git a/main/lipton/lepton_system.cpp b/main/lipton/lepton_system.c similarity index 99% rename from main/lipton/lepton_system.cpp rename to main/lipton/lepton_system.c index d262b93..db58710 100644 --- a/main/lipton/lepton_system.cpp +++ b/main/lipton/lepton_system.c @@ -6,7 +6,7 @@ #include "lepton_utilities.h" #include "i2c.h" #include "vospi.h" -#include +#include // lepton image and telem buffer diff --git a/main/lipton/lepton_utilities.cpp b/main/lipton/lepton_utilities.c similarity index 100% rename from main/lipton/lepton_utilities.cpp rename to main/lipton/lepton_utilities.c diff --git a/main/lipton/vospi.cpp b/main/lipton/vospi.c similarity index 100% rename from main/lipton/vospi.cpp rename to main/lipton/vospi.c diff --git a/main/main.c b/main/main.c index c0ef076..92e9204 100644 --- a/main/main.c +++ b/main/main.c @@ -1,5 +1,10 @@ #include +#define LEPTON_ENABLE + +#include "mbedtls/base64.h" + + #define TAG "CAM" #include "esp_camera.h" #define PWDN_GPIO_NUM -1 @@ -27,6 +32,8 @@ #define MAX_PAYLOAD_SIZE 1472 +// uint8_t hehe[10000]; + #include "esp_event.h" #include "esp_system.h" #include "esp_event.h" @@ -41,6 +48,8 @@ #include "lipton/vospi.h" #include "lipton/lepton_system.h" #include "lipton/lepton_utilities.h" +#include +#include // Number of consecutive VoSPI resynchronization attempts before attempting to reset #define LEP_SYNC_FAIL_FAULT_LIMIT 10 @@ -48,6 +57,13 @@ // Reset fail delay before attempting a re-init (seconds) #define LEP_RESET_FAIL_RETRY_SECS 5 +#define BUF_SIZE 20000 + +typedef struct { + int x, y, width, height; +} Rectangle; + + // // Code start // @@ -176,7 +192,7 @@ void send_data_frame(uint8_t type, uint8_t *data, size_t len) 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); + // ESP_LOG_BUFFER_HEXDUMP(TAG, full_packet, full_len, ESP_LOG_INFO); if (ret == ESP_OK) { @@ -188,7 +204,7 @@ void send_data_frame(uint8_t type, uint8_t *data, size_t len) } free(full_packet); - vTaskDelay(60 / portTICK_PERIOD_MS); + vTaskDelay(80 / portTICK_PERIOD_MS); offset += chunk_size; } } @@ -298,21 +314,103 @@ esp_err_t camera_capture() return ESP_OK; } +uint8_t *take_picture() +{ + gpio_set_level(LEP_CSN_PIN, 0); + vTaskDelay(pdMS_TO_TICKS(10)); + + while (1) + { + // Spin waiting for vsync to be asserted + int t = 0; + while (gpio_get_level(LEP_VSYNC_PIN) == 0 && t++ < 11000) + { + } + + vsyncDetectedUsec = esp_timer_get_time(); + + // Attempt to process a segment + if (t >= 22000 || vospi_transfer_segment(vsyncDetectedUsec)) + { + // Got image + vsync_count = 0; + if (t >= 11000) + printf("LEPOVF%d\n", t); + else + { + printf("LEPNOVF%d\n", t); + } + + // Copy the frame to the current half of the shared buffer and let rsp_task know + vospi_get_frame(&rsp_lep_buffer); + + // Hold fault counters reset while operating + sync_fail_count = 0; + + // disable lepton again + gpio_set_level(LEP_CSN_PIN, 1); + return rsp_lep_buffer.lep_bufferP; + } + else + { + if (++vsync_count >= 36) + { + vsync_count = 0; + printf("[MAIN] Could not get lepton image\n"); + + vTaskDelay(pdMS_TO_TICKS(185)); + + if (sync_fail_count++ == LEP_SYNC_FAIL_FAULT_LIMIT) + { + return NULL; + } + } + } + } + + return NULL; +} + +uint8_t *lepton_get_frame() +{ + + uint8_t *picture = take_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); + return picture; +} + void app_main(void) { + + // for (size_t i = 0; i < sizeof(hehe); i++) + // { + // hehe[i] = i & 0xFF; + // } + 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, + .parity = UART_PARITY_DISABLE, .stop_bits = UART_STOP_BITS_1, - .flow_ctrl = UART_HW_FLOWCTRL_DISABLE - }; + .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); + uint8_t *buf = malloc(BUF_SIZE); + char *line = malloc(BUF_SIZE); int line_len = 0; nvs_flash_init(); @@ -347,7 +445,25 @@ void app_main(void) ESP_LOGI(TAG, "ESP32 MAC: %02X:%02X:%02X:%02X:%02X:%02X", mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]); - camera_init(); + // size_t outLen = 0; + + // PreData pre; + + // struct timeval tv; + // gettimeofday(&tv, NULL); + + // pre.format = PIXFORMAT_JPEG; + // pre.width = 100; + // pre.height = 100; + // pre.isVisible = true; + // pre.len = sizeof(hehe); + // pre.timestamp = tv; + + // uint8_t *imgFrame = create_frame(hehe, &outLen, pre); + + // send_data_frame(0x10, imgFrame, outLen); + + // free(imgFrame); #ifdef LEPTON_ENABLE gpio_config_t usb_phy_conf = { @@ -412,6 +528,7 @@ void app_main(void) gpio_set_level(LEP_CSN_PIN, 1); printf("LEPTON INIT DONE"); + camera_init(); #endif @@ -421,24 +538,63 @@ void app_main(void) int64_t start = esp_timer_get_time(); camera_capture(); #ifdef LEPTON_ENABLE - uint8_t *picture = take_picture(); + uint8_t *picture = lepton_get_frame(); if (picture) { + int maxBrightness = -1; + Rectangle brightest = {0, 0, 0, 0}; + + for (int y = 0; y < LEP_HEIGHT; y++) { + for (int x = 0; x < LEP_WIDTH; x++) { + int idx = y * LEP_WIDTH + x; + int brightness = picture[idx]; // grayscale + + if (brightness > maxBrightness) { + maxBrightness = brightness; + brightest.x = x; + brightest.y = y; + } + } + } + + // Rozšírenie oblasti + int padding = 10; + brightest.x = (brightest.x - padding < 0) ? 0 : brightest.x - padding; + brightest.y = (brightest.y - padding < 0) ? 0 : brightest.y - padding; + brightest.width = ((brightest.x + padding * 2) > LEP_WIDTH) ? (LEP_WIDTH - brightest.x) : (padding * 2); + brightest.height = ((brightest.y + padding * 2) > LEP_HEIGHT) ? (LEP_HEIGHT - brightest.y) : (padding * 2); + + // Výpočet stredu oblasti + double middleX = brightest.x + brightest.width / 2.0; + double middleY = brightest.y + brightest.height / 2.0; + + // Výpočet uholných odchýlok + double rotationX = 3 * M_PI / 180.0; + double rotationY = 0 * M_PI / 180.0; + double rotationZ = 45 * M_PI / 180.0; + double posX = 100, posY = 100, posZ = 500; + + double plusX = ((middleX - LEP_HEIGHT / 2.0) / LEP_HEIGHT) * 57 * M_PI / 180.0; + double plusY = ((middleY - LEP_WIDTH / 2.0) / LEP_WIDTH) * 42.75 * M_PI / 180.0; + + double posGX = posZ * tan(rotationX + plusX) * cos(rotationZ) + posX; + double posGY = posZ * tan(rotationY + plusY) * sin(rotationZ) + posY; + + //printf("Stred najjasnejšej oblasti: (%.2f, %.2f)\n", middleX, middleY); + //printf("Výpočet pozície: posGX = %.2f, posGY = %.2f\n", posGX, posGY); - struct timeval tv_now; - gettimeofday(&tv_now, NULL); + uint8_t messageBuffer[32]; - 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); + uint8_t messageBufferEncoded[50]; + size_t outputLen; + mbedtls_base64_encode(messageBufferEncoded, sizeof(messageBufferEncoded), &outputLen, messageBuffer, sizeof(messageBuffer)); + + uint8_t messageBufferPrefixed[60] = "NavData:"; + strcat(messageBufferPrefixed, messageBufferEncoded); + strcat(messageBufferPrefixed, "\n"); + + uart_write_bytes(uart_num, messageBufferPrefixed, strlen(messageBufferPrefixed)); + } else {