joppe/BBOT
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
00e354c2b388f7fdb02a3941df74da478803a3ad
BBOT/main/bmi160.c

343 lines
8.9 KiB
C
Raw Blame History

#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"
#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));
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);
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);
}
Reference in New Issue View Git Blame Copy Permalink