Formatted and added bmi task
This commit is contained in:
59
main/bbot.c
59
main/bbot.c
@@ -5,14 +5,13 @@
|
||||
#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_INPUT_PIN GPIO_NUM_1
|
||||
#define BBOT_BLINK_PIN GPIO_NUM_8
|
||||
#define BBOT_BLINK_DELAY_MS 500
|
||||
#define BBOT_DRDY_INPUT_PIN GPIO_NUM_1
|
||||
|
||||
#define BBOT_I2C_PORT I2C_NUM_0
|
||||
#define BBOT_I2C_SCL_IO GPIO_NUM_3
|
||||
@@ -20,33 +19,10 @@
|
||||
#define BBOT_I2C_FREQ_HZ 100000
|
||||
|
||||
static bmi160_t imu;
|
||||
static QueueHandle_t imu_queue;
|
||||
|
||||
static void init_blink_gpio(){
|
||||
const gpio_config_t io_conf = {
|
||||
.pin_bit_mask = (1ULL << BBOT_BLINK_PIN),
|
||||
.mode = GPIO_MODE_OUTPUT,
|
||||
.pull_up_en = GPIO_PULLUP_DISABLE,
|
||||
.pull_down_en = GPIO_PULLDOWN_DISABLE,
|
||||
.intr_type = GPIO_INTR_DISABLE,
|
||||
};
|
||||
|
||||
gpio_config(&io_conf);
|
||||
gpio_set_level(BBOT_BLINK_PIN, 0);
|
||||
}
|
||||
|
||||
static void init_input_gpio(){
|
||||
const gpio_config_t io_conf = {
|
||||
.pin_bit_mask = (1ULL << BBOT_INPUT_PIN),
|
||||
.mode = GPIO_MODE_INPUT,
|
||||
.pull_up_en = GPIO_PULLUP_DISABLE,
|
||||
.pull_down_en = GPIO_PULLDOWN_DISABLE,
|
||||
.intr_type = GPIO_INTR_DISABLE,
|
||||
};
|
||||
|
||||
gpio_config(&io_conf);
|
||||
}
|
||||
|
||||
static void init_i2c(){
|
||||
static esp_err_t init_i2c()
|
||||
{
|
||||
const i2c_config_t i2c_conf = {
|
||||
.mode = I2C_MODE_MASTER,
|
||||
.sda_io_num = BBOT_I2C_SDA_IO,
|
||||
@@ -58,22 +34,21 @@ static void init_i2c(){
|
||||
|
||||
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){
|
||||
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));
|
||||
|
||||
init_input_gpio();
|
||||
init_blink_gpio();
|
||||
|
||||
init_motors();
|
||||
|
||||
init_i2c();
|
||||
imu_init(&imu, BBOT_I2C_PORT);
|
||||
|
||||
while (1) {
|
||||
vTaskDelay(pdMS_TO_TICKS(10));
|
||||
while (1)
|
||||
{
|
||||
bmi160_value_t v;
|
||||
imu_read(&imu, &v);
|
||||
ESP_LOGI("[<IMU>]", "%d %d %d %d %d %d", v.acc.x, v.acc.y, v.acc.z, v.gyr.x, v.gyr.y, v.gyr.z);
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
230
main/bmi160.c
230
main/bmi160.c
@@ -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;
|
||||
}
|
||||
|
||||
|
||||
@@ -4,43 +4,59 @@
|
||||
#include <stdint.h>
|
||||
|
||||
#include "driver/i2c.h"
|
||||
#include "driver/gpio.h"
|
||||
#include "esp_err.h"
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/queue.h"
|
||||
#include "freertos/task.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#define BMI160_I2C_ADDRESS_LOW 0x68
|
||||
#define BMI160_I2C_ADDRESS_LOW 0x68
|
||||
#define BMI160_I2C_ADDRESS_HIGH 0x69
|
||||
|
||||
#define BMI160_REG_CHIP_ID 0x00
|
||||
#define BMI160_REG_PMU_STATUS 0x03
|
||||
#define BMI160_REG_DATA 0x04
|
||||
#define BMI160_SIZE_REG_DATA 20
|
||||
#define BMI160_REG_STATUS 0x1B
|
||||
#define BMI160_REG_ACC_CONF 0x40
|
||||
#define BMI160_REG_ACC_RANGE 0x41
|
||||
#define BMI160_REG_GYR_CONF 0x42
|
||||
#define BMI160_REG_GYR_RANGE 0x43
|
||||
#define BMI160_REG_CMD 0x7e
|
||||
#define BMI160_REG_CHIP_ID 0x00
|
||||
#define BMI160_REG_PMU_STATUS 0x03
|
||||
#define BMI160_REG_DATA 0x04
|
||||
#define BMI160_SIZE_REG_DATA 20
|
||||
#define BMI160_REG_SENSORTIME 0x18
|
||||
#define BMI160_SIZE_SENSORTIME 3
|
||||
#define BMI160_REG_STATUS 0x1B
|
||||
#define BMI160_REG_ACC_CONF 0x40
|
||||
#define BMI160_REG_ACC_RANGE 0x41
|
||||
#define BMI160_REG_GYR_CONF 0x42
|
||||
#define BMI160_REG_GYR_RANGE 0x43
|
||||
#define BMI160_REG_INT_EN0 0x50
|
||||
#define BMI160_REG_INT_EN1 0x51
|
||||
#define BMI160_REG_INT_EN2 0x52
|
||||
#define BMI160_REG_INT_OUT_CTRL 0x53
|
||||
#define BMI160_REG_INT_LATCH 0x54
|
||||
#define BMI160_REG_INT_MAP0 0x55
|
||||
#define BMI160_REG_INT_MAP1 0x56
|
||||
#define BMI160_REG_INT_MAP2 0x57
|
||||
#define BMI160_REG_CMD 0x7e
|
||||
|
||||
#define BMI160_CHIP_ID 0xD1
|
||||
#define BMI160_CHIP_ID 0xD1
|
||||
|
||||
#define BMI160_DEFAULT_TIMEOUT_MS 100
|
||||
|
||||
typedef struct {
|
||||
typedef struct
|
||||
{
|
||||
i2c_port_t i2c_port;
|
||||
uint8_t i2c_address;
|
||||
TickType_t timeout_ticks;
|
||||
gpio_num_t drdy_io;
|
||||
QueueHandle_t sample_queue;
|
||||
TaskHandle_t read_task;
|
||||
} bmi160_t;
|
||||
|
||||
typedef struct {
|
||||
struct {
|
||||
typedef struct
|
||||
{
|
||||
struct
|
||||
{
|
||||
int16_t x;
|
||||
int16_t y;
|
||||
int16_t z;
|
||||
} acc, gyr;
|
||||
uint32_t time;
|
||||
} bmi160_value_t;
|
||||
|
||||
esp_err_t bmi160_init(bmi160_t *dev, i2c_port_t i2c_port, uint8_t i2c_address);
|
||||
@@ -49,9 +65,5 @@ esp_err_t bmi160_read_registers(const bmi160_t *dev, uint8_t start_reg, uint8_t
|
||||
esp_err_t bmi160_write_register(const bmi160_t *dev, uint8_t reg, uint8_t value);
|
||||
esp_err_t bmi160_write_registers(const bmi160_t *dev, uint8_t start_reg, const uint8_t *data, size_t len);
|
||||
|
||||
esp_err_t imu_init(bmi160_t *dev, i2c_port_t port);
|
||||
esp_err_t imu_read(const bmi160_t* dev, bmi160_value_t * value);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
esp_err_t imu_init(bmi160_t *dev, i2c_port_t port, gpio_num_t drdy_io, QueueHandle_t *queue_handle);
|
||||
esp_err_t imu_read(const bmi160_t *dev, bmi160_value_t *value);
|
||||
|
||||
@@ -13,7 +13,8 @@
|
||||
#define MOTOR_PWM_MAX_DUTY ((1 << 10) - 1)
|
||||
#define MOTOR_MIN_VALUE 60
|
||||
|
||||
typedef struct {
|
||||
typedef struct
|
||||
{
|
||||
int fwd_pin;
|
||||
int bak_pin;
|
||||
ledc_channel_t fwd_channel;
|
||||
@@ -39,13 +40,17 @@ static uint32_t percentile_to_duty(percentile_t percentile)
|
||||
{
|
||||
int clamped = percentile;
|
||||
const uint32_t min_duty = (MOTOR_PWM_MAX_DUTY * MOTOR_MIN_VALUE) / 100;
|
||||
if (clamped > 100) {
|
||||
if (clamped > 100)
|
||||
{
|
||||
clamped = 100;
|
||||
} else if (clamped < -100) {
|
||||
}
|
||||
else if (clamped < -100)
|
||||
{
|
||||
clamped = -100;
|
||||
}
|
||||
|
||||
if (clamped == 0) {
|
||||
if (clamped == 0)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -58,7 +63,7 @@ static void set_channel_duty(ledc_channel_t channel, uint32_t duty)
|
||||
ESP_ERROR_CHECK(ledc_update_duty(MOTOR_PWM_MODE, channel));
|
||||
}
|
||||
|
||||
void init_motors(void)
|
||||
esp_err_t init_motors(void)
|
||||
{
|
||||
const ledc_timer_config_t timer_config = {
|
||||
.speed_mode = MOTOR_PWM_MODE,
|
||||
@@ -70,7 +75,8 @@ void init_motors(void)
|
||||
|
||||
ESP_ERROR_CHECK(ledc_timer_config(&timer_config));
|
||||
|
||||
for (size_t i = 0; i < (sizeof(s_motor_configs) / sizeof(s_motor_configs[0])); ++i) {
|
||||
for (size_t i = 0; i < (sizeof(s_motor_configs) / sizeof(s_motor_configs[0])); ++i)
|
||||
{
|
||||
const motor_config_t *motor = &s_motor_configs[i];
|
||||
const ledc_channel_config_t fwd_channel_config = {
|
||||
.gpio_num = motor->fwd_pin,
|
||||
@@ -96,24 +102,31 @@ void init_motors(void)
|
||||
}
|
||||
|
||||
set_motors(0, 0);
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
void set_motor(motor_t motor, percentile_t percentile)
|
||||
{
|
||||
if (motor < MOTOR1 || motor > MOTOR2) {
|
||||
if (motor < MOTOR1 || motor > MOTOR2)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
const motor_config_t *config = &s_motor_configs[motor];
|
||||
const uint32_t duty = percentile_to_duty(percentile);
|
||||
|
||||
if (percentile > 0) {
|
||||
if (percentile > 0)
|
||||
{
|
||||
set_channel_duty(config->bak_channel, 0);
|
||||
set_channel_duty(config->fwd_channel, duty);
|
||||
} else if (percentile < 0) {
|
||||
}
|
||||
else if (percentile < 0)
|
||||
{
|
||||
set_channel_duty(config->fwd_channel, 0);
|
||||
set_channel_duty(config->bak_channel, duty);
|
||||
} else {
|
||||
}
|
||||
else
|
||||
{
|
||||
set_channel_duty(config->fwd_channel, 0);
|
||||
set_channel_duty(config->bak_channel, 0);
|
||||
}
|
||||
|
||||
@@ -1,19 +1,21 @@
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include "esp_err.h"
|
||||
|
||||
#define MOTOR1_FWD_PIN 4
|
||||
#define MOTOR1_BAK_PIN 5
|
||||
#define MOTOR2_FWD_PIN 6
|
||||
#define MOTOR2_BAK_PIN 7
|
||||
|
||||
typedef enum {
|
||||
typedef enum
|
||||
{
|
||||
MOTOR1,
|
||||
MOTOR2,
|
||||
} motor_t;
|
||||
|
||||
typedef int8_t percentile_t;
|
||||
|
||||
void init_motors();
|
||||
esp_err_t init_motors();
|
||||
void set_motor(motor_t motor, percentile_t percentile);
|
||||
void set_motors(percentile_t left, percentile_t right);
|
||||
|
||||
37
plot.py
37
plot.py
@@ -30,36 +30,6 @@ TEXT = (230, 235, 240)
|
||||
ERROR = (255, 110, 110)
|
||||
LINE_RE = re.compile(r"\[<([^>]+)>\]:\s*(.*)")
|
||||
|
||||
DEFAULT_CONFIG = {
|
||||
"title": "IMU stream",
|
||||
"history_seconds": 10.0,
|
||||
"graphs": [
|
||||
{
|
||||
"title": "Accelerometer",
|
||||
"streams": [
|
||||
{"key": "acc_x", "label": "ACC X", "color": [90, 170, 255]},
|
||||
{"key": "acc_y", "label": "ACC Y", "color": [80, 220, 140]},
|
||||
{"key": "acc_z", "label": "ACC Z", "color": [255, 200, 60]},
|
||||
],
|
||||
},
|
||||
{
|
||||
"title": "Gyroscope",
|
||||
"streams": [
|
||||
{"key": "gyr_x", "label": "GYR X", "color": [240, 80, 80]},
|
||||
{"key": "gyr_y", "label": "GYR Y", "color": [220, 80, 220]},
|
||||
{"key": "gyr_z", "label": "GYR Z", "color": [240, 240, 240]},
|
||||
],
|
||||
},
|
||||
],
|
||||
"sources": [
|
||||
{
|
||||
"tag": "IMU",
|
||||
"fields": ["acc_x", "acc_y", "acc_z", "gyr_x", "gyr_y", "gyr_z"],
|
||||
}
|
||||
],
|
||||
}
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Read tagged serial values and draw them live from a JSON config."
|
||||
@@ -77,7 +47,7 @@ def parse_args() -> argparse.Namespace:
|
||||
"--config",
|
||||
type=Path,
|
||||
default=None,
|
||||
help="Path to JSON plot config (default: built-in IMU config)",
|
||||
help="Path to JSON plot config (default: ./plot_config.json)",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--history-seconds",
|
||||
@@ -108,7 +78,10 @@ def parse_args() -> argparse.Namespace:
|
||||
|
||||
def load_config(config_path: Path | None) -> dict:
|
||||
if config_path is None:
|
||||
return DEFAULT_CONFIG
|
||||
config_path = Path("plot_config.json")
|
||||
|
||||
if not config_path.exists():
|
||||
raise SystemExit(f"Config file not found: {config_path}")
|
||||
|
||||
with config_path.open("r", encoding="utf-8") as handle:
|
||||
return json.load(handle)
|
||||
|
||||
@@ -4,7 +4,7 @@
|
||||
"sources": [
|
||||
{
|
||||
"tag": "IMU",
|
||||
"fields": ["acc_x", "acc_y", "acc_z", "gyr_x", "gyr_y", "gyr_z"]
|
||||
"fields": ["time", "acc_x", "acc_y", "acc_z", "gyr_x", "gyr_y", "gyr_z"]
|
||||
}
|
||||
],
|
||||
"graphs": [
|
||||
@@ -19,9 +19,9 @@
|
||||
{
|
||||
"title": "Gyroscope",
|
||||
"streams": [
|
||||
{ "key": "gyr_x", "label": "GYR X", "color": [240, 80, 80] },
|
||||
{ "key": "gyr_y", "label": "GYR Y", "color": [220, 80, 220] },
|
||||
{ "key": "gyr_z", "label": "GYR Z", "color": [240, 240, 240] }
|
||||
{ "key": "gyr_x", "label": "GYR X", "color": [90, 170, 255] },
|
||||
{ "key": "gyr_y", "label": "GYR Y", "color": [80, 220, 140] },
|
||||
{ "key": "gyr_z", "label": "GYR Z", "color": [225, 200, 60] }
|
||||
]
|
||||
}
|
||||
]
|
||||
|
||||
Reference in New Issue
Block a user