Working IMU with polling

main/bmi160.c

@@ -0,0 +1,184 @@
+#include "bmi160.h"
+
+#include <string.h>
+#include "esp_log.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+
+esp_err_t imu_init(bmi160_t *dev, i2c_port_t port){
+    esp_err_t err;
+
+    err = bmi160_init(dev, port, BMI160_I2C_ADDRESS_LOW);
+    if (err != ESP_OK) {
+        ESP_LOGW("bmi160", "BMI160 not found at 0x%02X: %s", BMI160_I2C_ADDRESS_LOW, esp_err_to_name(err));
+        err = bmi160_init(dev, port, BMI160_I2C_ADDRESS_HIGH);
+    }
+    if (err != ESP_OK) {
+        ESP_LOGE("bmi160", "BMI160 probe failed at 0x%02X and 0x%02X: %s",
+                 BMI160_I2C_ADDRESS_LOW,
+                 BMI160_I2C_ADDRESS_HIGH,
+                 esp_err_to_name(err));
+    }
+
+    // Soft reset
+    ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_CMD, 0xB6));
+    vTaskDelay(pdMS_TO_TICKS(50));
+
+    // Check chip ID
+    uint8_t chip_id;
+    ESP_ERROR_CHECK(bmi160_read_register(dev, BMI160_REG_CHIP_ID, &chip_id));
+    ESP_LOGI("bmi160", "BMI160 chip id: 0x%02x at 0x%02x", chip_id, dev->i2c_address);
+    if(chip_id != BMI160_CHIP_ID){
+        ESP_LOGE("bmi160", "BMI160 chip id not right");
+        return ESP_FAIL;
+    }
+    
+    // Configure ACC: 100Hz, normal mode filter, no undersampling
+    ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_ACC_CONF, 0x28));
+    // Configure ACC range: +-16g
+    ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_ACC_RANGE, 0x0C));
+
+    // Configure GYR: 100Hz, normal mode filter, no undersampling
+    ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_GYR_CONF, 0x28));
+    // Configure GYR range: +-2000dps
+    ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_GYR_RANGE, 0x00));
+
+    // Both sensors in normal mode
+    ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_CMD, 0x11));
+    vTaskDelay(pdMS_TO_TICKS(100));
+    ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_CMD, 0x15));
+    vTaskDelay(pdMS_TO_TICKS(100));
+
+    // Poll until active
+    bool startup_complete = false;
+    for(int timeout=0; timeout<1000; timeout+=10){
+        uint8_t status;
+        ESP_ERROR_CHECK(bmi160_read_register(dev, BMI160_REG_PMU_STATUS, &status));
+        int acc_pmu_status = (status & 0b00110000)>>4;
+        int gyr_pmu_status = (status & 0b00001100)>>2;
+        if(acc_pmu_status==0b01 && gyr_pmu_status==0b01){
+            startup_complete = true;
+            break;
+        }
+        vTaskDelay(pdMS_TO_TICKS(10));
+    }
+    if(!startup_complete){
+        ESP_LOGE("bmi160", "Acc or gyr not set in normal mode");
+    }
+
+    // Clear data registers
+    uint8_t data[20];
+    ESP_ERROR_CHECK(bmi160_read_registers(dev, BMI160_REG_DATA, data, BMI160_SIZE_REG_DATA));
+
+    return ESP_OK;
+}
+
+esp_err_t imu_read(const bmi160_t* dev, bmi160_value_t * value){
+    if (dev == NULL || value == NULL) {
+        return ESP_ERR_INVALID_ARG;
+    }
+
+    uint8_t data[20];
+    ESP_ERROR_CHECK(bmi160_read_registers(dev, BMI160_REG_DATA, data, BMI160_SIZE_REG_DATA));
+    value->acc.x = data[14] | (data[15]<<8);
+    value->acc.y = data[16] | (data[17]<<8);
+    value->acc.z = data[18] | (data[19]<<8);
+    value->gyr.x = data[8] | (data[9]<<8);
+    value->gyr.y = data[10] | (data[11]<<8);
+    value->gyr.z = data[12] | (data[13]<<8);
+
+    return ESP_OK;
+}
+
+
+// ----------------------------------------------------
+// Low level driver
+
+#define BMI160_MAX_WRITE_LEN 32
+
+static esp_err_t bmi160_check_dev(const bmi160_t *dev) {
+    if (dev == NULL) {
+        return ESP_ERR_INVALID_ARG;
+    }
+
+    return ESP_OK;
+}
+
+esp_err_t bmi160_init(bmi160_t *dev, i2c_port_t i2c_port, uint8_t i2c_address) {
+    uint8_t chip_id = 0;
+    esp_err_t err;
+
+    if (dev == NULL) {
+        return ESP_ERR_INVALID_ARG;
+    }
+
+    dev->i2c_port = i2c_port;
+    dev->i2c_address = i2c_address;
+    dev->timeout_ticks = pdMS_TO_TICKS(BMI160_DEFAULT_TIMEOUT_MS);
+
+    err = bmi160_read_register(dev, BMI160_REG_CHIP_ID, &chip_id);
+    if (err != ESP_OK) {
+        return err;
+    }
+
+    if (chip_id != BMI160_CHIP_ID) {
+        return ESP_ERR_NOT_FOUND;
+    }
+
+    return ESP_OK;
+}
+
+esp_err_t bmi160_read_register(const bmi160_t *dev, uint8_t reg, uint8_t *value) {
+    if (value == NULL) {
+        return ESP_ERR_INVALID_ARG;
+    }
+
+    return bmi160_read_registers(dev, reg, value, 1);
+}
+
+esp_err_t bmi160_read_registers(const bmi160_t *dev, uint8_t start_reg, uint8_t *data, size_t len) {
+    esp_err_t err = bmi160_check_dev(dev);
+    if (err != ESP_OK) {
+        return err;
+    }
+
+    if (data == NULL || len == 0) {
+        return ESP_ERR_INVALID_ARG;
+    }
+
+    return i2c_master_write_read_device(
+        dev->i2c_port,
+        dev->i2c_address,
+        &start_reg,
+        1,
+        data,
+        len,
+        dev->timeout_ticks);
+}
+
+esp_err_t bmi160_write_register(const bmi160_t *dev, uint8_t reg, uint8_t value) {
+    return bmi160_write_registers(dev, reg, &value, 1);
+}
+
+esp_err_t bmi160_write_registers(const bmi160_t *dev, uint8_t start_reg, const uint8_t *data, size_t len) {
+    uint8_t buffer[1 + BMI160_MAX_WRITE_LEN];
+    esp_err_t err = bmi160_check_dev(dev);
+
+    if (err != ESP_OK) {
+        return err;
+    }
+
+    if (data == NULL || len == 0 || len > BMI160_MAX_WRITE_LEN) {
+        return ESP_ERR_INVALID_ARG;
+    }
+
+    buffer[0] = start_reg;
+    memcpy(&buffer[1], data, len);
+
+    return i2c_master_write_to_device(
+        dev->i2c_port,
+        dev->i2c_address,
+        buffer,
+        len + 1,
+        dev->timeout_ticks);
+}