diff --git a/components/flow3r_bsp/flow3r_bsp_imu.c b/components/flow3r_bsp/flow3r_bsp_imu.c
index d2a020975d232ed3981c808307a8694329e552c4..a30a69b6e648e8f605d717e3dd18d370cb8b1dfa 100644
--- a/components/flow3r_bsp/flow3r_bsp_imu.c
+++ b/components/flow3r_bsp/flow3r_bsp_imu.c
@@ -25,14 +25,17 @@ static const char *TAG = "flow3r-imu";
static void bmi2_error_codes_print_result(int8_t rslt);
static void bmp5_error_codes_print_result(const char api_name[], int8_t rslt);
static int8_t set_accel_config(struct bmi2_dev *bmi);
+static int8_t set_gyro_config(struct bmi2_dev *bmi);
static int8_t set_bmp_config(
struct bmp5_osr_odr_press_config *osr_odr_press_cfg, struct bmp5_dev *dev);
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);
-// TODO: expose this, once we also expose the range of the acc
+// TODO: expose this, once we also expose the range of the acc / gyro
static esp_err_t flow3r_bsp_imu_read_acc(flow3r_bsp_imu_t *imu, int *x, int *y,
int *z);
-
+static esp_err_t flow3r_bsp_imu_read_gyro(flow3r_bsp_imu_t *imu, int *x, int *y,
+ int *z);
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;
@@ -150,11 +153,14 @@ esp_err_t flow3r_bsp_imu_init(flow3r_bsp_imu_t *imu) {
if (rslt != BMI2_OK) return ESP_FAIL;
rslt = set_accel_config(&(imu->bmi));
+ bmi2_error_codes_print_result(rslt);
+ if (rslt != BMI2_OK) return ESP_FAIL;
+ rslt = set_gyro_config(&(imu->bmi));
bmi2_error_codes_print_result(rslt);
if (rslt != BMI2_OK) return ESP_FAIL;
- uint8_t sensor_list[] = { BMI2_ACCEL, BMI2_AUX };
+ uint8_t sensor_list[] = { BMI2_ACCEL, BMI2_GYRO, BMI2_AUX };
rslt = bmi2_sensor_enable(sensor_list, sizeof(sensor_list), &(imu->bmi));
bmi2_error_codes_print_result(rslt);
if (rslt != BMI2_OK) return ESP_FAIL;
@@ -252,6 +258,43 @@ esp_err_t flow3r_bsp_imu_read_acc_mps(flow3r_bsp_imu_t *imu, float *x, float *y,
return res;
}
+static 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;
+ }
+ return ESP_FAIL;
+}
+
+esp_err_t flow3r_bsp_imu_read_gyro_dps(flow3r_bsp_imu_t *imu, float *x,
+ float *y, float *z) {
+ int ix, iy, iz;
+
+ esp_err_t res = flow3r_bsp_imu_read_gyro(imu, &ix, &iy, &iz);
+
+ if (res == ESP_OK) {
+ // TODO: un-hardcode the 2000 dps range
+ *x = lsb_to_dps(ix, 2000.f, imu->bmi.resolution);
+ *y = lsb_to_dps(iy, 2000.f, imu->bmi.resolution);
+ *z = lsb_to_dps(iz, 2000.f, imu->bmi.resolution);
+ }
+
+ return res;
+}
+
esp_err_t flow3r_bsp_imu_read_pressure(flow3r_bsp_imu_t *imu, float *pressure,
float *temperature) {
int8_t rslt = 0;
@@ -619,6 +662,40 @@ static int8_t set_accel_config(struct bmi2_dev *bmi) {
return rslt;
}
+static int8_t set_gyro_config(struct bmi2_dev *bmi) {
+ int8_t rslt;
+ struct bmi2_sens_config config;
+
+ config.type = BMI2_GYRO;
+
+ /* Get default configurations for the type of feature selected. */
+ rslt = bmi2_get_sensor_config(&config, 1, bmi);
+ bmi2_error_codes_print_result(rslt);
+
+ if (rslt == BMI2_OK) {
+ // Output Data Rate
+ config.cfg.gyr.odr = BMI2_GYR_ODR_100HZ;
+
+ // Measurement Range. By default the range is 2000 deg per second.
+ config.cfg.gyr.range = BMI2_GYR_RANGE_2000;
+
+ config.cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE;
+
+ // Full performance mode
+ config.cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE;
+ config.cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE;
+
+ rslt = bmi2_set_sensor_config(&config, 1, bmi);
+ bmi2_error_codes_print_result(rslt);
+
+ /* Map data ready interrupt to interrupt pin. */
+ rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi);
+ bmi2_error_codes_print_result(rslt);
+ }
+
+ return rslt;
+}
+
static int8_t set_bmp_config(
struct bmp5_osr_odr_press_config *osr_odr_press_cfg, struct bmp5_dev *dev) {
int8_t rslt;
@@ -689,3 +766,13 @@ static float lsb_to_mps(int16_t val, float g_range, uint8_t bit_width) {
return (GRAVITY_EARTH * val * g_range) / half_scale;
}
+
+// Convert lsb to degree per second for 16 bit gyro at
+// range 125, 250, 500, 1000 or 2000dps.
+static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) {
+ double power = 2;
+
+ float half_scale = (float)((pow((double)power, (double)bit_width) / 2.0f));
+
+ return (dps / (half_scale)) * (val);
+}
diff --git a/components/flow3r_bsp/flow3r_bsp_imu.h b/components/flow3r_bsp/flow3r_bsp_imu.h
index b65248860e237d23a87df732c1de01c2a6d200b2..8b857e519230f36f6bc10624b0ba5d4ecf9c6de2 100644
--- a/components/flow3r_bsp/flow3r_bsp_imu.h
+++ b/components/flow3r_bsp/flow3r_bsp_imu.h
@@ -30,7 +30,13 @@ esp_err_t flow3r_bsp_imu_init(flow3r_bsp_imu_t *imu);
// 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.
+//
+// This directly calls the I2C bus and need to lock the bus for that.
+// 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.
//
// Return cached data if no new reading is available.
diff --git a/components/micropython/usermodule/mp_imu.c b/components/micropython/usermodule/mp_imu.c
index e72de8e1c6ac1d5d1ab12a3cf122fd8ee3421fa6..654289b1c1fe387ee586d928eb9b84e6f42bf2f0 100644
--- a/components/micropython/usermodule/mp_imu.c
+++ b/components/micropython/usermodule/mp_imu.c
@@ -16,6 +16,19 @@ STATIC mp_obj_t mp_imu_acc_read(void) {
STATIC MP_DEFINE_CONST_FUN_OBJ_0(mp_imu_acc_read_obj, mp_imu_acc_read);
+STATIC mp_obj_t mp_imu_gyro_read(void) {
+ static float x, y, z;
+
+ // Will not overwrite old data if there is an error
+ st3m_imu_read_gyro_dps(&x, &y, &z);
+
+ mp_obj_t items[3] = { mp_obj_new_float(x), mp_obj_new_float(y),
+ mp_obj_new_float(z) };
+ return mp_obj_new_tuple(3, items);
+}
+
+STATIC MP_DEFINE_CONST_FUN_OBJ_0(mp_imu_gyro_read_obj, mp_imu_gyro_read);
+
STATIC mp_obj_t mp_imu_pressure_read(void) {
static float pressure, temperature;
@@ -32,6 +45,7 @@ STATIC MP_DEFINE_CONST_FUN_OBJ_0(mp_imu_pressure_read_obj,
STATIC const mp_rom_map_elem_t globals_table[] = {
{ MP_ROM_QSTR(MP_QSTR_acc_read), MP_ROM_PTR(&mp_imu_acc_read_obj) },
+ { MP_ROM_QSTR(MP_QSTR_gyro_read), MP_ROM_PTR(&mp_imu_gyro_read_obj) },
{ MP_ROM_QSTR(MP_QSTR_pressure_read),
MP_ROM_PTR(&mp_imu_pressure_read_obj) },
};
diff --git a/components/st3m/st3m_imu.c b/components/st3m/st3m_imu.c
index 4cd9b0cb9c35963ce70448a90d3e693491a66b94..dcf975a3facd7257367f3e8220da338232ad0ce6 100644
--- a/components/st3m/st3m_imu.c
+++ b/components/st3m/st3m_imu.c
@@ -23,6 +23,10 @@ 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);
}
+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);
+}
+
esp_err_t st3m_imu_read_pressure(float *pressure, float *temperature) {
return flow3r_bsp_imu_read_pressure(&imu, pressure, temperature);
}
diff --git a/components/st3m/st3m_imu.h b/components/st3m/st3m_imu.h
index 13e31b8c55d2e0ec28048d9760a48a45b812fd60..8af13fbb6b1e5c137668de9fbe8f070a00452182 100644
--- a/components/st3m/st3m_imu.h
+++ b/components/st3m/st3m_imu.h
@@ -8,4 +8,5 @@
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);