354 lines
9.4 KiB
C
354 lines
9.4 KiB
C
#include "bmi160.h"
|
|
|
|
#include <stdbool.h>
|
|
#include <string.h>
|
|
|
|
#include "driver/gpio.h"
|
|
#include "esp_log.h"
|
|
#include "freertos/FreeRTOS.h"
|
|
#include "freertos/queue.h"
|
|
#include "freertos/task.h"
|
|
|
|
#include "fixedpoint.h"
|
|
|
|
#define BMI160_MAX_WRITE_LEN 32
|
|
#define BMI160_SAMPLE_QUEUE_LEN 16
|
|
#define BMI160_QUEUE_MIN_FREE_SLOTS 1
|
|
#define BMI160_READ_TASK_STACK_SIZE 4096
|
|
#define BMI160_READ_TASK_PRIORITY 5
|
|
|
|
static bool s_gpio_isr_service_installed = false;
|
|
|
|
static void IRAM_ATTR bmi160_drdy_isr(void *arg)
|
|
{
|
|
bmi160_t *dev = (bmi160_t *)arg;
|
|
BaseType_t higher_priority_task_woken = pdFALSE;
|
|
|
|
if (dev != NULL && dev->read_task != NULL)
|
|
{
|
|
vTaskNotifyGiveFromISR(dev->read_task, &higher_priority_task_woken);
|
|
}
|
|
|
|
if (higher_priority_task_woken == pdTRUE)
|
|
{
|
|
portYIELD_FROM_ISR();
|
|
}
|
|
}
|
|
|
|
static void bmi160_read_task(void *arg)
|
|
{
|
|
bmi160_t *dev = (bmi160_t *)arg;
|
|
|
|
for (;;)
|
|
{
|
|
bmi160_value_t value;
|
|
|
|
ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
|
|
if (imu_read(dev, &value) != ESP_OK)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
if (dev->sample_queue == NULL)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
if (uxQueueSpacesAvailable(dev->sample_queue) <= BMI160_QUEUE_MIN_FREE_SLOTS)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
xQueueSend(dev->sample_queue, &value, 0);
|
|
}
|
|
}
|
|
|
|
static esp_err_t bmi160_init_streaming(bmi160_t *dev, gpio_num_t drdy_io, QueueHandle_t *queue_handle)
|
|
{
|
|
const gpio_config_t io_conf = {
|
|
.pin_bit_mask = (1ULL << drdy_io),
|
|
.mode = GPIO_MODE_INPUT,
|
|
.pull_up_en = GPIO_PULLUP_DISABLE,
|
|
.pull_down_en = GPIO_PULLDOWN_DISABLE,
|
|
.intr_type = GPIO_INTR_POSEDGE,
|
|
};
|
|
|
|
if (queue_handle == NULL)
|
|
{
|
|
return ESP_ERR_INVALID_ARG;
|
|
}
|
|
|
|
ESP_ERROR_CHECK(gpio_config(&io_conf));
|
|
|
|
if (!s_gpio_isr_service_installed)
|
|
{
|
|
esp_err_t err = gpio_install_isr_service(0);
|
|
if (err != ESP_OK && err != ESP_ERR_INVALID_STATE)
|
|
{
|
|
return err;
|
|
}
|
|
s_gpio_isr_service_installed = true;
|
|
}
|
|
|
|
dev->sample_queue = xQueueCreate(BMI160_SAMPLE_QUEUE_LEN, sizeof(bmi160_value_t));
|
|
if (dev->sample_queue == NULL)
|
|
{
|
|
return ESP_ERR_NO_MEM;
|
|
}
|
|
|
|
*queue_handle = dev->sample_queue;
|
|
|
|
BaseType_t task_ok = xTaskCreate(
|
|
bmi160_read_task,
|
|
"bmi160_read",
|
|
BMI160_READ_TASK_STACK_SIZE,
|
|
dev,
|
|
BMI160_READ_TASK_PRIORITY,
|
|
&dev->read_task);
|
|
if (task_ok != pdPASS)
|
|
{
|
|
vQueueDelete(dev->sample_queue);
|
|
dev->sample_queue = NULL;
|
|
*queue_handle = NULL;
|
|
return ESP_ERR_NO_MEM;
|
|
}
|
|
|
|
ESP_ERROR_CHECK(gpio_isr_handler_add(drdy_io, bmi160_drdy_isr, dev));
|
|
return ESP_OK;
|
|
}
|
|
|
|
esp_err_t imu_init(bmi160_t *dev, i2c_port_t port, gpio_num_t drdy_io, QueueHandle_t *queue_handle)
|
|
{
|
|
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));
|
|
return err;
|
|
}
|
|
|
|
dev->drdy_io = drdy_io;
|
|
dev->sample_queue = NULL;
|
|
dev->read_task = NULL;
|
|
if (queue_handle != NULL)
|
|
{
|
|
*queue_handle = NULL;
|
|
}
|
|
|
|
// 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");
|
|
}
|
|
|
|
// enable DRDY interrupt
|
|
ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_INT_EN1, 0x10));
|
|
// enable INT1, active high push pull
|
|
ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_INT_OUT_CTRL, 0x0a));
|
|
// Disable INT1 input, non-latched
|
|
ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_INT_LATCH, 0x00));
|
|
// Map DRDY to INT1
|
|
ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_INT_MAP1, 0x80));
|
|
|
|
// Clear data registers
|
|
uint8_t data[20];
|
|
ESP_ERROR_CHECK(bmi160_read_registers(dev, BMI160_REG_DATA, data, BMI160_SIZE_REG_DATA));
|
|
|
|
if (drdy_io >= 0 && queue_handle != NULL)
|
|
{
|
|
return bmi160_init_streaming(dev, drdy_io, queue_handle);
|
|
}
|
|
|
|
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));
|
|
|
|
int16_t acc_x = data[14] | (data[15] << 8); // unbiased raw
|
|
int16_t acc_y = data[16] | (data[17] << 8); // unbiased raw
|
|
int16_t acc_z = data[18] | (data[19] << 8); // unbiased raw
|
|
int16_t gyr_x = data[8] | (data[9] << 8); // unbiased raw
|
|
int16_t gyr_y = data[10] | (data[11] << 8); // unbiased raw
|
|
int16_t gyr_z = data[12] | (data[13] << 8); // unbiased raw
|
|
|
|
value->acc.x = FIXED_FROM_RATIO(acc_x, 2048, 12);
|
|
value->acc.y = FIXED_FROM_RATIO(acc_y, 2048, 12);
|
|
value->acc.z = FIXED_FROM_RATIO(acc_z, 2048, 12);
|
|
|
|
value->gyr.x = FIXED_FROM_RATIO((int32_t)gyr_x * 5, 82, 12);
|
|
value->gyr.y = FIXED_FROM_RATIO((int32_t)gyr_y * 5, 82, 12);
|
|
value->gyr.z = FIXED_FROM_RATIO((int32_t)gyr_z * 5, 82, 12);
|
|
|
|
uint8_t time[3];
|
|
ESP_ERROR_CHECK(bmi160_read_registers(dev, BMI160_REG_SENSORTIME, time, BMI160_SIZE_SENSORTIME));
|
|
value->time = (time[0] | (time[1]<<8) | (time[2]<<16)) >> 8; // shift right to set resolution
|
|
|
|
return ESP_OK;
|
|
}
|
|
|
|
// ----------------------------------------------------
|
|
// Low level driver
|
|
|
|
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);
|
|
}
|