From bf2fd957af642b70749e2e0781d2c21e50e2a999 Mon Sep 17 00:00:00 2001
From: Rune Harlyk <rune@harlyk.dk>
Date: Mon, 2 Feb 2026 20:42:51 +0100
Subject: [PATCH] =?UTF-8?q?=E2=9C=A8=20Adds=20support=20for=20esp32=20P4?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .vscode/settings.json                         |  2 +-
 boards/esp32p4_dev.json                       | 33 +++++++++++++++
 esp32/include/features.h                      |  1 +
 esp32/include/global.h                        | 21 ++++++++++
 esp32/include/motion_states/walk_state.h      |  1 +
 esp32/include/peripherals/camera_service.h    | 18 ++++++--
 esp32/include/peripherals/led_service.h       |  5 +++
 esp32/include/settings/camera_settings.h      | 11 +++--
 esp32/include/settings/peripherals_settings.h | 10 +++++
 esp32/partition_table/default_32MB.csv        |  8 ++++
 esp32/src/CMakeLists.txt                      | 35 ++++++++++------
 esp32/src/idf_component.yml                   | 13 ++++++
 esp32/src/main.cpp                            | 20 +++++++++
 esp32/src/peripherals/camera_service.cpp      | 25 ++++++++---
 esp32/src/system_service.cpp                  |  6 ++-
 platformio.ini                                | 42 +++++++++++++++++++
 16 files changed, 224 insertions(+), 27 deletions(-)
 create mode 100644 boards/esp32p4_dev.json
 create mode 100644 esp32/partition_table/default_32MB.csv

diff --git a/.vscode/settings.json b/.vscode/settings.json
index 2ff8141..36540a8 100644
--- a/.vscode/settings.json
+++ b/.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",
diff --git a/boards/esp32p4_dev.json b/boards/esp32p4_dev.json
new file mode 100644
index 0000000..417467a
--- /dev/null
+++ b/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"
+}
diff --git a/esp32/include/features.h b/esp32/include/features.h
index c06103f..04c3659 100644
--- a/esp32/include/features.h
+++ b/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"
diff --git a/esp32/include/global.h b/esp32/include/global.h
index 8494410..e801ea5 100644
--- a/esp32/include/global.h
+++ b/esp32/include/global.h
@@ -23,16 +23,37 @@
 #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
 
+#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 100000UL
 #endif
diff --git a/esp32/include/motion_states/walk_state.h b/esp32/include/motion_states/walk_state.h
index 6437f74..cc8ab53 100644
--- a/esp32/include/motion_states/walk_state.h
+++ b/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>
 
