Fix IMU ang MAG, added MAG chart to svelte

Early stages of magnetometer from ICM20948
Add void pointer for initializing sensors
2025-11-30 12:43:05 +01:00 · 2025-11-30 12:43:05 +01:00 · 2025-11-30 12:43:05 +01:00 · 2025-11-30 12:43:04 +01:00 · 2025-11-30 12:41:52 +01:00 · 2025-11-30 12:41:52 +01:00
52 changed files with 1072 additions and 1190 deletions
@@ -1,156 +0,0 @@
-<script lang="ts">
-    import { skill } from '$lib/stores'
-    import { onMount, onDestroy } from 'svelte'
-
-    let targetX = $state(0.5)
-    let targetZ = $state(0)
-    let targetYaw = $state(0)
-    let speed = $state(0.5)
-
-    const status = skill.status
-    const isActive = skill.isActive
-    const progress = skill.progress
-
-    const presets = [
-        { name: 'Forward 0.5m', x: 0.5, z: 0, yaw: 0 },
-        { name: 'Forward 1m', x: 1, z: 0, yaw: 0 },
-        { name: 'Back 0.5m', x: -0.5, z: 0, yaw: 0 },
-        { name: 'Left 0.5m', x: 0, z: 0.5, yaw: 0 },
-        { name: 'Right 0.5m', x: 0, z: -0.5, yaw: 0 },
-        { name: 'Turn Left 90°', x: 0, z: 0, yaw: 1.57 },
-        { name: 'Turn Right 90°', x: 0, z: 0, yaw: -1.57 },
-        { name: 'Turn 180°', x: 0, z: 0, yaw: 3.14 }
-    ]
-
-    onMount(() => skill.init())
-    onDestroy(() => skill.destroy())
-
-    function executeSkill() {
-        skill.walk(targetX, targetZ, targetYaw, speed)
-    }
-
-    function runPreset(preset: (typeof presets)[0]) {
-        skill.walk(preset.x, preset.z, preset.yaw, speed)
-    }
-
-    function formatMeters(val: number): string {
-        return val.toFixed(3) + 'm'
-    }
-
-    function formatDegrees(rad: number): string {
-        return ((rad * 180) / Math.PI).toFixed(1) + '°'
-    }
-</script>
-
-<div class="card bg-base-200 shadow-xl">
-    <div class="card-body p-4">
-        <h2 class="card-title text-sm flex justify-between">
-            Skill Control
-            <span class="badge" class:badge-success={$isActive} class:badge-ghost={!$isActive}>
-                {$isActive ? 'Active' : 'Idle'}
-            </span>
-        </h2>
-
-        <div class="grid grid-cols-2 gap-2 text-xs mb-2">
-            <div class="stat bg-base-300 rounded-lg p-2">
-                <div class="stat-title text-xs">Position</div>
-                <div class="stat-value text-sm">
-                    {formatMeters($status.x)}, {formatMeters($status.z)}
-                </div>
-                <div class="stat-desc">Yaw: {formatDegrees($status.yaw)}</div>
-            </div>
-            <div class="stat bg-base-300 rounded-lg p-2">
-                <div class="stat-title text-xs">Distance</div>
-                <div class="stat-value text-sm">{formatMeters($status.distance)}</div>
-                <div class="stat-desc">Total traveled</div>
-            </div>
-        </div>
-
-        {#if $isActive}
-            <div class="mb-2">
-                <div class="flex justify-between text-xs mb-1">
-                    <span>Progress</span>
-                    <span>{($progress * 100).toFixed(0)}%</span>
-                </div>
-                <progress class="progress progress-primary w-full" value={$progress} max="1"></progress>
-                <div class="text-xs text-base-content/60 mt-1">
-                    Target: ({$status.skill_target_x.toFixed(2)}, {$status.skill_target_z.toFixed(2)}, {formatDegrees(
-                        $status.skill_target_yaw
-                    )})
-                </div>
-            </div>
-        {/if}
-
-        <div class="divider my-1 text-xs">Presets</div>
-
-        <div class="grid grid-cols-4 gap-1">
-            {#each presets as preset}
-                <button class="btn btn-xs btn-outline" onclick={() => runPreset(preset)}>
-                    {preset.name}
-                </button>
-            {/each}
-        </div>
-
-        <div class="divider my-1 text-xs">Custom</div>
-
-        <div class="grid grid-cols-3 gap-2">
-            <div class="form-control">
-                <label class="label py-0" for="skill-x">
-                    <span class="label-text text-xs">X (m)</span>
-                </label>
-                <input
-                    id="skill-x"
-                    type="number"
-                    step="0.1"
-                    bind:value={targetX}
-                    class="input input-bordered input-xs w-full"
-                />
-            </div>
-            <div class="form-control">
-                <label class="label py-0" for="skill-z">
-                    <span class="label-text text-xs">Z (m)</span>
-                </label>
-                <input
-                    id="skill-z"
-                    type="number"
-                    step="0.1"
-                    bind:value={targetZ}
-                    class="input input-bordered input-xs w-full"
-                />
-            </div>
-            <div class="form-control">
-                <label class="label py-0" for="skill-yaw">
-                    <span class="label-text text-xs">Yaw (rad)</span>
-                </label>
-                <input
-                    id="skill-yaw"
-                    type="number"
-                    step="0.1"
-                    bind:value={targetYaw}
-                    class="input input-bordered input-xs w-full"
-                />
-            </div>
-        </div>
-
-        <div class="form-control mt-2">
-            <label class="label py-0" for="skill-speed">
-                <span class="label-text text-xs">Speed: {speed.toFixed(2)}</span>
-            </label>
-            <input id="skill-speed" type="range" min="0.1" max="1" step="0.05" bind:value={speed} class="range range-xs range-primary" />
-        </div>
-
-        <div class="card-actions justify-between mt-2">
-            <div class="flex gap-1">
-                <button class="btn btn-xs btn-ghost" onclick={() => skill.resetPosition()}>Reset Pos</button>
-            </div>
-            <div class="flex gap-1">
-                <button class="btn btn-xs btn-error" onclick={() => skill.stop()} disabled={!$isActive}>
-                    Stop
-                </button>
-                <button class="btn btn-xs btn-primary" onclick={executeSkill} disabled={$isActive}>
-                    Execute
-                </button>
-            </div>
-        </div>
-    </div>
-</div>
@@ -74,14 +74,13 @@
    let lastTick = performance.now()

    const dir = [1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1]
-    const THREEJS_SCALE = 10

    let body_state = {
        omega: 0,
        phi: 0,
        psi: 0,
        xm: 0,
-        ym: 0.15,
+        ym: 0.5,
        zm: 0,
        feet: kinematic.getDefaultFeetPos(),
        cumulative_x: 0,
@@ -105,7 +104,7 @@
        phi: 0,
        psi: 0,
        xm: 0,
-        ym: 0.15,
+        ym: 0.7,
        zm: 0,
        Background: defaultColor
    }
@@ -240,16 +239,8 @@
        const rotatedXm = settings.xm * cosYaw - settings.zm * sinYaw
        const rotatedZm = settings.xm * sinYaw + settings.zm * cosYaw

-        robot.position.x = smooth(
-            robot.position.x,
-            (-rotatedZm - body_state.cumulative_z) * THREEJS_SCALE,
-            0.1
-        )
-        robot.position.z = smooth(
-            robot.position.z,
-            (-rotatedXm - body_state.cumulative_x) * THREEJS_SCALE,
-            0.1
-        )
+        robot.position.x = smooth(robot.position.x, -rotatedZm - body_state.cumulative_z * 1.2, 0.1)
+        robot.position.z = smooth(robot.position.z, -rotatedXm - body_state.cumulative_x * 1.2, 0.1)

        const pitch = degToRad(settings.psi - 90) + body_state.cumulative_pitch
        const roll = degToRad(settings.omega) + body_state.cumulative_roll
@@ -284,6 +275,7 @@
            s: controlData[5],
            s1: controlData[6]
        }
+        body_state.ym = data.h

        let planner = planners[get(mode)]
        const delta = performance.now() - lastTick
@@ -304,8 +296,8 @@
        settings.omega = radToDeg(robot.rotation.y)
        settings.phi = radToDeg(robot.rotation.z) + $mpu.heading - 90
        settings.psi = radToDeg(robot.rotation.x) + 90
-        settings.xm = robot.position.z / THREEJS_SCALE
-        settings.zm = -robot.position.x / THREEJS_SCALE
+        settings.xm = robot.position.z * 100
+        settings.zm = -robot.position.x * 100
    }

    const updateTargetPosition = () => {
@@ -26,34 +26,21 @@ export abstract class GaitState {

    protected dt = 0.02
    protected body_state!: body_state_t
-
-    protected get kinematic() {
-        return get(currentKinematic)
-    }
-
    protected gait_state: gait_state_t = {
-        step_height: 0,
+        step_height: 0.4,
        step_x: 0,
        step_z: 0,
        step_angle: 0,
        step_velocity: 1,
-        step_depth: 0
+        step_depth: 0.002
    }

    public get default_feet_pos() {
-        return this.kinematic.getDefaultFeetPos()
+        return get(currentKinematic).getDefaultFeetPos()
    }

    protected get default_height() {
-        return this.kinematic.default_body_height
-    }
-
-    protected get default_step_depth() {
-        return this.kinematic.default_step_depth
-    }
-
-    protected get default_step_height() {
-        return this.kinematic.default_step_height
+        return 0.5
    }

    begin() {
@@ -80,15 +67,16 @@ export abstract class GaitState {
    }

    map_command(command: ControllerCommand) {
-        const kin = this.kinematic
-        this.gait_state = {
-            step_height: command.s1 * kin.max_step_height,
-            step_x: command.ly * kin.max_step_length,
-            step_z: -command.lx * kin.max_step_length,
+        const newCommand = {
+            step_height: 0.4 + (command.s1 + 1) / 2,
+            step_x: command.ly,
+            step_z: -command.lx,
            step_velocity: command.s,
            step_angle: command.rx,
-            step_depth: kin.default_step_depth
+            step_depth: 0.002
        }
+
+        this.gait_state = newCommand
    }
 }

@@ -104,13 +92,14 @@ export class IdleState extends GaitState {
 export class CalibrationState extends GaitState {
    protected name = 'Calibration'

+    // eslint-disable-next-line @typescript-eslint/no-unused-vars
    step(body_state: body_state_t, _command: ControllerCommand) {
        super.step(body_state, _command)
        body_state.omega = 0
        body_state.phi = 0
        body_state.psi = 0
        body_state.xm = 0
-        body_state.ym = this.kinematic.max_body_height
+        body_state.ym = this.default_height * 10
        body_state.zm = 0
        body_state.feet = this.default_feet_pos
        return body_state
@@ -120,13 +109,14 @@ export class CalibrationState extends GaitState {
 export class RestState extends GaitState {
    protected name = 'Rest'

+    // eslint-disable-next-line @typescript-eslint/no-unused-vars
    step(body_state: body_state_t, _command: ControllerCommand) {
        super.step(body_state, _command)
        body_state.omega = 0
        body_state.phi = 0
        body_state.psi = 0
        body_state.xm = 0
-        body_state.ym = this.kinematic.min_body_height
+        body_state.ym = this.default_height / 2
        body_state.zm = 0
        body_state.feet = this.default_feet_pos
        return body_state
@@ -138,13 +128,11 @@ export class StandState extends GaitState {

    step(body_state: body_state_t, command: ControllerCommand) {
        super.step(body_state, command)
-        const kin = this.kinematic
        body_state.omega = 0
-        body_state.ym = kin.min_body_height + command.h * kin.body_height_range
-        body_state.psi = command.ry * kin.max_pitch
-        body_state.phi = command.rx * kin.max_roll
-        body_state.xm = command.ly * kin.max_body_shift_x
-        body_state.zm = command.lx * kin.max_body_shift_z
+        body_state.phi = command.rx * 10 * (Math.PI / 2)
+        body_state.psi = command.ry * 10 * (Math.PI / 2)
+        body_state.xm = command.ly / 4
+        body_state.zm = command.lx / 4
        body_state.feet = this.default_feet_pos
        return body_state
    }
@@ -203,8 +191,6 @@ export class BezierState extends GaitState {

    step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
        super.step(body_state, command, dt)
-        const kin = this.kinematic
-        this.body_state.ym = kin.min_body_height + command.h * kin.body_height_range
        this.step_length = Math.sqrt(this.gait_state.step_x ** 2 + this.gait_state.step_z ** 2)
        if (this.gait_state.step_x < 0) this.step_length = -this.step_length
        this.update_phase()
@@ -353,8 +339,7 @@ export class BezierState extends GaitState {
        let angle = Math.atan2(this.gait_state.step_z, this.step_length) * 2
        const delta_pos = controller(length, angle, ...args, phase)

-        const kin = this.kinematic
-        length = this.gait_state.step_angle * kin.max_step_length
+        length = this.gait_state.step_angle * 2
        angle = yawArc(this.default_feet_pos[index], this.body_state.feet[index])

        const delta_rot = controller(length, angle, ...args, phase)
@@ -50,22 +50,7 @@ export default class Kinematic {

    DEG2RAD = DEG2RAD

-    max_roll: number
-    max_pitch: number
-    max_body_shift_x: number
-    max_body_shift_z: number
-    max_leg_reach: number
-    min_body_height: number
-    max_body_height: number
-    body_height_range: number
-    max_step_length: number
-    max_step_height: number
-    default_step_depth: number
-    default_body_height: number
-    default_step_height: number
-
    mountOffsets: number[][]
-    default_feet_positions: number[][]

    invMountRot = [
        [0, 0, -1],
@@ -81,34 +66,18 @@ export default class Kinematic {
        this.L = params.L
        this.W = params.W

-        this.max_roll = 15 * (Math.PI / 2)
-        this.max_pitch = 15 * (Math.PI / 2)
-        this.max_body_shift_x = this.W / 3
-        this.max_body_shift_z = this.W / 3
-        this.max_leg_reach = this.femur + this.tibia - this.coxa_offset
-        this.min_body_height = this.max_leg_reach * 0.45
-        this.max_body_height = this.max_leg_reach * 1
-        this.body_height_range = this.max_body_height - this.min_body_height
-        this.max_step_length = this.max_leg_reach * 0.8
-        this.max_step_height = this.max_leg_reach / 2
-        this.default_step_depth = 0.002
-        this.default_body_height = this.min_body_height + this.body_height_range / 2
-        this.default_step_height = this.default_body_height / 2
-
        this.mountOffsets = [
            [this.L / 2, 0, this.W / 2],
            [this.L / 2, 0, -this.W / 2],
            [-this.L / 2, 0, this.W / 2],
            [-this.L / 2, 0, -this.W / 2]
        ]
-
-        this.default_feet_positions = this.mountOffsets.map((offset, i) => {
-            return [offset[0], 0, offset[2] + (i % 2 === 1 ? -this.coxa : this.coxa)]
-        })
    }

    getDefaultFeetPos(): number[][] {
-        return this.default_feet_positions.map(pos => [...pos])
+        return this.mountOffsets.map((offset, i) => {
+            return [offset[0], -1, offset[2] + (i % 2 === 1 ? -this.coxa : this.coxa)]
+        })
    }

    calcIK(p: body_state_t): number[] {
@@ -20,7 +20,7 @@ import {
    Group,
    MeshBasicMaterial,
    RepeatWrapping,
-    type Object3D
+    Object3D
 } from 'three'
 import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls'
 import { TransformControls } from 'three/examples/jsm/controls/TransformControls'
@@ -4,7 +4,6 @@ import { get, type Writable } from 'svelte/store'
 import Visualization from '$lib/components/Visualization.svelte'
 import Stream from '$lib/components/Stream.svelte'
 import ChartWidget from '$lib/components/widget/ChartWidget.svelte'
-import SkillPanel from '$lib/components/SkillPanel.svelte'

 export interface WidgetConfig {
    id: string | number
@@ -26,8 +25,7 @@ export const isWidgetConfig = (
 export const WidgetComponents = {
    Visualization,
    Stream,
-    ChartWidget,
-    SkillPanel
+    ChartWidget
 }

 interface View {
@@ -61,16 +59,6 @@ const defaultViews: View[] = [
                { id: 2, component: 'Visualization', props: { debug: true } }
            ]
        }
-    },
-    {
-        name: 'Skills',
-        content: {
-            id: 'root',
-            widgets: [
-                { id: 1, component: 'Visualization', props: { debug: true } },
-                { id: 2, component: 'SkillPanel' }
-            ]
-        }
    }
 ]

@@ -29,24 +29,24 @@ export const variants = {
        model: `${base}spot_micro.urdf.xacro`,
        stl: `${base}stl.zip`,
        kinematics: {
-            coxa: 0.0605,
-            coxa_offset: 0.01,
-            femur: 0.1112,
-            tibia: 0.1185,
-            L: 0.2075,
-            W: 0.078
+            coxa: 60.5 / 100,
+            coxa_offset: 10 / 100,
+            femur: 111.7 / 100,
+            tibia: 118.5 / 100,
+            L: 207.5 / 100,
+            W: 78 / 100
        }
    },
    SPOTMICRO_YERTLE: {
        model: `${base}yertle.URDF`,
        stl: `${base}URDF.zip`,
        kinematics: {
-            coxa: 0.035,
-            coxa_offset: 0.0,
-            femur: 0.13,
-            tibia: 0.13,
-            L: 0.24,
-            W: 0.078
+            coxa: 35 / 100,
+            coxa_offset: 0 / 100,
+            femur: 130 / 100,
+            tibia: 130 / 100,
+            L: 240 / 100,
+            W: 78 / 100
        }
    }
 }
@@ -7,4 +7,3 @@ export * from './telemetry'
 export * from './analytics'
 export * from './featureFlags'
 export * from './location-store'
-export * from './skill'
@@ -50,5 +50,5 @@ export const input: Writable<ControllerInput> = writable({
    right: { x: 0, y: 0 },
    height: 0.5,
    speed: 0.5,
-    s1: 0.5
+    s1: 0.05
 })
@@ -1,85 +0,0 @@
-import { writable, derived } from 'svelte/store'
-import { socket } from './socket'
-import { MessageTopic, type SkillStatus, type SkillCommand } from '$lib/types/models'
-
-const defaultStatus: SkillStatus = {
-    x: 0,
-    y: 0,
-    z: 0,
-    yaw: 0,
-    distance: 0,
-    skill_active: false,
-    skill_target_x: 0,
-    skill_target_z: 0,
-    skill_target_yaw: 0,
-    skill_traveled_x: 0,
-    skill_traveled_z: 0,
-    skill_rotated: 0,
-    skill_progress: 0,
-    skill_complete: false
-}
-
-function createSkillStore() {
-    const status = writable<SkillStatus>(defaultStatus)
-    const history = writable<SkillCommand[]>([])
-    let unsubscribe: (() => void) | null = null
-
-    function init() {
-        if (unsubscribe) return
-        unsubscribe = socket.on<SkillStatus>(MessageTopic.skillStatus, data => {
-            status.set(data)
-            if (data.event === 'complete') {
-                history.update(h => [...h.slice(-9), getCurrentTarget(data)])
-            }
-        })
-    }
-
-    function getCurrentTarget(s: SkillStatus): SkillCommand {
-        return { x: s.skill_target_x, z: s.skill_target_z, yaw: s.skill_target_yaw }
-    }
-
-    function execute(cmd: SkillCommand) {
-        socket.sendEvent(MessageTopic.skill, cmd)
-    }
-
-    function walk(x: number, z: number = 0, yaw: number = 0, speed: number = 0.5) {
-        execute({ x, z, yaw, speed })
-    }
-
-    function turn(yaw: number, speed: number = 0.5) {
-        execute({ x: 0, z: 0, yaw, speed })
-    }
-
-    function stop() {
-        socket.sendEvent(MessageTopic.displacement, { action: 'clear' })
-    }
-
-    function resetPosition() {
-        socket.sendEvent(MessageTopic.displacement, { action: 'reset' })
-    }
-
-    function destroy() {
-        if (unsubscribe) {
-            unsubscribe()
-            unsubscribe = null
-        }
-    }
-
-    return {
-        status,
-        history,
-        init,
-        destroy,
-        execute,
-        walk,
-        turn,
-        stop,
-        resetPosition,
-        isActive: derived(status, $s => $s.skill_active),
-        progress: derived(status, $s => $s.skill_progress),
-        position: derived(status, $s => ({ x: $s.x, y: $s.y, z: $s.z, yaw: $s.yaw }))
-    }
-}
-
-export const skill = createSkillStore()
-
@@ -1,6 +1,5 @@
 export enum MessageTopic {
    imu = 'imu',
-    imuCalibrate = 'imuCalibrate',
    mode = 'mode',
    input = 'input',
    analytics = 'analytics',
@@ -14,10 +13,7 @@ export enum MessageTopic {
    servoPWM = 'servoPWM',
    WiFiSettings = 'WiFiSettings',
    sonar = 'sonar',
-    rssi = 'rssi',
-    skill = 'skill',
-    skillStatus = 'skill_status',
-    displacement = 'displacement'
+    rssi = 'rssi'
 }

 export type vector = { x: number; y: number }
@@ -164,10 +160,6 @@ export type IMUMsg = {
    bmp: [number, number, number, boolean]
 }

-export type IMUCalibrationResult = {
-    success: boolean
-}
-
 export interface I2CDevice {
    address: number
    part_number: string
@@ -251,28 +243,3 @@ export interface MDNSStatus {
    services: MDNSService[]
    global_txt_records: MDNSTxtRecord[]
 }
-
-export interface SkillCommand {
-    x: number
-    z: number
-    yaw: number
-    speed?: number
-}
-
-export interface SkillStatus {
-    x: number
-    y: number
-    z: number
-    yaw: number
-    distance: number
-    skill_active: boolean
-    skill_target_x: number
-    skill_target_z: number
-    skill_target_yaw: number
-    skill_traveled_x: number
-    skill_traveled_z: number
-    skill_rotated: number
-    skill_progress: number
-    skill_complete: boolean
-    event?: string
-}
@@ -1,4 +1,4 @@
-import { Color, Vector3 } from 'three'
+import { Color, LoaderUtils, Vector3 } from 'three'
 import URDFLoader, { type URDFRobot } from 'urdf-loader'
 import { XacroLoader } from 'xacro-parser'
 import { Result } from '$lib/utilities'
@@ -5,7 +5,7 @@

    const update = () => {
        const ws = $apiLocation ? $apiLocation : window.location.host
-        socket.init(`ws://${ws}/api/ws`)
+        socket.init(`ws://${ws}/api/ws/events`)
    }
 </script>

@@ -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' }
    ]
@@ -6,7 +6,7 @@
    import { slide } from 'svelte/transition'
    import { onDestroy, onMount } from 'svelte'
    import { socket } from '$lib/stores'
-    import { MessageTopic, type IMUMsg, type IMUCalibrationResult } from '$lib/types/models'
+    import { MessageTopic, type IMUMsg } from '$lib/types/models'
    import { useFeatureFlags } from '$lib/stores/featureFlags'
    import { Rotate3d } from '$lib/components/icons'

@@ -14,16 +14,16 @@

    const features = useFeatureFlags()
    let intervalId: ReturnType<typeof setInterval> | number
-    let isCalibrating = $state(false)
-    let calibrationResult = $state<IMUCalibrationResult | null>(null)

    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)
@@ -173,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[]) => {
@@ -196,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)
@@ -208,26 +243,14 @@
            imu.addData(data)
        })

-        socket.on(MessageTopic.imuCalibrate, (data: IMUCalibrationResult) => {
-            isCalibrating = false
-            calibrationResult = data
-        })
-
        initializeCharts()
        intervalId = setInterval(updateData, 200)
    })

    onDestroy(() => {
        socket.off(MessageTopic.imu)
-        socket.off(MessageTopic.imuCalibrate)
        clearInterval(intervalId)
    })
-
-    function startCalibration() {
-        isCalibrating = true
-        calibrationResult = null
-        socket.sendEvent(MessageTopic.imuCalibrate, {})
-    }
 </script>

 <SettingsCard collapsible={false}>
@@ -238,26 +261,6 @@
        <span>IMU</span>
    {/snippet}

-    <div class="flex items-center gap-2 mb-4">
-        <button
-            class="btn btn-sm btn-primary"
-            onclick={startCalibration}
-            disabled={isCalibrating || !$features.imu}
-        >
-            {#if isCalibrating}
-                <span class="loading loading-spinner loading-xs"></span>
-                Calibrating...
-            {:else}
-                Calibrate IMU
-            {/if}
-        </button>
-        {#if calibrationResult}
-            <span class="badge" class:badge-success={calibrationResult.success} class:badge-error={!calibrationResult.success}>
-                {calibrationResult.success ? 'Calibrated' : 'Failed'}
-            </span>
-        {/if}
-    </div>
-
    {#if $features.imu}
        <div class="w-full overflow-x-auto">
            <div
@@ -269,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
@@ -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
@@ -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;
@@ -55,16 +55,8 @@ class CommAdapterBase {
    void send(const char *data, int cid = -1) { send(reinterpret_cast<const uint8_t *>(data), strlen(data), cid); }
    virtual void send(const uint8_t *data, size_t len, int cid = -1) = 0;

-    void subscribe(const char *event, int cid = 0) {
-        xSemaphoreTake(mutex_, portMAX_DELAY);
-        client_subscriptions[event].push_back(cid);
-        xSemaphoreGive(mutex_);
-    }
-    void unsubscribe(const char *event, int cid = 0) {
-        xSemaphoreTake(mutex_, portMAX_DELAY);
-        client_subscriptions[event].remove(cid);
-        xSemaphoreGive(mutex_);
-    }
+    void subscribe(const char *event, int cid = 0) { client_subscriptions[event].push_back(cid); }
+    void unsubscribe(const char *event, int cid = 0) { client_subscriptions[event].push_back(cid); }

    void handleEventCallbacks(std::string event, JsonVariant &jsonObject, int originId) {
        for (auto &callback : event_callbacks[event]) {
@@ -110,21 +102,35 @@ class CommAdapterBase {
            }
            case message_type_t::PING: {
                ESP_LOGI("Comm Base", "PING (cid=%d)", cid);
-                ping(cid);
+#if USE_MSGPACK
+                static const uint8_t pong[] = {0x91, 0x04};
+                send(pong, sizeof(pong), cid);
+#else
+                send("[4]", cid);
+#endif
                break;
            }
-
            case message_type_t::PONG: ESP_LOGI("Comm Base", "PONG (cid=%d)", cid); break;
            default: ESP_LOGW("Comm Base", "Unknown message type: %d", static_cast<int>(type)); break;
        }
+
+        if (type == PONG) {
+            ESP_LOGV("EventSocket", "Pong");
+            return;
+        } else if (type == PING) {
+            ESP_LOGV("EventSocket", "Ping");
+            ping(cid);
+            return;
+        }
    }

    void ping(int cid) {
 #if USE_MSGPACK
-        static const uint8_t pong[] = {0x91, 0x04};
-        send(pong, sizeof(pong), cid);
+        const uint8_t out[] = {0x91, 0x04};
+        send(out, sizeof(out), cid);
 #else
-        send("[4]", cid);
+        const char *out = "[4]";
+        send(out, strlen(out), cid);
 #endif
    }

@@ -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>
@@ -12,7 +13,7 @@

 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 : public 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
-
@@ -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
@@ -42,11 +49,6 @@
 #define USE_PCA9685 1
 #endif

-// WS2812 LED strip off by default
-#ifndef USE_WS2812
-#define USE_WS2812 0
-#endif
-
 // ESP32 MDNS on by default
 #ifndef USE_MDNS
 #define USE_MDNS 1
@@ -88,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

@@ -25,7 +25,6 @@ esp_err_t uploadFile(PsychicRequest *request, const std::string &filename, uint6
                     bool last);

 esp_err_t getFiles(PsychicRequest *request);
-esp_err_t getConfigFile(PsychicRequest *request);
 esp_err_t handleDelete(PsychicRequest *request, JsonVariant &json);
 esp_err_t handleEdit(PsychicRequest *request, JsonVariant &json);

@@ -6,26 +6,26 @@
 class KinConfig {
  public:
 #if defined(SPOTMICRO_ESP32)
-    static constexpr float coxa = 0.0605f;
-    static constexpr float coxa_offset = 0.010f;
-    static constexpr float femur = 0.1112f;
-    static constexpr float tibia = 0.1185f;
-    static constexpr float L = 0.2075f;
-    static constexpr float W = 0.078f;
+    static constexpr float coxa = 60.5f / 100.0f;
+    static constexpr float coxa_offset = 10.0f / 100.0f;
+    static constexpr float femur = 111.2f / 100.0f;
+    static constexpr float tibia = 118.5f / 100.0f;
+    static constexpr float L = 207.5f / 100.0f;
+    static constexpr float W = 78.0f / 100.0f;
 #elif defined(SPOTMICRO_ESP32_MINI)
-    static constexpr float coxa = 0.035f;
-    static constexpr float coxa_offset = 0.0f;
-    static constexpr float femur = 0.060f;
-    static constexpr float tibia = 0.060f;
-    static constexpr float L = 0.160f;
-    static constexpr float W = 0.080f;
+    static constexpr float coxa = 35.0f / 100.0f;
+    static constexpr float coxa_offset = 0.0f / 100.0f;
+    static constexpr float femur = 60.0f / 100.0f;
+    static constexpr float tibia = 60.0f / 100.0f;
+    static constexpr float L = 160.0f / 100.0f;
+    static constexpr float W = 80.0f / 100.0f;
 #elif defined(SPOTMICRO_YERTLE)
-    static constexpr float coxa = 0.035f;
+    static constexpr float coxa = 35.0f / 100.0f;
    static constexpr float coxa_offset = 0.0f;
-    static constexpr float femur = 0.130f;
-    static constexpr float tibia = 0.130f;
-    static constexpr float L = 0.240f;
-    static constexpr float W = 0.078f;
+    static constexpr float femur = 130.0f / 100.0f;
+    static constexpr float tibia = 130.0f / 100.0f;
+    static constexpr float L = 240.0f / 100.0f;
+    static constexpr float W = 78.0f / 100.0f;
 #endif

    static constexpr float mountOffsets[4][3] = {
@@ -38,8 +38,9 @@ class KinConfig {
        {mountOffsets[3][0], 0, mountOffsets[3][2] - coxa, 1},
    };

-    static constexpr float max_roll = 15 * DEG2RAD_F;
-    static constexpr float max_pitch = 15 * DEG2RAD_F;
+    // Max constants
+    static constexpr float max_roll = 15 * (float)M_PI_2;
+    static constexpr float max_pitch = 15 * (float)M_PI_2;

    static constexpr float max_body_shift_x = W / 3;
    static constexpr float max_body_shift_z = W / 3;
@@ -59,85 +60,12 @@ class KinConfig {
    static constexpr float default_step_height = default_body_height / 2;
 };

-struct displacement_state_t {
-    float x {0};
-    float y {0};
-    float z {0};
-    float roll {0};
-    float pitch {0};
-    float yaw {0};
-
-    void reset() { x = y = z = roll = pitch = yaw = 0; }
-    float distance() const { return std::sqrt(x * x + z * z); }
-};
-
-struct skill_target_t {
-    float target_x {0};
-    float target_z {0};
-    float target_yaw {0};
-
-    float traveled_x {0};
-    float traveled_z {0};
-    float rotated {0};
-
-    bool active {false};
-
-    void set(float x, float z, float yaw) {
-        target_x = x;
-        target_z = z;
-        target_yaw = yaw;
-        traveled_x = traveled_z = rotated = 0;
-        active = true;
-    }
-
-    void reset() {
-        target_x = target_z = target_yaw = 0;
-        traveled_x = traveled_z = rotated = 0;
-        active = false;
-    }
-
-    void accumulate(float dx, float dz, float dyaw) {
-        traveled_x += dx;
-        traveled_z += dz;
-        rotated += dyaw;
-    }
-
-    bool isComplete() const {
-        if (!active) return false;
-        bool x_ok = (target_x == 0) || (target_x > 0 ? traveled_x >= target_x : traveled_x <= target_x);
-        bool z_ok = (target_z == 0) || (target_z > 0 ? traveled_z >= target_z : traveled_z <= target_z);
-        bool yaw_ok = (target_yaw == 0) || (target_yaw > 0 ? rotated >= target_yaw : rotated <= target_yaw);
-        return x_ok && z_ok && yaw_ok;
-    }
-
-    float progress() const {
-        if (!active) return 0;
-        float total_target = std::fabs(target_x) + std::fabs(target_z) + std::fabs(target_yaw);
-        if (total_target == 0) return 1;
-
-        auto clampProgress = [](float traveled, float target) -> float {
-            if (target == 0) return 0;
-            float p = traveled / target;
-            return std::clamp(p, 0.0f, 1.0f) * std::fabs(target);
-        };
-
-        float total_progress = clampProgress(traveled_x, target_x) + clampProgress(traveled_z, target_z) +
-                               clampProgress(rotated, target_yaw);
-        return total_progress / total_target;
-    }
-};
-
 struct alignas(16) body_state_t {
    float omega {0}, phi {0}, psi {0}, xm {0}, ym {KinConfig::default_body_height}, zm {0};
    float feet[4][4];

-    displacement_state_t cumulative;
-    skill_target_t skill;
-
    void updateFeet(const float newFeet[4][4]) { COPY_2D_ARRAY_4x4(feet, newFeet); }

-    void resetDisplacement() { cumulative.reset(); }
-
    bool operator==(const body_state_t &other) const {
        if (!IS_ALMOST_EQUAL(omega, other.omega) || !IS_ALMOST_EQUAL(phi, other.phi) ||
            !IS_ALMOST_EQUAL(psi, other.psi) || !IS_ALMOST_EQUAL(xm, other.xm) || !IS_ALMOST_EQUAL(ym, other.ym) ||
@@ -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
@@ -3,7 +3,6 @@

 #include <ArduinoJson.h>
 #include "esp_timer.h"
-#include <functional>

 #include <kinematics.h>
 #include <peripherals/gesture.h>
@@ -19,8 +18,6 @@

 enum class MOTION_STATE { DEACTIVATED, IDLE, CALIBRATION, REST, STAND, WALK };

-using SkillCompleteCallback = std::function<void()>;
-
 class MotionService {
  public:
    void begin();
@@ -33,10 +30,6 @@ class MotionService {

    void handleMode(JsonVariant &root, int originId);

-    void handleDisplacement(JsonVariant &root, int originId);
-
-    void handleSkill(JsonVariant &root, int originId);
-
    void setState(MotionState *newState);

    void handleGestures(const gesture_t ges);
@@ -49,39 +42,6 @@ class MotionService {

    inline bool isActive() { return state != nullptr; }

-    void resetDisplacement() { body_state.resetDisplacement(); }
-
-    void setSkillTarget(float x, float z, float yaw) { body_state.skill.set(x, z, yaw); }
-
-    void clearSkill() { body_state.skill.reset(); }
-
-    bool isSkillActive() const { return body_state.skill.active; }
-
-    bool isSkillComplete() const { return body_state.skill.isComplete(); }
-
-    const displacement_state_t &getDisplacement() const { return body_state.cumulative; }
-
-    const skill_target_t &getSkill() const { return body_state.skill; }
-
-    void getDisplacementResult(JsonVariant &root) const {
-        root["x"] = body_state.cumulative.x;
-        root["y"] = body_state.cumulative.y;
-        root["z"] = body_state.cumulative.z;
-        root["yaw"] = body_state.cumulative.yaw;
-        root["distance"] = body_state.cumulative.distance();
-        root["skill_active"] = body_state.skill.active;
-        root["skill_target_x"] = body_state.skill.target_x;
-        root["skill_target_z"] = body_state.skill.target_z;
-        root["skill_target_yaw"] = body_state.skill.target_yaw;
-        root["skill_traveled_x"] = body_state.skill.traveled_x;
-        root["skill_traveled_z"] = body_state.skill.traveled_z;
-        root["skill_rotated"] = body_state.skill.rotated;
-        root["skill_progress"] = body_state.skill.progress();
-        root["skill_complete"] = body_state.skill.isComplete();
-    }
-
-    void setSkillCompleteCallback(SkillCompleteCallback callback) { skillCompleteCallback = callback; }
-
  private:
    Kinematics kinematics;

@@ -103,11 +63,6 @@ class MotionService {
    float dir[12] = {1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1};

    int64_t lastUpdate = esp_timer_get_time();
-
-    SkillCompleteCallback skillCompleteCallback = nullptr;
-    bool skillWasComplete = false;
-
-    void checkSkillComplete();
 };

 #endif
@@ -98,33 +98,11 @@ class WalkState : public MotionState {

        step_length = std::hypot(gait_state.step_x, gait_state.step_z);
        if (gait_state.step_x < 0.0f) step_length = -step_length;
-
-        const bool moving = !isZero(gait_state.step_x) || !isZero(gait_state.step_z) || !isZero(gait_state.step_angle);
-        updateDisplacement(body_state, dt, moving);
-
        updatePhase(dt);
        updateBodyPosition(body_state, dt);
        updateFeetPositions(body_state);
    }

-    void updateDisplacement(body_state_t &body_state, float dt, bool moving) {
-        if (!moving) return;
-
-        float dx_local = gait_state.step_x * gait_state.step_velocity * dt * speed_factor;
-        float dz_local = gait_state.step_z * gait_state.step_velocity * dt * speed_factor;
-        float dyaw = gait_state.step_angle * gait_state.step_velocity * dt * speed_factor;
-
-        if (body_state.skill.active) {
-            body_state.skill.accumulate(dx_local, dz_local, dyaw);
-        }
-
-        float cos_yaw = std::cos(body_state.cumulative.yaw);
-        float sin_yaw = std::sin(body_state.cumulative.yaw);
-        body_state.cumulative.x += dx_local * cos_yaw - dz_local * sin_yaw;
-        body_state.cumulative.z += dx_local * sin_yaw + dz_local * cos_yaw;
-        body_state.cumulative.yaw += dyaw;
-    }
-
  protected:
    void handleCommand(const CommandMsg &cmd) override {
        target_body_state.ym = KinConfig::min_body_height + cmd.h * KinConfig::body_height_range;
@@ -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;
    }
@@ -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;

@@ -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,7 +48,7 @@ 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();
@@ -82,11 +86,33 @@ class IMU : public SensorBase<IMUAnglesMsg> {
        }
        _imu.setExtCrystalUse(true);
 #endif
-        return _msg.success;
+#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)
        uint16_t fifoCount = _imu.getFIFOCount();
        uint8_t intStatus = _imu.getIntStatus();
@@ -105,12 +131,31 @@ class IMU : public SensorBase<IMUAnglesMsg> {
        }
        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;
    }
@@ -123,26 +168,6 @@ class IMU : public SensorBase<IMUAnglesMsg> {

    float getAngleZ() { return _msg.rpy[0]; }

-    bool calibrate() {
-#if FT_ENABLED(USE_MPU6050)
-        if (!_msg.success) return false;
-        ESP_LOGI("IMU", "Starting calibration...");
-        _imu.CalibrateGyro(6);
-        _imu.CalibrateAccel(6);
-        ESP_LOGI("IMU", "Calibration complete");
-        return true;
-#elif FT_ENABLED(USE_BNO055)
-        if (!_msg.success) return false;
-        ESP_LOGI("IMU", "Starting calibration...");
-        adafruit_bno055_offsets_t offsets;
-        bool result = _imu.getSensorOffsets(offsets);
-        ESP_LOGI("IMU", "Calibration complete");
-        return result;
-#else
-        return false;
-#endif
-    }
-
  private:
 #if FT_ENABLED(USE_MPU6050)
    MPU6050 _imu;
@@ -154,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
 };
@@ -11,6 +11,7 @@

 #include <peripherals/sensor.hpp>

+
 struct MagnetometerMsg : public SensorMessageBase {
    float rpy[3] {0, 0, 0};
    float heading {-1};
@@ -38,20 +39,45 @@ struct MagnetometerMsg : public SensorMessageBase {

 class Magnetometer : public SensorBase<MagnetometerMsg> {
  public:
-    bool initialize() override {
-        _msg.success = _mag.begin();
+    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() override {
        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);
+        #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;
@@ -68,6 +94,18 @@ class Magnetometer : public SensorBase<MagnetometerMsg> {
    float getHeading() { return _msg.heading; }

  private:
-    Adafruit_HMC5883_Unified _mag {12345};
+    
+    #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;
 };
@@ -74,8 +74,6 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
    float leftDistance();
    float rightDistance();

-    bool calibrateIMU();
-
    StatefulHttpEndpoint<PeripheralsConfiguration> endpoint;

  private:
@@ -89,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)
@@ -17,7 +17,7 @@ class SensorBase {
  public:
    SensorBase() {}

-    virtual bool initialize() = 0;
+    virtual bool initialize(void* _arg) = 0;

    virtual bool update() = 0;

@@ -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 <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();
@@ -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);
    }
 };
@@ -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
@@ -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
@@ -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;
 };
@@ -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);
@@ -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) {
@@ -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);
+}
@@ -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
@@ -22,20 +22,6 @@ static Initializer initializer;

 esp_err_t getFiles(PsychicRequest *request) { return request->reply(200, "application/json", listFiles("/").c_str()); }

-esp_err_t getConfigFile(PsychicRequest *request) {
-    String path = "/config" + request->uri().substring(11);
-    if (!ESP_FS.exists(path)) {
-        return request->reply(404, "text/plain", "File not found");
-    }
-    File file = ESP_FS.open(path, "r");
-    if (!file) {
-        return request->reply(500, "text/plain", "Failed to open file");
-    }
-    String content = file.readString();
-    file.close();
-    return request->reply(200, "application/json", content.c_str());
-}
-
 esp_err_t handleDelete(PsychicRequest *request, JsonVariant &json) {
    if (json.is<JsonObject>()) {
        const char *filename = json["file"].as<const char *>();
@@ -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;
@@ -33,110 +30,10 @@ LEDService ledService;
 #if FT_ENABLED(USE_CAMERA)
 Camera::CameraService cameraService;
 #endif
-#if FT_ENABLED(USE_MDNS)
-MDNSService mdnsService;
-#endif

-// Service
 WiFiService wifiService;
 APService apService;

-void setupServer() {
-    server.config.max_uri_handlers = 32 + WWW_ASSETS_COUNT;
-    server.maxUploadSize = 1000000; // 1 MB;
-    server.listen(80);
-    server.on("/api/features", feature_service::getFeatures);
-    server.on("/api/system/status", HTTP_GET,
-              [&](PsychicRequest *request) { return system_service::getStatus(request); });
-    server.on("/api/system/reset", HTTP_POST,
-              [&](PsychicRequest *request, JsonVariant &json) { return system_service::handleReset(request); });
-    server.on("/api/system/restart", HTTP_POST,
-              [&](PsychicRequest *request, JsonVariant &json) { return system_service::handleRestart(request); });
-    server.on("/api/system/sleep", HTTP_POST,
-              [&](PsychicRequest *request, JsonVariant &json) { return system_service::handleSleep(request); });
-    server.on("/api/system/metrics", HTTP_GET,
-              [&](PsychicRequest *request) { return system_service::getMetrics(request); });
-#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);
-    });
-
-    // WiFi
-    server.on("/api/wifi/sta/settings", HTTP_GET,
-              [&](PsychicRequest *request) { return wifiService.endpoint.getState(request); });
-    server.on("/api/wifi/sta/settings", HTTP_POST, [&](PsychicRequest *request, JsonVariant &json) {
-        return wifiService.endpoint.handleStateUpdate(request, json);
-    });
-    server.on("/api/wifi/scan", HTTP_GET, [&](PsychicRequest *request) { return wifiService.handleScan(request); });
-    server.on("/api/wifi/networks", HTTP_GET,
-              [&](PsychicRequest *request) { return wifiService.getNetworks(request); });
-    server.on("/api/wifi/sta/status", HTTP_GET,
-              [&](PsychicRequest *request) { return wifiService.getNetworkStatus(request); });
-
-    // AP
-    server.on("/api/ap/status", HTTP_GET, [&](PsychicRequest *request) { return apService.getStatus(request); });
-    server.on("/api/ap/settings", HTTP_GET,
-              [&](PsychicRequest *request) { return apService.endpoint.getState(request); });
-    server.on("/api/ap/settings", HTTP_POST, [&](PsychicRequest *request, JsonVariant &json) {
-        return apService.endpoint.handleStateUpdate(request, json);
-    });
-
-    // Peripherals
-    server.on("/api/peripherals", HTTP_GET,
-              [&](PsychicRequest *request) { return peripherals.endpoint.getState(request); });
-    server.on("/api/peripherals", HTTP_POST, [&](PsychicRequest *request, JsonVariant &json) {
-        return peripherals.endpoint.handleStateUpdate(request, json);
-    });
-
-    // MDNS
-#if FT_ENABLED(USE_MDNS)
-    server.on("/api/mdns", HTTP_GET, [&](PsychicRequest *request) { return mdnsService.endpoint.getState(request); });
-    server.on("/api/mdns", HTTP_POST, [&](PsychicRequest *request, JsonVariant &json) {
-        return mdnsService.endpoint.handleStateUpdate(request, json);
-    });
-    server.on("/api/mdns/status", HTTP_GET, [&](PsychicRequest *request) { return mdnsService.getStatus(request); });
-    server.on("/api/mdns/query", HTTP_POST,
-              [&](PsychicRequest *request, JsonVariant &json) { return mdnsService.queryServices(request, json); });
-#endif
-
-
-    // Filesystem
-    server.on("/api/config/*", HTTP_GET, [](PsychicRequest *request) { return FileSystem::getConfigFile(request); });
-    server.on("/api/files", HTTP_GET, [&](PsychicRequest *request) { return FileSystem::getFiles(request); });
-    server.on("/api/files", HTTP_POST, FileSystem::uploadHandler);
-    server.on("/api/files/delete", HTTP_POST,
-              [&](PsychicRequest *request, JsonVariant &json) { return FileSystem::handleDelete(request, json); });
-    server.on("/api/files/edit", HTTP_POST,
-              [&](PsychicRequest *request, JsonVariant &json) { return FileSystem::handleEdit(request, json); });
-    server.on("/api/files/mkdir", HTTP_POST,
-              [&](PsychicRequest *request, JsonVariant &json) { return FileSystem::mkdir(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"
@@ -144,13 +41,104 @@ void setupServer() {
 #define EVENT_I2C_SCAN "i2cScan"
 #define EVENT_SERVO_CONFIGURATION_SETTINGS "servoPWM"
 #define EVENT_SERVO_STATE "servoState"
-#define EVENT_DISPLACEMENT "displacement"
-#define EVENT_SKILL "skill"
-#define EVENT_SKILL_STATUS "skill_status"
-#define EVENT_IMU_CALIBRATE "imuCalibrate"
+
+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) {
@@ -164,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;
@@ -173,50 +160,21 @@ void setupEventSocket() {
        socket.emit(EVENT_I2C_SCAN, results);
    });

-    socket.onEvent(EVENT_IMU_CALIBRATE, [&](JsonVariant &root, int originId) {
-        JsonDocument doc;
-        JsonVariant results = doc.to<JsonVariant>();
-        results["success"] = peripherals.calibrateIMU();
-        socket.emit(EVENT_IMU_CALIBRATE, 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); });
-
-    // Skill events
-    socket.onEvent(EVENT_DISPLACEMENT,
-                   [&](JsonVariant &root, int originId) { motionService.handleDisplacement(root, originId); });
-
-    socket.onEvent(EVENT_SKILL, [&](JsonVariant &root, int originId) { motionService.handleSkill(root, originId); });
-
-    socket.onEvent(EVENT_SKILL_STATUS, [&](JsonVariant &root, int originId) {
-        JsonDocument doc;
-        JsonVariant results = doc.to<JsonVariant>();
-        motionService.getDisplacementResult(results);
-        socket.emit(EVENT_SKILL_STATUS, results);
-    });
-
-    motionService.setSkillCompleteCallback([&]() {
-        JsonDocument doc;
-        JsonVariant results = doc.to<JsonVariant>();
-        motionService.getDisplacementResult(results);
-        results["event"] = "complete";
-        socket.emit(EVENT_SKILL_STATUS, results);
-    });
 }

 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;

    peripherals.begin();
    servoController.begin();
    motionService.begin();
-    peripherals.calibrateIMU();

    for (;;) {
        CALLS_PER_SECOND(SpotControlLoopEntry);
@@ -235,8 +193,7 @@ 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)
@@ -259,21 +216,14 @@ void IRAM_ATTR serviceLoopEntry(void *) {
            peripherals.getIMUResult(results);
            socket.emit(EVENT_IMU, results);
        });
-        EXECUTE_EVERY_N_MS(200, {
-            if (motionService.isSkillActive()) {
-                JsonDocument doc;
-                JsonVariant results = doc.to<JsonVariant>();
-                motionService.getDisplacementResult(results);
-                socket.emit(EVENT_SKILL_STATUS, 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();

@@ -281,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); }
@@ -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
@@ -46,65 +46,6 @@ void MotionService::handleMode(JsonVariant &root, int originId) {
    }
 }

-void MotionService::handleDisplacement(JsonVariant &root, int originId) {
-    std::string action = root["action"] | "";
-    if (action == "reset") {
-        resetDisplacement();
-        ESP_LOGI("MotionService", "Displacement reset");
-    } else if (action == "clear") {
-        clearSkill();
-        skillWasComplete = false;
-        ESP_LOGI("MotionService", "Skill cleared");
-    }
-}
-
-void MotionService::handleSkill(JsonVariant &root, int originId) {
-    float x = root["x"] | 0.0f;
-    float z = root["z"] | 0.0f;
-    float yaw = root["yaw"] | 0.0f;
-    float speed = root["speed"] | 0.5f;
-
-    setSkillTarget(x, z, yaw);
-    skillWasComplete = false;
-
-    float linear_mag = std::hypot(x, z);
-    bool has_linear = linear_mag > 0.001f;
-    bool has_yaw = std::fabs(yaw) > 0.001f;
-
-    if (has_linear || has_yaw) {
-        if (has_linear) {
-            float norm_x = x / linear_mag;
-            float norm_z = z / linear_mag;
-            command.ly = norm_x;
-            command.lx = -norm_z;
-        } else {
-            command.ly = 0;
-            command.lx = 0;
-        }
-        command.rx = has_yaw ? (yaw > 0 ? 1.0f : -1.0f) : 0;
-        command.s = speed;
-        if (state) state->handleCommand(command);
-    }
-
-    ESP_LOGI("MotionService", "Skill set: Walk(%.3f, %.3f, %.3f) speed=%.2f", x, z, yaw, speed);
-}
-
-void MotionService::checkSkillComplete() {
-    if (!body_state.skill.active) return;
-    if (skillWasComplete) return;
-
-    if (body_state.skill.isComplete()) {
-        skillWasComplete = true;
-        command = {0, 0, 0, 0, command.h, 0, command.s1};
-        if (state) state->handleCommand(command);
-        ESP_LOGI("MotionService", "Skill complete! Traveled: (%.3f, %.3f), Rotated: %.3f", body_state.skill.traveled_x,
-                 body_state.skill.traveled_z, body_state.skill.rotated);
-        if (skillCompleteCallback) {
-            skillCompleteCallback();
-        }
-    }
-}
-
 void MotionService::handleGestures(const gesture_t ges) {
    if (ges != gesture_t::eGestureNone) {
        ESP_LOGI("Motion", "Gesture: %d", ges);
@@ -123,12 +64,10 @@ bool MotionService::update(Peripherals *peripherals) {
    handleGestures(peripherals->takeGesture());
    if (!state) return false;
    int64_t now = esp_timer_get_time();
-    float dt = (now - lastUpdate) / 1000000.0f;
+    float dt = (now - lastUpdate) / 1000000.0f; // Convert microseconds to seconds
    lastUpdate = now;
    state->updateImuOffsets(peripherals->angleY(), peripherals->angleX());
-    ESP_LOGI("MotionService", "IMU Offsets: %.3f, %.3f", peripherals->angleY(), peripherals->angleX());
    state->step(body_state, dt);
-    checkSkillComplete();
    kinematics.calculate_inverse_kinematics(body_state, new_angles);

    return update_angles(new_angles, angles);
@@ -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;
 }
@@ -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);
@@ -34,8 +56,8 @@ void Peripherals::begin() {
 };

 void Peripherals::update() {
-    EXECUTE_EVERY_N_MS(20, { readImu(); });
-    EXECUTE_EVERY_N_MS(100, { readMag(); });
+    readImu();
+    readMag();
    EXECUTE_EVERY_N_MS(100, { readGesture(); });
    EXECUTE_EVERY_N_MS(500, { readBMP(); });
    EXECUTE_EVERY_N_MS(500, { readSonar(); });
@@ -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,11 +187,11 @@ float Peripherals::leftDistance() { return _left_distance; }
 float Peripherals::rightDistance() { return _right_distance; }

 void Peripherals::getIMUResult(JsonVariant &root) {
-#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055 || USE_ICM20948)
    JsonVariant imu = root["imu"].to<JsonVariant>();
    _imu.getResults(imu);
 #endif
-#if FT_ENABLED(USE_HMC5883)
+#if FT_ENABLED(USE_HMC5883 || USE_ICM20948) // TODO:
    JsonVariant mag = root["mag"].to<JsonVariant>();
    _mag.getResults(mag);
 #endif
@@ -185,15 +207,4 @@ void Peripherals::getSonarResult(JsonVariant &root) {
    array[0] = _left_distance;
    array[1] = _right_distance;
 #endif
-}
-
-bool Peripherals::calibrateIMU() {
-#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
-    beginTransaction();
-    bool result = _imu.calibrate();
-    endTransaction();
-    return result;
-#else
-    return false;
-#endif
 }
@@ -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);

@@ -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
@@ -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
@@ -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) {
@@ -1,30 +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];
+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++) {
-        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);
-        if (strcmp(a->uri, WWW_OPT.default_uri) == 0) {
-            server.defaultEndpoint->setHandler(handle);
+        if (strcmp(WWW_ASSETS[i].uri, WWW_OPT.default_uri) == 0) {
+            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);
+}
@@ -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
Author	SHA1	Message	Date
Niklas Jensen	7376ecf270	Fix IMU ang MAG, added MAG chart to svelte	2025-11-30 12:43:05 +01:00
Niklas Jensen	5481a598d9	Early stages of magnetometer from ICM20948	2025-11-30 12:43:05 +01:00
Niklas Jensen	0d379a8013	Add void pointer for initializing sensors	2025-11-30 12:43:05 +01:00
Rune Harlyk	868ff0446a	🐛 Fix imu and magnotometer	2025-11-30 12:43:04 +01:00
Niklas Jensen	081c1e7046	Ignore weird success flag on IMU update	2025-11-30 12:41:52 +01:00
Rune Harlyk	042548412d	🐛 Imu temp in message	2025-11-30 12:41:52 +01:00
Niklas Jensen	5c4dc51093	Added PAJ7620U2 defs, set mag for ICM20948 temp	2025-11-30 12:41:52 +01:00
Rune Harlyk	94a50302cc	🐛 Fix socket deadlock	2025-11-30 12:41:52 +01:00
Rune Harlyk	e17382c505	✨ Emit imu, mag and bmp data	2025-11-30 12:41:51 +01:00
Rune Harlyk	106c20418c	🐛 Fix system metric emit	2025-11-30 12:41:04 +01:00
Rune Harlyk	413097db1c	🐛 Call begin on camera service	2025-11-30 12:40:44 +01:00
Niklas Jensen	f9c28ed42a	Fix USE_ICM20948 checks in peripherals.cpp	2025-11-30 12:40:14 +01:00
Rune Harlyk	69dbea3fae	🐛 Call begin on camera service	2025-11-30 12:40:14 +01:00
Niklas Jensen	a24ab44b17	Added ICM20948 support	2025-11-30 12:40:14 +01:00
Rune Harlyk	9e02f8b8ee	Remove psychichttp	2025-11-27 16:45:30 +01:00