Add step counter to IMU API

components/flow3r_bsp/flow3r_bsp_imu.c

@@ -23,10 +23,12 @@ static const char *TAG = "flow3r-imu";
 static void bmi2_error_codes_print_result(int8_t rslt);
 static int8_t set_accel_config(flow3r_bsp_imu_t *imu);
 static int8_t set_gyro_config(flow3r_bsp_imu_t *imu);
+static int8_t set_step_counter_config(flow3r_bsp_imu_t *imu);
 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 struct bmi2_feat_sensor_data _bmi_feat_data = {.type = BMI2_STEP_COUNTER};
 
 static tildagon_mux_i2c_obj_t* mux;
 
@@ -120,8 +122,18 @@ esp_err_t flow3r_bsp_imu_init(flow3r_bsp_imu_t *imu) {
     bmi2_error_codes_print_result(rslt);
     if (rslt != BMI2_OK) return ESP_FAIL;
 
-    uint8_t sensor_list[] = { BMI2_ACCEL, BMI2_GYRO };
-    rslt = bmi2_sensor_enable(sensor_list, sizeof(sensor_list), &(imu->bmi));
+    uint8_t sensor_list[] = { BMI2_ACCEL, BMI2_GYRO, BMI2_STEP_COUNTER };
+
+    rslt = bmi270_sensor_enable(sensor_list, sizeof(sensor_list), &(imu->bmi));
+    bmi2_error_codes_print_result(rslt);
+    if (rslt != BMI2_OK) return ESP_FAIL;
+
+    rslt = set_step_counter_config(imu);
+    bmi2_error_codes_print_result(rslt);
+    if (rslt != BMI2_OK) return ESP_FAIL;
+
+    struct bmi2_sens_int_config sens_int = { .type = BMI2_STEP_COUNTER, .hw_int_pin = BMI2_INT1 };
+    rslt = bmi270_map_feat_int(&sens_int, 1, &(imu->bmi));
     bmi2_error_codes_print_result(rslt);
     if (rslt != BMI2_OK) return ESP_FAIL;
 
@@ -205,6 +217,26 @@ esp_err_t flow3r_bsp_imu_read_gyro_dps(flow3r_bsp_imu_t *imu, float *x,
     return res;
 }
 
+esp_err_t flow3r_bsp_imu_read_steps(flow3r_bsp_imu_t *imu, uint32_t *steps) {
+    uint16_t int_status;
+
+    int8_t rslt = bmi2_get_int_status(&int_status, &(imu->bmi));
+    bmi2_error_codes_print_result(rslt);
+    if (rslt != BMI2_OK) return ESP_FAIL;
+
+    if (int_status & BMI270_STEP_CNT_STATUS_MASK)
+    {
+        /* Step counter interrupt occurred when watermark level (20 steps) is reached */
+        rslt = bmi270_get_feature_data(&_bmi_feat_data, 1, &(imu->bmi));
+        bmi2_error_codes_print_result(rslt);
+        if (rslt != BMI2_OK) return ESP_FAIL;
+
+        *steps = _bmi_feat_data.sens_data.step_counter_output;
+        return ESP_OK;
+    }
+    return ESP_ERR_NOT_FOUND;
+}
+
 /*!
  *  @brief Prints the execution status of the APIs.
  */
@@ -593,6 +625,27 @@ static int8_t set_gyro_config(flow3r_bsp_imu_t *imu) {
     return rslt;
 }
 
+static int8_t set_step_counter_config(flow3r_bsp_imu_t *imu)
+{
+    int8_t rslt;
+    struct bmi2_sens_config config;
+
+    config.type = BMI2_STEP_COUNTER;
+
+    rslt = bmi270_get_sensor_config(&config, 1, &imu->bmi);
+    bmi2_error_codes_print_result(rslt);
+
+    if (rslt == BMI2_OK)
+    {
+        config.cfg.step_counter.watermark_level = 1;
+
+        rslt = bmi270_set_sensor_config(&config, 1, &imu->bmi);
+        bmi2_error_codes_print_result(rslt);
+    }
+
+    return rslt;
+}
+
 // Convert raw measurements to meters / second
 static float lsb_to_mps(int16_t val, float g_range, uint8_t bit_width) {
     double power = 2;

components/flow3r_bsp/flow3r_bsp_imu.h

@@ -59,4 +59,11 @@ esp_err_t flow3r_bsp_imu_read_gyro(flow3r_bsp_imu_t *imu, int *x, int *y,
 // 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);
+                                       float *y, float *z);
+
+// Get step count.
+//
+// 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).
+// Retrurns the number of steps counted.
+esp_err_t flow3r_bsp_imu_read_steps(flow3r_bsp_imu_t *imu, uint32_t *steps);

components/st3m/st3m_imu.c

@@ -20,6 +20,7 @@ static SemaphoreHandle_t _mu;
 
 static float _acc_x, _acc_y, _acc_z;
 static float _gyro_x, _gyro_y, _gyro_z;
+static uint32_t _steps;
 
 void st3m_imu_init() {
     _mu = xSemaphoreCreateMutex();
@@ -51,11 +52,17 @@ void st3m_imu_read_gyro_dps(float *x, float *y, float *z) {
     UNLOCK;
 }
 
+void st3m_imu_read_steps(uint32_t *steps) {
+    LOCK;
+    *steps = _steps;
+    UNLOCK;
+}
 
 static void _task(void *data) {
     TickType_t last_wake = xTaskGetTickCount();
     esp_err_t ret;
     float a, b, c;
+    uint32_t steps;
     while (1) {
         vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(10));  // 100 Hz
 
@@ -72,13 +79,18 @@ static void _task(void *data) {
             _acc_z = c;
         }
 
-        flow3r_bsp_imu_read_gyro_dps(&_imu, &a, &b, &c);
+        ret = flow3r_bsp_imu_read_gyro_dps(&_imu, &a, &b, &c);
         if (ret == ESP_OK) {
             _gyro_x = a;
             _gyro_y = b;
             _gyro_z = c;
         }
 
+        ret = flow3r_bsp_imu_read_steps(&_imu, &steps);
+        if (ret == ESP_OK) {
+            _steps = steps;
+        }
+
         UNLOCK;
     }
 }

components/st3m/st3m_imu.h

@@ -10,3 +10,4 @@ void st3m_imu_init(void);
 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);
+void st3m_imu_read_steps(uint32_t *steps);

drivers/tildagon_helpers/mp_imu.c

@@ -29,9 +29,20 @@ static mp_obj_t mp_imu_gyro_read(void) {
 
 static MP_DEFINE_CONST_FUN_OBJ_0(mp_imu_gyro_read_obj, mp_imu_gyro_read);
 
+static mp_obj_t mp_imu_step_counter_read(void) {
+    static uint32_t steps;
+
+    st3m_imu_read_steps(&steps);
+
+    return mp_obj_new_int_from_uint(steps);
+}
+
+static MP_DEFINE_CONST_FUN_OBJ_0(mp_imu_step_counter_read_obj, mp_imu_step_counter_read);
+
 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_step_counter_read), MP_ROM_PTR(&mp_imu_step_counter_read_obj) },
 };
 
 static MP_DEFINE_CONST_DICT(globals, globals_table);