✨ Switch to UV

.vscode/settings.json

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

app/src/lib/components/Visualization.svelte

@@ -262,8 +262,9 @@
         const rotatedXm = settings.xm * cosTotal - settings.zm * sinTotal
         const rotatedZm = settings.xm * sinTotal + settings.zm * cosTotal
 
-        const cosHead = Math.cos(headingYaw)
-        const sinHead = Math.sin(headingYaw)
+        const mpuHeadingRad = degToRad($mpu.heading)
+        const cosHead = Math.cos(mpuHeadingRad)
+        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
 

boards/esp32p4_dev.json

@@ -0,0 +1,33 @@
+{
+  "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_pb.h>
+#include <template/stateful_persistence.h>
 #include <settings/ap_settings.h>
 #include <utils/timing.h>
 #include <wifi/wifi_idf.h>

esp32/include/features.h

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

esp32/include/global.h

@@ -23,16 +23,39 @@
 #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
-#ifndef I2C_FREQUENCY
-#define I2C_FREQUENCY 100000UL
+#endif
+#ifndef I2C_FREQUENCY
+#define I2C_FREQUENCY 1000000UL
 #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_pb.h>
+#include <template/stateful_persistence.h>
 #include <settings/mdns_settings.h>
 #include <utils/timing.h>
 

esp32/include/motion_states/walk_state.h

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

esp32/include/peripherals/camera_service.h

@@ -5,25 +5,31 @@
 #include <features.h>
 #include <template/stateful_service.h>
 #include <template/stateful_proto_endpoint.h>
-#include <template/stateful_persistence_pb.h>
+#include <template/stateful_persistence.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>
-
-#if USE_CAMERA
 #include <peripherals/camera_pins.h>
-#endif
-
-#define PART_BOUNDARY "frame"
 
 camera_fb_t *safe_camera_fb_get();
 sensor_t *safe_sensor_get();
 void safe_sensor_return();
+#endif
 
-class CameraService : public StatefulService<CameraSettings> {
+#define PART_BOUNDARY "frame"
+
+class CameraService
+#if USE_DVP_CAMERA
+    : public StatefulService<CameraSettings>
+#endif
+{
   public:
     CameraService();
 
@@ -32,10 +38,12 @@ class CameraService : public StatefulService<CameraSettings> {
     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,11 +1,12 @@
 #pragma once
 
-#include <driver/i2c.h>
+#include <driver/i2c_master.h>
 #include <esp_log.h>
 #include <freertos/FreeRTOS.h>
 #include <freertos/semphr.h>
 #include <functional>
 #include <vector>
+#include <cstring>
 
 class I2CBus {
   public:
@@ -24,30 +25,21 @@ class I2CBus {
         _scl = scl;
         _freq = freq;
 
-        i2c_config_t conf = {};
-        conf.mode = I2C_MODE_MASTER;
-        conf.sda_io_num = sda;
-        conf.scl_io_num = scl;
-        conf.sda_pullup_en = GPIO_PULLUP_ENABLE;
-        conf.scl_pullup_en = GPIO_PULLUP_ENABLE;
-        conf.master.clk_speed = freq;
+        i2c_master_bus_config_t bus_cfg = {};
+        bus_cfg.i2c_port = port;
+        bus_cfg.sda_io_num = sda;
+        bus_cfg.scl_io_num = scl;
+        bus_cfg.clk_source = I2C_CLK_SRC_DEFAULT;
+        bus_cfg.glitch_ignore_cnt = 7;
+#if CONFIG_IDF_TARGET_ESP32P4
+        bus_cfg.flags.enable_internal_pullup = false;
+#else
+        bus_cfg.flags.enable_internal_pullup = true;
+#endif
 
-        esp_err_t err = i2c_param_config(_port, &conf);
+        esp_err_t err = i2c_new_master_bus(&bus_cfg, &_bus);
         if (err != ESP_OK) {
-            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));
+            ESP_LOGE(TAG, "i2c_new_master_bus failed: %s", esp_err_to_name(err));
             return err;
         }
 
@@ -57,73 +49,51 @@ class I2CBus {
 
     void end() {
         if (_initialized) {
-            i2c_driver_delete(_port);
+            if (_dev) {
+                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;
-
-        i2c_cmd_handle_t cmd = i2c_cmd_link_create();
-        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 err = ensureDevice(addr);
+        if (err != ESP_OK) return err;
+        return i2c_master_transmit(_dev, data, len, pdMS_TO_TICKS(200));
     }
 
     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;
 
-        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_write_byte(cmd, reg, true);
+        uint8_t buf[len + 1];
+        buf[0] = reg;
         if (len > 0 && data != nullptr) {
-            i2c_master_write(cmd, data, len, true);
+            memcpy(buf + 1, data, len);
         }
-        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;
+        return i2c_master_transmit(_dev, buf, len + 1, pdMS_TO_TICKS(200));
     }
 
     esp_err_t readReg(uint8_t addr, uint8_t reg, uint8_t* data, size_t len) {
         if (!_initialized) return ESP_ERR_INVALID_STATE;
-
-        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_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;
+        esp_err_t err = ensureDevice(addr);
+        if (err != ESP_OK) return err;
+        return i2c_master_transmit_receive(_dev, &reg, 1, data, len, pdMS_TO_TICKS(200));
     }
 
     bool probe(uint8_t addr) {
         if (!_initialized) return false;
-
-        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;
+        return i2c_master_probe(_bus, addr, pdMS_TO_TICKS(200)) == ESP_OK;
     }
 
     std::vector<uint8_t> scan(uint8_t lower = 1, uint8_t upper = 127) {
@@ -157,4 +127,23 @@ 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,6 +1,7 @@
 #ifndef LEDService_h
 #define LEDService_h
 
+#include <sdkconfig.h>
 #include <driver/rmt_tx.h>
 #include <led_strip.h>
 #include <led_strip_rmt.h>
@@ -9,8 +10,12 @@
 #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_pb.h>
+#include <template/stateful_persistence.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_pb.h>
+#include <template/stateful_persistence.h>
 #include <template/stateful_proto_endpoint.h>
 #include <template/stateful_service.h>
 #include <utils/math_utils.h>
@@ -24,22 +24,17 @@ 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, "Servo4"},
-        {306, -1, 45, 2.0f, "Servo5"}, {306, -1, -90, 2.0f, "Servo6"},
-        {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"}
-    };
+        {306, -1, 0, 2.0f, "Servo1"}, {306, 1, -45, 2.0f, "Servo2"},  {306, 1, 90, 2.0f, "Servo3"},
+        {306, -1, 0, 2.0f, "Servo4"}, {306, -1, 45, 2.0f, "Servo5"},  {306, -1, -90, 2.0f, "Servo6"},
+        {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;
@@ -52,9 +47,8 @@ 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, api_ServoSettings_size,
-                       ServoSettings_defaults()) {}
+          _persistence(ServoSettings_read, ServoSettings_update, this, SERVO_SETTINGS_FILE, api_ServoSettings_fields,
+                       api_ServoSettings_size, ServoSettings_defaults()) {}
 
     void begin() {
         _persistence.readFromFS();

esp32/include/settings/camera_settings.h

@@ -1,15 +1,20 @@
 #pragma once
 
-#include <template/state_result.h>
+#include <sdkconfig.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,20 +1,9 @@
 #pragma once
 
 #include <template/state_result.h>
+#include <sdkconfig.h>
 #include <platform_shared/api.pb.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
+#include <global.h>
 
 // Use proto types directly
 using PinConfig = api_PinConfig;

esp32/include/template/stateful_persistence.h

@@ -1,36 +1,66 @@
-#ifndef FSPersistence_h
-#define FSPersistence_h
+#pragma once
 
 #include <template/stateful_service.h>
+#include <template/state_result.h>
 #include <filesystem.h>
-#include <ArduinoJson.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 FSPersistence {
+class FSPersistencePB {
   public:
-    FSPersistence(JsonStateReader<T> stateReader, JsonStateUpdater<T> stateUpdater, StatefulService<T> *statefulService,
-                  const char *filePath)
+    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() {
-        std::string content = FileSystem::readFile(_filePath);
+        FILE *file = fopen(_filePath, "rb");
 
-        if (!content.empty()) {
-            JsonDocument jsonDocument;
-            DeserializationError error = deserializeJson(jsonDocument, content);
-            if (error == DeserializationError::Ok) {
-                JsonVariant jsonObject = jsonDocument.as<JsonVariant>();
-                _statefulService->updateWithoutPropagation(jsonObject, _stateUpdater);
+        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();
@@ -38,16 +68,35 @@ class FSPersistence {
     }
 
     bool writeToFS() {
-        JsonDocument jsonDocument;
-        JsonVariant jsonObject = jsonDocument.to<JsonVariant>();
-        _statefulService->read(jsonObject, _stateReader);
+        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();
 
-        std::string content;
-        serializeJson(jsonDocument, content);
+        FILE *file = fopen(_filePath, "wb");
+        if (!file) {
+            ESP_LOGE(TAG_PERSISTENCE, "Failed to open file for writing: %s", _filePath);
+            delete[] buffer;
+            return false;
+        }
 
-        return FileSystem::writeFile(_filePath, content.c_str());
+        size_t written = fwrite(buffer, 1, stream.bytes_written, file);
+        fclose(file);
+        delete[] buffer;
+
+        return written == stream.bytes_written;
     }
 
     void disableUpdateHandler() {
@@ -64,11 +113,13 @@ class FSPersistence {
     }
 
   private:
-    JsonStateReader<T> _stateReader;
-    JsonStateUpdater<T> _stateUpdater;
+    ProtoStateReader _stateReader;
+    ProtoStateUpdater _stateUpdater;
     StatefulService<T> *_statefulService;
     const char *_filePath;
-    size_t _bufferSize;
+    const pb_msgdesc_t *_msgDescriptor;
+    size_t _maxSize;
+    T _defaultState;
     HandlerId _updateHandlerId;
 
     void mkdirs() {
@@ -84,11 +135,7 @@ class FSPersistence {
     }
 
   protected:
-    virtual void applyDefaults() {
-        JsonDocument jsonDocument;
-        JsonVariant jsonObject = jsonDocument.as<JsonVariant>();
-        _statefulService->updateWithoutPropagation(jsonObject, _stateUpdater);
+    void applyDefaults() {
+        _statefulService->updateWithoutPropagation([this](T &state) { return _stateUpdater(_defaultState, state); });
     }
 };
-
-#endif

esp32/include/template/stateful_persistence_pb.h

@@ -1,141 +0,0 @@
-#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/wifi_service.h

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

esp32/partition_table/default_32MB.csv

@@ -0,0 +1,8 @@
+# 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,3 +1,25 @@
+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 
         "."
@@ -10,17 +32,5 @@ idf_component_register(
         "../include"
         "../../submodules/nanopb"
     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
+        ${COMPONENT_REQUIRES}
 )

esp32/src/idf_component.yml

@@ -16,3 +16,21 @@ 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

@@ -21,6 +21,10 @@
 #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"};
@@ -55,11 +59,13 @@ 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); });
@@ -275,6 +281,21 @@ 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();
@@ -291,6 +312,7 @@ void IRAM_ATTR serviceLoopEntry(void *) {
     setupEventSocket();
 
     ESP_LOGI("main", "Service task started");
+
     for (;;) {
         wifiService.loop();
         apService.loop();

esp32/src/peripherals/camera_service.cpp

@@ -6,9 +6,13 @@ 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();
 
@@ -35,9 +39,8 @@ 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, api_CameraSettings_size,
-                   CameraSettings_defaults()) {
+      _persistence(CameraSettings_read, CameraSettings_update, this, CAMERA_SETTINGS_FILE, api_CameraSettings_fields,
+                   api_CameraSettings_size, CameraSettings_defaults()) {
     addUpdateHandler([&](const std::string &originId) { updateCamera(); }, false);
 }
 
@@ -46,7 +49,6 @@ 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;
@@ -63,7 +65,6 @@ 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;
 
@@ -181,4 +182,463 @@ 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,7 +8,8 @@
 #include <esp_sleep.h>
 #include <soc/soc.h>
 
-#if CONFIG_IDF_TARGET_ESP32S2 || CONFIG_IDF_TARGET_ESP32S3 || CONFIG_IDF_TARGET_ESP32C3 || CONFIG_IDF_TARGET_ESP32C6
+#if CONFIG_IDF_TARGET_ESP32S2 || CONFIG_IDF_TARGET_ESP32S3 || CONFIG_IDF_TARGET_ESP32C3 || \
+    CONFIG_IDF_TARGET_ESP32C6 || CONFIG_IDF_TARGET_ESP32P4
 #include <driver/temperature_sensor.h>
 
 static float temperatureRead() {
@@ -100,7 +101,7 @@ void sleep() {
 
                 uint64_t bitmask = (uint64_t)1 << (WAKEUP_PIN_NUMBER);
 
-#ifdef CONFIG_IDF_TARGET_ESP32C3
+#if CONFIG_IDF_TARGET_ESP32C3 || CONFIG_IDF_TARGET_ESP32C6 || CONFIG_IDF_TARGET_ESP32P4
                 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);
@@ -124,6 +125,7 @@ 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";
     }
 }

platformio.ini

@@ -15,6 +15,7 @@ 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
@@ -73,6 +74,45 @@ 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

readme.md

@@ -143,8 +143,8 @@ A PyBullet-based physics simulation is available for algorithm development and r
 
 ```bash
 cd simulation
-pip install -r requirements.txt
-python play.py
+uv sync
+uv run play.py
 ```
 
 Features:
@@ -245,7 +245,7 @@ Complete build instructions are available in the documentation:
 **Build and flash:**
 
 ```bash
-git clone https://github.com/runeharlyk/SpotMicroESP32-Leika
+git clone --recurse-submodules 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
-pip install -r requirements.txt
-python play.py
+uv sync
+uv run 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

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

simulation/pyproject.toml

@@ -0,0 +1,21 @@
+[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

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

simulation/test.py

@@ -1,24 +0,0 @@
-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()