Fix task and added timings for write

.vscode/settings.json

@@ -13,7 +13,7 @@
     },
     "editor.tabSize": 4,
     "editor.detectIndentation": false,
-    "cmake.sourceDirectory": "C:/data/repos/Hardware/Spot_Micro_Leika",
+    "cmake.sourceDirectory": "C:/data/repos/Hardware/Spot Micro - Leika/.pio/libdeps/esp32cam/esp32-camera",
     "cSpell.words": [
       "Adafruit",
       "IRAM",

app/src/lib/components/Visualization.svelte

@@ -262,9 +262,8 @@
         const rotatedXm = settings.xm * cosTotal - settings.zm * sinTotal
         const rotatedZm = settings.xm * sinTotal + settings.zm * cosTotal
 
-        const mpuHeadingRad = degToRad($mpu.heading)
+        const cosHead = Math.cos(headingYaw)
-        const cosHead = Math.cos(mpuHeadingRad)
+        const sinHead = Math.sin(headingYaw)
-        const sinHead = Math.sin(mpuHeadingRad)
         const rotatedCumX = body_state.cumulative_x * cosHead - body_state.cumulative_z * sinHead
         const rotatedCumZ = body_state.cumulative_x * sinHead + body_state.cumulative_z * cosHead
 

app/src/lib/filesystem/chunkedTransfer.ts

@@ -15,7 +15,7 @@ import type {
 } from '$lib/platform_shared/filesystem'
 import type { Result, DataResult, ListResult, ProgressCallback } from '$lib/types/models'
 
-const MAX_CHUNK_SIZE = 2 ** 14
+const MAX_CHUNK_SIZE = 1024 * 64 // 64KB - must match ESP32 FS_MAX_CHUNK_SIZE
 
 type TimeoutId = ReturnType<typeof setTimeout>
 type CleanupFn = (() => void) | null
@@ -51,7 +51,7 @@ export class FileSystemClient {
 	private downloadListenerCleanup: CleanupFn = null
 	private completeListenerCleanup: CleanupFn = null
 	private uploadCompleteListenerCleanup: CleanupFn = null
-	private transferTimeout = 60000
+	private transferTimeout = 300000
 
 	constructor() {
 		this.setupListeners()

app/src/lib/stores/socket.ts

@@ -153,6 +153,8 @@ function createWebSocket() {
             }
 
             const { tag, msg } = decodeMessage(frame.data)
+            const key: keyof Message = (MESSAGE_TAG_TO_KEY.get(tag) ?? "") as keyof Message;
+            console.log(key + ": ", msg[key])
             if (msg.correlationResponse) {
                 const pending = pending_requests.get(msg.correlationResponse.correlationId)
                 if (pending) {

app/vite.config.ts

@@ -21,7 +21,7 @@ export default defineConfig({
     server: {
         proxy: {
             '/api': {
-                target: 'http://spot-micro.local/',
+                target: 'http://192.168.50.141/',
                 changeOrigin: true,
                 ws: true
             }

boards/esp32p4_dev.json

@@ -1,33 +0,0 @@
-{
-  "build": {
-    "core": "esp32",
-    "extra_flags": [
-      "-DBOARD_HAS_PSRAM"
-    ],
-    "f_cpu": "360000000L",
-    "f_flash": "80000000L",
-    "f_psram": "200000000L",
-    "flash_mode": "qio",
-    "mcu": "esp32p4",
-    "variant": "esp32p4"
-  },
-  "connectivity": [
-    "wifi"
-  ],
-  "debug": {
-    "openocd_target": "esp32p4.cfg"
-  },
-  "frameworks": [
-    "espidf"
-  ],
-  "name": "ESP32-P4 Dev Board (32MB PSRAM + 32MB Flash, C6 coprocessor)",
-  "upload": {
-    "flash_size": "32MB",
-    "maximum_ram_size": 786432,
-    "maximum_size": 33554432,
-    "require_upload_port": true,
-    "speed": 1500000
-  },
-  "url": "https://docs.espressif.com/projects/esp-dev-kits/en/latest/esp32p4/",
-  "vendor": "Espressif"
-}

esp32/include/ap_service.h

@@ -2,7 +2,7 @@
 
 #include <template/stateful_service.h>
 #include <template/stateful_proto_endpoint.h>
-#include <template/stateful_persistence.h>
+#include <template/stateful_persistence_pb.h>
 #include <settings/ap_settings.h>
 #include <utils/timing.h>
 #include <wifi/wifi_idf.h>

esp32/include/communication/comm_base.hpp

@@ -8,6 +8,7 @@
 #include <map>
 #include <type_traits>
 #include <communication/proto_helpers.h>
+#include <utils/timing.h>
 
 class CommAdapterBase {
   public:
@@ -54,7 +55,8 @@ class CommAdapterBase {
 
         pb_ostream_t stream = pb_ostream_from_buffer(buffer, out_size);
         if (!pb_encode(&stream, socket_message_Message_fields, &msg_)) {
-            ESP_LOGE("ProtoComm", "Failed to encode message (tag %d), buffer too small?", (int)tag);
+            ESP_LOGE("ProtoComm", "Failed to encode message (tag %d): %s (calc=%u, written=%u)",
+                     (int)tag, PB_GET_ERROR(&stream), out_size, stream.bytes_written);
             return;
         }
 
@@ -95,9 +97,11 @@ class CommAdapterBase {
     }
 
     void handleIncoming(const uint8_t* data, size_t len, int cid) {
+        TIME_IT(
         if (!decoder_.decode(data, len, cid)) {
             ESP_LOGE("ProtoComm", "Failed to decode incoming message from client %d", cid);
         }
+        , INCOMING_DECODE)
     }
 
     void sendPong(int cid) {

esp32/include/communication/proto_helpers.h

@@ -3,6 +3,7 @@
 #include <pb_encode.h>
 #include <pb_decode.h>
 #include <platform_shared/message.pb.h>
+#include <esp_log.h>
 #include <functional>
 #include <map>
 
@@ -71,32 +72,48 @@ class ProtoDecoder {
     bool decode(const uint8_t* data, size_t len, int clientId) {
         pb_istream_t stream = pb_istream_from_buffer(data, len);
 
-        if (!pb_decode(&stream, socket_message_Message_fields, &msg_)) {
+        // Reset message before decoding (nanopb will malloc FT_POINTER fields)
+        msg_ = socket_message_Message_init_zero;
+
+        bool success = pb_decode(&stream, socket_message_Message_fields, &msg_);
+
+        if (!success) {
+            ESP_LOGE("ProtoHelpers", "Decode failed: %s (len=%u)", PB_GET_ERROR(&stream), len);
+            pb_release(socket_message_Message_fields, &msg_);
             return false;
         }
 
+        bool handled = false;
         switch (msg_.which_message) {
             case socket_message_Message_sub_notif_tag:
                 if (subscribeHandler_) subscribeHandler_(msg_.message.sub_notif.tag, clientId);
-                return true;
+                handled = true;
+                break;
 
             case socket_message_Message_unsub_notif_tag:
                 if (unsubscribeHandler_) unsubscribeHandler_(msg_.message.unsub_notif.tag, clientId);
-                return true;
+                handled = true;
+                break;
 
             case socket_message_Message_pingmsg_tag:
                 if (pingHandler_) pingHandler_(clientId);
-                return true;
+                handled = true;
+                break;
 
             default: {
                 auto it = handlers_.find(msg_.which_message);
                 if (it != handlers_.end()) {
                     it->second(clientId);
-                    return true;
+                    handled = true;
                 }
-                return false;
+                break;
             }
         }
+
+        // Free any malloc'd FT_POINTER fields
+        pb_release(socket_message_Message_fields, &msg_);
+
+        return handled;
     }
 
   private:

esp32/include/features.h

@@ -1,6 +1,5 @@
 #pragma once
 
-#include <sdkconfig.h>
 #include <wifi/wifi_idf.h>
 #include <esp_http_server.h>
 #include "platform_shared/message.pb.h"

esp32/include/filesystem.h

@@ -9,16 +9,18 @@
 #include <cstdio>
 #include <platform_shared/api.pb.h>
 
-#define MOUNT_POINT "/littlefs"
+#define MOUNT_POINT "/"
+#define LITTLEFS_MOUNT_POINT "/littlefs"
+#define SD_MOUNT_POINT "/sdcard"
 
-#define FS_CONFIG_DIRECTORY MOUNT_POINT "/config"
+#define FS_CONFIG_DIRECTORY LITTLEFS_MOUNT_POINT "/config"
-#define DEVICE_CONFIG_FILE MOUNT_POINT "/config/peripheral.pb"
+#define DEVICE_CONFIG_FILE LITTLEFS_MOUNT_POINT "/config/peripheral.pb"
-#define CAMERA_SETTINGS_FILE MOUNT_POINT "/config/cameraSettings.pb"
+#define CAMERA_SETTINGS_FILE LITTLEFS_MOUNT_POINT "/config/cameraSettings.pb"
-#define AP_SETTINGS_FILE MOUNT_POINT "/config/apSettings.pb"
+#define AP_SETTINGS_FILE LITTLEFS_MOUNT_POINT "/config/apSettings.pb"
-#define MDNS_SETTINGS_FILE MOUNT_POINT "/config/mdnsSettings.pb"
+#define MDNS_SETTINGS_FILE LITTLEFS_MOUNT_POINT "/config/mdnsSettings.pb"
-#define WIFI_SETTINGS_FILE MOUNT_POINT "/config/wifiSettings.pb"
+#define WIFI_SETTINGS_FILE LITTLEFS_MOUNT_POINT "/config/wifiSettings.pb"
-#define PERIPHERAL_SETTINGS_FILE MOUNT_POINT "/config/peripheralSettings.pb"
+#define PERIPHERAL_SETTINGS_FILE LITTLEFS_MOUNT_POINT "/config/peripheralSettings.pb"
-#define SERVO_SETTINGS_FILE MOUNT_POINT "/config/servoSettings.pb"
+#define SERVO_SETTINGS_FILE LITTLEFS_MOUNT_POINT "/config/servoSettings.pb"
 
 namespace FileSystem {
 

esp32/include/filesystem_ws.h

@@ -6,9 +6,16 @@
 #include <string>
 #include <functional>
 #include <cstdio>
+#include <freertos/FreeRTOS.h>
+#include <freertos/queue.h>
+#include <freertos/task.h>
+#include <freertos/semphr.h>
 
-#define FS_MAX_CHUNK_SIZE 16384
+#define FS_MAX_CHUNK_SIZE (1024 * 64)
 #define FS_TRANSFER_TIMEOUT_MS 30000
+#define FS_WRITE_QUEUE_SIZE 4
+#define FS_WRITER_TASK_STACK_SIZE 4096
+#define FS_WRITER_TASK_PRIORITY 5
 
 namespace FileSystemWS {
 
@@ -29,6 +36,7 @@ struct UploadState {
     uint32_t fileSize;
     uint32_t totalChunks;
     uint32_t chunksReceived;
+    uint32_t chunksWritten;
     uint32_t bytesReceived;
     uint32_t lastActivityTime;
     int clientId;
@@ -36,6 +44,14 @@ struct UploadState {
     std::string errorMessage;
 };
 
+struct WriteRequest {
+    uint32_t transferId;
+    uint8_t* data;
+    size_t size;
+    uint32_t chunkIndex;
+    bool isLastChunk;
+};
+
 using SendMetadataCallback = std::function<void(const socket_message_FSDownloadMetadata&, int clientId)>;
 using SendCallback = std::function<void(const socket_message_FSDownloadData&, int clientId)>;
 using SendCompleteCallback = std::function<void(const socket_message_FSDownloadComplete&, int clientId)>;
@@ -44,6 +60,10 @@ using SendUploadCompleteCallback = std::function<void(const socket_message_FSUpl
 class FileSystemHandler {
   public:
     FileSystemHandler();
+    ~FileSystemHandler();
+
+    void startWriterTask();
+    void stopWriterTask();
 
     void setSendCallbacks(SendMetadataCallback sendMetadata, SendCallback sendData, SendCompleteCallback sendComplete,
                           SendUploadCompleteCallback sendUploadComplete);
@@ -63,6 +83,12 @@ class FileSystemHandler {
     std::map<uint32_t, UploadState> uploads_;
     uint32_t transferIdCounter_;
 
+    // Async writer task
+    QueueHandle_t writeQueue_;
+    TaskHandle_t writerTaskHandle_;
+    SemaphoreHandle_t uploadsMutex_;
+    volatile bool writerTaskRunning_;
+
     inline uint32_t generateTransferId() { return ++transferIdCounter_; }
 
     SendMetadataCallback sendMetadataCallback_;
@@ -74,6 +100,8 @@ class FileSystemHandler {
     bool deleteRecursive(const std::string& path);
     bool sendNextDownloadChunk(uint32_t transferId);
     void finalizeUpload(uint32_t transferId, bool success, const std::string& error = "");
+    void processWriteRequest(const WriteRequest& req);
+    static void writerTaskFunc(void* param);
 };
 
 extern FileSystemHandler fsHandler;

esp32/include/global.h

@@ -23,39 +23,29 @@
 #ifndef ESP_PLATFORM_NAME
 #define ESP_PLATFORM_NAME "ESP32-S3"
 #endif
-#elif CONFIG_IDF_TARGET_ESP32C6
-#include "esp32c6/rom/rtc.h"
-#ifndef ESP_PLATFORM_NAME
-#define ESP_PLATFORM_NAME "ESP32-C6"
-#endif
-#elif CONFIG_IDF_TARGET_ESP32P4
-#include "esp32p4/rom/rtc.h"
-#ifndef ESP_PLATFORM_NAME
-#define ESP_PLATFORM_NAME "ESP32-P4"
-#endif
-#define ESP32P4_USES_C6_COPROCESSOR 1
 #else
 #error Target CONFIG_IDF_TARGET is not supported
 #endif
 
-/*
- * I2C software connection
- */
-#if CONFIG_IDF_TARGET_ESP32P4
-#ifndef SDA_PIN
-#define SDA_PIN 7
-#endif
-#ifndef SCL_PIN
-#define SCL_PIN 8
-#endif
-#else
 #ifndef SDA_PIN
 #define SDA_PIN 21
 #endif
 #ifndef SCL_PIN
 #define SCL_PIN 22
 #endif
-#endif
 #ifndef I2C_FREQUENCY
-#define I2C_FREQUENCY 1000000UL
+#define I2C_FREQUENCY 100000UL
 #endif
+
+// Optional SD card mounting via SDMMC (1-bit mode for ESP32-S3-CAM)
+// Pin definitions - override in build flags if needed
+#ifndef SD_CMD_PIN
+#define SD_CMD_PIN GPIO_NUM_38
+#endif
+#ifndef SD_CLK_PIN
+#define SD_CLK_PIN GPIO_NUM_39
+#endif
+#ifndef SD_DATA_PIN
+#define SD_DATA_PIN GPIO_NUM_40
+#endif
+

esp32/include/mdns_service.h

@@ -4,7 +4,7 @@
 #include <mdns.h>
 #include <template/stateful_service.h>
 #include <template/stateful_proto_endpoint.h>
-#include <template/stateful_persistence.h>
+#include <template/stateful_persistence_pb.h>
 #include <settings/mdns_settings.h>
 #include <utils/timing.h>
 

esp32/include/motion_states/walk_state.h

@@ -2,7 +2,6 @@
 
 #include <motion_states/state.h>
 #include <utils/math_utils.h>
-#include <algorithm>
 #include <array>
 #include <functional>
 

esp32/include/peripherals/camera_service.h

@@ -5,31 +5,25 @@
 #include <features.h>
 #include <template/stateful_service.h>
 #include <template/stateful_proto_endpoint.h>
-#include <template/stateful_persistence.h>
+#include <template/stateful_persistence_pb.h>
 
 #include <settings/camera_settings.h>
 
 namespace Camera {
 
-#define USE_DVP_CAMERA (USE_CAMERA && !CONFIG_IDF_TARGET_ESP32P4)
-#define USE_CSI_CAMERA (USE_CAMERA && CONFIG_IDF_TARGET_ESP32P4)
-
-#if USE_DVP_CAMERA
 #include <esp_camera.h>
-#include <peripherals/camera_pins.h>
 
-camera_fb_t *safe_camera_fb_get();
+#if USE_CAMERA
-sensor_t *safe_sensor_get();
+#include <peripherals/camera_pins.h>
-void safe_sensor_return();
 #endif
 
 #define PART_BOUNDARY "frame"
 
-class CameraService
+camera_fb_t *safe_camera_fb_get();
-#if USE_DVP_CAMERA
+sensor_t *safe_sensor_get();
-    : public StatefulService<CameraSettings>
+void safe_sensor_return();
-#endif
+
-{
+class CameraService : public StatefulService<CameraSettings> {
   public:
     CameraService();
 
@@ -38,12 +32,10 @@ class CameraService
     esp_err_t cameraStill(httpd_req_t *request);
     esp_err_t cameraStream(httpd_req_t *request);
 
-#if USE_DVP_CAMERA
     StatefulProtoEndpoint<CameraSettings, api_CameraSettings> protoEndpoint;
 
   private:
     FSPersistencePB<CameraSettings> _persistence;
     void updateCamera();
-#endif
 };
 } // namespace Camera

esp32/include/peripherals/i2c_bus.h

@@ -1,12 +1,11 @@
 #pragma once
 
-#include <driver/i2c_master.h>
+#include <driver/i2c.h>
 #include <esp_log.h>
 #include <freertos/FreeRTOS.h>
 #include <freertos/semphr.h>
 #include <functional>
 #include <vector>
-#include <cstring>
 
 class I2CBus {
   public:
@@ -25,21 +24,30 @@ class I2CBus {
         _scl = scl;
         _freq = freq;
 
-        i2c_master_bus_config_t bus_cfg = {};
+        i2c_config_t conf = {};
-        bus_cfg.i2c_port = port;
+        conf.mode = I2C_MODE_MASTER;
-        bus_cfg.sda_io_num = sda;
+        conf.sda_io_num = sda;
-        bus_cfg.scl_io_num = scl;
+        conf.scl_io_num = scl;
-        bus_cfg.clk_source = I2C_CLK_SRC_DEFAULT;
+        conf.sda_pullup_en = GPIO_PULLUP_ENABLE;
-        bus_cfg.glitch_ignore_cnt = 7;
+        conf.scl_pullup_en = GPIO_PULLUP_ENABLE;
-#if CONFIG_IDF_TARGET_ESP32P4
+        conf.master.clk_speed = freq;
-        bus_cfg.flags.enable_internal_pullup = false;
-#else
-        bus_cfg.flags.enable_internal_pullup = true;
-#endif
 
-        esp_err_t err = i2c_new_master_bus(&bus_cfg, &_bus);
+        esp_err_t err = i2c_param_config(_port, &conf);
         if (err != ESP_OK) {
-            ESP_LOGE(TAG, "i2c_new_master_bus failed: %s", esp_err_to_name(err));
+            ESP_LOGE(TAG, "i2c_param_config failed: %s", esp_err_to_name(err));
+            return err;
+        }
+
+        err = i2c_driver_install(_port, I2C_MODE_MASTER, 0, 0, 0);
+        if (err == ESP_ERR_INVALID_STATE) {
+            ESP_LOGW(TAG, "I2C driver already installed for port %d, reconfiguring", _port);
+            i2c_driver_delete(_port);
+            err = i2c_param_config(_port, &conf);
+            if (err != ESP_OK) return err;
+            err = i2c_driver_install(_port, I2C_MODE_MASTER, 0, 0, 0);
+        }
+        if (err != ESP_OK) {
+            ESP_LOGE(TAG, "i2c_driver_install failed: %s", esp_err_to_name(err));
             return err;
         }
 
@@ -49,51 +57,73 @@ class I2CBus {
 
     void end() {
         if (_initialized) {
-            if (_dev) {
+            i2c_driver_delete(_port);
-                i2c_master_bus_rm_device(_dev);
-                _dev = NULL;
-                _dev_addr = 0xFF;
-            }
-            i2c_del_master_bus(_bus);
-            _bus = NULL;
             _initialized = false;
         }
     }
 
     bool isInitialized() const { return _initialized; }
 
-    i2c_master_bus_handle_t busHandle() const { return _bus; }
-
     esp_err_t writeBytes(uint8_t addr, const uint8_t* data, size_t len) {
         if (!_initialized) return ESP_ERR_INVALID_STATE;
-        esp_err_t err = ensureDevice(addr);
+
-        if (err != ESP_OK) return err;
+        i2c_cmd_handle_t cmd = i2c_cmd_link_create();
-        return i2c_master_transmit(_dev, data, len, pdMS_TO_TICKS(200));
+        i2c_master_start(cmd);
+        i2c_master_write_byte(cmd, (addr << 1) | I2C_MASTER_WRITE, true);
+        if (len > 0 && data != nullptr) {
+            i2c_master_write(cmd, data, len, true);
+        }
+        i2c_master_stop(cmd);
+        esp_err_t ret = i2c_master_cmd_begin(_port, cmd, pdMS_TO_TICKS(100));
+        i2c_cmd_link_delete(cmd);
+        return ret;
     }
 
     esp_err_t writeReg(uint8_t addr, uint8_t reg, const uint8_t* data, size_t len) {
         if (!_initialized) return ESP_ERR_INVALID_STATE;
-        esp_err_t err = ensureDevice(addr);
-        if (err != ESP_OK) return err;
 
-        uint8_t buf[len + 1];
+        i2c_cmd_handle_t cmd = i2c_cmd_link_create();
-        buf[0] = reg;
+        i2c_master_start(cmd);
+        i2c_master_write_byte(cmd, (addr << 1) | I2C_MASTER_WRITE, true);
+        i2c_master_write_byte(cmd, reg, true);
         if (len > 0 && data != nullptr) {
-            memcpy(buf + 1, data, len);
+            i2c_master_write(cmd, data, len, true);
         }
-        return i2c_master_transmit(_dev, buf, len + 1, pdMS_TO_TICKS(200));
+        i2c_master_stop(cmd);
+        esp_err_t ret = i2c_master_cmd_begin(_port, cmd, pdMS_TO_TICKS(100));
+        i2c_cmd_link_delete(cmd);
+        return ret;
     }
 
     esp_err_t readReg(uint8_t addr, uint8_t reg, uint8_t* data, size_t len) {
         if (!_initialized) return ESP_ERR_INVALID_STATE;
-        esp_err_t err = ensureDevice(addr);
+
-        if (err != ESP_OK) return err;
+        i2c_cmd_handle_t cmd = i2c_cmd_link_create();
-        return i2c_master_transmit_receive(_dev, &reg, 1, data, len, pdMS_TO_TICKS(200));
+        i2c_master_start(cmd);
+        i2c_master_write_byte(cmd, (addr << 1) | I2C_MASTER_WRITE, true);
+        i2c_master_write_byte(cmd, reg, true);
+        i2c_master_start(cmd);
+        i2c_master_write_byte(cmd, (addr << 1) | I2C_MASTER_READ, true);
+        if (len > 1) {
+            i2c_master_read(cmd, data, len - 1, I2C_MASTER_ACK);
+        }
+        i2c_master_read_byte(cmd, data + len - 1, I2C_MASTER_NACK);
+        i2c_master_stop(cmd);
+        esp_err_t ret = i2c_master_cmd_begin(_port, cmd, pdMS_TO_TICKS(100));
+        i2c_cmd_link_delete(cmd);
+        return ret;
     }
 
     bool probe(uint8_t addr) {
         if (!_initialized) return false;
-        return i2c_master_probe(_bus, addr, pdMS_TO_TICKS(200)) == ESP_OK;
+
+        i2c_cmd_handle_t cmd = i2c_cmd_link_create();
+        i2c_master_start(cmd);
+        i2c_master_write_byte(cmd, (addr << 1) | I2C_MASTER_WRITE, true);
+        i2c_master_stop(cmd);
+        esp_err_t ret = i2c_master_cmd_begin(_port, cmd, pdMS_TO_TICKS(50));
+        i2c_cmd_link_delete(cmd);
+        return ret == ESP_OK;
     }
 
     std::vector<uint8_t> scan(uint8_t lower = 1, uint8_t upper = 127) {
@@ -127,23 +157,4 @@ class I2CBus {
     gpio_num_t _scl = GPIO_NUM_NC;
     uint32_t _freq = 100000;
     bool _initialized = false;
-
-    i2c_master_bus_handle_t _bus = NULL;
-    i2c_master_dev_handle_t _dev = NULL;
-    uint8_t _dev_addr = 0xFF;
-
-    esp_err_t ensureDevice(uint8_t addr) {
-        if (_dev && _dev_addr == addr) return ESP_OK;
-        if (_dev) {
-            i2c_master_bus_rm_device(_dev);
-            _dev = NULL;
-        }
-        i2c_device_config_t dev_cfg = {};
-        dev_cfg.dev_addr_length = I2C_ADDR_BIT_LEN_7;
-        dev_cfg.device_address = addr;
-        dev_cfg.scl_speed_hz = _freq;
-        esp_err_t err = i2c_master_bus_add_device(_bus, &dev_cfg, &_dev);
-        if (err == ESP_OK) _dev_addr = addr;
-        return err;
-    }
 };

esp32/include/peripherals/led_service.h

@@ -1,7 +1,6 @@
 #ifndef LEDService_h
 #define LEDService_h
 
-#include <sdkconfig.h>
 #include <driver/rmt_tx.h>
 #include <led_strip.h>
 #include <led_strip_rmt.h>
@@ -10,12 +9,8 @@
 #include <esp_log.h>
 
 #ifndef WS2812_PIN
-#if CONFIG_IDF_TARGET_ESP32P4
-#define WS2812_PIN 27
-#else
 #define WS2812_PIN 12
 #endif
-#endif
 
 #ifndef WS2812_NUM_LEDS
 #define WS2812_NUM_LEDS 13

esp32/include/peripherals/peripherals.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include <template/stateful_persistence.h>
+#include <template/stateful_persistence_pb.h>
 #include <template/stateful_service.h>
 #include <template/stateful_proto_endpoint.h>
 #include <utils/math_utils.h>

esp32/include/peripherals/servo_controller.h

@@ -2,7 +2,7 @@
 #define ServoController_h
 
 #include <peripherals/drivers/pca9685.h>
-#include <template/stateful_persistence.h>
+#include <template/stateful_persistence_pb.h>
 #include <template/stateful_proto_endpoint.h>
 #include <template/stateful_service.h>
 #include <utils/math_utils.h>
@@ -24,17 +24,22 @@ inline ServoSettings ServoSettings_defaults() {
     ServoSettings settings = {};
     settings.servos_count = 12;
     const api_Servo defaults[12] = {
-        {306, -1, 0, 2.0f, "Servo1"}, {306, 1, -45, 2.0f, "Servo2"},  {306, 1, 90, 2.0f, "Servo3"},
+        {306, -1, 0, 2.0f, "Servo1"},   {306, 1, -45, 2.0f, "Servo2"},
-        {306, -1, 0, 2.0f, "Servo4"}, {306, -1, 45, 2.0f, "Servo5"},  {306, -1, -90, 2.0f, "Servo6"},
+        {306, 1, 90, 2.0f, "Servo3"}, {306, -1, 0, 2.0f, "Servo4"},
-        {306, 1, 0, 2.0f, "Servo7"},  {306, 1, -45, 2.0f, "Servo8"},  {306, 1, 90, 2.0f, "Servo9"},
+        {306, -1, 45, 2.0f, "Servo5"}, {306, -1, -90, 2.0f, "Servo6"},
-        {306, 1, 0, 2.0f, "Servo10"}, {306, -1, 45, 2.0f, "Servo11"}, {306, -1, -90, 2.0f, "Servo12"}};
+        {306, 1, 0, 2.0f, "Servo7"},   {306, 1, -45, 2.0f, "Servo8"},
+        {306, 1, 90, 2.0f, "Servo9"}, {306, 1, 0, 2.0f, "Servo10"},
+        {306, -1, 45, 2.0f, "Servo11"}, {306, -1, -90, 2.0f, "Servo12"}
+    };
     for (int i = 0; i < 12; i++) {
         settings.servos[i] = defaults[i];
     }
     return settings;
 }
 
-inline void ServoSettings_read(const ServoSettings &settings, ServoSettings &proto) { proto = settings; }
+inline void ServoSettings_read(const ServoSettings &settings, ServoSettings &proto) {
+    proto = settings;
+}
 
 inline StateUpdateResult ServoSettings_update(const ServoSettings &proto, ServoSettings &settings) {
     settings = proto;
@@ -47,8 +52,9 @@ class ServoController : public StatefulService<ServoSettings> {
         : protoEndpoint(ServoSettings_read, ServoSettings_update, this,
                         API_REQUEST_EXTRACTOR(servo_settings, ServoSettings),
                         API_RESPONSE_ASSIGNER(servo_settings, ServoSettings)),
-          _persistence(ServoSettings_read, ServoSettings_update, this, SERVO_SETTINGS_FILE, api_ServoSettings_fields,
+          _persistence(ServoSettings_read, ServoSettings_update, this,
-                       api_ServoSettings_size, ServoSettings_defaults()) {}
+                       SERVO_SETTINGS_FILE, api_ServoSettings_fields, api_ServoSettings_size,
+                       ServoSettings_defaults()) {}
 
     void begin() {
         _persistence.readFromFS();

esp32/include/settings/camera_settings.h

@@ -1,20 +1,15 @@
 #pragma once
 
-#include <sdkconfig.h>
+#include <template/state_result.h>
 #include <platform_shared/api.pb.h>
-
-#if !CONFIG_IDF_TARGET_ESP32P4
 #include <esp_camera.h>
-#else
-#define PIXFORMAT_JPEG 0
-#define FRAMESIZE_VGA 0
-#define GAINCEILING_2X 0
-#endif
 
 namespace Camera {
 
+// Use proto type directly as settings type
 using CameraSettings = api_CameraSettings;
 
+// Default factory settings
 inline CameraSettings CameraSettings_defaults() {
     CameraSettings settings = api_CameraSettings_init_zero;
     settings.pixformat = PIXFORMAT_JPEG;

esp32/include/settings/peripherals_settings.h

@@ -1,9 +1,20 @@
 #pragma once
 
 #include <template/state_result.h>
-#include <sdkconfig.h>
 #include <platform_shared/api.pb.h>
-#include <global.h>
+
+/*
+ * I2C software connection
+ */
+#ifndef SDA_PIN
+#define SDA_PIN 21
+#endif
+#ifndef SCL_PIN
+#define SCL_PIN 22
+#endif
+#ifndef I2C_FREQUENCY
+#define I2C_FREQUENCY 1000000UL
+#endif
 
 // Use proto types directly
 using PinConfig = api_PinConfig;

esp32/include/template/stateful_persistence.h

@@ -1,65 +1,35 @@
-#pragma once
+#ifndef FSPersistence_h
+#define FSPersistence_h
 
 #include <template/stateful_service.h>
-#include <template/state_result.h>
 #include <filesystem.h>
-#include <pb_encode.h>
+#include <ArduinoJson.h>
-#include <pb_decode.h>
 #include <cstdio>
 #include <sys/stat.h>
-#include <esp_log.h>
-
-static const char *TAG_PERSISTENCE = "FSPersistencePB";
 
 template <class T>
-class FSPersistencePB {
+class FSPersistence {
   public:
-    using ProtoStateReader = std::function<void(const T &, T &)>;
+    FSPersistence(JsonStateReader<T> stateReader, JsonStateUpdater<T> stateUpdater, StatefulService<T> *statefulService,
-    using ProtoStateUpdater = std::function<StateUpdateResult(const T &, T &)>;
+                  const char *filePath)
-
-    FSPersistencePB(ProtoStateReader stateReader, ProtoStateUpdater stateUpdater, StatefulService<T> *statefulService,
-                    const char *filePath, const pb_msgdesc_t *msgDescriptor, size_t maxSize, const T &defaultState)
         : _stateReader(stateReader),
           _stateUpdater(stateUpdater),
           _statefulService(statefulService),
           _filePath(filePath),
-          _msgDescriptor(msgDescriptor),
-          _maxSize(maxSize),
-          _defaultState(defaultState),
           _updateHandlerId(0) {
         enableUpdateHandler();
     }
 
     void readFromFS() {
-        FILE *file = fopen(_filePath, "rb");
+        std::string content = FileSystem::readFile(_filePath);
 
-        if (file) {
+        if (!content.empty()) {
-            fseek(file, 0, SEEK_END);
+            JsonDocument jsonDocument;
-            size_t fileSize = ftell(file);
+            DeserializationError error = deserializeJson(jsonDocument, content);
-            fseek(file, 0, SEEK_SET);
+            if (error == DeserializationError::Ok) {
-
+                JsonVariant jsonObject = jsonDocument.as<JsonVariant>();
-            if (fileSize > 0 && fileSize <= _maxSize) {
+                _statefulService->updateWithoutPropagation(jsonObject, _stateUpdater);
-                uint8_t *buffer = new uint8_t[fileSize];
+                return;
-                size_t bytesRead = fread(buffer, 1, fileSize, file);
-                fclose(file);
-
-                if (bytesRead == fileSize) {
-                    T *protoMsg = new T();
-                    *protoMsg = {};
-                    pb_istream_t stream = pb_istream_from_buffer(buffer, bytesRead);
-
-                    if (pb_decode(&stream, _msgDescriptor, protoMsg)) {
-                        _statefulService->updateWithoutPropagation(
-                            [this, protoMsg](T &state) { return _stateUpdater(*protoMsg, state); });
-                        delete protoMsg;
-                        delete[] buffer;
-                        return;
-                    }
-                    delete protoMsg;
-                }
-                delete[] buffer;
-            } else {
-                fclose(file);
             }
         }
 
@@ -68,35 +38,16 @@ class FSPersistencePB {
     }
 
     bool writeToFS() {
-        uint8_t *buffer = new uint8_t[_maxSize];
+        JsonDocument jsonDocument;
-        pb_ostream_t stream = pb_ostream_from_buffer(buffer, _maxSize);
+        JsonVariant jsonObject = jsonDocument.to<JsonVariant>();
-
+        _statefulService->read(jsonObject, _stateReader);
-        T *protoMsg = new T();
-        *protoMsg = {};
-        _statefulService->read([this, protoMsg](const T &state) { _stateReader(state, *protoMsg); });
-
-        bool encodeSuccess = pb_encode(&stream, _msgDescriptor, protoMsg);
-        delete protoMsg;
-
-        if (!encodeSuccess) {
-            delete[] buffer;
-            return false;
-        }
 
         mkdirs();
 
-        FILE *file = fopen(_filePath, "wb");
+        std::string content;
-        if (!file) {
+        serializeJson(jsonDocument, content);
-            ESP_LOGE(TAG_PERSISTENCE, "Failed to open file for writing: %s", _filePath);
-            delete[] buffer;
-            return false;
-        }
 
-        size_t written = fwrite(buffer, 1, stream.bytes_written, file);
+        return FileSystem::writeFile(_filePath, content.c_str());
-        fclose(file);
-        delete[] buffer;
-
-        return written == stream.bytes_written;
     }
 
     void disableUpdateHandler() {
@@ -113,13 +64,11 @@ class FSPersistencePB {
     }
 
   private:
-    ProtoStateReader _stateReader;
+    JsonStateReader<T> _stateReader;
-    ProtoStateUpdater _stateUpdater;
+    JsonStateUpdater<T> _stateUpdater;
     StatefulService<T> *_statefulService;
     const char *_filePath;
-    const pb_msgdesc_t *_msgDescriptor;
+    size_t _bufferSize;
-    size_t _maxSize;
-    T _defaultState;
     HandlerId _updateHandlerId;
 
     void mkdirs() {
@@ -135,7 +84,11 @@ class FSPersistencePB {
     }
 
   protected:
-    void applyDefaults() {
+    virtual void applyDefaults() {
-        _statefulService->updateWithoutPropagation([this](T &state) { return _stateUpdater(_defaultState, state); });
+        JsonDocument jsonDocument;
+        JsonVariant jsonObject = jsonDocument.as<JsonVariant>();
+        _statefulService->updateWithoutPropagation(jsonObject, _stateUpdater);
     }
 };
+
+#endif

esp32/include/template/stateful_persistence_pb.h

@@ -0,0 +1,141 @@
+#pragma once
+
+#include <template/stateful_service.h>
+#include <template/state_result.h>
+#include <filesystem.h>
+#include <pb_encode.h>
+#include <pb_decode.h>
+#include <cstdio>
+#include <sys/stat.h>
+#include <esp_log.h>
+
+static const char *TAG_PERSISTENCE = "FSPersistencePB";
+
+template <class T>
+class FSPersistencePB {
+  public:
+    using ProtoStateReader = std::function<void(const T &, T &)>;
+    using ProtoStateUpdater = std::function<StateUpdateResult(const T &, T &)>;
+
+    FSPersistencePB(ProtoStateReader stateReader, ProtoStateUpdater stateUpdater, StatefulService<T> *statefulService,
+                    const char *filePath, const pb_msgdesc_t *msgDescriptor, size_t maxSize, const T &defaultState)
+        : _stateReader(stateReader),
+          _stateUpdater(stateUpdater),
+          _statefulService(statefulService),
+          _filePath(filePath),
+          _msgDescriptor(msgDescriptor),
+          _maxSize(maxSize),
+          _defaultState(defaultState),
+          _updateHandlerId(0) {
+        enableUpdateHandler();
+    }
+
+    void readFromFS() {
+        FILE *file = fopen(_filePath, "rb");
+
+        if (file) {
+            fseek(file, 0, SEEK_END);
+            size_t fileSize = ftell(file);
+            fseek(file, 0, SEEK_SET);
+
+            if (fileSize > 0 && fileSize <= _maxSize) {
+                uint8_t *buffer = new uint8_t[fileSize];
+                size_t bytesRead = fread(buffer, 1, fileSize, file);
+                fclose(file);
+
+                if (bytesRead == fileSize) {
+                    T *protoMsg = new T();
+                    *protoMsg = {};
+                    pb_istream_t stream = pb_istream_from_buffer(buffer, bytesRead);
+
+                    if (pb_decode(&stream, _msgDescriptor, protoMsg)) {
+                        _statefulService->updateWithoutPropagation(
+                            [this, protoMsg](T &state) { return _stateUpdater(*protoMsg, state); });
+                        delete protoMsg;
+                        delete[] buffer;
+                        return;
+                    }
+                    delete protoMsg;
+                }
+                delete[] buffer;
+            } else {
+                fclose(file);
+            }
+        }
+
+        applyDefaults();
+        writeToFS();
+    }
+
+    bool writeToFS() {
+        uint8_t *buffer = new uint8_t[_maxSize];
+        pb_ostream_t stream = pb_ostream_from_buffer(buffer, _maxSize);
+
+        T *protoMsg = new T();
+        *protoMsg = {};
+        _statefulService->read([this, protoMsg](const T &state) { _stateReader(state, *protoMsg); });
+
+        bool encodeSuccess = pb_encode(&stream, _msgDescriptor, protoMsg);
+        delete protoMsg;
+
+        if (!encodeSuccess) {
+            delete[] buffer;
+            return false;
+        }
+
+        mkdirs();
+
+        FILE *file = fopen(_filePath, "wb");
+        if (!file) {
+            ESP_LOGE(TAG_PERSISTENCE, "Failed to open file for writing: %s", _filePath);
+            delete[] buffer;
+            return false;
+        }
+
+        size_t written = fwrite(buffer, 1, stream.bytes_written, file);
+        fclose(file);
+        delete[] buffer;
+
+        return written == stream.bytes_written;
+    }
+
+    void disableUpdateHandler() {
+        if (_updateHandlerId) {
+            _statefulService->removeUpdateHandler(_updateHandlerId);
+            _updateHandlerId = 0;
+        }
+    }
+
+    void enableUpdateHandler() {
+        if (!_updateHandlerId) {
+            _updateHandlerId = _statefulService->addUpdateHandler([&](const std::string &originId) { writeToFS(); });
+        }
+    }
+
+  private:
+    ProtoStateReader _stateReader;
+    ProtoStateUpdater _stateUpdater;
+    StatefulService<T> *_statefulService;
+    const char *_filePath;
+    const pb_msgdesc_t *_msgDescriptor;
+    size_t _maxSize;
+    T _defaultState;
+    HandlerId _updateHandlerId;
+
+    void mkdirs() {
+        std::string path(_filePath);
+        size_t index = 0;
+        while ((index = path.find('/', index + 1)) != std::string::npos) {
+            std::string segment = path.substr(0, index);
+            struct stat st;
+            if (stat(segment.c_str(), &st) != 0) {
+                FileSystem::mkdirRecursive(segment.c_str());
+            }
+        }
+    }
+
+  protected:
+    void applyDefaults() {
+        _statefulService->updateWithoutPropagation([this](T &state) { return _stateUpdater(_defaultState, state); });
+    }
+};

esp32/include/utils/timing.h

@@ -16,17 +16,18 @@
         }                                                                                         \
     } while (0)
 
-#define TIME_IT(code)                                                                       \
+    // Note: name must be a valid variable name too
+#define TIME_IT(code, name)                                                                       \
     {                                                                                       \
-        uint64_t time_it_start = esp_timer_get_time();                                      \
+        uint64_t time_it_start##name = esp_timer_get_time();                                      \
         code;                                                                               \
-        uint64_t time_it_elapsed = esp_timer_get_time() - time_it_start;                    \
+        uint64_t time_it_elapsed##name = esp_timer_get_time() - time_it_start##name;                    \
-        if (time_it_elapsed < 1000) {                                                       \
+        if (time_it_elapsed##name < 1000) {                                                       \
-            ESP_LOGI("Time It", "Time elapsed: %llu microseconds", time_it_elapsed);        \
+            ESP_LOGI("Time It - " #name, "Time elapsed: %llu microseconds", time_it_elapsed##name);        \
-        } else if (time_it_elapsed < 1000000) {                                             \
+        } else if (time_it_elapsed##name < 1000000) {                                             \
-            ESP_LOGI("Time It", "Time elapsed: %llu milliseconds", time_it_elapsed / 1000); \
+            ESP_LOGI("Time It - " #name, "Time elapsed: %llu milliseconds", time_it_elapsed##name / 1000); \
         } else {                                                                            \
-            ESP_LOGI("Time It", "Time elapsed: %.2f seconds", time_it_elapsed / 1000000.0); \
+            ESP_LOGI("Time It - " #name, "Time elapsed: %.2f seconds", time_it_elapsed##name / 1000000.0); \
         }                                                                                   \
     }
 

esp32/include/wifi_service.h

@@ -8,7 +8,7 @@
 #include <filesystem.h>
 #include <utils/timing.h>
 #include <template/stateful_service.h>
-#include <template/stateful_persistence.h>
+#include <template/stateful_persistence_pb.h>
 #include <template/stateful_proto_endpoint.h>
 #include <settings/wifi_settings.h>
 

esp32/partition_table/default_32MB.csv

@@ -1,8 +0,0 @@
-# Name,   Type, SubType, Offset,   Size, Flags
-# 32MB Flash partition table for ESP32-P4
-nvs,      data, nvs,     0x9000,   0x5000,
-otadata,  data, ota,     0xe000,   0x2000,
-app0,     app,  ota_0,   0x10000,  0xC80000,
-app1,     app,  ota_1,   0xC90000, 0xC80000,
-spiffs,   data, spiffs,  0x1910000,0x6E0000,
-coredump, data, coredump,0x1FF0000,0x10000,

esp32/src/CMakeLists.txt

@@ -1,25 +1,3 @@
-set(COMPONENT_REQUIRES
-    driver
-    esp_driver_i2c
-    esp_http_server
-    nvs_flash
-    esp_wifi
-    esp_event
-    esp_netif
-    mdns
-    esp_timer
-    esp_psram
-    spi_flash
-    littlefs
-    esp-dsp
-)
-
-if(IDF_TARGET STREQUAL "esp32p4")
-    list(APPEND COMPONENT_REQUIRES esp_wifi_remote esp_hosted esp_driver_cam esp_driver_isp esp_driver_jpeg espressif__esp_sccb_intf espressif__esp_cam_sensor)
-else()
-    list(APPEND COMPONENT_REQUIRES esp32-camera)
-endif()
-
 idf_component_register(
     SRC_DIRS 
         "."
@@ -32,5 +10,17 @@ idf_component_register(
         "../include"
         "../../submodules/nanopb"
     REQUIRES
-        ${COMPONENT_REQUIRES}
+        driver
+        esp_http_server
+        nvs_flash
+        esp_wifi
+        esp_event
+        esp_netif
+        mdns
+        esp_timer
+        esp_psram
+        spi_flash
+        littlefs
+        esp32-camera
+        esp-dsp
 )

esp32/src/communication/webserver.cpp

@@ -2,6 +2,7 @@
 #include <esp_log.h>
 #include <cstring>
 #include <algorithm>
+#include <utils/timing.h>
 
 static const char* TAG = "WebServer";
 
@@ -121,6 +122,10 @@ esp_err_t WebServer::httpHandler(httpd_req_t* req) {
 }
 
 esp_err_t WebServer::wsHandler(httpd_req_t* req) {
+    esp_err_t result;
+    httpd_ws_frame_t frame;
+    esp_err_t ret;
+    TIME_IT(
     WebServer* self = static_cast<WebServer*>(req->user_ctx);
 
     if (req->method == HTTP_GET) {
@@ -133,17 +138,21 @@ esp_err_t WebServer::wsHandler(httpd_req_t* req) {
         return ESP_OK;
     }
 
-    httpd_ws_frame_t frame;
+    
     memset(&frame, 0, sizeof(httpd_ws_frame_t));
     frame.type = HTTPD_WS_TYPE_BINARY;
 
-    esp_err_t ret = httpd_ws_recv_frame(req, &frame, 0);
+    
+    TIME_IT(
+    ret = httpd_ws_recv_frame(req, &frame, 0);
     if (ret != ESP_OK) {
         ESP_LOGE(TAG, "Failed to get frame len: %s", esp_err_to_name(ret));
         return ret;
     }
+    , FRAME_LEN)
 
     if (frame.len > 0) {
+        TIME_IT(
         frame.payload = (uint8_t*)malloc(frame.len);
         if (!frame.payload) {
             ESP_LOGE(TAG, "Failed to allocate frame payload");
@@ -156,6 +165,7 @@ esp_err_t WebServer::wsHandler(httpd_req_t* req) {
             free(frame.payload);
             return ret;
         }
+        , FRAME_RECEIVE)
     }
 
     if (frame.type == HTTPD_WS_TYPE_CLOSE) {
@@ -169,15 +179,18 @@ esp_err_t WebServer::wsHandler(httpd_req_t* req) {
         return ESP_OK;
     }
 
-    esp_err_t result = ESP_OK;
+    result = ESP_OK;
     if (self->wsFrameHandler_) {
+        TIME_IT(
         result = self->wsFrameHandler_(req, &frame);
+        , FRAME_HANDLER)
     }
 
     if (frame.payload) {
         free(frame.payload);
     }
 
+    , WS_HANDLER)
     return result;
 }
 
@@ -265,7 +278,19 @@ esp_err_t WebServer::wsSend(int sockfd, const uint8_t* data, size_t len) {
                               .type = HTTPD_WS_TYPE_BINARY,
                               .payload = const_cast<uint8_t*>(data),
                               .len = len};
-    return httpd_ws_send_frame_async(server_, sockfd, &frame);
+    xSemaphoreTake(wsMutex_, portMAX_DELAY);
+    esp_err_t ret = httpd_ws_send_frame_async(server_, sockfd, &frame);
+    xSemaphoreGive(wsMutex_);
+
+    if (ret != ESP_OK) {
+        if (httpd_ws_get_fd_info(server_, sockfd) != HTTPD_WS_CLIENT_WEBSOCKET) {
+        ESP_LOGW(TAG, "Removing disconnected client %d", sockfd);
+        removeWsClient(sockfd);
+            return ESP_ERR_INVALID_STATE;
+        } 
+    }
+
+    return ret;
 }
 
 esp_err_t WebServer::wsSendAll(const uint8_t* data, size_t len) {

esp32/src/communication/websocket.cpp

@@ -24,8 +24,26 @@ void Websocket::onWsClose(int sockfd) {
 }
 
 esp_err_t Websocket::onFrame(httpd_req_t* req, httpd_ws_frame_t* frame) {
+    // Handle PING - respond with PONG
+    if (frame->type == HTTPD_WS_TYPE_PING) {
+        httpd_ws_frame_t pong = {
+            .final = true,
+            .fragmented = false,
+            .type = HTTPD_WS_TYPE_PONG,
+            .payload = frame->payload,
+            .len = frame->len
+        };
+        return httpd_ws_send_frame(req, &pong);
+    }
+
+    // Ignore PONG frames
+    if (frame->type == HTTPD_WS_TYPE_PONG) {
+        return ESP_OK;
+    }
+
+    // Ignore other non-binary frames
     if (frame->type != HTTPD_WS_TYPE_BINARY) {
-        ESP_LOGW(TAG, "Expected binary frame, got type %d", frame->type);
+        ESP_LOGD(TAG, "Ignoring frame type %d", frame->type);
         return ESP_OK;
     }
 

esp32/src/filesystem.cpp

@@ -6,6 +6,9 @@
 #include <esp_log.h>
 #include <pb_encode.h>
 #include <pb_decode.h>
+#include "esp_vfs_fat.h"
+#include <sdmmc_cmd.h>
+#include <driver/sdmmc_host.h>
 
 static const char *TAG = "FileSystem";
 
@@ -79,7 +82,7 @@ void listFilesProto(const std::string &directory, api_FileEntry *entry) {
     if (path.empty() || path[0] != '/') {
         path = "/" + directory;
     }
-    std::string fullPath = std::string(MOUNT_POINT) + path;
+    std::string fullPath = path;
     listFilesProtoRecursive(fullPath, entry);
 }
 
@@ -108,7 +111,7 @@ esp_err_t getFilesProto(httpd_req_t *request) {
 
 bool init() {
     esp_vfs_littlefs_conf_t conf = {
-        .base_path = MOUNT_POINT,
+        .base_path = LITTLEFS_MOUNT_POINT,
         .partition_label = "spiffs",
         .format_if_mount_failed = true,
         .dont_mount = false,
@@ -134,6 +137,38 @@ bool init() {
 
     mkdirRecursive(FS_CONFIG_DIRECTORY);
 
+    esp_vfs_fat_sdmmc_mount_config_t sd_mount_config = {
+        .format_if_mount_failed = false,
+        .max_files = 4,
+        .allocation_unit_size = 16 * 1024,
+    };
+
+    sdmmc_host_t host = SDMMC_HOST_DEFAULT();
+    host.flags = SDMMC_HOST_FLAG_1BIT;  // Use 1-bit mode
+    host.max_freq_khz = SDMMC_FREQ_DEFAULT;
+
+    sdmmc_slot_config_t slot_config = SDMMC_SLOT_CONFIG_DEFAULT();
+    slot_config.width = 1;  // 1-bit mode
+    slot_config.clk = SD_CLK_PIN;
+    slot_config.cmd = SD_CMD_PIN;
+    slot_config.d0 = SD_DATA_PIN;
+    slot_config.flags |= SDMMC_SLOT_FLAG_INTERNAL_PULLUP;
+
+    sdmmc_card_t *card = nullptr;
+    esp_err_t err = esp_vfs_fat_sdmmc_mount(SD_MOUNT_POINT, &host, &slot_config, &sd_mount_config, &card);
+    if (err != ESP_OK) {
+        if (err == ESP_FAIL) {
+            ESP_LOGW(TAG, "Failed to mount SD card filesystem");
+        } else {
+            ESP_LOGW(TAG, "SD card not present or failed to initialize (%s)", esp_err_to_name(err));
+        }
+        // Don't fail - SD card is optional
+    } else {
+        ESP_LOGI(TAG, "SD card mounted at %s", SD_MOUNT_POINT);
+        ESP_LOGI(TAG, "SD card: %s, %lluMB", card->cid.name,
+                 ((uint64_t)card->csd.capacity) * card->csd.sector_size / (1024 * 1024));
+    }
+
     return true;
 }
 
@@ -231,7 +266,7 @@ esp_err_t getFiles(httpd_req_t *request) {
 
 esp_err_t getConfigFile(httpd_req_t *request) {
     const char *uri = request->uri;
-    std::string path = std::string(MOUNT_POINT) + "/config" + std::string(uri).substr(11);
+    std::string path = std::string(LITTLEFS_MOUNT_POINT) + "/config" + std::string(uri).substr(11);
 
     if (!fileExists(path.c_str())) {
         return WebServer::sendError(request, 404, "File not found");
@@ -253,7 +288,7 @@ esp_err_t getConfigFile(httpd_req_t *request) {
 }
 
 esp_err_t handleDelete(httpd_req_t *request, const api_FileDeleteRequest &req) {
-    std::string fullPath = std::string(MOUNT_POINT) + req.path;
+    std::string fullPath = req.path;
     ESP_LOGI(TAG, "Deleting file: %s", fullPath.c_str());
 
     api_Response res = api_Response_init_zero;
@@ -267,7 +302,7 @@ esp_err_t handleDelete(httpd_req_t *request, const api_FileDeleteRequest &req) {
 }
 
 esp_err_t handleEdit(httpd_req_t *request, const api_FileEditRequest &req) {
-    std::string fullPath = std::string(MOUNT_POINT) + req.path;
+    std::string fullPath = req.path;
     ESP_LOGI(TAG, "Editing file: %s", fullPath.c_str());
 
     api_Response res = api_Response_init_zero;
@@ -326,7 +361,7 @@ bool editFile(const char *filename, const uint8_t *content, size_t size) { retur
 bool editFile(const char *filename, const char *content) { return writeFile(filename, content); }
 
 esp_err_t mkdir(httpd_req_t *request, const api_FileMkdirRequest &req) {
-    std::string fullPath = std::string(MOUNT_POINT) + req.path;
+    std::string fullPath = req.path;
     ESP_LOGI(TAG, "Creating directory: %s", fullPath.c_str());
 
     api_Response res = api_Response_init_zero;

esp32/src/filesystem_ws.cpp

@@ -7,6 +7,8 @@
 #include <dirent.h>
 #include <sys/stat.h>
 #include <cstring>
+#include <cerrno>
+#include <utils/timing.h>
 
 static const char* TAG = "FileSystemWS";
 
@@ -14,7 +16,137 @@ namespace FileSystemWS {
 
 FileSystemHandler fsHandler;
 
-FileSystemHandler::FileSystemHandler() : transferIdCounter_(0) {}
+FileSystemHandler::FileSystemHandler()
+    : transferIdCounter_(0), writeQueue_(nullptr), writerTaskHandle_(nullptr), uploadsMutex_(nullptr),
+      writerTaskRunning_(false) {
+    uploadsMutex_ = xSemaphoreCreateMutex();
+}
+
+FileSystemHandler::~FileSystemHandler() {
+    stopWriterTask();
+    if (uploadsMutex_) {
+        vSemaphoreDelete(uploadsMutex_);
+    }
+}
+
+void FileSystemHandler::startWriterTask() {
+    if (writerTaskHandle_ != nullptr) {
+        return;
+    }
+
+    writeQueue_ = xQueueCreate(FS_WRITE_QUEUE_SIZE, sizeof(WriteRequest));
+    if (!writeQueue_) {
+        ESP_LOGE(TAG, "Failed to create write queue");
+        return;
+    }
+
+    writerTaskRunning_ = true;
+    BaseType_t result =
+        xTaskCreate(writerTaskFunc, "fs_writer", FS_WRITER_TASK_STACK_SIZE, this, FS_WRITER_TASK_PRIORITY, &writerTaskHandle_);
+
+    if (result != pdPASS) {
+        ESP_LOGE(TAG, "Failed to create writer task");
+        vQueueDelete(writeQueue_);
+        writeQueue_ = nullptr;
+        writerTaskRunning_ = false;
+    } else {
+        ESP_LOGI(TAG, "Writer task started");
+    }
+}
+
+void FileSystemHandler::stopWriterTask() {
+    if (!writerTaskRunning_) {
+        return;
+    }
+
+    writerTaskRunning_ = false;
+
+    // Send a poison pill to wake up the task
+    WriteRequest poison = {0, nullptr, 0, 0, false};
+    if (writeQueue_) {
+        xQueueSend(writeQueue_, &poison, portMAX_DELAY);
+    }
+
+    // Wait for task to finish
+    if (writerTaskHandle_) {
+        vTaskDelay(pdMS_TO_TICKS(100));
+        writerTaskHandle_ = nullptr;
+    }
+
+    if (writeQueue_) {
+        // Drain any remaining requests and free their data
+        WriteRequest req;
+        while (xQueueReceive(writeQueue_, &req, 0) == pdTRUE) {
+            if (req.data) {
+                free(req.data);
+            }
+        }
+        vQueueDelete(writeQueue_);
+        writeQueue_ = nullptr;
+    }
+
+    ESP_LOGI(TAG, "Writer task stopped");
+}
+
+void FileSystemHandler::writerTaskFunc(void* param) {
+    FileSystemHandler* self = static_cast<FileSystemHandler*>(param);
+    WriteRequest req;
+
+    while (self->writerTaskRunning_) {
+        if (xQueueReceive(self->writeQueue_, &req, pdMS_TO_TICKS(10)) == pdTRUE) {
+            if (req.data == nullptr) {
+                // Poison pill - exit
+                break;
+            }
+            self->processWriteRequest(req);
+            free(req.data);
+        }
+    }
+
+    vTaskDelete(nullptr);
+}
+
+void FileSystemHandler::processWriteRequest(const WriteRequest& req) {
+    xSemaphoreTake(uploadsMutex_, portMAX_DELAY);
+
+    auto it = uploads_.find(req.transferId);
+    if (it == uploads_.end()) {
+        xSemaphoreGive(uploadsMutex_);
+        return;
+    }
+
+    UploadState& state = it->second;
+
+    if (state.hasError) {
+        xSemaphoreGive(uploadsMutex_);
+        return;
+    }
+
+    size_t bytesWritten = fwrite(req.data, 1, req.size, state.file);
+
+    if (bytesWritten != req.size) {
+        state.hasError = true;
+        state.errorMessage = "Failed to write chunk";
+        xSemaphoreGive(uploadsMutex_);
+        finalizeUpload(req.transferId, false, state.errorMessage);
+        return;
+    }
+
+    state.chunksWritten++;
+    ESP_LOGD(TAG, "Async write chunk %u/%u: %u bytes", state.chunksWritten, state.totalChunks, bytesWritten);
+
+    // Periodic flush
+    if (state.chunksWritten > 0 && state.chunksWritten % 64 == 0) {
+        fflush(state.file);
+    }
+
+    bool shouldFinalize = req.isLastChunk;
+    xSemaphoreGive(uploadsMutex_);
+
+    if (shouldFinalize) {
+        finalizeUpload(req.transferId, true);
+    }
+}
 
 void FileSystemHandler::setSendCallbacks(SendMetadataCallback sendMetadata, SendCallback sendData,
                                          SendCompleteCallback sendComplete,
@@ -52,35 +184,46 @@ void FileSystemHandler::cleanupExpiredTransfers() {
         }
     }
 
+    xSemaphoreTake(uploadsMutex_, portMAX_DELAY);
     auto ulIt = uploads_.begin();
     while (ulIt != uploads_.end()) {
         if (now - ulIt->second.lastActivityTime > FS_TRANSFER_TIMEOUT_MS) {
             if (ulIt->second.file) {
                 fclose(ulIt->second.file);
+                ulIt->second.file = nullptr;
             }
-            remove(ulIt->second.path.c_str());
+            std::string path = ulIt->second.path;
-            ESP_LOGW(TAG, "Upload %u timed out, deleted partial file", ulIt->first);
+            uint32_t chunksReceived = ulIt->second.chunksReceived;
+            int clientId = ulIt->second.clientId;
+            uint32_t transferId = ulIt->first;
+
+            ulIt = uploads_.erase(ulIt);
+            xSemaphoreGive(uploadsMutex_);
+
+            remove(path.c_str());
+            ESP_LOGW(TAG, "Upload %u timed out, deleted partial file", transferId);
 
             if (sendUploadCompleteCallback_) {
                 socket_message_FSUploadComplete complete = socket_message_FSUploadComplete_init_zero;
-                complete.transfer_id = ulIt->first;
+                complete.transfer_id = transferId;
                 complete.success = false;
                 strncpy(complete.error, "Transfer timed out", sizeof(complete.error) - 1);
-                complete.chunks_received = ulIt->second.chunksReceived;
+                complete.chunks_received = chunksReceived;
-                sendUploadCompleteCallback_(complete, ulIt->second.clientId);
+                sendUploadCompleteCallback_(complete, clientId);
             }
 
-            ulIt = uploads_.erase(ulIt);
+            xSemaphoreTake(uploadsMutex_, portMAX_DELAY);
         } else {
             ++ulIt;
         }
     }
+    xSemaphoreGive(uploadsMutex_);
 }
 
 socket_message_FSDeleteResponse FileSystemHandler::handleDelete(const socket_message_FSDeleteRequest& req) {
     socket_message_FSDeleteResponse response = socket_message_FSDeleteResponse_init_zero;
 
-    std::string path = std::string(MOUNT_POINT) + req.path;
+    std::string path = req.path;
     ESP_LOGI(TAG, "Delete request: %s", path.c_str());
 
     struct stat st;
@@ -129,7 +272,7 @@ bool FileSystemHandler::deleteRecursive(const std::string& path) {
 socket_message_FSMkdirResponse FileSystemHandler::handleMkdir(const socket_message_FSMkdirRequest& req) {
     socket_message_FSMkdirResponse response = socket_message_FSMkdirResponse_init_zero;
 
-    std::string path = std::string(MOUNT_POINT) + req.path;
+    std::string path = req.path;
     ESP_LOGI(TAG, "Mkdir request: %s", path.c_str());
 
     struct stat st;
@@ -150,6 +293,15 @@ socket_message_FSMkdirResponse FileSystemHandler::handleMkdir(const socket_messa
 }
 
 void FileSystemHandler::listDirectory(const std::string& path, socket_message_FSListResponse& response) {
+
+    // Root "/" is virtual - list mount points instead
+    if (strcmp(path.c_str(), "/") == 0) {
+        strncpy(response.directories[0].name, LITTLEFS_MOUNT_POINT + 1, sizeof(response.directories[0].name) - 1);
+        strncpy(response.directories[1].name, SD_MOUNT_POINT + 1, sizeof(response.directories[1].name) - 1);
+        response.directories_count = 2;
+        return;
+    }
+
     DIR* dir = opendir(path.c_str());
     if (!dir) {
         return;
@@ -191,15 +343,13 @@ void FileSystemHandler::listDirectory(const std::string& path, socket_message_FS
 socket_message_FSListResponse FileSystemHandler::handleList(const socket_message_FSListRequest& req) {
     socket_message_FSListResponse response = socket_message_FSListResponse_init_zero;
 
-    std::string path = std::string(MOUNT_POINT);
+    std::string path = req.path;
-    if (strlen(req.path) > 0 && req.path[0] != '\0') {
-        path += req.path;
-    }
 
     ESP_LOGI(TAG, "List request: %s", path.c_str());
 
     struct stat st;
-    if (stat(path.c_str(), &st) != 0) {
+    // Make sure that path exists, or that it is a root listing
+    if (strcmp(path.c_str(), "/") != 0 && stat(path.c_str(), &st) != 0) {
         response.success = false;
         strncpy(response.error, "Path not found", sizeof(response.error) - 1);
         return response;
@@ -212,7 +362,7 @@ socket_message_FSListResponse FileSystemHandler::handleList(const socket_message
 }
 
 void FileSystemHandler::handleDownloadRequest(const socket_message_FSDownloadRequest& req, int clientId) {
-    std::string path = std::string(MOUNT_POINT) + req.path;
+    std::string path = req.path;
     ESP_LOGI(TAG, "Download request: %s", path.c_str());
 
     struct stat st;
@@ -308,8 +458,28 @@ bool FileSystemHandler::sendNextDownloadChunk(uint32_t transferId) {
         bytesToRead = state.fileSize - position;
     }
 
-    size_t bytesRead = fread(data->data.bytes, 1, bytesToRead, state.file);
+    // Allocate buffer for FT_POINTER data field
+    data->data = (pb_bytes_array_t*)malloc(PB_BYTES_ARRAY_T_ALLOCSIZE(bytesToRead));
+    if (!data->data) {
+        delete data;
+        if (sendCompleteCallback_) {
+            socket_message_FSDownloadComplete complete = socket_message_FSDownloadComplete_init_zero;
+            complete.transfer_id = transferId;
+            complete.success = false;
+            strncpy(complete.error, "Memory allocation failed", sizeof(complete.error) - 1);
+            complete.total_chunks = state.chunksSent;
+            complete.file_size = state.fileSize;
+            sendCompleteCallback_(complete, state.clientId);
+        }
+        fclose(state.file);
+        downloads_.erase(it);
+        ESP_LOGE(TAG, "Download failed - memory allocation: %u", transferId);
+        return false;
+    }
+
+    size_t bytesRead = fread(data->data->bytes, 1, bytesToRead, state.file);
     if (bytesRead == 0 && bytesToRead > 0) {
+        free(data->data);
         delete data;
         if (sendCompleteCallback_) {
             socket_message_FSDownloadComplete complete = socket_message_FSDownloadComplete_init_zero;
@@ -326,12 +496,13 @@ bool FileSystemHandler::sendNextDownloadChunk(uint32_t transferId) {
         ESP_LOGE(TAG, "Download failed - read error: %u", transferId);
         return false;
     }
-    data->data.size = bytesRead;
+    data->data->size = bytesRead;
 
     if (sendDataCallback_) {
         sendDataCallback_(*data, state.clientId);
     }
 
+    free(data->data);
     delete data;
     state.chunksSent++;
     ESP_LOGD(TAG, "Download chunk %u/%u sent: %u bytes", state.chunksSent, state.totalChunks, bytesRead);
@@ -343,17 +514,22 @@ socket_message_FSUploadStartResponse FileSystemHandler::handleUploadStart(const
                                                                           int clientId) {
     socket_message_FSUploadStartResponse response = socket_message_FSUploadStartResponse_init_zero;
 
-    std::string path = std::string(MOUNT_POINT) + req.path;
+    std::string path = req.path;
     ESP_LOGI(TAG, "Upload start request: %s, size=%u, chunks=%u", path.c_str(), req.file_size, req.total_chunks);
 
-    size_t fs_total = 0, fs_used = 0;
+    // Check available space on the target filesystem
-    esp_littlefs_info("spiffs", &fs_total, &fs_used);
+    if (path.find(SD_MOUNT_POINT) != 0) {
-    size_t freeSpace = fs_total - fs_used;
+        // LittleFS path
-    if (freeSpace < req.file_size + 4096) {
+        size_t fs_total = 0, fs_used = 0;
-        response.success = false;
+        esp_littlefs_info("spiffs", &fs_total, &fs_used);
-        strncpy(response.error, "Insufficient storage space", sizeof(response.error) - 1);
+        size_t freeSpace = fs_total - fs_used;
-        return response;
+        if (freeSpace < req.file_size + 4096) {
+            response.success = false;
+            strncpy(response.error, "Insufficient storage space", sizeof(response.error) - 1);
+            return response;
+        }
     }
+    // TODO: SD card space check skipped - FAT doesn't have a simple API for this
 
     size_t lastSlash = path.find_last_of('/');
     if (lastSlash != std::string::npos && lastSlash > 0) {
@@ -368,11 +544,15 @@ socket_message_FSUploadStartResponse FileSystemHandler::handleUploadStart(const
 
     FILE* file = fopen(path.c_str(), "wb");
     if (!file) {
+        ESP_LOGE(TAG, "fopen failed for '%s': %s (errno=%d)", path.c_str(), strerror(errno), errno);
         response.success = false;
-        strncpy(response.error, "Cannot open file for writing", sizeof(response.error) - 1);
+        snprintf(response.error, sizeof(response.error) - 1, "Cannot open file: %s", strerror(errno));
         return response;
     }
 
+    // Set larger buffer for better write performance
+    setvbuf(file, nullptr, _IOFBF, 32 * 1024);
+
     uint32_t transferId = generateTransferId();
 
     UploadState state;
@@ -381,12 +561,15 @@ socket_message_FSUploadStartResponse FileSystemHandler::handleUploadStart(const
     state.fileSize = req.file_size;
     state.totalChunks = req.total_chunks;
     state.chunksReceived = 0;
+    state.chunksWritten = 0;
     state.bytesReceived = 0;
     state.lastActivityTime = esp_timer_get_time() / 1000;
     state.clientId = clientId;
     state.hasError = false;
 
+    xSemaphoreTake(uploadsMutex_, portMAX_DELAY);
     uploads_[transferId] = state;
+    xSemaphoreGive(uploadsMutex_);
 
     response.success = true;
     response.transfer_id = transferId;
@@ -399,8 +582,16 @@ socket_message_FSUploadStartResponse FileSystemHandler::handleUploadStart(const
 void FileSystemHandler::handleUploadData(const socket_message_FSUploadData& req) {
     uint32_t transferId = req.transfer_id;
 
+    // Auto-start writer task if not running
+    if (!writerTaskRunning_) {
+        startWriterTask();
+    }
+
+    xSemaphoreTake(uploadsMutex_, portMAX_DELAY);
+
     auto it = uploads_.find(transferId);
     if (it == uploads_.end()) {
+        xSemaphoreGive(uploadsMutex_);
         ESP_LOGW(TAG, "Upload data for unknown transfer: %u", transferId);
         return;
     }
@@ -409,6 +600,7 @@ void FileSystemHandler::handleUploadData(const socket_message_FSUploadData& req)
     state.lastActivityTime = esp_timer_get_time() / 1000;
 
     if (state.hasError) {
+        xSemaphoreGive(uploadsMutex_);
         return;
     }
 
@@ -416,32 +608,61 @@ void FileSystemHandler::handleUploadData(const socket_message_FSUploadData& req)
         ESP_LOGW(TAG, "Upload chunk out of order: expected %u, got %u", state.chunksReceived, req.chunk_index);
     }
 
-    size_t bytesWritten = fwrite(req.data.bytes, 1, req.data.size, state.file);
+    if (!req.data || req.data->size == 0) {
-    if (bytesWritten != req.data.size) {
         state.hasError = true;
-        state.errorMessage = "Failed to write chunk";
+        state.errorMessage = "Empty or invalid data chunk";
+        xSemaphoreGive(uploadsMutex_);
         finalizeUpload(transferId, false, state.errorMessage);
         return;
     }
 
-    state.chunksReceived++;
+    // Copy data for async write
-    state.bytesReceived += bytesWritten;
+    WriteRequest writeReq;
+    writeReq.transferId = transferId;
+    writeReq.size = req.data->size;
+    writeReq.chunkIndex = req.chunk_index;
+    writeReq.data = static_cast<uint8_t*>(malloc(req.data->size));
 
-    if (state.chunksReceived > 0 && state.chunksReceived % 64 == 0) {
+    if (!writeReq.data) {
-        ESP_LOGD(TAG, "Flushing file at chunk %u", state.chunksReceived);
+        state.hasError = true;
-        fflush(state.file);
+        state.errorMessage = "Memory allocation failed";
+        xSemaphoreGive(uploadsMutex_);
+        finalizeUpload(transferId, false, state.errorMessage);
+        return;
     }
 
-    ESP_LOGD(TAG, "Upload chunk %u/%u: %u bytes", state.chunksReceived, state.totalChunks, bytesWritten);
+    memcpy(writeReq.data, req.data->bytes, req.data->size);
 
-    if (state.chunksReceived >= state.totalChunks) {
+    state.chunksReceived++;
-        finalizeUpload(transferId, true);
+    state.bytesReceived += req.data->size;
+    writeReq.isLastChunk = (state.chunksReceived >= state.totalChunks);
+
+    ESP_LOGD(TAG, "Queuing chunk %u/%u: %u bytes", state.chunksReceived, state.totalChunks, req.data->size);
+
+    xSemaphoreGive(uploadsMutex_);
+
+    // Check queue is valid
+    if (!writeQueue_) {
+        ESP_LOGE(TAG, "Write queue not initialized");
+        free(writeReq.data);
+        finalizeUpload(transferId, false, "Write queue not initialized");
+        return;
+    }
+
+    // Try to queue (non-blocking) - if full, do sync write to avoid blocking HTTP server
+    if (xQueueSend(writeQueue_, &writeReq, 0) != pdTRUE) {
+        ESP_LOGD(TAG, "Queue full, doing sync write for chunk %u", writeReq.chunkIndex);
+        processWriteRequest(writeReq);
+        free(writeReq.data);
     }
 }
 
 void FileSystemHandler::finalizeUpload(uint32_t transferId, bool success, const std::string& error) {
+    xSemaphoreTake(uploadsMutex_, portMAX_DELAY);
+
     auto it = uploads_.find(transferId);
     if (it == uploads_.end()) {
+        xSemaphoreGive(uploadsMutex_);
         return;
     }
 
@@ -449,13 +670,22 @@ void FileSystemHandler::finalizeUpload(uint32_t transferId, bool success, const
 
     if (state.file) {
         fclose(state.file);
+        state.file = nullptr;
     }
 
+    std::string path = state.path;
+    uint32_t bytesReceived = state.bytesReceived;
+    uint32_t chunksReceived = state.chunksReceived;
+    int clientId = state.clientId;
+
+    uploads_.erase(it);
+    xSemaphoreGive(uploadsMutex_);
+
     if (!success) {
-        remove(state.path.c_str());
+        remove(path.c_str());
-        ESP_LOGW(TAG, "Upload failed, deleted partial file: %s", state.path.c_str());
+        ESP_LOGW(TAG, "Upload failed, deleted partial file: %s", path.c_str());
     } else {
-        ESP_LOGI(TAG, "Upload completed: %s (%u bytes)", state.path.c_str(), state.bytesReceived);
+        ESP_LOGI(TAG, "Upload completed: %s (%u bytes)", path.c_str(), bytesReceived);
     }
 
     if (sendUploadCompleteCallback_) {
@@ -465,11 +695,9 @@ void FileSystemHandler::finalizeUpload(uint32_t transferId, bool success, const
         if (!error.empty()) {
             strncpy(complete.error, error.c_str(), sizeof(complete.error) - 1);
         }
-        complete.chunks_received = state.chunksReceived;
+        complete.chunks_received = chunksReceived;
-        sendUploadCompleteCallback_(complete, state.clientId);
+        sendUploadCompleteCallback_(complete, clientId);
     }
-
-    uploads_.erase(it);
 }
 
 socket_message_FSCancelTransferResponse FileSystemHandler::handleCancelTransfer(
@@ -489,17 +717,22 @@ socket_message_FSCancelTransferResponse FileSystemHandler::handleCancelTransfer(
         return response;
     }
 
+    xSemaphoreTake(uploadsMutex_, portMAX_DELAY);
     auto ulIt = uploads_.find(transferId);
     if (ulIt != uploads_.end()) {
         if (ulIt->second.file) {
             fclose(ulIt->second.file);
+            ulIt->second.file = nullptr;
         }
-        remove(ulIt->second.path.c_str());
+        std::string path = ulIt->second.path;
         uploads_.erase(ulIt);
+        xSemaphoreGive(uploadsMutex_);
+        remove(path.c_str());
         response.success = true;
         ESP_LOGI(TAG, "Upload cancelled: %u", transferId);
         return response;
     }
+    xSemaphoreGive(uploadsMutex_);
 
     response.success = false;
     return response;

esp32/src/idf_component.yml

@@ -16,21 +16,3 @@ dependencies:
     
   espressif/esp32-camera:
     version: "^2.0.0"
-    rules:
-      - if: "idf_version >=5.0.0"
-      - if: "target not in [esp32p4]"
-      
-  espressif/esp_wifi_remote:
-    version: ">=0.3.0"
-    rules:
-      - if: "target in [esp32p4]"
-      
-  espressif/esp_hosted:
-    version: ">=0.0.6"
-    rules:
-      - if: "target in [esp32p4]"
-
-  espressif/esp_cam_sensor:
-    version: ">=0.5.0"
-    rules:
-      - if: "target in [esp32p4]"

esp32/src/main.cpp

@@ -1,6 +1,7 @@
 #include <freertos/FreeRTOS.h>
 #include <freertos/task.h>
 #include <esp_log.h>
+#include <esp_heap_caps.h>
 #include <nvs_flash.h>
 #include <wifi/wifi_idf.h>
 #include <mdns.h>
@@ -21,10 +22,6 @@
 #include <mdns_service.h>
 #include <system_service.h>
 
-#if CONFIG_IDF_TARGET_ESP32P4
-#include <esp_hosted.h>
-#endif
-
 #include <www_mount.hpp>
 
 Websocket wsSocket {server, "/api/ws"};
@@ -46,6 +43,8 @@ WiFiService wifiService;
 APService apService;
 
 void setupServer() {
+    ESP_LOGI("Main", "Free heap before server: %lu, largest block: %lu",
+             esp_get_free_heap_size(), heap_caps_get_largest_free_block(MALLOC_CAP_8BIT));
     server.config(50 + WWW_ASSETS_COUNT, 16384);
     server.listen(80);
 
@@ -59,13 +58,11 @@ void setupServer() {
     server.on("/api/camera/still", HTTP_GET, [&](httpd_req_t *request) { return cameraService.cameraStill(request); });
     server.on("/api/camera/stream", HTTP_GET,
               [&](httpd_req_t *request) { return cameraService.cameraStream(request); });
-#if USE_DVP_CAMERA
     server.on("/api/camera/settings", HTTP_GET,
               [&](httpd_req_t *request) { return cameraService.protoEndpoint.getState(request); });
     server.on("/api/camera/settings", HTTP_POST, [&](httpd_req_t *request, api_Request *protoReq) {
         return cameraService.protoEndpoint.handleStateUpdate(request, protoReq);
     });
-#endif
 #endif
     server.on("/api/servo/config", HTTP_GET,
               [&](httpd_req_t *request) { return servoController.protoEndpoint.getState(request); });
@@ -160,7 +157,7 @@ void setupEventSocket() {
     });
 
     wsSocket.on<socket_message_FSUploadData>(
-        [&](const socket_message_FSUploadData &data, int clientId) { FileSystemWS::fsHandler.handleUploadData(data); });
+        [&](const socket_message_FSUploadData &data, int clientId) { TIME_IT(FileSystemWS::fsHandler.handleUploadData(data), handle_upload) });
 
     using CorrelationHandler =
         std::function<void(const socket_message_CorrelationRequest &, socket_message_CorrelationResponse &, int)>;
@@ -281,21 +278,6 @@ void IRAM_ATTR SpotControlLoopEntry(void *) {
 
 void IRAM_ATTR serviceLoopEntry(void *) {
     ESP_LOGI("main", "Service task starting");
-#if CONFIG_IDF_TARGET_ESP32P4
-    ESP_LOGI("main", "Initializing ESP-Hosted for C6 coprocessor WiFi...");
-    int ret = esp_hosted_init();
-    if (ret != 0) {
-        ESP_LOGE("main", "ESP-Hosted init failed: %d", ret);
-    } else {
-        ESP_LOGI("main", "ESP-Hosted initialized, connecting to C6...");
-        ret = esp_hosted_connect_to_slave();
-        if (ret != 0) {
-            ESP_LOGW("main", "ESP-Hosted connect failed: %d - WiFi may not work", ret);
-        } else {
-            ESP_LOGI("main", "ESP-Hosted link established with C6");
-        }
-    }
-#endif
 
     WiFi.init();
     wifiService.begin();
@@ -312,7 +294,6 @@ void IRAM_ATTR serviceLoopEntry(void *) {
     setupEventSocket();
 
     ESP_LOGI("main", "Service task started");
-
     for (;;) {
         wifiService.loop();
         apService.loop();

esp32/src/peripherals/camera_service.cpp

@@ -6,13 +6,9 @@ namespace Camera {
 
 static const char *const TAG = "CameraService";
 
-#if USE_DVP_CAMERA || USE_CSI_CAMERA
 static constexpr const char *_STREAM_CONTENT_TYPE = "multipart/x-mixed-replace;boundary=" PART_BOUNDARY;
 static constexpr const char *_STREAM_BOUNDARY = "\r\n--" PART_BOUNDARY "\r\n";
 static constexpr const char *_STREAM_PART = "Content-Type: image/jpeg\r\nContent-Length: %u\r\n\r\n";
-#endif
-
-#if USE_DVP_CAMERA
 
 SemaphoreHandle_t cameraMutex = xSemaphoreCreateMutex();
 
@@ -39,8 +35,9 @@ CameraService::CameraService()
     : protoEndpoint(CameraSettings_read, CameraSettings_update, this,
                     API_REQUEST_EXTRACTOR(camera_settings, api_CameraSettings),
                     API_RESPONSE_ASSIGNER(camera_settings, api_CameraSettings)),
-      _persistence(CameraSettings_read, CameraSettings_update, this, CAMERA_SETTINGS_FILE, api_CameraSettings_fields,
+      _persistence(CameraSettings_read, CameraSettings_update, this,
-                   api_CameraSettings_size, CameraSettings_defaults()) {
+                   CAMERA_SETTINGS_FILE, api_CameraSettings_fields, api_CameraSettings_size,
+                   CameraSettings_defaults()) {
     addUpdateHandler([&](const std::string &originId) { updateCamera(); }, false);
 }
 
@@ -49,6 +46,7 @@ esp_err_t CameraService::begin() {
     camera_config_t camera_config;
     camera_config.ledc_channel = LEDC_CHANNEL_0;
     camera_config.ledc_timer = LEDC_TIMER_0;
+#if FT_ENABLED(USE_CAMERA)
     camera_config.pin_d0 = Y2_GPIO_NUM;
     camera_config.pin_d1 = Y3_GPIO_NUM;
     camera_config.pin_d2 = Y4_GPIO_NUM;
@@ -65,6 +63,7 @@ esp_err_t CameraService::begin() {
     camera_config.pin_sccb_scl = SIOC_GPIO_NUM;
     camera_config.pin_pwdn = PWDN_GPIO_NUM;
     camera_config.pin_reset = RESET_GPIO_NUM;
+#endif
     camera_config.xclk_freq_hz = 20000000;
     camera_config.pixel_format = PIXFORMAT_JPEG;
 
@@ -182,463 +181,4 @@ void CameraService::updateCamera() {
     safe_sensor_return();
 }
 
-#elif USE_CSI_CAMERA
-}
-
-extern "C" {
-#include "esp_cam_ctlr.h"
-#include "esp_cam_ctlr_csi.h"
-#include "esp_cam_ctlr_types.h"
-#include "driver/isp.h"
-#include "driver/jpeg_encode.h"
-#include "esp_sccb_intf.h"
-#include "esp_sccb_i2c.h"
-#include "esp_cam_sensor.h"
-#include "ov5647.h"
-#include "esp_ldo_regulator.h"
-#include "driver/isp_demosaic.h"
-#include "driver/isp_bf.h"
-#include "driver/isp_sharpen.h"
-}
-
-#include <peripherals/i2c_bus.h>
-
-namespace Camera {
-
-#ifndef MIPI_CSI_HRES
-#define MIPI_CSI_HRES 640
-#endif
-#ifndef MIPI_CSI_VRES
-#define MIPI_CSI_VRES 480
-#endif
-#ifndef MIPI_CSI_LANE_BITRATE_MBPS
-#define MIPI_CSI_LANE_BITRATE_MBPS 200
-#endif
-#ifndef MIPI_CSI_DATA_LANES
-#define MIPI_CSI_DATA_LANES 2
-#endif
-#ifndef CAM_SCCB_FREQ_HZ
-#define CAM_SCCB_FREQ_HZ 100000
-#endif
-#ifndef CAM_SENSOR_ADDR
-#define CAM_SENSOR_ADDR 0x36
-#endif
-#ifndef CAM_XCLK_PIN
-#define CAM_XCLK_PIN -1
-#endif
-#ifndef CAM_XCLK_FREQ_HZ
-#define CAM_XCLK_FREQ_HZ 25000000
-#endif
-#ifndef CAM_RESET_PIN
-#define CAM_RESET_PIN -1
-#endif
-#ifndef CAM_PWDN_PIN
-#define CAM_PWDN_PIN -1
-#endif
-#ifndef CSI_JPEG_QUALITY
-#define CSI_JPEG_QUALITY 80
-#endif
-
-#define NUM_FRAME_BUFS 2
-static constexpr size_t CACHE_LINE_SIZE = 64;
-#define ALIGN_UP(n, a) (((n) + ((a) - 1)) & ~((a) - 1))
-
-static esp_cam_ctlr_handle_t s_cam_handle = NULL;
-static isp_proc_handle_t s_isp_proc = NULL;
-static jpeg_encoder_handle_t s_jpeg_enc = NULL;
-
-static uint8_t *s_frame_bufs[NUM_FRAME_BUFS] = {};
-static size_t s_frame_buf_size = 0;
-static uint8_t *s_jpeg_bufs[NUM_FRAME_BUFS] = {};
-static size_t s_jpeg_buf_alloc = 0;
-
-static bool s_cam_initialized = false;
-static uint16_t s_frame_hres = MIPI_CSI_HRES;
-static uint16_t s_frame_vres = MIPI_CSI_VRES;
-
-static SemaphoreHandle_t s_frame_done = NULL;
-static SemaphoreHandle_t s_jpeg_lock = NULL;
-static SemaphoreHandle_t s_jpeg_ready = NULL;
-static TaskHandle_t s_capture_task = NULL;
-static volatile bool s_capture_running = false;
-
-static int s_write_idx = 0;
-static int s_ready_idx = -1;
-static size_t s_ready_jpeg_len = 0;
-
-static uint8_t *s_send_buf = NULL;
-static size_t s_send_buf_size = 0;
-
-static bool on_trans_finished(esp_cam_ctlr_handle_t handle, esp_cam_ctlr_trans_t *trans, void *user_data) {
-    BaseType_t woken = pdFALSE;
-    xSemaphoreGiveFromISR(s_frame_done, &woken);
-    return (woken == pdTRUE);
-}
-
-static void capture_task_fn(void *arg) {
-    while (s_capture_running) {
-        int idx = s_write_idx;
-
-        esp_cam_ctlr_trans_t trans = {};
-        trans.buffer = s_frame_bufs[idx];
-        trans.buflen = s_frame_buf_size;
-
-        if (esp_cam_ctlr_receive(s_cam_handle, &trans, 2000) != ESP_OK) {
-            vTaskDelay(pdMS_TO_TICKS(5));
-            continue;
-        }
-
-        if (xSemaphoreTake(s_frame_done, pdMS_TO_TICKS(2000)) != pdTRUE) {
-            continue;
-        }
-
-        jpeg_encode_cfg_t enc_cfg = {};
-        enc_cfg.src_type = JPEG_ENCODE_IN_FORMAT_RGB565;
-        enc_cfg.sub_sample = JPEG_DOWN_SAMPLING_YUV420;
-        enc_cfg.image_quality = CSI_JPEG_QUALITY;
-        enc_cfg.width = s_frame_hres;
-        enc_cfg.height = s_frame_vres;
-
-        uint32_t out_size = 0;
-        esp_err_t err = jpeg_encoder_process(s_jpeg_enc, &enc_cfg, s_frame_bufs[idx], trans.received_size,
-                                             s_jpeg_bufs[idx], s_jpeg_buf_alloc, &out_size);
-        if (err != ESP_OK) {
-            continue;
-        }
-
-        xSemaphoreTake(s_jpeg_lock, portMAX_DELAY);
-        s_ready_idx = idx;
-        s_ready_jpeg_len = out_size;
-        xSemaphoreGive(s_jpeg_lock);
-
-        s_write_idx = (idx + 1) % NUM_FRAME_BUFS;
-
-        xSemaphoreGive(s_jpeg_ready);
-    }
-    vTaskDelete(NULL);
-}
-
-CameraService::CameraService() {
-    s_frame_done = xSemaphoreCreateBinary();
-    s_jpeg_lock = xSemaphoreCreateMutex();
-    s_jpeg_ready = xSemaphoreCreateBinary();
-}
-
-esp_err_t CameraService::begin() {
-    ESP_LOGI(TAG, "Initializing MIPI-CSI camera for ESP32-P4");
-
-    esp_ldo_channel_handle_t ldo_mipi_phy = NULL;
-    esp_ldo_channel_config_t ldo_cfg = {};
-    ldo_cfg.chan_id = 3;
-    ldo_cfg.voltage_mv = 2500;
-    esp_err_t ldo_err = esp_ldo_acquire_channel(&ldo_cfg, &ldo_mipi_phy);
-    if (ldo_err != ESP_OK) {
-        ESP_LOGE(TAG, "Failed to acquire MIPI PHY LDO: %s", esp_err_to_name(ldo_err));
-        return ldo_err;
-    }
-
-    i2c_master_bus_handle_t i2c_bus = I2CBus::instance().busHandle();
-    if (!i2c_bus) {
-        ESP_LOGE(TAG, "I2C bus not initialized, cannot init camera SCCB");
-        return ESP_ERR_INVALID_STATE;
-    }
-
-    esp_sccb_io_handle_t sccb_io = NULL;
-    sccb_i2c_config_t sccb_cfg = {};
-    sccb_cfg.scl_speed_hz = CAM_SCCB_FREQ_HZ;
-    sccb_cfg.device_address = CAM_SENSOR_ADDR;
-    sccb_cfg.dev_addr_length = I2C_ADDR_BIT_LEN_7;
-
-    esp_err_t err = sccb_new_i2c_io(i2c_bus, &sccb_cfg, &sccb_io);
-    if (err != ESP_OK) {
-        ESP_LOGE(TAG, "Failed to create SCCB I/O handle: %s", esp_err_to_name(err));
-        return err;
-    }
-
-    esp_cam_sensor_config_t cam_sensor_cfg = {};
-    cam_sensor_cfg.sccb_handle = sccb_io;
-    cam_sensor_cfg.reset_pin = static_cast<gpio_num_t>(CAM_RESET_PIN);
-    cam_sensor_cfg.pwdn_pin = static_cast<gpio_num_t>(CAM_PWDN_PIN);
-    cam_sensor_cfg.xclk_pin = static_cast<gpio_num_t>(CAM_XCLK_PIN);
-    cam_sensor_cfg.xclk_freq_hz = CAM_XCLK_FREQ_HZ;
-    cam_sensor_cfg.sensor_port = ESP_CAM_SENSOR_MIPI_CSI;
-
-    esp_cam_sensor_device_t *cam_sensor = ov5647_detect(&cam_sensor_cfg);
-    if (!cam_sensor) {
-        ESP_LOGE(TAG, "OV5647 detection failed");
-        return ESP_FAIL;
-    }
-    ESP_LOGI(TAG, "OV5647 camera sensor detected");
-
-    esp_cam_sensor_format_array_t fmt_array = {};
-    err = esp_cam_sensor_query_format(cam_sensor, &fmt_array);
-    if (err != ESP_OK) {
-        ESP_LOGE(TAG, "Failed to query sensor formats: %s", esp_err_to_name(err));
-        return err;
-    }
-
-    const esp_cam_sensor_format_t *selected_format = NULL;
-    uint32_t best_area = 0;
-    for (uint32_t i = 0; i < fmt_array.count; i++) {
-        const auto &f = fmt_array.format_array[i];
-        ESP_LOGI(TAG, "Sensor format[%u]: %dx%d mipi_clk=%uHz lanes=%d", (unsigned)i, f.width, f.height,
-                 (unsigned)f.mipi_info.mipi_clk, f.mipi_info.lane_num);
-    }
-    for (uint32_t i = 0; i < fmt_array.count; i++) {
-        const uint16_t w = fmt_array.format_array[i].width;
-        const uint16_t h = fmt_array.format_array[i].height;
-        if (w <= MIPI_CSI_HRES && h <= MIPI_CSI_VRES) {
-            const uint32_t area = (uint32_t)w * (uint32_t)h;
-            if (!selected_format || area > best_area) {
-                selected_format = &fmt_array.format_array[i];
-                best_area = area;
-            }
-        }
-    }
-    if (!selected_format && fmt_array.count > 0) {
-        uint32_t min_area = UINT32_MAX;
-        for (uint32_t i = 0; i < fmt_array.count; i++) {
-            const uint32_t area =
-                (uint32_t)fmt_array.format_array[i].width * (uint32_t)fmt_array.format_array[i].height;
-            if (area < min_area) {
-                selected_format = &fmt_array.format_array[i];
-                min_area = area;
-            }
-        }
-    }
-    if (!selected_format) {
-        ESP_LOGE(TAG, "No sensor formats available");
-        return ESP_FAIL;
-    }
-
-    err = esp_cam_sensor_set_format(cam_sensor, selected_format);
-    if (err != ESP_OK) {
-        ESP_LOGE(TAG, "Failed to set sensor format: %s", esp_err_to_name(err));
-        return err;
-    }
-
-    s_frame_hres = selected_format->width;
-    s_frame_vres = selected_format->height;
-    ESP_LOGI(TAG, "Sensor format set: %dx%d", s_frame_hres, s_frame_vres);
-
-    esp_cam_sensor_format_t cur_fmt = {};
-    if (esp_cam_sensor_get_format(cam_sensor, &cur_fmt) == ESP_OK) {
-        ESP_LOGI(TAG, "Active format: %dx%d, mipi_clk=%uHz, lanes=%d", cur_fmt.width, cur_fmt.height,
-                 (unsigned)cur_fmt.mipi_info.mipi_clk, cur_fmt.mipi_info.lane_num);
-    }
-
-    int stream_on = 1;
-    err = esp_cam_sensor_ioctl(cam_sensor, ESP_CAM_SENSOR_IOC_S_STREAM, &stream_on);
-    if (err != ESP_OK) {
-        ESP_LOGE(TAG, "Failed to start sensor stream: %s", esp_err_to_name(err));
-        return err;
-    }
-
-    esp_isp_processor_cfg_t isp_cfg = {};
-    isp_cfg.clk_src = static_cast<isp_clk_src_t>(0);
-    isp_cfg.clk_hz = 80 * 1000 * 1000;
-    isp_cfg.input_data_source = ISP_INPUT_DATA_SOURCE_CSI;
-    isp_cfg.input_data_color_type = ISP_COLOR_RAW8;
-    isp_cfg.output_data_color_type = ISP_COLOR_RGB565;
-    isp_cfg.has_line_start_packet = false;
-    isp_cfg.has_line_end_packet = false;
-    isp_cfg.h_res = s_frame_hres;
-    isp_cfg.v_res = s_frame_vres;
-    isp_cfg.bayer_order = COLOR_RAW_ELEMENT_ORDER_GBRG;
-
-    err = esp_isp_new_processor(&isp_cfg, &s_isp_proc);
-    if (err != ESP_OK) {
-        ESP_LOGE(TAG, "ISP processor init failed: %s", esp_err_to_name(err));
-        return err;
-    }
-
-    err = esp_isp_enable(s_isp_proc);
-    if (err != ESP_OK) {
-        ESP_LOGE(TAG, "ISP enable failed: %s", esp_err_to_name(err));
-        return err;
-    }
-
-    esp_isp_demosaic_config_t demosaic_cfg = {};
-    demosaic_cfg.grad_ratio.val = 16;
-    demosaic_cfg.padding_mode = ISP_DEMOSAIC_EDGE_PADDING_MODE_SRND_DATA;
-    esp_isp_demosaic_configure(s_isp_proc, &demosaic_cfg);
-    esp_isp_demosaic_enable(s_isp_proc);
-
-    esp_isp_bf_config_t bf_cfg = {};
-    bf_cfg.denoising_level = 10;
-    bf_cfg.padding_mode = ISP_BF_EDGE_PADDING_MODE_SRND_DATA;
-    uint8_t bf_tpl[ISP_BF_TEMPLATE_X_NUMS][ISP_BF_TEMPLATE_Y_NUMS] = {{1, 2, 1}, {2, 4, 2}, {1, 2, 1}};
-    memcpy(bf_cfg.bf_template, bf_tpl, sizeof(bf_tpl));
-    esp_isp_bf_configure(s_isp_proc, &bf_cfg);
-    esp_isp_bf_enable(s_isp_proc);
-
-    esp_isp_sharpen_config_t sharp_cfg = {};
-    sharp_cfg.h_thresh = 255;
-    sharp_cfg.l_thresh = 20;
-    sharp_cfg.padding_mode = ISP_SHARPEN_EDGE_PADDING_MODE_SRND_DATA;
-    uint8_t sharp_m[ISP_SHARPEN_TEMPLATE_X_NUMS][ISP_SHARPEN_TEMPLATE_Y_NUMS] = {{1, 2, 1}, {2, 4, 2}, {1, 2, 1}};
-    memcpy(sharp_cfg.sharpen_template, sharp_m, sizeof(sharp_m));
-    sharp_cfg.h_freq_coeff.integer = 1;
-    sharp_cfg.h_freq_coeff.decimal = 0;
-    sharp_cfg.m_freq_coeff.integer = 1;
-    sharp_cfg.m_freq_coeff.decimal = 0;
-    esp_isp_sharpen_configure(s_isp_proc, &sharp_cfg);
-    esp_isp_sharpen_enable(s_isp_proc);
-
-    esp_cam_ctlr_csi_config_t csi_cfg = {};
-    csi_cfg.ctlr_id = 0;
-    csi_cfg.h_res = s_frame_hres;
-    csi_cfg.v_res = s_frame_vres;
-    csi_cfg.lane_bit_rate_mbps = MIPI_CSI_LANE_BITRATE_MBPS;
-    csi_cfg.input_data_color_type = CAM_CTLR_COLOR_RAW8;
-    csi_cfg.output_data_color_type = CAM_CTLR_COLOR_RGB565;
-    csi_cfg.data_lane_num = MIPI_CSI_DATA_LANES;
-    csi_cfg.byte_swap_en = false;
-    csi_cfg.queue_items = NUM_FRAME_BUFS;
-
-    err = esp_cam_new_csi_ctlr(&csi_cfg, &s_cam_handle);
-    if (err != ESP_OK) {
-        ESP_LOGE(TAG, "CSI controller init failed: %s", esp_err_to_name(err));
-        return err;
-    }
-
-    esp_cam_ctlr_evt_cbs_t cbs = {};
-    cbs.on_trans_finished = on_trans_finished;
-    err = esp_cam_ctlr_register_event_callbacks(s_cam_handle, &cbs, NULL);
-    if (err != ESP_OK) {
-        ESP_LOGE(TAG, "CSI register callbacks failed: %s", esp_err_to_name(err));
-        return err;
-    }
-
-    s_frame_buf_size = ALIGN_UP((size_t)s_frame_hres * s_frame_vres * 2, CACHE_LINE_SIZE);
-    for (int i = 0; i < NUM_FRAME_BUFS; i++) {
-        s_frame_bufs[i] = (uint8_t *)heap_caps_aligned_alloc(CACHE_LINE_SIZE, s_frame_buf_size, MALLOC_CAP_SPIRAM);
-        if (!s_frame_bufs[i]) {
-            ESP_LOGE(TAG, "Failed to allocate frame buffer %d (%d bytes)", i, (int)s_frame_buf_size);
-            return ESP_ERR_NO_MEM;
-        }
-    }
-
-    jpeg_encode_memory_alloc_cfg_t jpeg_mem_cfg = {};
-    jpeg_mem_cfg.buffer_direction = JPEG_ENC_ALLOC_OUTPUT_BUFFER;
-    for (int i = 0; i < NUM_FRAME_BUFS; i++) {
-        size_t alloc_sz = 0;
-        s_jpeg_bufs[i] = (uint8_t *)jpeg_alloc_encoder_mem(s_frame_hres * s_frame_vres, &jpeg_mem_cfg, &alloc_sz);
-        if (!s_jpeg_bufs[i]) {
-            ESP_LOGE(TAG, "Failed to allocate JPEG buffer %d", i);
-            return ESP_ERR_NO_MEM;
-        }
-        if (i == 0) s_jpeg_buf_alloc = alloc_sz;
-    }
-
-    s_send_buf_size = s_jpeg_buf_alloc;
-    s_send_buf = (uint8_t *)heap_caps_aligned_alloc(CACHE_LINE_SIZE, s_send_buf_size, MALLOC_CAP_SPIRAM);
-    if (!s_send_buf) {
-        ESP_LOGE(TAG, "Failed to allocate send buffer");
-        return ESP_ERR_NO_MEM;
-    }
-
-    jpeg_encode_engine_cfg_t enc_eng_cfg = {};
-    enc_eng_cfg.timeout_ms = 500;
-    err = jpeg_new_encoder_engine(&enc_eng_cfg, &s_jpeg_enc);
-    if (err != ESP_OK) {
-        ESP_LOGE(TAG, "JPEG encoder init failed: %s", esp_err_to_name(err));
-        return err;
-    }
-
-    err = esp_cam_ctlr_enable(s_cam_handle);
-    if (err != ESP_OK) {
-        ESP_LOGE(TAG, "CSI controller enable failed: %s", esp_err_to_name(err));
-        return err;
-    }
-
-    err = esp_cam_ctlr_start(s_cam_handle);
-    if (err != ESP_OK) {
-        ESP_LOGE(TAG, "CSI controller start failed: %s", esp_err_to_name(err));
-        return err;
-    }
-
-    s_cam_initialized = true;
-
-    s_capture_running = true;
-    xTaskCreatePinnedToCore(capture_task_fn, "csi_cap", 4096, NULL, 6, &s_capture_task, 1);
-
-    ESP_LOGI(TAG, "MIPI-CSI camera initialized (%dx%d, %d-lane, %d Mbps)", s_frame_hres, s_frame_vres,
-             MIPI_CSI_DATA_LANES, MIPI_CSI_LANE_BITRATE_MBPS);
-    return ESP_OK;
-}
-
-esp_err_t CameraService::cameraStill(httpd_req_t *request) {
-    if (!s_cam_initialized) {
-        return WebServer::sendError(request, 503, "Camera not initialized");
-    }
-
-    if (xSemaphoreTake(s_jpeg_ready, pdMS_TO_TICKS(3000)) != pdTRUE) {
-        return WebServer::sendError(request, 500, "Camera capture timed out");
-    }
-
-    xSemaphoreTake(s_jpeg_lock, portMAX_DELAY);
-    size_t len = s_ready_jpeg_len;
-    if (s_ready_idx >= 0 && len > 0) {
-        memcpy(s_send_buf, s_jpeg_bufs[s_ready_idx], len);
-    }
-    xSemaphoreGive(s_jpeg_lock);
-
-    if (len == 0) {
-        return WebServer::sendError(request, 500, "No frame available");
-    }
-
-    httpd_resp_set_type(request, "image/jpeg");
-    httpd_resp_set_hdr(request, "Content-Disposition", "inline; filename=capture.jpg");
-    return httpd_resp_send(request, (const char *)s_send_buf, len);
-}
-
-esp_err_t CameraService::cameraStream(httpd_req_t *request) {
-    if (!s_cam_initialized) {
-        return WebServer::sendError(request, 503, "Camera not initialized");
-    }
-
-    httpd_resp_set_type(request, _STREAM_CONTENT_TYPE);
-
-    char part_buf[64];
-    esp_err_t res = ESP_OK;
-
-    while (res == ESP_OK) {
-        if (xSemaphoreTake(s_jpeg_ready, pdMS_TO_TICKS(3000)) != pdTRUE) {
-            break;
-        }
-
-        xSemaphoreTake(s_jpeg_lock, portMAX_DELAY);
-        size_t jpeg_len = s_ready_jpeg_len;
-        if (s_ready_idx >= 0 && jpeg_len > 0) {
-            memcpy(s_send_buf, s_jpeg_bufs[s_ready_idx], jpeg_len);
-        }
-        xSemaphoreGive(s_jpeg_lock);
-
-        if (jpeg_len == 0) continue;
-
-        size_t hlen = snprintf(part_buf, 64, _STREAM_PART, (unsigned int)jpeg_len);
-        res = httpd_resp_send_chunk(request, part_buf, hlen);
-        if (res == ESP_OK) res = httpd_resp_send_chunk(request, (const char *)s_send_buf, jpeg_len);
-        if (res == ESP_OK) res = httpd_resp_send_chunk(request, _STREAM_BOUNDARY, strlen(_STREAM_BOUNDARY));
-    }
-
-    ESP_LOGI(TAG, "Stream ended");
-    httpd_resp_send_chunk(request, NULL, 0);
-    return ESP_OK;
-}
-
-#else
-
-CameraService::CameraService() {}
-esp_err_t CameraService::begin() { return ESP_ERR_NOT_SUPPORTED; }
-esp_err_t CameraService::cameraStill(httpd_req_t *request) {
-    return WebServer::sendError(request, 501, "Camera not supported on this platform");
-}
-esp_err_t CameraService::cameraStream(httpd_req_t *request) {
-    return WebServer::sendError(request, 501, "Camera not supported on this platform");
-}
-
-#endif
-
 } // namespace Camera

esp32/src/system_service.cpp

@@ -8,8 +8,7 @@
 #include <esp_sleep.h>
 #include <soc/soc.h>
 
-#if CONFIG_IDF_TARGET_ESP32S2 || CONFIG_IDF_TARGET_ESP32S3 || CONFIG_IDF_TARGET_ESP32C3 || \
+#if CONFIG_IDF_TARGET_ESP32S2 || CONFIG_IDF_TARGET_ESP32S3 || CONFIG_IDF_TARGET_ESP32C3 || CONFIG_IDF_TARGET_ESP32C6
-    CONFIG_IDF_TARGET_ESP32C6 || CONFIG_IDF_TARGET_ESP32P4
 #include <driver/temperature_sensor.h>
 
 static float temperatureRead() {
@@ -101,7 +100,7 @@ void sleep() {
 
                 uint64_t bitmask = (uint64_t)1 << (WAKEUP_PIN_NUMBER);
 
-#if CONFIG_IDF_TARGET_ESP32C3 || CONFIG_IDF_TARGET_ESP32C6 || CONFIG_IDF_TARGET_ESP32P4
+#ifdef CONFIG_IDF_TARGET_ESP32C3
                 esp_deep_sleep_enable_gpio_wakeup(bitmask, (esp_deepsleep_gpio_wake_up_mode_t)WAKEUP_SIGNAL);
 #else
                 esp_sleep_enable_ext1_wakeup(bitmask, (esp_sleep_ext1_wakeup_mode_t)WAKEUP_SIGNAL);
@@ -125,7 +124,6 @@ static const char *getChipModel() {
         case CHIP_ESP32C2: return "ESP32-C2";
         case CHIP_ESP32C6: return "ESP32-C6";
         case CHIP_ESP32H2: return "ESP32-H2";
-        case CHIP_ESP32P4: return "ESP32-P4";
         default: return "Unknown";
     }
 }

platform_shared/filesystem.options

@@ -16,11 +16,11 @@ socket_message.FSListResponse.directories max_count:20
 # Streaming download messages
 socket_message.FSDownloadRequest.path max_size:256
 socket_message.FSDownloadMetadata.error max_size:128
-socket_message.FSDownloadData.data max_size:16384
+socket_message.FSDownloadData.data type:FT_POINTER
 socket_message.FSDownloadComplete.error max_size:128
 
 # Streaming upload messages
 socket_message.FSUploadStart.path max_size:256
 socket_message.FSUploadStartResponse.error max_size:128
-socket_message.FSUploadData.data max_size:16384
+socket_message.FSUploadData.data type:FT_POINTER
 socket_message.FSUploadComplete.error max_size:128

platformio.ini

@@ -15,7 +15,6 @@ src_dir = esp32/src
 include_dir = esp32/include
 lib_dir = esp32/lib
 test_dir = esp32/test
-boards_dir = boards
 extra_configs = 
 	esp32/factory_settings.ini
 	esp32/features.ini
@@ -55,6 +54,12 @@ build_flags =
 	-D USS_RIGHT_PIN=14
 	-D SDA_PIN=47
 	-D SCL_PIN=21
+    -D SD_CMD_PIN=GPIO_NUM_38
+    -D SD_CLK_PIN=GPIO_NUM_39
+    -D SD_DATA_PIN=GPIO_NUM_40
+
+
+    
 
 [env:seeed-xiao-esp32s3]
 platform = espressif32
@@ -74,45 +79,6 @@ board_build.partitions = esp32/partition_table/min_spiffs.csv
 build_flags =
     ${env.build_flags}
 
-[env:esp32-p4]
-platform = https://github.com/pioarduino/platform-espressif32.git
-framework = espidf
-board = esp32p4_dev
-board_build.mcu = esp32p4
-board_build.f_cpu = 360000000L
-board_build.f_flash = 80000000L
-board_build.f_psram = 200000000L
-board_build.flash_mode = qio
-board_upload.flash_size = 32MB
-board_build.partitions = esp32/partition_table/default_32MB.csv
-board_build.filesystem = littlefs
-board_build.sdkconfig_defaults =
-    esp32/sdkconfig.defaults
-    esp32/sdkconfig.defaults.esp32p4
-upload_speed = 921600
-monitor_speed = 115200
-monitor_filters =
-    direct
-    esp32_exception_decoder
-extra_scripts =
-    pre:esp32/scripts/pre_build.py
-    pre:esp32/scripts/build_app.py
-build_flags =
-    ${env.build_flags}
-    -D USE_CAMERA=1
-    -D CAM_XCLK_PIN=-1
-    -D CAM_RESET_PIN=-1
-    -D CAM_PWDN_PIN=-1
-    -D MIPI_CSI_HRES=800
-    -D MIPI_CSI_VRES=640
-    -D MIPI_CSI_LANE_BITRATE_MBPS=400
-    -D MIPI_CSI_DATA_LANES=2
-    -D CSI_JPEG_QUALITY=65
-    -D BOARD_HAS_PSRAM
-    -D SDA_PIN=7
-    -D SCL_PIN=8
-    -D WS2812_PIN=27
-
 
 ; ================================================================
 ; General environment section
@@ -122,8 +88,8 @@ platform = espressif32 @ 6.8.1
 framework = espidf
 monitor_speed = 115200
 monitor_filters =
-    direct
+	direct
-    esp32_exception_decoder
+	esp32_exception_decoder
 build_flags =
 	${factory_settings.build_flags}
     ${features.build_flags}
@@ -136,6 +102,8 @@ build_flags =
     -fdata-sections
     -Wl,--gc-sections
     -I submodules/nanopb
+    -D PB_FIELD_32BIT=1
+    -D PB_ENABLE_MALLOC=1
     -Wno-missing-braces
     -Wno-format
     -D CONFIG_HTTPD_WS_SUPPORT=1

readme.md

@@ -143,8 +143,8 @@ A PyBullet-based physics simulation is available for algorithm development and r
 
 ```bash
 cd simulation
-uv sync
+pip install -r requirements.txt
-uv run play.py
+python play.py
 ```
 
 Features:
@@ -245,7 +245,7 @@ Complete build instructions are available in the documentation:
 **Build and flash:**
 
 ```bash
-git clone --recurse-submodules https://github.com/runeharlyk/SpotMicroESP32-Leika
+git clone https://github.com/runeharlyk/SpotMicroESP32-Leika
 cd SpotMicroESP32-Leika
 
 cd app
@@ -264,8 +264,8 @@ To experiment with the simulation environments without hardware:
 
 ```bash
 cd simulation
-uv sync
+pip install -r requirements.txt
-uv run play.py
+python play.py
 ```
 
 For development workflows and contribution guidelines, see [docs/6_developing.md](docs/6_developing.md) and [docs/7_contributing.md](docs/7_contributing.md).

simulation/.python-version

@@ -1 +0,0 @@
-3.13

simulation/pyproject.toml

@@ -1,21 +0,0 @@
-[project]
-name = "simulation-leika"
-version = "0.1.0"
-description = "Add your description here"
-readme = "README.md"
-requires-python = ">=3.13"
-dependencies = [
-    "gymnasium>=1.2.3",
-    "matplotlib>=3.10.9",
-    "msgpack>=1.1.2",
-    "numpy>=2.4.4",
-    "onnx>=1.21.0",
-    "onnxruntime>=1.26.0",
-    "pybullet>=3.2.7",
-    "stable-baselines3[extra]>=2.0.0",
-    "tensorboard>=2.20.0",
-    "tensorflow>=2.21.0",
-    "torch>=2.11.0",
-    "tqdm>=4.67.3",
-    "websockets>=16.0",
-]

simulation/requirements.txt

@@ -0,0 +1,14 @@
+torch
+onnx
+onnxruntime
+tensorflow
+numpy
+matplotlib
+pybullet
+websockets
+msgpack
+asyncio
+gymnasium
+stable-baselines3[extra]>=2.0.0
+tensorboard
+tqdm

simulation/test.py

@@ -0,0 +1,24 @@
+from src.envs.quadruped_env import QuadrupedEnv
+from training.model import SimpleNN
+
+import resources as resources
+
+
+def main():
+    env = QuadrupedEnv(resources.getDataPath() + "/spot.urdf")
+    env.reset()
+
+    input_size = env.robot.get_observation().shape[0]
+    output_size = env.robot.get_observation().shape[0]
+    agent = SimpleNN(input_size, output_size)
+
+    done = False
+    observation = []
+
+    while not done:
+        action = agent.select_action(observation)
+        observation, reward, done = env.step(action)
+
+
+if __name__ == "__main__":
+    main()