55 lines
1.4 KiB
C
55 lines
1.4 KiB
C
#include <stdio.h>
|
|
|
|
#include "driver/gpio.h"
|
|
#include "driver/i2c.h"
|
|
#include "esp_err.h"
|
|
#include "esp_log.h"
|
|
#include "freertos/FreeRTOS.h"
|
|
#include "freertos/queue.h"
|
|
#include "freertos/task.h"
|
|
|
|
#include "motors.h"
|
|
#include "bmi160.h"
|
|
|
|
#define BBOT_DRDY_INPUT_PIN GPIO_NUM_1
|
|
|
|
#define BBOT_I2C_PORT I2C_NUM_0
|
|
#define BBOT_I2C_SCL_IO GPIO_NUM_3
|
|
#define BBOT_I2C_SDA_IO GPIO_NUM_2
|
|
#define BBOT_I2C_FREQ_HZ 100000
|
|
|
|
static bmi160_t imu;
|
|
static QueueHandle_t imu_queue;
|
|
|
|
static esp_err_t init_i2c()
|
|
{
|
|
const i2c_config_t i2c_conf = {
|
|
.mode = I2C_MODE_MASTER,
|
|
.sda_io_num = BBOT_I2C_SDA_IO,
|
|
.scl_io_num = BBOT_I2C_SCL_IO,
|
|
.sda_pullup_en = GPIO_PULLUP_ENABLE,
|
|
.scl_pullup_en = GPIO_PULLUP_ENABLE,
|
|
.master.clk_speed = BBOT_I2C_FREQ_HZ,
|
|
};
|
|
|
|
ESP_ERROR_CHECK(i2c_param_config(BBOT_I2C_PORT, &i2c_conf));
|
|
ESP_ERROR_CHECK(i2c_driver_install(BBOT_I2C_PORT, i2c_conf.mode, 0, 0, 0));
|
|
return ESP_OK;
|
|
}
|
|
|
|
void app_main(void)
|
|
{
|
|
ESP_ERROR_CHECK(init_motors());
|
|
ESP_ERROR_CHECK(init_i2c());
|
|
ESP_ERROR_CHECK(imu_init(&imu, BBOT_I2C_PORT, BBOT_DRDY_INPUT_PIN, &imu_queue));
|
|
|
|
while (1)
|
|
{
|
|
bmi160_value_t v;
|
|
while (xQueueReceive(imu_queue, &v, pdMS_TO_TICKS(1000)) == pdTRUE)
|
|
{
|
|
ESP_LOGI("[<IMU>]", "%d %d %d %d %d %d %d", v.time, v.acc.x, v.acc.y, v.acc.z, v.gyr.x, v.gyr.y, v.gyr.z);
|
|
}
|
|
}
|
|
}
|