diff --git a/app/src/lib/stores/imu.ts b/app/src/lib/stores/imu.ts index 12ee5e1..3bf1491 100644 --- a/app/src/lib/stores/imu.ts +++ b/app/src/lib/stores/imu.ts @@ -1,5 +1,5 @@ import { writable } from 'svelte/store' -import type { IMU } from '$lib/types/models' +import type { IMUMsg } from '$lib/types/models' const maxIMUData = 100 @@ -14,11 +14,24 @@ export const imu = (() => { bmp_temp: [] as number[] }) - const addData = (content: IMU) => { + const addData = (content: IMUMsg) => { update(data => { - ;(Object.keys(content) as (keyof IMU)[]).forEach(key => { - data[key] = [...data[key], content[key]].slice(-maxIMUData) - }) + if (content.imu && content.imu[3]) { + data.x = [...data.x, content.imu[0]].slice(-maxIMUData) + data.y = [...data.y, content.imu[1]].slice(-maxIMUData) + data.z = [...data.z, content.imu[2]].slice(-maxIMUData) + } + + if (content.mag && content.mag[4]) { + data.heading = [...data.heading, content.mag[3]].slice(-maxIMUData) + } + + if (content.bmp && content.bmp[3]) { + data.pressure = [...data.pressure, content.bmp[0]].slice(-maxIMUData) + data.altitude = [...data.altitude, content.bmp[1]].slice(-maxIMUData) + data.bmp_temp = [...data.bmp_temp, content.bmp[2]].slice(-maxIMUData) + } + return data }) } diff --git a/app/src/lib/types/models.ts b/app/src/lib/types/models.ts index 616b222..85ab164 100644 --- a/app/src/lib/types/models.ts +++ b/app/src/lib/types/models.ts @@ -154,6 +154,12 @@ export type IMU = { pressure: number } +export type IMUMsg = { + imu: [number, number, number, boolean] + mag: [number, number, number, number, boolean] + bmp: [number, number, number, boolean] +} + export interface I2CDevice { address: number part_number: string diff --git a/app/src/routes/peripherals/imu/imu.svelte b/app/src/routes/peripherals/imu/imu.svelte index f038a8e..38b75d0 100644 --- a/app/src/routes/peripherals/imu/imu.svelte +++ b/app/src/routes/peripherals/imu/imu.svelte @@ -6,7 +6,7 @@ import { slide } from 'svelte/transition' import { onDestroy, onMount } from 'svelte' import { socket } from '$lib/stores' - import { MessageTopic, type IMU } from '$lib/types/models' + import { MessageTopic, type IMUMsg } from '$lib/types/models' import { useFeatureFlags } from '$lib/stores/featureFlags' import { Rotate3d } from '$lib/components/icons' @@ -201,7 +201,7 @@ } onMount(() => { - socket.on(MessageTopic.imu, (data: IMU) => { + socket.on(MessageTopic.imu, (data: IMUMsg) => { console.log(data) imu.addData(data) }) diff --git a/esp32/src/main.cpp b/esp32/src/main.cpp index a536cca..044446e 100644 --- a/esp32/src/main.cpp +++ b/esp32/src/main.cpp @@ -185,6 +185,12 @@ void IRAM_ATTR SpotControlLoopEntry(void *) { #if FT_ENABLED(USE_WS2812) ledService.loop(); #endif + EXECUTE_EVERY_N_MS(250, [&]() { + JsonDocument doc; + JsonVariant results = doc.to(); + peripherals.getIMUResult(results); + socket.emit(EVENT_IMU, results); + }); vTaskDelayUntil(&xLastWakeTime, xFrequency); } } diff --git a/esp32/src/peripherals/peripherals.cpp b/esp32/src/peripherals/peripherals.cpp index 87be590..ff02c26 100644 --- a/esp32/src/peripherals/peripherals.cpp +++ b/esp32/src/peripherals/peripherals.cpp @@ -166,13 +166,16 @@ float Peripherals::rightDistance() { return _right_distance; } void Peripherals::getIMUResult(JsonVariant &root) { #if FT_ENABLED(USE_MPU6050 || USE_BNO055 || USE_ICM20948) - _imu.getResults(root); + JsonVariant imu = root["imu"].to(); + _imu.getResults(imu); #endif #if FT_ENABLED(USE_HMC5883 || USE_ICM20948) - _mag.getResults(root); + JsonVariant mag = root["mag"].to(); + _mag.getResults(mag); #endif #if FT_ENABLED(USE_BMP180) - _bmp.getResults(root); + JsonVariant bmp = root["bmp"].to(); + _bmp.getResults(bmp); #endif }