joppe/BBOT
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Formatted and added bmi task

Browse Source
This commit is contained in:
Joppe Blondel
2026-03-19 19:27:06 +01:00
parent f5f47c26fd
commit 00e354c2b3
7 changed files with 284 additions and 151 deletions
Download Patch File Download Diff File Expand all files Collapse all files

230
main/bmi160.c
View File

@@ -1,23 +1,145 @@
#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"
esp_err_t imu_init(bmi160_t *dev, i2c_port_t port){
#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) {
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) {
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
@@ -28,11 +150,12 @@ esp_err_t imu_init(bmi160_t *dev, i2c_port_t port){
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){
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
@@ -51,64 +174,88 @@ esp_err_t imu_init(bmi160_t *dev, i2c_port_t port){
// Poll until active
bool startup_complete = false;
for(int timeout=0; timeout<1000; timeout+=10){
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){
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){
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) {
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);
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
#define BMI160_MAX_WRITE_LEN 32
static esp_err_t bmi160_check_dev(const bmi160_t *dev) {
if (dev == NULL) {
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) {
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) {
if (dev == NULL)
{
return ESP_ERR_INVALID_ARG;
}
@@ -117,32 +264,39 @@ esp_err_t bmi160_init(bmi160_t *dev, i2c_port_t i2c_port, uint8_t 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) {
if (err != ESP_OK)
{
return err;
}
if (chip_id != BMI160_CHIP_ID) {
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) {
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 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) {
if (err != ESP_OK)
{
return err;
}
if (data == NULL || len == 0) {
if (data == NULL || len == 0)
{
return ESP_ERR_INVALID_ARG;
}
@@ -156,19 +310,23 @@ esp_err_t bmi160_read_registers(const bmi160_t *dev, uint8_t start_reg, uint8_t
dev->timeout_ticks);
}
esp_err_t bmi160_write_register(const bmi160_t *dev, uint8_t reg, uint8_t value) {
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) {
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) {
if (err != ESP_OK)
{
return err;
}
if (data == NULL || len == 0 || len > BMI160_MAX_WRITE_LEN) {
if (data == NULL || len == 0 || len > BMI160_MAX_WRITE_LEN)
{
return ESP_ERR_INVALID_ARG;
}