Fix IMU ang MAG, added MAG chart to svelte

app/src/lib/stores/imu.ts

@@ -1,5 +1,5 @@
 import { writable } from 'svelte/store'
-import type { IMU } from '$lib/types/models'
+import type { IMUMsg } from '$lib/types/models'
 
 const maxIMUData = 100
 
@@ -14,11 +14,24 @@ export const imu = (() => {
         bmp_temp: [] as number[]
     })
 
-    const addData = (content: IMU) => {
+    const addData = (content: IMUMsg) => {
         update(data => {
-            ;(Object.keys(content) as (keyof IMU)[]).forEach(key => {
-                data[key] = [...data[key], content[key]].slice(-maxIMUData)
-            })
+            if (content.imu && content.imu[4]) {
+                data.x = [...data.x, content.imu[0]].slice(-maxIMUData)
+                data.y = [...data.y, content.imu[1]].slice(-maxIMUData)
+                data.z = [...data.z, content.imu[2]].slice(-maxIMUData)
+            }
+
+            if (content.mag && content.mag[4]) {
+                data.heading = [...data.heading, content.mag[3]].slice(-maxIMUData)
+            }
+
+            if (content.bmp && content.bmp[3]) {
+                data.pressure = [...data.pressure, content.bmp[0]].slice(-maxIMUData)
+                data.altitude = [...data.altitude, content.bmp[1]].slice(-maxIMUData)
+                data.bmp_temp = [...data.bmp_temp, content.bmp[2]].slice(-maxIMUData)
+            }
+
             return data
         })
     }

app/src/lib/types/models.ts

@@ -154,6 +154,12 @@ export type IMU = {
     pressure: number
 }
 
+export type IMUMsg = {
+    imu: [number, number, number, number, boolean]
+    mag: [number, number, number, number, boolean]
+    bmp: [number, number, number, boolean]
+}
+
 export interface I2CDevice {
     address: number
     part_number: string

app/src/routes/peripherals/i2c/i2c.svelte

@@ -16,6 +16,11 @@
             part_number: 'MPU6050',
             name: 'Six-Axis (Gyro + Accelerometer) MEMS MotionTracking™ Devices'
         },
+        {
+            address: 105,
+            part_number: 'ICM20948',
+            name: 'Nine-Axis (Gyro + Accelerometer + Magnetometer) MEMS MotionTracking™ Device'
+        },
         { address: 115, part_number: 'PAJ7620U2', name: 'Gesture sensor' },
         { address: 119, part_number: 'BMP085', name: 'Temp/Barometric' }
     ]

app/src/routes/peripherals/imu/imu.svelte

@@ -6,7 +6,7 @@
     import { slide } from 'svelte/transition'
     import { onDestroy, onMount } from 'svelte'
     import { socket } from '$lib/stores'
-    import { MessageTopic, type IMU } from '$lib/types/models'
+    import { MessageTopic, type IMUMsg } from '$lib/types/models'
     import { useFeatureFlags } from '$lib/stores/featureFlags'
     import { Rotate3d } from '$lib/components/icons'
 
@@ -18,10 +18,12 @@
     let angleChartElement: HTMLCanvasElement
     let tempChartElement: HTMLCanvasElement
     let altitudeChartElement: HTMLCanvasElement
+    let magnetometerChartElement: HTMLCanvasElement
 
     let angleChart: Chart
     let tempChart: Chart
     let altitudeChart: Chart
+    let magnetometerChart: Chart
 
     const getChartColors = () => {
         const style = getComputedStyle(document.body)
@@ -171,6 +173,37 @@
                 }
             }
         })
+
+        magnetometerChart = new Chart(magnetometerChartElement, {
+            type: 'line',
+            data: {
+                datasets: [
+                    {
+                        label: 'Heading',
+                        borderColor: colors.primary,
+                        backgroundColor: colors.primary,
+                        borderWidth: 2,
+                        data: $imu.heading,
+                        yAxisID: 'y'
+                    }
+                ]
+            },
+            options: {
+                ...baseConfig,
+                scales: {
+                    ...baseConfig.scales,
+                    y: {
+                        ...baseConfig.scales.y,
+                        title: {
+                            display: true,
+                            text: 'Heading [°]',
+                            color: colors.background,
+                            font: { size: 16, weight: 'bold' }
+                        }
+                    }
+                }
+            }
+        })
     }
 
     const updateChartData = (chart: Chart, data: number[]) => {
@@ -194,6 +227,10 @@
             angleChart.update('none')
         }
 
