reduce latency

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;
             }
 

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;

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

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

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
 

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
 }