update

.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"
     },
 }

main/CMakeLists.txt

@@ -1,12 +1,12 @@
 idf_component_register(SRCS "main.c"
-                        "lipton/cci.cpp"
+    "lipton/cci.c"
-"lipton/i2c.cpp"
-                        "lipton/lepton_system.cpp"
-                        "lipton/lepton_utilities.cpp"
-                        "lipton/vospi.cpp"
     "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 ".")

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 >> 8 & 0xff),
-		uint8_t (reg & 0xff),
+		(uint8_t) (reg & 0xff),
-		uint8_t (value >> 8 & 0xff),
+		(uint8_t) (value >> 8 & 0xff),
-		uint8_t (value & 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);

main/lipton/lepton_system.c

@@ -6,7 +6,7 @@
 #include "lepton_utilities.h"
 #include "i2c.h"
 #include "vospi.h"
-#include <cstring>
+#include <string.h>
 
 
 // lepton image and telem buffer

main/main.c

@@ -1,5 +1,10 @@
 #include <stdio.h>
 
+#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 <driver/uart.h>
+#include <math.h>
 
 // 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,
         .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);
+    uint8_t *buf = malloc(BUF_SIZE);
-    char* line = 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};
         
-            struct timeval tv_now;
+            for (int y = 0; y < LEP_HEIGHT; y++) {
-            gettimeofday(&tv_now, NULL);
+                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);
+
+            uint8_t messageBuffer[32];
+
+            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));
         
-            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
         {