+        if ($features.mag) {
+            updateChartData(magnetometerChart, $imu.heading)
+        }
+
         if ($features.bmp) {
             updateChartData(tempChart, $imu.bmp_temp)
             updateChartData(altitudeChart, $imu.altitude)
@@ -201,7 +238,7 @@
     }
 
     onMount(() => {
-        socket.on(MessageTopic.imu, (data: IMU) => {
+        socket.on(MessageTopic.imu, (data: IMUMsg) => {
             console.log(data)
             imu.addData(data)
         })
@@ -235,6 +272,17 @@
         </div>
     {/if}
 
+    {#if $features.mag}
+        <div class="w-full overflow-x-auto">
+            <div
+                class="flex w-full flex-col space-y-1 h-60"
+                transition:slide|local={{ duration: 300, easing: cubicOut }}
+            >
+                <canvas bind:this={magnetometerChartElement}></canvas>
+            </div>
+        </div>
+    {/if}
+
     {#if $features.bmp}
         <div class="w-full overflow-x-auto">
             <div

esp32/features.ini

@@ -13,6 +13,8 @@ build_flags =
   -D USE_HMC5883=0
   -D USE_BMP180=0
   -D USE_MPU6050=0
+  -D USE_ICM20948=1
+  -D USE_ICM20948_SPIMODE=0
   -D USE_WS2812=1
   -D USE_BNO055=0
   -D USE_USS=0

esp32/include/ap_service.h

@@ -3,8 +3,9 @@
 #include <template/stateful_persistence.h>
 #include <settings/ap_settings.h>
 #include <utils/timing.h>
-#include <WiFi.h>
-#include "esp_timer.h"
+#include <utils/http_utils.h>
+#include <esp_http_server.h>
+#include <esp_timer.h>
 #include <string>
 
 class APService : public StatefulService<APSettings> {
@@ -16,14 +17,13 @@ class APService : public StatefulService<APSettings> {
     void loop();
     void recoveryMode();
 
-    esp_err_t getStatus(PsychicRequest *request);
+    esp_err_t getStatus(httpd_req_t *req);
     void status(JsonObject &root);
     APNetworkStatus getAPNetworkStatus();
 
     StatefulHttpEndpoint<APSettings> endpoint;
 
   private:
-    PsychicHttpServer *_server;
     FSPersistence<APSettings> _persistence;
 
     DNSServer *_dnsServer;

esp32/include/communication/comm_base.hpp

@@ -38,16 +38,16 @@ class CommAdapterBase {
         array.add(event);
         array.add(payload);
 
-        // TODO: Only send to subscribed
-
 #if USE_MSGPACK
         std::string bin;
         serializeMsgPack(doc, bin);
-        send(reinterpret_cast<const uint8_t *>(bin.data()), bin.size(), -1); // TODO: Make CID dynamic
+        xSemaphoreGive(mutex_);
+        send(reinterpret_cast<const uint8_t *>(bin.data()), bin.size(), -1);
 #else
         String out;
         serializeJson(doc, out);
-        send(out.c_str(), cid);
+        xSemaphoreGive(mutex_);
+        send(out.c_str(), -1);
 #endif
     }
 

esp32/include/communication/websocket_adapter.h

@@ -1,8 +1,9 @@
 #ifndef Socket_h
 #define Socket_h
 
-#include <PsychicHttp.h>
+#include <esp_http_server.h>
 #include <template/stateful_service.h>
+#include <utils/websocket_server.h>
 #include <list>
 #include <map>
 #include <vector>
@@ -10,9 +11,9 @@
 
 #include <communication/comm_base.hpp>
 
-class Websocket : CommAdapterBase {
+class Websocket : public CommAdapterBase {
   public:
-    Websocket(PsychicHttpServer &server, const char *route = "/api/ws");
+    Websocket(httpd_handle_t *server, const char *route = "/api/ws");
 
     void begin() override;
 
@@ -20,17 +21,21 @@ class Websocket : CommAdapterBase {
 
     void emit(const char *event, JsonVariant &payload, const char *originId = "", bool onlyToSameOrigin = false);
 
-  private:
-    PsychicWebSocketHandler _socket;
-    PsychicHttpServer &_server;
-    const char *_route;
+    httpd_uri_t *getUriHandler() { return &_ws_uri; }
 
-    void onWSOpen(PsychicWebSocketClient *client);
-    void onWSClose(PsychicWebSocketClient *client);
-    esp_err_t onFrame(PsychicWebSocketRequest *request, httpd_ws_frame *frame);
+  private:
+    websocket::WebSocketServer _socket;
+    httpd_handle_t *_server;
+    const char *_route;
+    httpd_uri_t _ws_uri;
+
+    void onWSOpen(int fd);
+    void onWSClose(int fd);
+    esp_err_t onFrame(httpd_req_t *req, httpd_ws_frame_t *frame);
 
     void send(const uint8_t *data, size_t len, int cid = -1) override;
+
+    static esp_err_t ws_handler_wrapper(httpd_req_t *req);
 };
 
 #endif
-

esp32/include/features.h

@@ -1,9 +1,8 @@
 #ifndef Features_h
 #define Features_h
 
-#include <WiFi.h>
 #include <ArduinoJson.h>
-#include <PsychicHttp.h>
+#include <esp_http_server.h>
 
 #define FT_ENABLED(feature) feature
 
@@ -12,7 +11,7 @@
 #define USE_CAMERA 0
 #endif
 
-// ESP32 IMU on by default
+// ESP32 IMU off by default
 #ifndef USE_MPU6050
 #define USE_MPU6050 0
 #endif
@@ -22,6 +21,14 @@
 #define USE_BNO055 1
 #endif
 
+// ESP32 IMU off by default
+#ifndef USE_ICM20948
+#define USE_ICM20948 0
+#endif
+#ifndef USE_ICM20948_SPIMODE // I2C on by default
+#define USE_ICM20948_SPIMODE 0
+#endif
+
 // ESP32 magnetometer on by default
 #ifndef USE_HMC5883
 #define USE_HMC5883 0
@@ -83,7 +90,7 @@ void printFeatureConfiguration();
 
 void features(JsonObject &root);
 
-esp_err_t getFeatures(PsychicRequest *request);
+esp_err_t getFeatures(httpd_req_t *req);
 
 } // namespace feature_service
 

esp32/include/mdns_service.h

@@ -1,34 +1,15 @@
 #pragma once
 
-#include <PsychicHttp.h>
-#include <ESPmDNS.h>
-#include <template/stateful_service.h>
-#include <template/stateful_endpoint.h>
-#include <template/stateful_persistence.h>
-#include <settings/mdns_settings.h>
-#include <utils/timing.h>
+#include <mdns.h>
+#include <esp_http_server.h>
+#include <utils/http_utils.h>
 #include <string>
 
-class MDNSService : public StatefulService<MDNSSettings> {
-  private:
-    FSPersistence<MDNSSettings> _persistence;
-    bool _started {false};
+namespace mdns_service {
 
-    void reconfigureMDNS();
-    void startMDNS();
-    void stopMDNS();
-    void addServices();
+void begin(const char *hostname);
+void end();
+void addService(const char *service, const char *proto, uint16_t port);
+void addServiceTxt(const char *service, const char *proto, const char *key, const char *value);
 
-  public:
-    MDNSService();
-    ~MDNSService();
-
-    void begin();
-
-    esp_err_t getStatus(PsychicRequest *request);
-    void getStatus(JsonVariant &root);
-
-    static esp_err_t queryServices(PsychicRequest *request, JsonVariant &json);
-
-    StatefulHttpEndpoint<MDNSSettings> endpoint;
-};
+} // namespace mdns_service

esp32/include/peripherals/barometer.h

@@ -38,7 +38,7 @@ struct BarometerMsg : public SensorMessageBase {
 
 class Barometer : public SensorBase<BarometerMsg> {
   public:
-    bool initialize() override {
+    bool initialize(void* _) override {
         _msg.success = _bmp.begin();
         return _msg.success;
     }

esp32/include/peripherals/camera_service.h

@@ -2,8 +2,8 @@
 #define CameraService_h
 
 #include <ArduinoJson.h>
-#include <PsychicHttp.h>
-#include <WiFi.h>
+#include <esp_http_server.h>
+#include <utils/http_utils.h>
 #include <async_worker.h>
 
 #include <features.h>
@@ -35,8 +35,8 @@ class CameraService : public StatefulService<CameraSettings> {
 
     esp_err_t begin();
 
-    esp_err_t cameraStill(PsychicRequest *request);
-    esp_err_t cameraStream(PsychicRequest *request);
+    esp_err_t cameraStill(httpd_req_t *req);
+    esp_err_t cameraStream(httpd_req_t *req);
 
     StatefulHttpEndpoint<CameraSettings> endpoint;
 

esp32/include/peripherals/imu.h

@@ -6,6 +6,10 @@
 #include <ArduinoJson.h>
 #include <utils/math_utils.h>
 
+#if FT_ENABLED(USE_ICM20948)
+#include "ICM_20948.h" 
+#endif
+
 #if FT_ENABLED(USE_MPU6050)
 #include <MPU6050_6Axis_MotionApps612.h>
 #endif
@@ -44,52 +48,114 @@ struct IMUAnglesMsg : public SensorMessageBase {
 
 class IMU : public SensorBase<IMUAnglesMsg> {
   public:
-    bool initialize() override {
+    bool initialize(void* _arg = nullptr) override {
 #if FT_ENABLED(USE_MPU6050)
         _imu.initialize();
         _msg.success = _imu.testConnection();
-        if (!_msg.success) return false;
+        if (!_msg.success) {
+            ESP_LOGE("IMU", "MPU6050 connection test failed");
+            return false;
+        }
         devStatus = _imu.dmpInitialize();
         if (devStatus == 0) {
-            _imu.setDMPEnabled(false);
-            _imu.setDMPConfig1(0x03);
-            _imu.setDMPEnabled(true);
+            _imu.setXGyroOffset(0);
+            _imu.setYGyroOffset(0);
+            _imu.setZGyroOffset(0);
+            _imu.setXAccelOffset(0);
+            _imu.setYAccelOffset(0);
+            _imu.setZAccelOffset(0);
+
             _imu.setI2CMasterModeEnabled(false);
             _imu.setI2CBypassEnabled(true);
             _imu.setSleepEnabled(false);
+            _imu.setRate(1);
+            _imu.resetFIFO();
+            _imu.setDMPEnabled(true);
+
+            ESP_LOGI("IMU", "MPU6050 DMP initialized successfully");
         } else {
-            return false;
+            ESP_LOGE("IMU", "DMP initialization failed (code %d)", devStatus);
+            _msg.success = false;
         }
 #endif
 #if FT_ENABLED(USE_BNO055)
         _msg.success = _imu.begin();
         if (!_msg.success) {
+            ESP_LOGE("IMU", "BNO055 connection test failed");
             return false;
         }
         _imu.setExtCrystalUse(true);
+#endif
+#if FT_ENABLED(USE_ICM20948)
+    #if USE_ICM20948_SPIMODE > 0
+        _imu = (ICM_20948_SPI*)_arg;
+        if (true || !_imu->isConnected()) { _imu->begin(CS_PIN, SPI_PORT); ESP_LOGI("IMU", "Beginning ICM20948 in SPI mode"); }
+    #else
+        _imu = (ICM_20948_I2C*)_arg;
+        if (true || !_imu->isConnected()) { _imu->begin(Wire, 1, 0xFF); ESP_LOGI("IMU", "Beginning ICM20948 in I2C mode"); }
+        
+    #endif
+    if (_imu->status != ICM_20948_Stat_Ok){ return false; }
+    
+    _imu->setSampleMode((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), ICM_20948_Sample_Mode_Continuous);
+    if (_imu->status != ICM_20948_Stat_Ok){ return false; }
+    
+    ICM_20948_fss_t myFSS;
+    myFSS.a = gpm2;
+    myFSS.g = dps250;
+    _imu->setFullScale((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), myFSS);
+    if (_imu->status != ICM_20948_Stat_Ok){ return false; }
+    // TODO: Setup low pass filter config
+    _msg.success = true;
 #endif
         return true;
     }
 
     bool update() override {
-        if (!_msg.success) return false;
+        //if (!_msg.success) return false;
 #if FT_ENABLED(USE_MPU6050)
-        if (_imu.dmpPacketAvailable()) {
-            if (_imu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
-                _imu.dmpGetQuaternion(&q, fifoBuffer);
-                _imu.dmpGetGravity(&gravity, &q);
-                _imu.dmpGetYawPitchRoll(_msg.rpy, &q, &gravity);
-                return true;
-            }
+        uint16_t fifoCount = _imu.getFIFOCount();
+        uint8_t intStatus = _imu.getIntStatus();
+
+        if (intStatus & 0x10) {
+            _imu.resetFIFO();
+            ESP_LOGW("IMU", "FIFO overflow, resetting");
+            return false;
+        }
+
+        if (_imu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
+            _imu.dmpGetQuaternion(&q, fifoBuffer);
+            _imu.dmpGetGravity(&gravity, &q);
+            _imu.dmpGetYawPitchRoll(_msg.rpy, &q, &gravity);
+            return true;
         }
         return false;
 #endif
+#if FT_ENABLED(USE_ICM20948)
+        if (_imu->dataReady())
+        {   
+            _imu->getAGMT();
+            _msg.rpy[0] = _imu->gyrX();
+            _msg.rpy[1] = _imu->gyrY();
+            _msg.rpy[2] = _imu->gyrZ();
+        }
+#endif
 #if FT_ENABLED(USE_BNO055)
         sensors_event_t event;
         _imu.getEvent(&event);
         _msg.rpy[0] = event.orientation.x;
         _msg.rpy[1] = event.orientation.y;
         _msg.rpy[2] = event.orientation.z;
+#endif
+#if FT_ENABLED(USE_ICM20948)
+    #if FT_ENABLED(USE_ICM20948_SPIMODE) > 0
+        #define SPI_PORT SPI // TODO in periphearals_seetings.h
+        #define CS_PIN 2 
+        ICM_20948_SPI _imu;
+    #else
+        //#define WIRE_PORT Wire 
+        ICM_20948_I2C _imu;
+    #endif
 #endif
         return true;
     }
@@ -105,7 +171,7 @@ class IMU : public SensorBase<IMUAnglesMsg> {
   private:
 #if FT_ENABLED(USE_MPU6050)
     MPU6050 _imu;
-    uint8_t devStatus {false};
+    uint8_t devStatus {0};
     Quaternion q;
     uint8_t fifoBuffer[64];
     VectorFloat gravity;
@@ -113,4 +179,14 @@ class IMU : public SensorBase<IMUAnglesMsg> {
 #if FT_ENABLED(USE_BNO055)
     Adafruit_BNO055 _imu {55, 0x29};
 #endif
+#if FT_ENABLED(USE_ICM20948)
+    #if FT_ENABLED(USE_ICM20948_SPIMODE) > 0
+        #define SPI_PORT SPI // TODO in periphearals_seetings.h
+        #define CS_PIN 2 
+        ICM_20948_SPI* _imu;
+    #else
+        //#define WIRE_PORT Wire 
+        ICM_20948_I2C* _imu;
+    #endif
+#endif
 };

esp32/include/peripherals/magnetometer.h

@@ -11,6 +11,7 @@
 
 #include <peripherals/sensor.hpp>
 
+
 struct MagnetometerMsg : public SensorMessageBase {
     float rpy[3] {0, 0, 0};
     float heading {-1};
@@ -38,37 +39,73 @@ struct MagnetometerMsg : public SensorMessageBase {
 
 class Magnetometer : public SensorBase<MagnetometerMsg> {
   public:
-    bool initialize() {
-        msg.success = _mag.begin();
-        return msg.success;
+    bool initialize(void* _arg) override {
+        #if FT_ENABLED(USE_ICM20948)
+            #if USE_ICM20948_SPIMODE > 0
+                _mag = (ICM_20948_SPI*)_arg;
+                if (true || !_mag->isConnected()) { _mag->begin(CS_PIN, SPI_PORT); ESP_LOGI("Magnetometer", "Beginning ICM20948 in SPI mode"); }
+            #else
+                _mag = (ICM_20948_I2C*)_arg;
+                if (true || !_mag->isConnected()) { _mag->begin(Wire, 1, 0xFF); ESP_LOGI("Magnetometer", "Beginning ICM20948 in I2C mode"); }
+                
+            #endif
+            if (_mag->status != ICM_20948_Stat_Ok){ return false; }
+            
+            _mag->startupMagnetometer();
+            if (_mag->status != ICM_20948_Stat_Ok){ return false; }
+            _msg.success = true;
+        #elif FT_ENABLED(USE_HMC5883)
+            _msg.success = _mag.begin();
+        #endif
+        return _msg.success;
     }
 
-    bool update() {
-        if (!msg.success) return false;
-        sensors_event_t event;
-        bool updated = _mag.getEvent(&event);
-        if (!updated) return false;
-        msg.rpy[0] = event.magnetic.x;
-        msg.rpy[1] = event.magnetic.y;
-        msg.rpy[2] = event.magnetic.z;
-        msg.heading = atan2(event.magnetic.y, event.magnetic.x);
-        msg.heading += declinationAngle;
-        if (msg.heading < 0) msg.heading += 2 * PI;
-        if (msg.heading > 2 * PI) msg.heading -= 2 * PI;
-        msg.heading *= 180 / M_PI;
+    bool update() override {
+        if (!_msg.success) return false;
+        #if FT_ENABLED(USE_ICM20948)
+            _mag->getAGMT();
+            if (_mag->status != ICM_20948_Stat_Ok){ return false; }
+            _msg.rpy[0] = _mag->magX();
+            _msg.rpy[1] = _mag->magY();
+            _msg.rpy[2] = _mag->magZ();
+
+        #elif FT_ENABLED(USE_HMC5883)
+            sensors_event_t event;
+            bool updated = _mag.getEvent(&event);
+            if (!updated) return false;
+            _msg.rpy[0] = event.magnetic.x;
+            _msg.rpy[1] = event.magnetic.y;
+            _msg.rpy[2] = event.magnetic.z;
+        #endif
+        _msg.heading = atan2(_msg.rpy[1], _msg.rpy[0]); // atan2(y, x)
+        _msg.heading += declinationAngle;
+        if (_msg.heading < 0) _msg.heading += 2 * PI;
+        if (_msg.heading > 2 * PI) _msg.heading -= 2 * PI;
+        _msg.heading *= 180 / M_PI;
         return true;
     }
 
-    float getMagX() { return msg.rpy[0]; }
+    float getMagX() { return _msg.rpy[0]; }
 
-    float getMagY() { return msg.rpy[1]; }
+    float getMagY() { return _msg.rpy[1]; }
 
-    float getMagZ() { return msg.rpy[2]; }
+    float getMagZ() { return _msg.rpy[2]; }
 
-    float getHeading() { return msg.heading; }
+    float getHeading() { return _msg.heading; }
 
   private:
-    Adafruit_HMC5883_Unified _mag {12345};
-    MagnetometerMsg msg;
+    
+    #if FT_ENABLED(USE_ICM20948)
+        #if FT_ENABLED(USE_ICM20948_SPIMODE) > 0
+            #define SPI_PORT SPI // TODO in periphearals_seetings.h
+            #define CS_PIN 2 
+            ICM_20948_SPI* _mag;
+        #else
+            //#define WIRE_PORT Wire 
+            ICM_20948_I2C* _mag;
+        #endif
+    #elif FT_ENABLED(USE_HMC5883)
+        Adafruit_HMC5883_Unified _mag {12345};
+    #endif
     const float declinationAngle = 0.22;
 };

esp32/include/peripherals/peripherals.h

@@ -87,10 +87,10 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
 
     JsonDocument doc;
     char message[MAX_ESP_IMU_SIZE];
-#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055 || USE_ICM20948)
     IMU _imu;
 #endif
-#if FT_ENABLED(USE_HMC5883)
+#if FT_ENABLED(USE_HMC5883) || FT_ENABLED(USE_ICM20948)
     Magnetometer _mag;
 #endif
 #if FT_ENABLED(USE_BMP180)

esp32/include/peripherals/sensor.hpp

@@ -17,7 +17,7 @@ class SensorBase {
   public:
     SensorBase() {}
 
-    virtual bool initialize() = 0;
+    virtual bool initialize(void* _arg) = 0;
 
     virtual bool update() = 0;
 

esp32/include/system_service.h

@@ -1,23 +1,24 @@
 #pragma once
 
 #include <ESPmDNS.h>
-#include <PsychicHttp.h>
+#include <esp_http_server.h>
 #include <WiFi.h>
-// #include <communication/websocket_adapter.h>
+#include <communication/websocket_adapter.h>
 #include <filesystem.h>
 #include <global.h>
-#include "esp_timer.h"
+#include <esp_timer.h>
+#include <utils/http_utils.h>
 #include <string>
 
 #define MAX_ESP_ANALYTICS_SIZE 2024
 #define EVENT_ANALYTICS "analytics"
 
 namespace system_service {
-esp_err_t handleReset(PsychicRequest *request);
-esp_err_t handleRestart(PsychicRequest *request);
-esp_err_t handleSleep(PsychicRequest *request);
-esp_err_t getStatus(PsychicRequest *request);
-esp_err_t getMetrics(PsychicRequest *request);
+esp_err_t handleReset(httpd_req_t *req);
+esp_err_t handleRestart(httpd_req_t *req);
+esp_err_t handleSleep(httpd_req_t *req);
+esp_err_t getStatus(httpd_req_t *req);
+esp_err_t getMetrics(httpd_req_t *req);
 
 void reset();
 void restart();
@@ -25,7 +26,7 @@ void sleep();
 void status(JsonObject &root);
 void metrics(JsonObject &root);
 
-void emitMetrics();
+void emitMetrics(Websocket &socket);
 
 const char *resetReason(esp_reset_reason_t reason);
 } // namespace system_service

esp32/include/template/stateful_endpoint.h

@@ -1,7 +1,8 @@
 #pragma once
 
-#include <PsychicHttp.h>
+#include <esp_http_server.h>
 #include <template/stateful_service.h>
+#include <utils/http_utils.h>
 
 #include <functional>
 
@@ -20,29 +21,27 @@ class StatefulHttpEndpoint {
                          StatefulService<T> *statefulService)
         : _stateReader(stateReader), _stateUpdater(stateUpdater), _statefulService(statefulService) {}
 
-    esp_err_t handleStateUpdate(PsychicRequest *request, JsonVariant &json) {
+    esp_err_t handleStateUpdate(httpd_req_t *req, JsonVariant &json) {
         JsonVariant jsonObject = json.as<JsonVariant>();
         StateUpdateResult outcome = _statefulService->updateWithoutPropagation(jsonObject, _stateUpdater);
 
-        if (outcome == StateUpdateResult::ERROR)
-            return request->reply(400);
-        else if ((outcome == StateUpdateResult::CHANGED)) {
-            // persist the changes to the FS
+        if (outcome == StateUpdateResult::ERROR) {
+            return http_utils::send_error(req, 400);
+        } else if ((outcome == StateUpdateResult::CHANGED)) {
             _statefulService->callUpdateHandlers(HTTP_ENDPOINT_ORIGIN_ID);
         }
 
-        PsychicJsonResponse response = PsychicJsonResponse(request, false);
-        jsonObject = response.getRoot();
-
+        JsonDocument doc;
+        jsonObject = doc.to<JsonVariant>();
         _statefulService->read(jsonObject, _stateReader);
 
-        return response.send();
+        return http_utils::send_json_response(req, doc);
     }
 
-    esp_err_t getState(PsychicRequest *request) {
-        PsychicJsonResponse response = PsychicJsonResponse(request, false);
-        JsonVariant jsonObject = response.getRoot();
+    esp_err_t getState(httpd_req_t *req) {
+        JsonDocument doc;
+        JsonVariant jsonObject = doc.to<JsonVariant>();
         _statefulService->read(jsonObject, _stateReader);
-        return response.send();
+        return http_utils::send_json_response(req, doc);
     }
 };

esp32/include/utils/http_utils.h

@@ -0,0 +1,24 @@
+#pragma once
+
+#include <esp_http_server.h>
+#include <ArduinoJson.h>
+
+namespace http_utils {
+
+esp_err_t send_json_response(httpd_req_t *req, JsonDocument &doc, int status_code = 200);
+
+esp_err_t send_error(httpd_req_t *req, int status_code, const char *message = nullptr);
+
+esp_err_t send_empty_response(httpd_req_t *req, int status_code = 200);
+
+esp_err_t add_cors_headers(httpd_req_t *req);
+
+esp_err_t add_standard_headers(httpd_req_t *req);
+
+esp_err_t parse_json_body(httpd_req_t *req, JsonDocument &doc);
+
+esp_err_t handle_options_cors(httpd_req_t *req);
+
+const char *get_client_ip(httpd_req_t *req);
+
+} // namespace http_utils

esp32/include/utils/websocket_server.h

@@ -0,0 +1,56 @@
+#pragma once
+
+#include <esp_http_server.h>
+#include <map>
+#include <list>
+#include <functional>
+
+namespace websocket {
+
+struct WebSocketClient {
+    int fd;
+    uint64_t last_seen;
+};
+
+typedef std::function<void(int fd)> ClientCallback;
+typedef std::function<esp_err_t(httpd_req_t *req, httpd_ws_frame_t *frame)> FrameCallback;
+
+class WebSocketServer {
+  public:
+    WebSocketServer();
+    ~WebSocketServer();
+
+    void setOpenCallback(ClientCallback callback);
+    void setCloseCallback(ClientCallback callback);
+    void setFrameCallback(FrameCallback callback);
+
+    esp_err_t handleWebSocket(httpd_req_t *req);
+
+    void addClient(int fd);
+    void removeClient(int fd);
+    WebSocketClient *getClient(int fd);
+
+    esp_err_t sendText(int fd, const char *data, size_t len);
+    esp_err_t sendBinary(int fd, const uint8_t *data, size_t len);
+    esp_err_t sendToAll(httpd_ws_type_t type, const uint8_t *data, size_t len);
+
+    bool hasClients() const { return !_clients.empty(); }
+    size_t clientCount() const { return _clients.size(); }
+
+  public:
+    void setServer(httpd_handle_t server) { _server = server; }
+
+  private:
+    std::map<int, WebSocketClient> _clients;
+    ClientCallback _onOpen;
+    ClientCallback _onClose;
+    FrameCallback _onFrame;
+    SemaphoreHandle_t _mutex;
+    httpd_handle_t _server;
+
+    friend esp_err_t websocket_handler(httpd_req_t *req);
+};
+
+esp_err_t websocket_handler(httpd_req_t *req);
+
+} // namespace websocket

esp32/include/wifi_service.h

@@ -1,12 +1,12 @@
 #pragma once
 
-#include <PsychicHttp.h>
-#include <WiFi.h>
-#include <ESPmDNS.h>
+#include <esp_http_server.h>
+#include <esp_wifi.h>
 #include <string>
 
 #include <filesystem.h>
 #include <utils/timing.h>
+#include <utils/http_utils.h>
 #include <template/stateful_service.h>
 #include <template/stateful_persistence.h>
 #include <template/stateful_endpoint.h>
@@ -43,9 +43,9 @@ class WiFiService : public StatefulService<WiFiSettings> {
 
     const char *getHostname() { return state().hostname.c_str(); }
 
-    static esp_err_t handleScan(PsychicRequest *request);
-    static esp_err_t getNetworks(PsychicRequest *request);
-    static esp_err_t getNetworkStatus(PsychicRequest *request);
+    static esp_err_t handleScan(httpd_req_t *req);
+    static esp_err_t getNetworks(httpd_req_t *req);
+    static esp_err_t getNetworkStatus(httpd_req_t *req);
 
     StatefulHttpEndpoint<WiFiSettings> endpoint;
 };

esp32/include/www_mount.hpp

@@ -1,5 +1,5 @@
 #pragma once
-#include <PsychicHttp.h>
+#include <esp_http_server.h>
 #include "WWWData.h"
 
-void mountStaticAssets(PsychicHttpServer& s);
+void mountStaticAssets(httpd_handle_t server);

esp32/src/ap_service.cpp

@@ -1,4 +1,5 @@
 #include <ap_service.h>
+#include <WiFi.h>
 
 static const char *TAG = "APService";
 
@@ -12,11 +13,11 @@ APService::~APService() {}
 
 void APService::begin() { _persistence.readFromFS(); }
 
-esp_err_t APService::getStatus(PsychicRequest *request) {
-    PsychicJsonResponse response = PsychicJsonResponse(request, false);
-    JsonObject root = response.getRoot();
+esp_err_t APService::getStatus(httpd_req_t *req) {
+    JsonDocument doc;
+    JsonObject root = doc.to<JsonObject>();
     status(root);
-    return response.send();
+    return http_utils::send_json_response(req, doc);
 }
 
 void APService::status(JsonObject &root) {

esp32/src/communication/websocket_adapter.cpp

@@ -3,13 +3,25 @@
 
 static const char *TAG = "Websocket";
 
-Websocket::Websocket(PsychicHttpServer &server, const char *route) : _server(server), _route(route) {
-    _socket.onOpen((std::bind(&Websocket::onWSOpen, this, std::placeholders::_1)));
-    _socket.onClose(std::bind(&Websocket::onWSClose, this, std::placeholders::_1));
-    _socket.onFrame(std::bind(&Websocket::onFrame, this, std::placeholders::_1, std::placeholders::_2));
+Websocket::Websocket(httpd_handle_t *server, const char *route) : _server(server), _route(route) {
+    _socket.setOpenCallback([this](int fd) { this->onWSOpen(fd); });
+    _socket.setCloseCallback([this](int fd) { this->onWSClose(fd); });
+    _socket.setFrameCallback([this](httpd_req_t *req, httpd_ws_frame_t *frame) { return this->onFrame(req, frame); });
+
+    _ws_uri.uri = _route;
+    _ws_uri.method = HTTP_GET;
+    _ws_uri.handler = ws_handler_wrapper;
+    _ws_uri.user_ctx = this;
+    _ws_uri.is_websocket = true;
+    _ws_uri.handle_ws_control_frames = false;
 }
 
-void Websocket::begin() { _server.on(_route, &_socket); }
+void Websocket::begin() {
+    if (_server && *_server) {
+        _socket.setServer(*_server);
+        httpd_register_uri_handler(*_server, &_ws_uri);
+    }
+}
 
 void Websocket::onEvent(std::string event, EventCallback callback) {
     CommAdapterBase::onEvent(std::move(event), std::move(callback));
@@ -19,38 +31,44 @@ void Websocket::emit(const char *event, JsonVariant &payload, const char *origin
     CommAdapterBase::emit(event, payload, originId, onlyToSameOrigin);
 }
 
-void Websocket::onWSOpen(PsychicWebSocketClient *client) {
-    ESP_LOGI("EventSocket", "ws[%s][%u] connect", client->remoteIP().toString().c_str(), client->socket());
-    ping(client->socket());
+void Websocket::onWSOpen(int fd) {
+    ESP_LOGI(TAG, "ws[%d] connect", fd);
+    ping(fd);
 }
 
-void Websocket::onWSClose(PsychicWebSocketClient *client) {
+void Websocket::onWSClose(int fd) {
     xSemaphoreTake(mutex_, portMAX_DELAY);
     for (auto &event_subscriptions : client_subscriptions) {
-        event_subscriptions.second.remove(client->socket());
+        event_subscriptions.second.remove(fd);
     }
     xSemaphoreGive(mutex_);
-    ESP_LOGI("EventSocket", "ws[%s][%u] disconnect", client->remoteIP().toString().c_str(), client->socket());
+    ESP_LOGI(TAG, "ws[%d] disconnect", fd);
 }
 
-esp_err_t Websocket::onFrame(PsychicWebSocketRequest *request, httpd_ws_frame *frame) {
-    ESP_LOGV(TAG, "ws[%s][%u] opcode[%d]", request->client()->remoteIP().toString().c_str(),
-             request->client()->socket(), frame->type);
+esp_err_t Websocket::onFrame(httpd_req_t *req, httpd_ws_frame_t *frame) {
+    int fd = httpd_req_to_sockfd(req);
+    ESP_LOGV(TAG, "ws[%d] opcode[%d]", fd, frame->type);
 
     if (frame->type != HTTPD_WS_TYPE_TEXT && frame->type != HTTPD_WS_TYPE_BINARY) {
         ESP_LOGE(TAG, "Unsupported frame type: %d", frame->type);
         return ESP_OK;
     }
 
-#if USE_MSGPACK
+#if USE_PROTOBUF
     if (frame->type == HTTPD_WS_TYPE_BINARY) {
-        handleIncoming(frame->payload, frame->len, request->client()->socket());
+        handleIncoming(frame->payload, frame->len, fd);
+    } else {
+        ESP_LOGE(TAG, "Expected binary, got text");
+    }
+#elif USE_MSGPACK
+    if (frame->type == HTTPD_WS_TYPE_BINARY) {
+        handleIncoming(frame->payload, frame->len, fd);
     } else {
         ESP_LOGE(TAG, "Expected binary, got text");
     }
 #else
     if (frame->type == HTTPD_WS_TYPE_TEXT) {
-        handleIncoming(frame->payload, frame->len, request->client()->socket());
+        handleIncoming(frame->payload, frame->len, fd);
     } else {
         ESP_LOGE(TAG, "Expected text, got binary");
     }
@@ -60,22 +78,35 @@ esp_err_t Websocket::onFrame(PsychicWebSocketRequest *request, httpd_ws_frame *f
 }
 
 void Websocket::send(const uint8_t *data, size_t len, int cid) {
+    if (!_server || !*_server) {
+        ESP_LOGW(TAG, "Server not initialized, cannot send");
+        return;
+    }
+
+    httpd_ws_type_t type;
+#if USE_PROTOBUF || USE_MSGPACK
+    type = HTTPD_WS_TYPE_BINARY;
+#else
+    type = HTTPD_WS_TYPE_TEXT;
+#endif
+
     if (cid != -1) {
-        auto *client = _socket.getClient(cid);
-        if (client) {
-            ESP_LOGV(TAG, "Sending to client %s: %s", client->remoteIP().toString().c_str(), data);
-#if USE_MSGPACK
-            client->sendMessage(HTTPD_WS_TYPE_BINARY, data, len);
-#else
-            client->sendMessage(HTTPD_WS_TYPE_TEXT, data, len);
-#endif
-        }
+        ESP_LOGV(TAG, "Sending to client %d: %.*s", cid, (int)len, data);
+
+        httpd_ws_frame_t ws_pkt;
+        memset(&ws_pkt, 0, sizeof(httpd_ws_frame_t));
+        ws_pkt.payload = (uint8_t *)data;
+        ws_pkt.len = len;
+        ws_pkt.type = type;
+
+        httpd_ws_send_frame_async(*_server, cid, &ws_pkt);
     } else {
-        ESP_LOGV(TAG, "Sending to all clients: %s", data);
-#if USE_MSGPACK
-        _socket.sendAll(HTTPD_WS_TYPE_BINARY, data, len);
-#else
-        _socket.sendAll(HTTPD_WS_TYPE_TEXT, data, len);
-#endif
+        ESP_LOGV(TAG, "Sending to all clients: %.*s", (int)len, data);
+        _socket.sendToAll(type, data, len);
     }
 }
+
+esp_err_t Websocket::ws_handler_wrapper(httpd_req_t *req) {
+    Websocket *socket = (Websocket *)req->user_ctx;
+    return socket->_socket.handleWebSocket(req);
+}

esp32/src/features.cpp

@@ -1,4 +1,5 @@
 #include <features.h>
+#include <utils/http_utils.h>
 
 namespace feature_service {
 
@@ -12,11 +13,14 @@ void printFeatureConfiguration() {
     ESP_LOGI("Features", "USE_MOTION: %s", USE_MOTION ? "enabled" : "disabled");
 
     // Sensors
+    ESP_LOGI("Features", "USE_ICM20948: %s", USE_ICM20948 ? "enabled" : "disabled");
     ESP_LOGI("Features", "USE_BNO055: %s", USE_BNO055 ? "enabled" : "disabled");
     ESP_LOGI("Features", "USE_MPU6050: %s", USE_MPU6050 ? "enabled" : "disabled");
     ESP_LOGI("Features", "USE_HMC5883: %s", USE_HMC5883 ? "enabled" : "disabled");
     ESP_LOGI("Features", "USE_BMP180: %s", USE_BMP180 ? "enabled" : "disabled");
     ESP_LOGI("Features", "USE_USS: %s", USE_USS ? "enabled" : "disabled");
+    ESP_LOGI("Features", "USE_PAJ7620U2: %s", USE_PAJ7620U2 ? "enabled" : "disabled");
+    
 
     // Peripherals
     ESP_LOGI("Features", "USE_PCA9685: %s", USE_PCA9685 ? "enabled" : "disabled");
@@ -31,8 +35,8 @@ void printFeatureConfiguration() {
 
 void features(JsonObject &root) {
     root["camera"] = USE_CAMERA ? true : false;
-    root["imu"] = (USE_MPU6050 || USE_BNO055) ? true : false;
-    root["mag"] = (USE_HMC5883 || USE_BNO055) ? true : false;
+    root["imu"] = (USE_MPU6050 || USE_BNO055 || USE_ICM20948) ? true : false;
+    root["mag"] = (USE_HMC5883 || USE_BNO055 || USE_ICM20948) ? true : false;
     root["bmp"] = USE_BMP180 ? true : false;
     root["sonar"] = USE_USS ? true : false;
     root["servo"] = USE_PCA9685 ? true : false;
@@ -45,11 +49,11 @@ void features(JsonObject &root) {
     root["variant"] = KINEMATICS_VARIANT_STR;
 }
 
-esp_err_t getFeatures(PsychicRequest *request) {
-    PsychicJsonResponse response = PsychicJsonResponse(request, false);
-    JsonObject root = response.getRoot();
+esp_err_t getFeatures(httpd_req_t *req) {
+    JsonDocument doc;
+    JsonObject root = doc.to<JsonObject>();
     features(root);
-    return response.send();
+    return http_utils::send_json_response(req, doc);
 }
 
 } // namespace feature_service

esp32/src/main.cpp

@@ -1,8 +1,6 @@
-#include <Arduino.h>
-#include <PsychicHttp.h>
-#include <ESPmDNS.h>
-#include <WiFi.h>
-#include <Wire.h>
+#include <esp_http_server.h>
+#include <esp_log.h>
+#include <driver/uart.h>
 
 #include <filesystem.h>
 #include <peripherals/peripherals.h>
@@ -16,14 +14,13 @@
 #include <ap_service.h>
 #include <mdns_service.h>
 #include <system_service.h>
+#include <utils/http_utils.h>
 
 #include <www_mount.hpp>
 
-// Communication
-PsychicHttpServer server;
-Websocket socket {server, "/api/ws"};
+httpd_handle_t server = NULL;
+Websocket socket(&server, "/api/ws");
 
-// Core
 Peripherals peripherals;
 ServoController servoController;
 MotionService motionService;
@@ -34,47 +31,9 @@ LEDService ledService;
 Camera::CameraService cameraService;
 #endif
 
-// Service
 WiFiService wifiService;
 APService apService;
 
-void setupServer() {
-    server.config.max_uri_handlers = 10 + WWW_ASSETS_COUNT;
-    server.maxUploadSize = 1000000; // 1 MB;
-    server.listen(80);
-    server.serveStatic("/api/config/", ESP_FS, "/config/");
-    server.on("/api/features", feature_service::getFeatures);
-#if USE_CAMERA
-    server.on("/api/camera/still", HTTP_GET,
-              [&](PsychicRequest *request) { return cameraService.cameraStill(request); });
-    server.on("/api/camera/stream", HTTP_GET,
-              [&](PsychicRequest *request) { return cameraService.cameraStream(request); });
-    server.on("/api/camera/settings", HTTP_GET,
-              [&](PsychicRequest *request) { return cameraService.endpoint.getState(request); });
-    server.on("/api/camera/settings", HTTP_POST, [&](PsychicRequest *request, JsonVariant &json) {
-        return cameraService.endpoint.handleStateUpdate(request, json);
-    });
-#endif
-    server.on("/api/servo/config", HTTP_GET,
-              [&](PsychicRequest *request) { return servoController.endpoint.getState(request); });
-    server.on("/api/servo/config", HTTP_POST, [&](PsychicRequest *request, JsonVariant &json) {
-        return servoController.endpoint.handleStateUpdate(request, json);
-    });
-#if EMBED_WEBAPP
-    mountStaticAssets(server);
-#endif
-    server.on("/*", HTTP_OPTIONS, [](PsychicRequest *request) { // CORS handling
-        PsychicResponse response(request);
-        response.setCode(200);
-        return response.send();
-    });
-    DefaultHeaders::Instance().addHeader("Server", APP_NAME);
-    DefaultHeaders::Instance().addHeader("Access-Control-Allow-Origin", "*");
-    DefaultHeaders::Instance().addHeader("Access-Control-Allow-Headers", "Accept, Content-Type, Authorization");
-    DefaultHeaders::Instance().addHeader("Access-Control-Allow-Methods", "GET, POST, OPTIONS");
-    DefaultHeaders::Instance().addHeader("Access-Control-Max-Age", "86400");
-}
-
 #define ANGLES_EVENT "angles"
 #define INPUT_EVENT "input"
 #define MODE_EVENT "mode"
@@ -83,8 +42,103 @@ void setupServer() {
 #define EVENT_SERVO_CONFIGURATION_SETTINGS "servoPWM"
 #define EVENT_SERVO_STATE "servoState"
 
+esp_err_t cors_options_handler(httpd_req_t *req) { return http_utils::handle_options_cors(req); }
+
+esp_err_t servo_config_get_handler(httpd_req_t *req) { return servoController.endpoint.getState(req); }
+
+esp_err_t servo_config_post_handler(httpd_req_t *req) {
+    JsonDocument doc;
+    if (http_utils::parse_json_body(req, doc) != ESP_OK) {
+        return http_utils::send_error(req, 400, "Invalid JSON");
+    }
+    JsonVariant json = doc.as<JsonVariant>();
+    return servoController.endpoint.handleStateUpdate(req, json);
+}
+
+#if USE_CAMERA
+esp_err_t camera_still_handler(httpd_req_t *req) { return cameraService.cameraStill(req); }
+
+esp_err_t camera_stream_handler(httpd_req_t *req) { return cameraService.cameraStream(req); }
+
+esp_err_t camera_settings_get_handler(httpd_req_t *req) { return cameraService.endpoint.getState(req); }
+
+esp_err_t camera_settings_post_handler(httpd_req_t *req) {
+    JsonDocument doc;
+    if (http_utils::parse_json_body(req, doc) != ESP_OK) {
+        return http_utils::send_error(req, 400, "Invalid JSON");
+    }
+    JsonVariant json = doc.as<JsonVariant>();
+    return cameraService.endpoint.handleStateUpdate(req, json);
+}
+#endif
+
+void setupServer() {
+    httpd_config_t config = HTTPD_DEFAULT_CONFIG();
+    config.max_uri_handlers = 20 + WWW_ASSETS_COUNT;
+    config.stack_size = 8192;
+    config.max_open_sockets = 7;
+    config.lru_purge_enable = true;
+
+    if (httpd_start(&server, &config) == ESP_OK) {
+        ESP_LOGI("main", "HTTP server started");
+
+        httpd_uri_t config_static = {.uri = "/api/config/*",
+                                     .method = HTTP_GET,
+                                     .handler = [](httpd_req_t *req) -> esp_err_t {
+                                         return http_utils::send_error(req, 501,
+                                                                       "Static file serving not yet implemented");
+                                     },
+                                     .user_ctx = nullptr};
+        httpd_register_uri_handler(server, &config_static);
+
+        httpd_uri_t features_uri = {
+            .uri = "/api/features", .method = HTTP_GET, .handler = feature_service::getFeatures, .user_ctx = nullptr};
+        httpd_register_uri_handler(server, &features_uri);
+
+#if USE_CAMERA
+        httpd_uri_t camera_still_uri = {
+            .uri = "/api/camera/still", .method = HTTP_GET, .handler = camera_still_handler, .user_ctx = nullptr};
+        httpd_register_uri_handler(server, &camera_still_uri);
+
+        httpd_uri_t camera_stream_uri = {
+            .uri = "/api/camera/stream", .method = HTTP_GET, .handler = camera_stream_handler, .user_ctx = nullptr};
+        httpd_register_uri_handler(server, &camera_stream_uri);
+
+        httpd_uri_t camera_settings_get_uri = {.uri = "/api/camera/settings",
+                                               .method = HTTP_GET,
+                                               .handler = camera_settings_get_handler,
+                                               .user_ctx = nullptr};
+        httpd_register_uri_handler(server, &camera_settings_get_uri);
+
+        httpd_uri_t camera_settings_post_uri = {.uri = "/api/camera/settings",
+                                                .method = HTTP_POST,
+                                                .handler = camera_settings_post_handler,
+                                                .user_ctx = nullptr};
+        httpd_register_uri_handler(server, &camera_settings_post_uri);
+#endif
+
+        httpd_uri_t servo_config_get_uri = {
+            .uri = "/api/servo/config", .method = HTTP_GET, .handler = servo_config_get_handler, .user_ctx = nullptr};
+        httpd_register_uri_handler(server, &servo_config_get_uri);
+
+        httpd_uri_t servo_config_post_uri = {
+            .uri = "/api/servo/config", .method = HTTP_POST, .handler = servo_config_post_handler, .user_ctx = nullptr};
+        httpd_register_uri_handler(server, &servo_config_post_uri);
+
+#if EMBED_WEBAPP
+        mountStaticAssets(server);
+#endif
+
+        httpd_uri_t options_uri = {
+            .uri = "/*", .method = HTTP_OPTIONS, .handler = cors_options_handler, .user_ctx = nullptr};
+        httpd_register_uri_handler(server, &options_uri);
+
+    } else {
+        ESP_LOGE("main", "Failed to start HTTP server");
+    }
+}
+
 void setupEventSocket() {
-    // Motion events
     socket.onEvent(INPUT_EVENT, [&](JsonVariant &root, int originId) { motionService.handleInput(root, originId); });
 
     socket.onEvent(MODE_EVENT, [&](JsonVariant &root, int originId) {
@@ -98,7 +152,6 @@ void setupEventSocket() {
 
     socket.onEvent(ANGLES_EVENT, [&](JsonVariant &root, int originId) { motionService.anglesEvent(root, originId); });
 
-    // Peripherals events
     socket.onEvent(EVENT_I2C_SCAN, [&](JsonVariant &root, int originId) {
         peripherals.scanI2C();
         JsonDocument doc;
@@ -107,15 +160,15 @@ void setupEventSocket() {
         socket.emit(EVENT_I2C_SCAN, results);
     });
 
-    // Servo controller events
     socket.onEvent(EVENT_SERVO_CONFIGURATION_SETTINGS,
                    [&](JsonVariant &root, int originId) { servoController.servoEvent(root, originId); });
+
     socket.onEvent(EVENT_SERVO_STATE,
                    [&](JsonVariant &root, int originId) { servoController.stateUpdate(root, originId); });
 }
 
 void IRAM_ATTR SpotControlLoopEntry(void *) {
-    ESP_LOGI("main", "Setup complete now runing tsk");
+    ESP_LOGI("main", "Setup complete now running control task");
     TickType_t xLastWakeTime = xTaskGetTickCount();
     const TickType_t xFrequency = 5 / portTICK_PERIOD_MS;
 
@@ -140,10 +193,13 @@ void IRAM_ATTR serviceLoopEntry(void *) {
     ESP_LOGI("main", "Service control task starting");
 
     wifiService.begin();
-    MDNS.begin(APP_NAME);
-    MDNS.setInstanceName(APP_NAME);
+    mdns_service::begin(APP_NAME);
     apService.begin();
 
+#if FT_ENABLED(USE_CAMERA)
+    cameraService.begin();
+#endif
+
     setupServer();
 
     socket.begin();
@@ -153,14 +209,21 @@ void IRAM_ATTR serviceLoopEntry(void *) {
     for (;;) {
         wifiService.loop();
         apService.loop();
-        EXECUTE_EVERY_N_MS(2000, system_service::emitMetrics());
+        EXECUTE_EVERY_N_MS(2000, system_service::emitMetrics(socket));
+        EXECUTE_EVERY_N_MS(500, {
+            JsonDocument doc;
+            JsonVariant results = doc.to<JsonVariant>();
+            peripherals.getIMUResult(results);
+            socket.emit(EVENT_IMU, results);
+        });
 
         vTaskDelay(100 / portTICK_PERIOD_MS);
     }
 }
 
-void setup() {
-    Serial.begin(115200);
+extern "C" void app_main() {
+    uart_driver_install(UART_NUM_0, 256, 0, 0, NULL, 0);
+    esp_log_level_set("*", ESP_LOG_INFO);
 
     ESP_FS.begin();
 
@@ -168,11 +231,9 @@ void setup() {
 
     feature_service::printFeatureConfiguration();
 
-    xTaskCreate(serviceLoopEntry, "Service task", 4096, nullptr, 2, nullptr);
+    xTaskCreate(serviceLoopEntry, "Service task", 8192, nullptr, 2, nullptr);
 
-    xTaskCreatePinnedToCore(SpotControlLoopEntry, "Control task", 4096, nullptr, 5, nullptr, 1);
+    xTaskCreatePinnedToCore(SpotControlLoopEntry, "Control task", 8192, nullptr, 5, nullptr, 1);
 
     ESP_LOGI("main", "Finished booting");
 }
-
-void loop() { vTaskDelete(nullptr); }

esp32/src/mdns_service.cpp

@@ -2,99 +2,54 @@
 
 static const char *TAG = "MDNSService";
 
-MDNSService::MDNSService()
-    : _persistence(MDNSSettings::read, MDNSSettings::update, this, MDNS_SETTINGS_FILE),
-      endpoint(MDNSSettings::read, MDNSSettings::update, this) {
-    addUpdateHandler([&](const std::string &originId) { reconfigureMDNS(); }, false);
-}
+namespace mdns_service {
 
-MDNSService::~MDNSService() {
-    if (_started) {
-        stopMDNS();
+void begin(const char *hostname) {
+    ESP_LOGI(TAG, "Starting mDNS with hostname: %s", hostname);
+
+    esp_err_t err = mdns_init();
+    if (err != ESP_OK) {
+        ESP_LOGE(TAG, "mDNS init failed: %d", err);
+        return;
     }
-}
 
-void MDNSService::begin() {
-    _persistence.readFromFS();
-    startMDNS();
-}
-
-void MDNSService::reconfigureMDNS() {
-    if (_started) {
-        stopMDNS();
+    err = mdns_hostname_set(hostname);
+    if (err != ESP_OK) {
+        ESP_LOGE(TAG, "Failed to set hostname: %d", err);
+        return;
     }
-    startMDNS();
+
+    err = mdns_instance_name_set(hostname);
+    if (err != ESP_OK) {
+        ESP_LOGE(TAG, "Failed to set instance name: %d", err);
+        return;
+    }
+
+    addService("http", "_tcp", 80);
+    addService("ws", "_tcp", 80);
+    addServiceTxt("http", "_tcp", "version", APP_VERSION);
+
+    ESP_LOGI(TAG, "mDNS started successfully");
 }
 
-void MDNSService::startMDNS() {
-    ESP_LOGV(TAG, "Starting MDNS with hostname: %s", state().hostname.c_str());
+void end() { mdns_free(); }
 
-    if (MDNS.begin(state().hostname.c_str())) {
-        _started = true;
-        MDNS.setInstanceName(state().instance.c_str());
-
-        addServices();
-
-        ESP_LOGI(TAG, "MDNS started successfully with hostname: %s", state().hostname.c_str());
+void addService(const char *service, const char *proto, uint16_t port) {
+    esp_err_t err = mdns_service_add(NULL, service, proto, port, NULL, 0);
+    if (err != ESP_OK) {
+        ESP_LOGE(TAG, "Failed to add service %s.%s: %d", service, proto, err);
     } else {
-        _started = false;
-        ESP_LOGE(TAG, "Failed to start MDNS");
+        ESP_LOGI(TAG, "Added mDNS service: %s.%s on port %d", service, proto, port);
     }
 }
 
-void MDNSService::stopMDNS() {
-    ESP_LOGV(TAG, "Stopping MDNS");
-    MDNS.end();
-    _started = false;
-}
+void addServiceTxt(const char *service, const char *proto, const char *key, const char *value) {
+    mdns_txt_item_t txt_data[1] = {{(char *)key, (char *)value}};
 
-void MDNSService::addServices() {
-    for (const auto &service : state().services) {
-        MDNS.addService(service.service.c_str(), service.protocol.c_str(), service.port);
-
-        for (const auto &txt : service.txtRecords) {
-            MDNS.addServiceTxt(service.service.c_str(), service.protocol.c_str(), txt.key.c_str(), txt.value.c_str());
-        }
-    }
-
-    for (const auto &txt : state().globalTxtRecords) {
-        for (const auto &service : state().services) {
-            MDNS.addServiceTxt(service.service.c_str(), service.protocol.c_str(), txt.key.c_str(), txt.value.c_str());
-        }
+    esp_err_t err = mdns_service_txt_set(service, proto, txt_data, 1);
+    if (err != ESP_OK) {
+        ESP_LOGE(TAG, "Failed to set TXT record for %s.%s: %d", service, proto, err);
     }
 }
 
-esp_err_t MDNSService::getStatus(PsychicRequest *request) {
-    PsychicJsonResponse response = PsychicJsonResponse(request, false);
-    JsonVariant root = response.getRoot();
-    getStatus(root);
-    return response.send();
-}
-
-void MDNSService::getStatus(JsonVariant &root) {
-    state().read(state(), root);
-    root["started"] = _started;
-}
-
-esp_err_t MDNSService::queryServices(PsychicRequest *request, JsonVariant &json) {
-    std::string service = json["service"].as<std::string>();
-    std::string proto = json["protocol"].as<std::string>();
-
-    PsychicJsonResponse response = PsychicJsonResponse(request, false);
-    JsonVariant root = response.getRoot();
-
-    ESP_LOGI(TAG, "Querying for service: %s, protocol: %s", service.c_str(), proto.c_str());
-
-    int n = MDNS.queryService(service.c_str(), proto.c_str());
-    ESP_LOGI(TAG, "Found %d services", n);
-
-    JsonArray servicesArray = root["services"].to<JsonArray>();
-    for (int i = 0; i < n; i++) {
-        JsonVariant serviceObj = servicesArray.add<JsonVariant>();
-        serviceObj["name"] = MDNS.hostname(i);
-        serviceObj["ip"] = MDNS.IP(i);
-        serviceObj["port"] = MDNS.port(i);
-    }
-
-    return response.send();
-}
+} // namespace mdns_service

esp32/src/peripherals/camera_service.cpp

@@ -85,44 +85,40 @@ esp_err_t CameraService::begin() {
     return err;
 }
 
-esp_err_t CameraService::cameraStill(PsychicRequest *request) {
+esp_err_t CameraService::cameraStill(httpd_req_t *req) {
     camera_fb_t *fb = safe_camera_fb_get();
     if (!fb) {
         ESP_LOGE(TAG, "Camera capture failed");
-        request->reply(500, "text/plain", "Camera capture failed");
-        return ESP_FAIL;
+        return http_utils::send_error(req, 500, "Camera capture failed");
     }
-    PsychicStreamResponse response = PsychicStreamResponse(request, "image/jpeg", "capture.jpg");
-    response.beginSend();
-    response.write(fb->buf, fb->len);
+
+    httpd_resp_set_type(req, "image/jpeg");
+    httpd_resp_set_hdr(req, "Content-Disposition", "inline; filename=capture.jpg");
+
+    esp_err_t res = httpd_resp_send(req, (const char *)fb->buf, fb->len);
     esp_camera_fb_return(fb);
-    return response.endSend();
+    return res;
 }
 
 void streamTask(void *pv) {
     esp_err_t res = ESP_OK;
 
-    PsychicRequest *request = static_cast<PsychicRequest *>(pv);
+    httpd_req_t *req = static_cast<httpd_req_t *>(pv);
 
     httpd_req_t *copy = nullptr;
-    res = httpd_req_async_handler_begin(request->request(), &copy);
+    res = httpd_req_async_handler_begin(req, &copy);
     if (res != ESP_OK) {
         return;
     }
-    PsychicHttpServer *server = request->server();
-    PsychicRequest new_request = PsychicRequest(server, copy);
-    request = &new_request;
 
-    PsychicStreamResponse response = PsychicStreamResponse(request, _STREAM_CONTENT_TYPE);
+    httpd_resp_set_type(copy, _STREAM_CONTENT_TYPE);
     camera_fb_t *fb = NULL;
 
-    char *part_buf[64];
+    char part_buf[64];
     size_t buf_len = 0;
     uint8_t *buf = NULL;
     int64_t fr_start = esp_timer_get_time();
 
-    response.beginSend();
-
     for (;;) {
         fb = safe_camera_fb_get();
         if (!fb) {
@@ -136,26 +132,28 @@ void streamTask(void *pv) {
             buf = fb->buf;
         }
 
-        size_t hlen = snprintf((char *)part_buf, 64, _STREAM_PART, buf_len);
-        size_t w = response.write((const char *)part_buf, hlen);
-        w += response.write((const char *)buf, buf_len);
-        w += response.write((char *)_STREAM_BOUNDARY, strlen(_STREAM_BOUNDARY));
-        if (w == 62) break;
+        size_t hlen = snprintf(part_buf, 64, _STREAM_PART, buf_len);
+        if (httpd_resp_send_chunk(copy, part_buf, hlen) != ESP_OK) break;
+        if (httpd_resp_send_chunk(copy, (const char *)buf, buf_len) != ESP_OK) break;
+        if (httpd_resp_send_chunk(copy, _STREAM_BOUNDARY, strlen(_STREAM_BOUNDARY)) != ESP_OK) break;
+
         esp_camera_fb_return(fb);
         safe_sensor_return();
         buf = NULL;
         taskYIELD();
-        int64_t delay = 30000ll - esp_timer_get_time() - fr_start;
-        if (delay > 0) vTaskDelay(pdMS_TO_TICKS(delay));
+        int64_t delay = 30000ll - (esp_timer_get_time() - fr_start);
+        if (delay > 0) vTaskDelay(pdMS_TO_TICKS(delay / 1000));
+        fr_start = esp_timer_get_time();
     }
+
     ESP_LOGI("Stream", "Stream ended");
-    response.endSend();
+    httpd_resp_send_chunk(copy, nullptr, 0);
     httpd_req_async_handler_complete(copy);
     vTaskDelete(NULL);
 }
 
-esp_err_t CameraService::cameraStream(PsychicRequest *request) {
-    xTaskCreate(streamTask, "Stream client task", 4096, request, 4, nullptr);
+esp_err_t CameraService::cameraStream(httpd_req_t *req) {
+    xTaskCreate(streamTask, "Stream client task", 4096, req, 4, nullptr);
     vTaskDelay(pdMS_TO_TICKS(100));
     return ESP_OK;
 }

esp32/src/peripherals/peripherals.cpp

@@ -15,18 +15,40 @@ void Peripherals::begin() {
 
     updatePins();
 
+#if FT_ENABLED(USE_ICM20948)
+    #if USE_ICM20948_SPIMODE > 0
+        ICM_20948_SPI* icm20948 = new ICM_20948_SPI;
+    #else
+        ICM_20948_I2C* icm20948 = new ICM_20948_I2C;
+    #endif
+#endif
+
+// --- IMU ---
 #if FT_ENABLED(USE_MPU6050 || USE_BNO055)
-    if (!_imu.initialize()) ESP_LOGE("IMUService", "IMU initialize failed");
+    if (!_imu.initialize(nullptr)) ESP_LOGE("Peripherals", "IMU initialize failed");
+#elif FT_ENABLED(USE_ICM20948)
+    if (!_imu.initialize(icm20948)) ESP_LOGE("Peripherals", "IMU initialize failed (ICM20948)");
 #endif
+
+// --- MAGNETOMETER ---
 #if FT_ENABLED(USE_HMC5883)
-    if (!_mag.initialize()) ESP_LOGE("IMUService", "MAG initialize failed");
+    if (!_mag.initialize(nullptr)) ESP_LOGE("Peripherals", "MAG initialize failed");
+#elif FT_ENABLED(USE_ICM20948)
+    if (!_mag.initialize(icm20948)) ESP_LOGE("Peripherals", "MAG initialize failed (ICM20948)");
 #endif
+
+// --- BMP ---
 #if FT_ENABLED(USE_BMP180)
-    if (!_bmp.initialize()) ESP_LOGE("IMUService", "BMP initialize failed");
+    if (!_bmp.initialize(nullptr)) ESP_LOGE("Peripherals", "BMP initialize failed");
 #endif
+
+
+// --- GESTURE ---
 #if FT_ENABLED(USE_PAJ7620U2)
-    if (!_gesture.initialize()) ESP_LOGE("IMUService", "Gesture initialize failed");
+    if (!_gesture.initialize(nullptr)) ESP_LOGE("Peripherals", "Gesture initialize failed");
 #endif
+
+// --- SONAR ---
 #if FT_ENABLED(USE_USS)
     _left_sonar = std::make_unique<NewPing>(USS_LEFT_PIN, USS_LEFT_PIN, MAX_DISTANCE);
     _right_sonar = std::make_unique<NewPing>(USS_RIGHT_PIN, USS_RIGHT_PIN, MAX_DISTANCE);
@@ -79,7 +101,7 @@ void Peripherals::scanI2C(uint8_t lower, uint8_t higher) {
 /* IMU FUNCTIONS */
 bool Peripherals::readImu() {
     bool updated = false;
-#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055 || USE_ICM20948)
     beginTransaction();
     updated = _imu.update();
     endTransaction();
@@ -89,7 +111,7 @@ bool Peripherals::readImu() {
 
 bool Peripherals::readMag() {
     bool updated = false;
-#if FT_ENABLED(USE_HMC5883)
+#if FT_ENABLED(USE_HMC5883 || USE_ICM20948)
     beginTransaction();
     updated = _mag.update();
     endTransaction();
@@ -127,7 +149,7 @@ void Peripherals::readSonar() {
 
 float Peripherals::angleX() {
     return
-#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055 || USE_ICM20948)
         _imu.getAngleX();
 #else
         0;
@@ -136,7 +158,7 @@ float Peripherals::angleX() {
 
 float Peripherals::angleY() {
     return
-#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055 || USE_ICM20948)
         _imu.getAngleY();
 #else
         0;
@@ -145,7 +167,7 @@ float Peripherals::angleY() {
 
 float Peripherals::angleZ() {
     return
-#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055 || USE_ICM20948)
         _imu.getAngleZ();
 #else
         0;
@@ -165,14 +187,17 @@ float Peripherals::leftDistance() { return _left_distance; }
 float Peripherals::rightDistance() { return _right_distance; }
 
 void Peripherals::getIMUResult(JsonVariant &root) {
-#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
-    _imu.getResults(root);
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055 || USE_ICM20948)
+    JsonVariant imu = root["imu"].to<JsonVariant>();
+    _imu.getResults(imu);
 #endif
-#if FT_ENABLED(USE_HMC5883)
-    _mag.getResults(root);
+#if FT_ENABLED(USE_HMC5883 || USE_ICM20948) // TODO:
+    JsonVariant mag = root["mag"].to<JsonVariant>();
+    _mag.getResults(mag);
 #endif
 #if FT_ENABLED(USE_BMP180)
-    _bmp.getResults(root);
+    JsonVariant bmp = root["bmp"].to<JsonVariant>();
+    _bmp.getResults(bmp);
 #endif
 }
 

esp32/src/system_service.cpp

@@ -4,33 +4,33 @@ namespace system_service {
 
 static const char *TAG = "SystemService";
 
-esp_err_t handleReset(PsychicRequest *request) {
+esp_err_t handleReset(httpd_req_t *req) {
     reset();
-    return request->reply(200);
+    return http_utils::send_empty_response(req, 200);
 }
 
-esp_err_t handleRestart(PsychicRequest *request) {
+esp_err_t handleRestart(httpd_req_t *req) {
     restart();
-    return request->reply(200);
+    return http_utils::send_empty_response(req, 200);
 }
 
-esp_err_t handleSleep(PsychicRequest *request) {
+esp_err_t handleSleep(httpd_req_t *req) {
     sleep();
-    return request->reply(200);
+    return http_utils::send_empty_response(req, 200);
 }
 
-esp_err_t getStatus(PsychicRequest *request) {
-    PsychicJsonResponse response = PsychicJsonResponse(request, false);
-    JsonObject root = response.getRoot();
+esp_err_t getStatus(httpd_req_t *req) {
+    JsonDocument doc;
+    JsonObject root = doc.to<JsonObject>();
     status(root);
-    return response.send();
+    return http_utils::send_json_response(req, doc);
 }
 
-esp_err_t getMetrics(PsychicRequest *request) {
-    PsychicJsonResponse response = PsychicJsonResponse(request, false);
-    JsonObject root = response.getRoot();
+esp_err_t getMetrics(httpd_req_t *req) {
+    JsonDocument doc;
+    JsonObject root = doc.to<JsonObject>();
     metrics(root);
-    return response.send();
+    return http_utils::send_json_response(req, doc);
 }
 
 void reset() {
@@ -49,12 +49,8 @@ void restart() {
     xTaskCreate(
         [](void *pvParameters) {
             for (;;) {
-                vTaskDelay(250 / portTICK_PERIOD_MS);
-                MDNS.end();
-                vTaskDelay(100 / portTICK_PERIOD_MS);
-                WiFi.disconnect(true);
-                vTaskDelay(500 / portTICK_PERIOD_MS);
-                ESP.restart();
+                vTaskDelay(1000 / portTICK_PERIOD_MS);
+                esp_restart();
             }
         },
         "Restart task", 4096, nullptr, 10, nullptr);
@@ -64,11 +60,7 @@ void sleep() {
     xTaskCreate(
         [](void *pvParameters) {
             for (;;) {
-                vTaskDelay(250 / portTICK_PERIOD_MS);
-                MDNS.end();
-                vTaskDelay(100 / portTICK_PERIOD_MS);
-                WiFi.disconnect(true);
-                vTaskDelay(500 / portTICK_PERIOD_MS);
+                vTaskDelay(1000 / portTICK_PERIOD_MS);
 
                 uint64_t bitmask = (uint64_t)1 << (WAKEUP_PIN_NUMBER);
 
@@ -121,13 +113,14 @@ void metrics(JsonObject &root) {
     root["core_temp"] = temperatureRead();
 }
 
-void emitMetrics() {
-    // if (!socket.hasSubscribers(EVENT_ANALYTICS)) return;
-    // analyticsDoc.clear();
-    // JsonObject root = analyticsDoc.to<JsonObject>();
-    // system_service::metrics(root);
-    // JsonVariant data = analyticsDoc.as<JsonVariant>();
-    // socket.emit(EVENT_ANALYTICS, data);
+void emitMetrics(Websocket &socket) {
+    if (!socket.hasSubscribers(EVENT_ANALYTICS)) return;
+
+    JsonDocument doc;
+    JsonObject root = doc.to<JsonObject>();
+    system_service::metrics(root);
+    JsonVariant data = doc.as<JsonVariant>();
+    socket.emit(EVENT_ANALYTICS, data);
 }
 
 const char *resetReason(esp_reset_reason_t reason) {

esp32/src/utils/http_utils.cpp

@@ -0,0 +1,138 @@
+#include <utils/http_utils.h>
+#include <global.h>
+#include <string.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+
+namespace http_utils {
+
+static const char *TAG = "HttpUtils";
+
+esp_err_t send_json_response(httpd_req_t *req, JsonDocument &doc, int status_code) {
+    add_standard_headers(req);
+    add_cors_headers(req);
+
+    httpd_resp_set_type(req, "application/json");
+
+    if (status_code != 200) {
+        char status_str[4];
+        snprintf(status_str, sizeof(status_str), "%d", status_code);
+        httpd_resp_set_status(req, status_str);
+    }
+
+    String json_str;
+    serializeJson(doc, json_str);
+
+    return httpd_resp_send(req, json_str.c_str(), json_str.length());
+}
+
+esp_err_t send_error(httpd_req_t *req, int status_code, const char *message) {
+    add_standard_headers(req);
+    add_cors_headers(req);
+
+    char status_str[4];
+    snprintf(status_str, sizeof(status_str), "%d", status_code);
+    httpd_resp_set_status(req, status_str);
+
+    if (message) {
+        httpd_resp_set_type(req, "text/plain");
+        return httpd_resp_send(req, message, strlen(message));
+    }
+
+    return httpd_resp_send(req, nullptr, 0);
+}
+
+esp_err_t send_empty_response(httpd_req_t *req, int status_code) {
+    add_standard_headers(req);
+    add_cors_headers(req);
+
+    if (status_code != 200) {
+        char status_str[4];
+        snprintf(status_str, sizeof(status_str), "%d", status_code);
+        httpd_resp_set_status(req, status_str);
+    }
+
+    return httpd_resp_send(req, nullptr, 0);
+}
+
+esp_err_t add_cors_headers(httpd_req_t *req) {
+    httpd_resp_set_hdr(req, "Access-Control-Allow-Origin", "*");
+    httpd_resp_set_hdr(req, "Access-Control-Allow-Headers", "Accept, Content-Type, Authorization");
+    httpd_resp_set_hdr(req, "Access-Control-Allow-Methods", "GET, POST, OPTIONS");
+    httpd_resp_set_hdr(req, "Access-Control-Max-Age", "86400");
+    return ESP_OK;
+}
+
+esp_err_t add_standard_headers(httpd_req_t *req) {
+    httpd_resp_set_hdr(req, "Server", APP_NAME);
+    return ESP_OK;
+}
+
+esp_err_t parse_json_body(httpd_req_t *req, JsonDocument &doc) {
+    size_t content_len = req->content_len;
+
+    if (content_len == 0) {
+        ESP_LOGW(TAG, "Empty request body");
+        return ESP_FAIL;
+    }
+
+    if (content_len > 16384) {
+        ESP_LOGE(TAG, "Request body too large: %d bytes", content_len);
+        return ESP_FAIL;
+    }
+
+    char *buf = (char *)malloc(content_len + 1);
+    if (!buf) {
+        ESP_LOGE(TAG, "Failed to allocate memory for request body");
+        return ESP_FAIL;
+    }
+
+    int ret = httpd_req_recv(req, buf, content_len);
+    if (ret <= 0) {
+        free(buf);
+        if (ret == HTTPD_SOCK_ERR_TIMEOUT) {
+            ESP_LOGE(TAG, "Socket timeout");
+            return ESP_ERR_TIMEOUT;
+        }
+        return ESP_FAIL;
+    }
+
+    buf[ret] = '\0';
+
+    DeserializationError error = deserializeJson(doc, buf, ret);
+    free(buf);
+
+    if (error) {
+        ESP_LOGE(TAG, "JSON parse error: %s", error.c_str());
+        return ESP_FAIL;
+    }
+
+    return ESP_OK;
+}
+
+esp_err_t handle_options_cors(httpd_req_t *req) { return send_empty_response(req, 200); }
+
+const char *get_client_ip(httpd_req_t *req) {
+    int sockfd = httpd_req_to_sockfd(req);
+    struct sockaddr_in6 addr;
+    socklen_t addr_size = sizeof(addr);
+
+    if (getpeername(sockfd, (struct sockaddr *)&addr, &addr_size) < 0) {
+        return "unknown";
+    }
+
+    static char ip_str[INET6_ADDRSTRLEN];
+    if (addr.sin6_family == AF_INET) {
+        struct sockaddr_in *addr_in = (struct sockaddr_in *)&addr;
+        snprintf(ip_str, sizeof(ip_str), "%d.%d.%d.%d", (addr_in->sin_addr.s_addr >> 0) & 0xFF,
+                 (addr_in->sin_addr.s_addr >> 8) & 0xFF, (addr_in->sin_addr.s_addr >> 16) & 0xFF,
+                 (addr_in->sin_addr.s_addr >> 24) & 0xFF);
+    } else {
+        inet_ntop(AF_INET6, &addr.sin6_addr, ip_str, sizeof(ip_str));
+    }
+
+    return ip_str;
+}
+
+} // namespace http_utils

esp32/src/utils/websocket_server.cpp

@@ -0,0 +1,152 @@
+#include <utils/websocket_server.h>
+#include <esp_log.h>
+#include <string.h>
+
+namespace websocket {
+
+static const char *TAG = "WebSocketServer";
+
+WebSocketServer::WebSocketServer() : _server(nullptr) { _mutex = xSemaphoreCreateMutex(); }
+
+WebSocketServer::~WebSocketServer() {
+    if (_mutex) {
+        vSemaphoreDelete(_mutex);
+    }
+}
+
+void WebSocketServer::setOpenCallback(ClientCallback callback) { _onOpen = callback; }
+
+void WebSocketServer::setCloseCallback(ClientCallback callback) { _onClose = callback; }
+
+void WebSocketServer::setFrameCallback(FrameCallback callback) { _onFrame = callback; }
+
+esp_err_t WebSocketServer::handleWebSocket(httpd_req_t *req) {
+    if (req->method == HTTP_GET) {
+        int fd = httpd_req_to_sockfd(req);
+        addClient(fd);
+        ESP_LOGI(TAG, "WebSocket client connected: fd=%d", fd);
+
+        if (_onOpen) {
+            _onOpen(fd);
+        }
+
+        return ESP_OK;
+    }
+
+    httpd_ws_frame_t ws_pkt;
+    memset(&ws_pkt, 0, sizeof(httpd_ws_frame_t));
+
+    esp_err_t ret = httpd_ws_recv_frame(req, &ws_pkt, 0);
+    if (ret != ESP_OK) {
+        ESP_LOGE(TAG, "httpd_ws_recv_frame failed to get frame len with %d", ret);
+        return ret;
+    }
+
+    if (ws_pkt.len) {
+        ws_pkt.payload = (uint8_t *)malloc(ws_pkt.len + 1);
+        if (!ws_pkt.payload) {
+            ESP_LOGE(TAG, "Failed to allocate memory for WebSocket payload");
+            return ESP_ERR_NO_MEM;
+        }
+
+        ret = httpd_ws_recv_frame(req, &ws_pkt, ws_pkt.len);
+        if (ret != ESP_OK) {
+            ESP_LOGE(TAG, "httpd_ws_recv_frame failed with %d", ret);
+            free(ws_pkt.payload);
+            return ret;
+        }
+
+        ((uint8_t *)ws_pkt.payload)[ws_pkt.len] = '\0';
+    }
+
+    if (ws_pkt.type == HTTPD_WS_TYPE_CLOSE) {
+        int fd = httpd_req_to_sockfd(req);
+        ESP_LOGI(TAG, "WebSocket client disconnected: fd=%d", fd);
+        removeClient(fd);
+
+        if (_onClose) {
+            _onClose(fd);
+        }
+
+        if (ws_pkt.payload) {
+            free(ws_pkt.payload);
+        }
+        return ESP_OK;
+    }
+
+    if (_onFrame) {
+        ret = _onFrame(req, &ws_pkt);
+    }
+
+    if (ws_pkt.payload) {
+        free(ws_pkt.payload);
+    }
+
+    return ret;
+}
+
+void WebSocketServer::addClient(int fd) {
+    xSemaphoreTake(_mutex, portMAX_DELAY);
+    WebSocketClient client;
+    client.fd = fd;
+    client.last_seen = esp_timer_get_time();
+    _clients[fd] = client;
+    xSemaphoreGive(_mutex);
+}
+
+void WebSocketServer::removeClient(int fd) {
+    xSemaphoreTake(_mutex, portMAX_DELAY);
+    _clients.erase(fd);
+    xSemaphoreGive(_mutex);
+}
+
+WebSocketClient *WebSocketServer::getClient(int fd) {
+    xSemaphoreTake(_mutex, portMAX_DELAY);
+    auto it = _clients.find(fd);
+    WebSocketClient *client = (it != _clients.end()) ? &it->second : nullptr;
+    xSemaphoreGive(_mutex);
+    return client;
+}
+
+esp_err_t WebSocketServer::sendText(int fd, const char *data, size_t len) {
+    return sendBinary(fd, (const uint8_t *)data, len);
+}
+
+esp_err_t WebSocketServer::sendBinary(int fd, const uint8_t *data, size_t len) {
+    if (!_server) {
+        ESP_LOGE(TAG, "Server handle not set");
+        return ESP_FAIL;
+    }
+
+    httpd_ws_frame_t ws_pkt;
+    memset(&ws_pkt, 0, sizeof(httpd_ws_frame_t));
+    ws_pkt.payload = (uint8_t *)data;
+    ws_pkt.len = len;
+    ws_pkt.type = HTTPD_WS_TYPE_TEXT;
+
+    return httpd_ws_send_frame_async(_server, fd, &ws_pkt);
+}
+
+esp_err_t WebSocketServer::sendToAll(httpd_ws_type_t type, const uint8_t *data, size_t len) {
+    if (!_server) {
+        ESP_LOGE(TAG, "Server handle not set");
+        return ESP_FAIL;
+    }
+
+    httpd_ws_frame_t ws_pkt;
+    memset(&ws_pkt, 0, sizeof(httpd_ws_frame_t));
+    ws_pkt.payload = (uint8_t *)data;
+    ws_pkt.len = len;
+    ws_pkt.type = type;
+
+    xSemaphoreTake(_mutex, portMAX_DELAY);
+    for (auto &client_pair : _clients) {
+        int fd = client_pair.first;
+        httpd_ws_send_frame_async(_server, fd, &ws_pkt);
+    }
+    xSemaphoreGive(_mutex);
+
+    return ESP_OK;
+}
+
+} // namespace websocket

esp32/src/wifi_service.cpp

@@ -1,4 +1,6 @@
 #include <wifi_service.h>
+#include <WiFi.h>
+#include <ESPmDNS.h>
 
 WiFiService::WiFiService()
     : _persistence(WiFiSettings::read, WiFiSettings::update, this, WIFI_SETTINGS_FILE),
@@ -38,25 +40,25 @@ void WiFiService::reconfigureWiFiConnection() {
 
 void WiFiService::loop() { EXECUTE_EVERY_N_MS(reconnectDelay, manageSTA()); }
 
-esp_err_t WiFiService::handleScan(PsychicRequest *request) {
+esp_err_t WiFiService::handleScan(httpd_req_t *req) {
     if (WiFi.scanComplete() != -1) {
         WiFi.scanDelete();
         WiFi.scanNetworks(true);
     }
-    return request->reply(202);
+    return http_utils::send_empty_response(req, 202);
 }
 
-esp_err_t WiFiService::getNetworks(PsychicRequest *request) {
+esp_err_t WiFiService::getNetworks(httpd_req_t *req) {
     int numNetworks = WiFi.scanComplete();
     if (numNetworks == -1)
-        return request->reply(202);
+        return http_utils::send_empty_response(req, 202);
     else if (numNetworks < -1)
-        return handleScan(request);
+        return handleScan(req);
 
-    PsychicJsonResponse response = PsychicJsonResponse(request, false);
-    JsonObject root = response.getRoot();
+    JsonDocument doc;
+    JsonObject root = doc.to<JsonObject>();
     getNetworks(root);
-    return response.send();
+    return http_utils::send_json_response(req, doc);
 }
 
 void WiFiService::setupMDNS(const char *hostname) {
@@ -80,11 +82,11 @@ void WiFiService::getNetworks(JsonObject &root) {
     }
 }
 
-esp_err_t WiFiService::getNetworkStatus(PsychicRequest *request) {
-    PsychicJsonResponse response = PsychicJsonResponse(request, false);
-    JsonObject root = response.getRoot();
+esp_err_t WiFiService::getNetworkStatus(httpd_req_t *req) {
+    JsonDocument doc;
+    JsonObject root = doc.to<JsonObject>();
     getNetworkStatus(root);
-    return response.send();
+    return http_utils::send_json_response(req, doc);
 }
 
 void WiFiService::getNetworkStatus(JsonObject &root) {

esp32/src/www_mount.cpp

@@ -1,34 +1,70 @@
 #include "www_mount.hpp"
+#include <esp_log.h>
+#include <string.h>
+
+static const char *TAG = "WWWMount";
+
+struct asset_handler_ctx {
+    const WebAsset *asset;
+};
+
+static esp_err_t web_send(httpd_req_t *req, const WebAsset &asset) {
+    httpd_resp_set_status(req, "200 OK");
+    httpd_resp_set_type(req, asset.mime);
+
+    if (asset.gz) {
+        httpd_resp_set_hdr(req, "Content-Encoding", "gzip");
+    }
+
+    if (WWW_OPT.add_vary) {
+        httpd_resp_set_hdr(req, "Vary", "Accept-Encoding");
+    }
 
-static esp_err_t web_send(PsychicRequest* req, const WebAsset& asset) {
-    PsychicResponse resp(req);
-    resp.setCode(200);
-    resp.setContentType(asset.mime);
-    if (asset.gz) resp.addHeader("Content-Encoding", "gzip");
-    if (WWW_OPT.add_vary) resp.addHeader("Vary", "Accept-Encoding");
     char cc[64];
     snprintf(cc, sizeof(cc), "public, immutable, max-age=%u", WWW_OPT.max_age);
-    resp.addHeader("Cache-Control", cc);
+    httpd_resp_set_hdr(req, "Cache-Control", cc);
+
     char et[34];
     snprintf(et, sizeof(et), "\"%08x\"", asset.etag);
-    resp.addHeader("ETag", et);
-    resp.setContent(asset.data, asset.len);
-    return resp.send();
+    httpd_resp_set_hdr(req, "ETag", et);
+
+    return httpd_resp_send(req, (const char *)asset.data, asset.len);
 }
 
-void mountStaticAssets(PsychicHttpServer& server) {
-    static uint8_t buf[sizeof(PsychicWebHandler) * WWW_ASSETS_COUNT];
-    for (size_t i = 0; i < WWW_ASSETS_COUNT; i++) {
-        const WebAsset* a = &WWW_ASSETS[i];
-        auto* handle = new (&buf[i * sizeof(PsychicWebHandler)]) PsychicWebHandler();
-        handle->onRequest([a](PsychicRequest* req) { return web_send(req, *a); });
-        server.on(a->uri, HTTP_GET, handle);
-    }
+static esp_err_t asset_handler(httpd_req_t *req) {
+    asset_handler_ctx *ctx = (asset_handler_ctx *)req->user_ctx;
+    return web_send(req, *ctx->asset);
+}
+
+static esp_err_t default_handler(httpd_req_t *req) {
     for (size_t i = 0; i < WWW_ASSETS_COUNT; i++) {
         if (strcmp(WWW_ASSETS[i].uri, WWW_OPT.default_uri) == 0) {
-            server.defaultEndpoint->setHandler(
-                reinterpret_cast<PsychicWebHandler*>(&buf[i * sizeof(PsychicWebHandler)]));
-            break;
+            return web_send(req, WWW_ASSETS[i]);
         }
     }
-}
+    httpd_resp_send_404(req);
+    return ESP_OK;
+}
+
+void mountStaticAssets(httpd_handle_t server) {
+    static asset_handler_ctx contexts[WWW_ASSETS_COUNT];
+
+    for (size_t i = 0; i < WWW_ASSETS_COUNT; i++) {
+        contexts[i].asset = &WWW_ASSETS[i];
+
+        httpd_uri_t uri_handler = {
+            .uri = WWW_ASSETS[i].uri, .method = HTTP_GET, .handler = asset_handler, .user_ctx = &contexts[i]};
+
+        esp_err_t err = httpd_register_uri_handler(server, &uri_handler);
+        if (err != ESP_OK) {
+            ESP_LOGE(TAG, "Failed to register handler for %s: %d", WWW_ASSETS[i].uri, err);
+        } else {
+            ESP_LOGD(TAG, "Registered handler for %s", WWW_ASSETS[i].uri);
+        }
+    }
+
+    httpd_uri_t default_uri_handler = {.uri = "/", .method = HTTP_GET, .handler = default_handler, .user_ctx = nullptr};
+    httpd_register_uri_handler(server, &default_uri_handler);
+
+    ESP_LOGI(TAG, "Mounted %d static assets", WWW_ASSETS_COUNT);
+}

platformio.ini

@@ -105,7 +105,6 @@ build_src_flags =
 test_ignore = test_embedded
 board_build.filesystem = littlefs
 lib_deps = 
-	hoeken/PsychicHttp@^1.2.1
 	ArduinoJson@>=7.0.0
 	teckel12/NewPing@^1.9.7
 	jrowberg/I2Cdevlib-MPU6050@^1.0.0
@@ -116,6 +115,7 @@ lib_deps =
 	adafruit/Adafruit Unified Sensor@^1.1.14
     adafruit/Adafruit BNO055@^1.6.4
 	FastLED@3.5.0
+    sparkfun/SparkFun 9DoF IMU Breakout - ICM 20948 - Arduino Library@^1.3.2
 extra_scripts = 
 	pre:esp32/scripts/pre_build.py
     pre:esp32/scripts/build_app.py