imu: read all sensors at once at 100 Hz

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