From a04656c7c4a2f84798b45e857c86ae22c95718df Mon Sep 17 00:00:00 2001
From: schneider <schneider@blinkenlichts.net>
Date: Fri, 4 Aug 2023 23:42:56 +0200
Subject: [PATCH] imu: read all sensors at once at 100 Hz
---
components/flow3r_bsp/flow3r_bsp_imu.c | 57 +++++++---------
components/flow3r_bsp/flow3r_bsp_imu.h | 33 +++++----
components/st3m/st3m_imu.c | 93 +++++++++++++++++++++-----
components/st3m/st3m_imu.h | 6 +-
4 files changed, 123 insertions(+), 66 deletions(-)
diff --git a/components/flow3r_bsp/flow3r_bsp_imu.c b/components/flow3r_bsp/flow3r_bsp_imu.c
index e4439465f0..846ac9bbc2 100644
--- a/components/flow3r_bsp/flow3r_bsp_imu.c
+++ b/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,
diff --git a/components/flow3r_bsp/flow3r_bsp_imu.h b/components/flow3r_bsp/flow3r_bsp_imu.h
index 93535c6ff6..bdfea3e13c 100644
--- a/components/flow3r_bsp/flow3r_bsp_imu.h
+++ b/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);
diff --git a/components/st3m/st3m_imu.c b/components/st3m/st3m_imu.c
index dcf975a3fa..c21ec0f2bb 100644
--- a/components/st3m/st3m_imu.c
+++ b/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;
+ }
+ }
}
diff --git a/components/st3m/st3m_imu.h b/components/st3m/st3m_imu.h
index 8af13fbb6b..9201b4c259 100644
--- a/components/st3m/st3m_imu.h
+++ b/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);
--
GitLab