imu: read all sensors at once at 100 Hz

Introduces an update function to the BSP IMU implementation which
fetches the complete state of the BMI270 at once. This includes memory
range of the auxilary pressure sensor.

Introduces a task in st3m which updates the sensor readings every 10 ms.
Priority is chosen to be below captouch and audio.

BSP functions let the user know if there actually was a new reading on
the specific sensor. st3m always return the last measurement without
this extra information.

components/flow3r_bsp/flow3r_bsp_imu.c

@@ -30,6 +30,8 @@ static int8_t set_bmp_config(
 static float lsb_to_mps(int16_t val, float g_range, uint8_t bit_width);
 static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width);
 
+static struct bmi2_sens_data _bmi_sens_data;
+
 static BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data,
                                            uint32_t len, void *intf_ptr) {
     flow3r_bsp_imu_t *imu = (flow3r_bsp_imu_t *)intf_ptr;
@@ -83,11 +85,11 @@ static BMP5_INTF_RET_TYPE bmp5_i2c_read(uint8_t reg_addr, uint8_t *reg_data,
     int8_t rslt;
 
     // The BMI270 is configured to automatically fetch the sensor readings
-    // from the BMP581. We can read them directly from BMI270 memory, avoiding
+    // from the BMP581. We can read them directly from BMI270 state, avoiding
     // extra I2C transactions.
-    if (reg_addr >= BMP5_REG_TEMP_DATA_XLSB &&
-        reg_addr + length <= BMP5_REG_TEMP_DATA_XLSB + 6) {
-        rslt = bmi2_i2c_read(BMI2_AUX_X_LSB_ADDR, reg_data, length, intf_ptr);
+    if (reg_addr == BMP5_REG_TEMP_DATA_XLSB && length <= 6) {
+        memcpy(reg_data, _bmi_sens_data.aux_data, length);
+        rslt = BMI2_OK;
     } else {
         rslt = bmi2_read_aux_man_mode(reg_addr, reg_data, (uint16_t)length,
                                       &imu->bmi);
@@ -221,25 +223,28 @@ esp_err_t flow3r_bsp_imu_init(flow3r_bsp_imu_t *imu) {
     return ESP_OK;
 }
 
-esp_err_t flow3r_bsp_imu_read_acc(flow3r_bsp_imu_t *imu, int *x, int *y,
-                                  int *z) {
+esp_err_t flow3r_bsp_imu_update(flow3r_bsp_imu_t *imu) {
     struct bmi2_sens_data sens_data = {
         0,
     };
-
     int8_t rslt = bmi2_get_sensor_data(&sens_data, &(imu->bmi));
     bmi2_error_codes_print_result(rslt);
 
     if (rslt == BMI2_OK) {
-        if (sens_data.status & BMI2_DRDY_ACC) {
-            *x = sens_data.acc.x;
-            *y = sens_data.acc.y;
-            *z = sens_data.acc.z;
-            return ESP_OK;
-        }
-        return ESP_ERR_NOT_FOUND;
+        _bmi_sens_data = sens_data;
+    }
+    return rslt;
+}
+
+esp_err_t flow3r_bsp_imu_read_acc(flow3r_bsp_imu_t *imu, int *x, int *y,
+                                  int *z) {
+    if (_bmi_sens_data.status & BMI2_DRDY_ACC) {
+        *x = _bmi_sens_data.acc.x;
+        *y = _bmi_sens_data.acc.y;
+        *z = _bmi_sens_data.acc.z;
+        return ESP_OK;
     }
-    return ESP_FAIL;
+    return ESP_ERR_NOT_FOUND;
 }
 
 esp_err_t flow3r_bsp_imu_read_acc_mps(flow3r_bsp_imu_t *imu, float *x, float *y,
@@ -259,23 +264,13 @@ esp_err_t flow3r_bsp_imu_read_acc_mps(flow3r_bsp_imu_t *imu, float *x, float *y,
 
 esp_err_t flow3r_bsp_imu_read_gyro(flow3r_bsp_imu_t *imu, int *x, int *y,
                                    int *z) {
-    struct bmi2_sens_data sens_data = {
-        0,
-    };
-
-    int8_t rslt = bmi2_get_sensor_data(&sens_data, &(imu->bmi));
-    bmi2_error_codes_print_result(rslt);
-
-    if (rslt == BMI2_OK) {
-        if (sens_data.status & BMI2_DRDY_ACC) {
-            *x = sens_data.gyr.x;
-            *y = sens_data.gyr.y;
-            *z = sens_data.gyr.z;
-            return ESP_OK;
-        }
-        return ESP_ERR_NOT_FOUND;
+    if (_bmi_sens_data.status & BMI2_DRDY_GYR) {
+        *x = _bmi_sens_data.gyr.x;
+        *y = _bmi_sens_data.gyr.y;
+        *z = _bmi_sens_data.gyr.z;
+        return ESP_OK;
     }
-    return ESP_FAIL;
+    return ESP_ERR_NOT_FOUND;
 }
 
 esp_err_t flow3r_bsp_imu_read_gyro_dps(flow3r_bsp_imu_t *imu, float *x,

components/flow3r_bsp/flow3r_bsp_imu.h

@@ -20,7 +20,7 @@ typedef struct {
     struct bmp5_osr_odr_press_config osr_odr_press_cfg;
 } flow3r_bsp_imu_t;
 
-// Init the IMU to default settings
+// Init the IMU with default settings
 //
 // Configures the IMU to:
 // Accelerometer: 100 Hz sample rate, 2 g range
@@ -28,47 +28,46 @@ typedef struct {
 // Pressure sensor: 50 Hz sample rate
 esp_err_t flow3r_bsp_imu_init(flow3r_bsp_imu_t *imu);
 
-// Query the IMU for an accelerometer reading.
+// Update the IMU readings by reading data from the I2C bus.
 //
 // This directly calls the I2C bus and waits until the bus is available (max 1
-// second). Returns ESP_ERR_NOT_FOUND if there is no new reading available.
+// second).
 // Returns ESP_FAIL if the sensor could not be read (e.g. I2C unavailable).
+esp_err_t flow3r_bsp_imu_update(flow3r_bsp_imu_t *imu);
+
+// Get an accelerometer reading.
+//
+// Returns ESP_ERR_NOT_FOUND if there is no new reading available.
 // Return values are raw data from the BMI270.
 // Use imu->acc_range and imu->bmi.resolution for interpretation.
 esp_err_t flow3r_bsp_imu_read_acc(flow3r_bsp_imu_t *imu, int *x, int *y,
                                   int *z);
 
-// Query the IMU for a converted accelerometer reading.
+// Get aa converted accelerometer reading.
 //
-// This directly calls the I2C bus and waits until the bus is available (max 1
-// second). Returns ESP_ERR_NOT_FOUND if there is no new reading available.
-// Returns ESP_FAIL if the sensor could not be read (e.g. I2C unavailable).
+// Returns ESP_ERR_NOT_FOUND if there is no new reading available.
 // Return values in m/s**2.
 esp_err_t flow3r_bsp_imu_read_acc_mps(flow3r_bsp_imu_t *imu, float *x, float *y,
                                       float *z);
 
-// Query the IMU for a gyroscope reading.
+// Get a gyroscope reading.
 //
-// This directly calls the I2C bus and waits until the bus is available (max 1
-// second). Returns ESP_ERR_NOT_FOUND if there is no new reading available.
-// Returns ESP_FAIL if the sensor could not be read (e.g. I2C unavailable).
+// Returns ESP_ERR_NOT_FOUND if there is no new reading available.
 // Return values are raw data from the BMI270.
 // Use imu->gyro_range and imu->bmi.resolution for interpretation.
 esp_err_t flow3r_bsp_imu_read_gyro(flow3r_bsp_imu_t *imu, int *x, int *y,
                                    int *z);
 
-// Query the IMU for a converted gyroscope reading.
+// Get converted gyroscope reading.
 //
-// This directly calls the I2C bus and waits until the bus is available (max 1
-// second). Returns ESP_ERR_NOT_FOUND if there is no new reading available.
-// Returns ESP_FAIL if the sensor could not be read (e.g. I2C unavailable).
+// Returns ESP_ERR_NOT_FOUND if there is no new reading available.
 // Return values in deg/s.
 esp_err_t flow3r_bsp_imu_read_gyro_dps(flow3r_bsp_imu_t *imu, float *x,
                                        float *y, float *z);
 
-// Query the IMU for a pressure sensor reading.
+// Get a pressure sensor reading.
 //
-// Returns cached data if no new reading is available.
+// Returns ESP_ERR_NOT_FOUND if there is no new reading available.
 // Presssure in Pa, temperature in deg C
 esp_err_t flow3r_bsp_imu_read_pressure(flow3r_bsp_imu_t *imu, float *pressure,
                                        float *temperature);

components/st3m/st3m_imu.c

@@ -2,31 +2,94 @@
 
 #include "flow3r_bsp_imu.h"
 
-#include "flow3r_bsp.h"
-
-#include "esp_system.h"
-#include "esp_task.h"
+static const char *TAG = "st3m-imu";
+#include "esp_err.h"
+#include "esp_log.h"
 
 #include "freertos/FreeRTOS.h"
 #include "freertos/semphr.h"
 #include "freertos/task.h"
 
-#include <math.h>
-#include <stdio.h>
-#include <string.h>
+static void _task(void *data);
+
+static flow3r_bsp_imu_t _imu;
+
+static SemaphoreHandle_t mutex;
+#define LOCK xSemaphoreTake(mutex, portMAX_DELAY)
+#define UNLOCK xSemaphoreGive(mutex)
 
-static flow3r_bsp_imu_t imu;
+static float _acc_x, _acc_y, _acc_z;
+static float _gyro_x, _gyro_y, _gyro_z;
+static float _pressure;
+static float _temperature;
 
-void st3m_imu_init() { flow3r_bsp_imu_init(&imu); }
+void st3m_imu_init() {
+    mutex = xSemaphoreCreateMutex();
+    assert(mutex != NULL);
+
+    esp_err_t ret = flow3r_bsp_imu_init(&_imu);
+    if (ret != ESP_OK) {
+        ESP_LOGE(TAG, "IMU init failed: %s", esp_err_to_name(ret));
+        return;
+    }
+
+    xTaskCreate(&_task, "imu", 4096, NULL, configMAX_PRIORITIES - 2, NULL);
+    ESP_LOGI(TAG, "IMU task started");
+}
 
-esp_err_t st3m_imu_read_acc_mps(float *x, float *y, float *z) {
-    return flow3r_bsp_imu_read_acc_mps(&imu, x, y, z);
+void st3m_imu_read_acc_mps(float *x, float *y, float *z) {
+    LOCK;
+    *x = _acc_x;
+    *y = _acc_y;
+    *z = _acc_z;
+    UNLOCK;
 }
 
-esp_err_t st3m_imu_read_gyro_dps(float *x, float *y, float *z) {
-    return flow3r_bsp_imu_read_gyro_dps(&imu, x, y, z);
+void st3m_imu_read_gyro_dps(float *x, float *y, float *z) {
+    LOCK;
+    *x = _gyro_x;
+    *y = _gyro_y;
+    *z = _gyro_z;
+    UNLOCK;
 }
 
-esp_err_t st3m_imu_read_pressure(float *pressure, float *temperature) {
-    return flow3r_bsp_imu_read_pressure(&imu, pressure, temperature);
+void st3m_imu_read_pressure(float *pressure, float *temperature) {
+    LOCK;
+    *pressure = _pressure;
+    *temperature = _temperature;
+    UNLOCK;
+}
+
+static void _task(void *data) {
+    TickType_t last_wake = xTaskGetTickCount();
+    esp_err_t ret;
+    float a, b, c;
+    while (1) {
+        vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(10));  // 100 Hz
+
+        ret = flow3r_bsp_imu_update(&_imu);
+        if (ret == ESP_OK) {
+            LOCK;
+            ret = flow3r_bsp_imu_read_acc_mps(&_imu, &a, &b, &c);
+            if (ret == ESP_OK) {
+                _acc_x = a;
+                _acc_y = b;
+                _acc_z = c;
+            }
+
+            flow3r_bsp_imu_read_gyro_dps(&_imu, &a, &b, &c);
+            if (ret == ESP_OK) {
+                _gyro_x = a;
+                _gyro_y = b;
+                _gyro_z = c;
+            }
+
+            flow3r_bsp_imu_read_pressure(&_imu, &a, &b);
+            if (ret == ESP_OK) {
+                _pressure = a;
+                _temperature = b;
+            }
+            UNLOCK;
+        }
+    }
 }

components/st3m/st3m_imu.h

@@ -7,6 +7,6 @@
 
 void st3m_imu_init(void);
 
-esp_err_t st3m_imu_read_acc_mps(float *x, float *y, float *z);
-esp_err_t st3m_imu_read_gyro_dps(float *x, float *y, float *z);
-esp_err_t st3m_imu_read_pressure(float *pressure, float *temperature);
+void st3m_imu_read_acc_mps(float *x, float *y, float *z);
+void st3m_imu_read_gyro_dps(float *x, float *y, float *z);
+void st3m_imu_read_pressure(float *pressure, float *temperature);