From 079bf808ce6cf984f18f6778f03b9127464a5836 Mon Sep 17 00:00:00 2001 From: "T.T. Tang" Date: Wed, 1 Jan 2025 22:31:29 +0800 Subject: [PATCH 1/2] reduce delay caused by timers --- components/apps/monitor.c | 3 ++- components/apps/network.c | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/components/apps/monitor.c b/components/apps/monitor.c index bb28114..893cfb7 100644 --- a/components/apps/monitor.c +++ b/components/apps/monitor.c @@ -80,10 +80,11 @@ _Noreturn void app_monitor(void *pvParameters) { esp_err_t err = g_imu.read(g_imu.p_imu, &imu_data, true); /** If the err occurs(most likely due to the empty uart buffer), wait **/ + /** This is theoretically impossible */ if (err != ESP_OK) { ESP_LOGD(TAG, "IMU read error: %d", err); taskYIELD(); - os_delay_ms(5); + os_delay_ms(1000 / g_mcu.target_fps / 2); continue; } diff --git a/components/apps/network.c b/components/apps/network.c index 5e0ba7a..d2fc22a 100644 --- a/components/apps/network.c +++ b/components/apps/network.c @@ -136,7 +136,7 @@ _Noreturn void app_data_client(void *pvParameters) { int64_t confirm_index = -1; xQueueReset(read_signal_queue); - esp_timer_start_periodic(read_timer, 1000000 / g_mcu.target_fps); + esp_timer_start_periodic(read_timer, 1000000 / g_mcu.target_fps / 2); imu_dgram_t imu_reading = {0}; char signal = 0; From 599a8b2701638ad39d4cb7328470f1a710fa8d2b Mon Sep 17 00:00:00 2001 From: "T.T. Tang" Date: Wed, 1 Jan 2025 22:31:58 +0800 Subject: [PATCH 2/2] prototype support for IMU callbacks --- components/sys/sys.c | 4 ++++ include/settings.h | 3 ++- lib/bno08x/bno08x.c | 6 +++--- main/app_main.c | 44 ++++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 53 insertions(+), 4 deletions(-) diff --git a/components/sys/sys.c b/components/sys/sys.c index f129701..aa4ef15 100644 --- a/components/sys/sys.c +++ b/components/sys/sys.c @@ -234,7 +234,9 @@ static _Noreturn void app_log_trace(void * pvParameters) { void sys_start_tasks(void) { #if CONFIG_EN_MULTI_CORE launch_task_multicore(app_data_client, "app_data_client", 4096, NULL, 10, g_mcu.tasks.app_data_client_task, 0x1); +#if !CONFIG_EN_IMU_CB launch_task_multicore(app_monitor, "app_monitor", 4096, NULL, 13, g_mcu.tasks.app_monitor_task, 0x0); +#endif launch_task_multicore(app_system_loop, "app_system_loop", 4096, NULL, 8, g_mcu.tasks.app_system_loop_task, 0x0); #if CONFIG_EN_PROFILING launch_task_multicore(app_log_trace, "app_log_trace", 4096, NULL, 5, g_mcu.tasks.app_log_trace_task, 0x0); @@ -257,8 +259,10 @@ void sys_start_tasks(void) { void sys_stop_tasks(void) { vTaskDelete(g_mcu.tasks.app_system_loop_task); g_mcu.tasks.app_system_loop_task = NULL; +#if !CONFIG_EN_IMU_CB vTaskDelete(g_mcu.tasks.app_monitor_task); g_mcu.tasks.app_monitor_task = NULL; +#endif vTaskDelete(g_mcu.tasks.app_data_client_task); g_mcu.tasks.app_data_client_task = NULL; #if CONFIG_PROFILING_ENABLED diff --git a/include/settings.h b/include/settings.h index 784dc30..fc694ad 100644 --- a/include/settings.h +++ b/include/settings.h @@ -6,8 +6,9 @@ #define CONFIG_MAX_FPS 400 #define CONFIG_EN_MULTI_CORE 1 #define CONFIG_EN_PROFILING 0 -#define CONFIG_EN_FPS_PROFILING 1 +#define CONFIG_EN_FPS_PROFILING 0 #define CONFIG_EN_READ_FPS_LIM 1 +#define CONFIG_EN_IMU_CB 0 /** Hardware related settings **/ #define CONFIG_DATA_BUF_LEN 128 diff --git a/lib/bno08x/bno08x.c b/lib/bno08x/bno08x.c index f7c5cba..89b31e8 100644 --- a/lib/bno08x/bno08x.c +++ b/lib/bno08x/bno08x.c @@ -42,11 +42,11 @@ static esp_err_t bno08x_config(imu_t *p_imu) { } #if CONFIG_USE_LINEAR_ACCELERATION - BNO08x_enable_linear_accelerometer(p_driver, 1000 / p_imu->target_fps); // 100Hz + BNO08x_enable_linear_accelerometer(p_driver, 1000000 / p_imu->target_fps); // 100Hz #else - BNO08x_enable_accelerometer(p_driver, 1000 / p_imu->target_fps); // 100Hz + BNO08x_enable_accelerometer(p_driver, 1000000 / p_imu->target_fps); // 100Hz #endif - BNO08x_enable_rotation_vector(p_driver, 1000 / p_imu->target_fps); // 100Hz + BNO08x_enable_rotation_vector(p_driver, 1000000 / p_imu->target_fps); // 100Hz BNO08x_enable_step_counter(p_driver, CONFIG_BNO08X_SLOW_INTERVAL_MS); // 2Hz BNO08x_enable_stability_classifier(p_driver, CONFIG_BNO08X_SLOW_INTERVAL_MS); // 2Hz diff --git a/main/app_main.c b/main/app_main.c index 737d9b8..85c7402 100644 --- a/main/app_main.c +++ b/main/app_main.c @@ -14,10 +14,47 @@ #include "sys.h" #include "ring_buf.h" +#if CONFIG_EN_IMU_CB +#include "bno08x.h" +#include "bno08x_driver.h" +#endif + static const char *TAG = "app_main"; static uint8_t s_data_buf[CONFIG_DATA_BUF_LEN * sizeof(imu_dgram_t)]; +#if CONFIG_EN_IMU_CB +typedef struct { + imu_t base; + BNO08x driver; +} bno08x_t; + +/** Get a ring buffer pointer **/ +ring_buf_t *serial_buf = &g_mcu.imu_ring_buf; + +/** Create a imu data structure **/ +imu_dgram_t imu_data = {0}; +uint32_t seq = 0; + +void imu_data_cb(void *arg) +{ + /** Read IMU data **/ + esp_err_t err = g_imu.read(g_imu.p_imu, &imu_data, true); + + /** If the err occurs(most likely due to the empty uart buffer), wait **/ + /** but it should be impossible */ + if (err != ESP_OK) { + ESP_LOGD(TAG, "IMU read error: %d", err); + } + + /** Tag seq number, timestamp, buffer_len(how many bits are left in the buffer) **/ + imu_data.seq = seq++; + + /** Add the imu data to the ring buffer **/ + ring_buf_push(serial_buf, (uint8_t *) &imu_data); +} +#endif + static void init() { sys_init_chip(); sys_init_nvs(); // Initialize NVS @@ -41,6 +78,13 @@ static void init() { /** Test IMU availability, must run at the end of init() **/ g_imu.self_test(g_imu.p_imu); + +#if CONFIG_EN_IMU_CB + bno08x_t *p_bno08x = (bno08x_t *) g_imu.p_imu; + BNO08x *p_driver = &p_bno08x->driver; + /** register the IMU callback */ + BNO08x_register_cb(p_driver, imu_data_cb); +#endif }