diff --git a/esp32/include/peripherals/camera_service.h b/esp32/include/peripherals/camera_service.h
index a9d97f2..44e4364 100644
--- a/esp32/include/peripherals/camera_service.h
+++ b/esp32/include/peripherals/camera_service.h
@@ -11,10 +11,16 @@
 
 namespace Camera {
 
+#if USE_CAMERA && !CONFIG_IDF_TARGET_ESP32P4
 #include <esp_camera.h>
-
-#if USE_CAMERA
 #include <peripherals/camera_pins.h>
+#else
+typedef struct {
+    uint8_t *buf;
+    size_t len;
+} camera_fb_t;
+typedef struct {
+} sensor_t;
 #endif
 
 #define PART_BOUNDARY "frame"
@@ -23,7 +29,11 @@ camera_fb_t *safe_camera_fb_get();
 sensor_t *safe_sensor_get();
 void safe_sensor_return();
 
-class CameraService : public StatefulService<CameraSettings> {
+class CameraService
+#if USE_CAMERA && !CONFIG_IDF_TARGET_ESP32P4
+    : public StatefulService<CameraSettings>
+#endif
+{
   public:
     CameraService();
 
@@ -32,10 +42,12 @@ class CameraService : public StatefulService<CameraSettings> {
     esp_err_t cameraStill(httpd_req_t *request);
     esp_err_t cameraStream(httpd_req_t *request);
 
+#if USE_CAMERA && !CONFIG_IDF_TARGET_ESP32P4
     StatefulProtoEndpoint<CameraSettings, api_CameraSettings> protoEndpoint;
 
   private:
     FSPersistencePB<CameraSettings> _persistence;
     void updateCamera();
+#endif
 };
 } // namespace Camera
diff --git a/esp32/include/peripherals/led_service.h b/esp32/include/peripherals/led_service.h
index 18fd541..b502a45 100644
--- a/esp32/include/peripherals/led_service.h
+++ b/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
diff --git a/esp32/include/settings/camera_settings.h b/esp32/include/settings/camera_settings.h
index 76ac494..d5ed451 100644
--- a/esp32/include/settings/camera_settings.h
+++ b/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;
diff --git a/esp32/include/settings/peripherals_settings.h b/esp32/include/settings/peripherals_settings.h
index f9b186e..24b89c2 100644
--- a/esp32/include/settings/peripherals_settings.h
+++ b/esp32/include/settings/peripherals_settings.h
@@ -1,17 +1,27 @@
 #pragma once
 
 #include <template/state_result.h>
+#include <sdkconfig.h>
 #include <platform_shared/api.pb.h>
 
 /*
  * 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
 #endif
diff --git a/esp32/partition_table/default_32MB.csv b/esp32/partition_table/default_32MB.csv
new file mode 100644
index 0000000..423331f
--- /dev/null
+++ b/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,
diff --git a/esp32/src/CMakeLists.txt b/esp32/src/CMakeLists.txt
index 5699452..bda5e5a 100644
--- a/esp32/src/CMakeLists.txt
+++ b/esp32/src/CMakeLists.txt
@@ -1,3 +1,24 @@
+set(COMPONENT_REQUIRES
+    driver
+    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)
+else()
+    list(APPEND COMPONENT_REQUIRES esp32-camera)
+endif()
+
 idf_component_register(
     SRC_DIRS 
         "."
@@ -10,17 +31,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}
 )
diff --git a/esp32/src/idf_component.yml b/esp32/src/idf_component.yml
index b94e3fb..a48db62 100644
--- a/esp32/src/idf_component.yml
+++ b/esp32/src/idf_component.yml
@@ -16,3 +16,16 @@ 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]"
\ No newline at end of file
diff --git a/esp32/src/main.cpp b/esp32/src/main.cpp
index 591d95c..ca9c2a8 100644
--- a/esp32/src/main.cpp
+++ b/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"};
@@ -275,6 +279,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 +310,7 @@ void IRAM_ATTR serviceLoopEntry(void *) {
     setupEventSocket();
 
     ESP_LOGI("main", "Service task started");
+
     for (;;) {
         wifiService.loop();
         apService.loop();
diff --git a/esp32/src/peripherals/camera_service.cpp b/esp32/src/peripherals/camera_service.cpp
index 7193f25..6d37e36 100644
--- a/esp32/src/peripherals/camera_service.cpp
+++ b/esp32/src/peripherals/camera_service.cpp
@@ -5,6 +5,7 @@
 namespace Camera {
 
 static const char *const TAG = "CameraService";
+#if USE_CAMERA && !CONFIG_IDF_TARGET_ESP32P4
 
 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";
@@ -35,9 +36,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 +46,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 +62,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 +179,21 @@ void CameraService::updateCamera() {
     safe_sensor_return();
 }
 
+#else
+
+camera_fb_t *safe_camera_fb_get() { return nullptr; }
+sensor_t *safe_sensor_get() { return nullptr; }
+void safe_sensor_return() {}
+
+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
diff --git a/esp32/src/system_service.cpp b/esp32/src/system_service.cpp
index 7df7188..4ab4cd4 100644
--- a/esp32/src/system_service.cpp
+++ b/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";
     }
 }
diff --git a/platformio.ini b/platformio.ini
index a1504bc..d556a95 100644
--- a/platformio.ini
+++ b/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,47 @@ 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 WAKEUP_PIN_NUMBER=0
+    -D BOARD_HAS_PSRAM
+    -D USE_BLE=0
+    -D SDA_PIN=7
+    -D SCL_PIN=8
+    -D WS2812_PIN=27
+
 
 ; ================================================================
 ; General environment section