From 9a2419223acde0c5a86b7ef5aced96ec35774673 Mon Sep 17 00:00:00 2001 From: Brendan Haines Date: Thu, 4 Jul 2024 23:21:16 -0600 Subject: [PATCH] split out gyro code --- app/src/main.c | 27 ++-- drivers/sensor/bma255/bma255.c | 103 +-------------- drivers/sensor/bmg160/CMakeLists.txt | 2 + drivers/sensor/bmg160/Kconfig | 17 +++ drivers/sensor/bmg160/bmg160.c | 189 +++++++++++++++++++++++++++ drivers/sensor/bmm150/CMakeLists.txt | 2 + 6 files changed, 224 insertions(+), 116 deletions(-) create mode 100644 drivers/sensor/bmg160/CMakeLists.txt create mode 100644 drivers/sensor/bmg160/Kconfig create mode 100644 drivers/sensor/bmg160/bmg160.c create mode 100644 drivers/sensor/bmm150/CMakeLists.txt diff --git a/app/src/main.c b/app/src/main.c index dcd0ede..2c653cb 100644 --- a/app/src/main.c +++ b/app/src/main.c @@ -195,14 +195,13 @@ int main(void) } LOG_INF("Initialized %s", accel->name); - gyro = accel; // FIXME: this is a hack while splitting apart BMX055 - // gyro = DEVICE_DT_GET(DT_NODELABEL(bmx055_gyro)); - // if (!device_is_ready(gyro)) - // { - // LOG_ERR("Device %s not ready", gyro->name); - // return 0; - // } - // LOG_INF("Initialized %s", gyro->name); + gyro = DEVICE_DT_GET(DT_NODELABEL(bmx055_gyro)); + if (!device_is_ready(gyro)) + { + LOG_ERR("Device %s not ready", gyro->name); + return 0; + } + LOG_INF("Initialized %s", gyro->name); mag = accel; // FIXME: this is a hack while splitting apart BMX055 // mag = DEVICE_DT_GET(DT_NODELABEL(bmx055_mag)); @@ -273,10 +272,10 @@ int main(void) LOG_ERR("Could not get acceleration"); } - // if (sensor_sample_fetch(gyro) < 0) - // { - // LOG_ERR("Could not fetch sample from gyro"); - // } + if (sensor_sample_fetch(gyro) < 0) + { + LOG_ERR("Could not fetch sample from gyro"); + } if (sensor_channel_get(gyro, SENSOR_CHAN_GYRO_X, &gyro_x) < 0) { LOG_ERR("Could not get gyro"); @@ -352,11 +351,11 @@ int main(void) { LOG_ERR("Failed to print a string"); } - if (cfb_print(display, str_my, 0, 16)) + if (cfb_print(display, str_gy, 0, 16)) { LOG_ERR("Failed to print a string"); } - if (cfb_print(display, str_mz, 0, 16 * 2)) + if (cfb_print(display, str_gz, 0, 16 * 2)) { LOG_ERR("Failed to print a string"); } diff --git a/drivers/sensor/bma255/bma255.c b/drivers/sensor/bma255/bma255.c index e7835be..e4c09b4 100644 --- a/drivers/sensor/bma255/bma255.c +++ b/drivers/sensor/bma255/bma255.c @@ -90,54 +90,6 @@ const uint8_t ACCEL_REG_TRIM_GP1 = 0x3C; const uint8_t ACCEL_REG_FIFO_CONFIG_1 = 0x3E; const uint8_t ACCEL_REG_FIFO_DATA = 0x3F; -const uint8_t GYRO_REG_CHIP_ID = 0x00; -const uint8_t GYRO_REG_RATE_X_LSB = 0x02; -const uint8_t GYRO_REG_RATE_X_MSB = 0x03; -const uint8_t GYRO_REG_RATE_Y_LSB = 0x04; -const uint8_t GYRO_REG_RATE_Y_MSB = 0x05; -const uint8_t GYRO_REG_RATE_Z_LSB = 0x06; -const uint8_t GYRO_REG_RATE_Z_MSB = 0x07; -const uint8_t GYRO_REG_INT_STATUS_0 = 0x09; -const uint8_t GYRO_REG_INT_STATUS_1 = 0x0A; -const uint8_t GYRO_REG_INT_STATUS_2 = 0x0B; -const uint8_t GYRO_REG_INT_STATUS_3 = 0x0C; -const uint8_t GYRO_REG_FIFO_STATUS = 0x0E; -const uint8_t GYRO_REG_RANGE = 0x0F; -const uint8_t GYRO_REG_BW = 0x10; -const uint8_t GYRO_REG_LPM1 = 0x11; -const uint8_t GYRO_REG_LPM2 = 0x12; -const uint8_t GYRO_REG_RATE_HBW = 0x13; -const uint8_t GYRO_REG_BGW_SOFTRESET = 0x14; -const uint8_t GYRO_REG_INT_EN_0 = 0x15; -const uint8_t GYRO_REG_INT_EN_1 = 0x16; -const uint8_t GYRO_REG_INT_MAP_0 = 0x17; -const uint8_t GYRO_REG_INT_MAP_1 = 0x18; -const uint8_t GYRO_REG_INT_MAP_2 = 0x19; -const uint8_t GYRO_REG_INT_SOURCE_1 = 0x1A; -const uint8_t GYRO_REG_INT_SOURCE_2 = 0x1B; -// const uint8_t GYRO_REG_ = 0x1C; -// const uint8_t GYRO_REG_ = 0x1E; -const uint8_t GYRO_REG_INT_RST_LATCH = 0x21; -const uint8_t GYRO_REG_HIGH_TH_X = 0x22; -const uint8_t GYRO_REG_HIGH_DUR_X = 0x23; -const uint8_t GYRO_REG_HIGH_TH_Y = 0x24; -const uint8_t GYRO_REG_HIGH_DUR_Y = 0x25; -const uint8_t GYRO_REG_HIGH_TH_Z = 0x26; -const uint8_t GYRO_REG_HIGH_DUR_Z = 0x27; -const uint8_t GYRO_REG_SOC = 0x31; -const uint8_t GYRO_REG_A_FOC = 0x32; -const uint8_t GYRO_REG_TRIM_NVM_CTRL = 0x33; -const uint8_t GYRO_REG_BGW_SPI3_WDT = 0x34; -const uint8_t GYRO_REG_OFC1 = 0x36; -const uint8_t GYRO_REG_OFC2 = 0x37; -const uint8_t GYRO_REG_OFC3 = 0x38; -const uint8_t GYRO_REG_OFC4 = 0x39; -const uint8_t GYRO_REG_TRIM_GP0 = 0x3A; -const uint8_t GYRO_REG_TRIM_GP1 = 0x3B; -const uint8_t GYRO_REG_BIST = 0x3C; -const uint8_t GYRO_REG_FIFO_CONFIG_0 = 0x3D; -const uint8_t GYRO_REG_FIFO_CONFIG_1 = 0x3E; - const uint8_t MAG_REG_CHIP_ID = 0x40; const uint8_t MAG_REG_DATA_X_LSB = 0x42; const uint8_t MAG_REG_DATA_X_MSB = 0x43; @@ -173,9 +125,6 @@ struct bma255_data int16_t accel_x; int16_t accel_y; int16_t accel_z; - int16_t gyro_x; - int16_t gyro_y; - int16_t gyro_z; int16_t mag_x; int16_t mag_y; int16_t mag_z; @@ -211,26 +160,13 @@ static int bma255_sample_fetch(const struct device *dev, data->accel_y = ((int16_t)sys_get_le16(&accel[2])) >> 4; data->accel_z = ((int16_t)sys_get_le16(&accel[4])) >> 4; - uint8_t gyro[6]; - struct i2c_dt_spec gyro_bus = config->bus; - gyro_bus.addr = 0x68; - ret = i2c_burst_read_dt(&gyro_bus, GYRO_REG_RATE_X_LSB, gyro, sizeof(gyro)); - if (ret < 0) - { - LOG_ERR("Failed to read gyro registers!"); - return ret; - } - data->gyro_x = ((int16_t)sys_get_le16(&gyro[0])); - data->gyro_y = ((int16_t)sys_get_le16(&gyro[2])); - data->gyro_z = ((int16_t)sys_get_le16(&gyro[4])); - uint8_t mag[6]; struct i2c_dt_spec mag_bus = config->bus; mag_bus.addr = 0x10; ret = i2c_burst_read_dt(&mag_bus, MAG_REG_DATA_X_LSB, mag, sizeof(mag)); if (ret < 0) { - LOG_ERR("Failed to read gyro registers!"); + LOG_ERR("Failed to read mag registers!"); return ret; } data->mag_x = ((int16_t)sys_get_le16(&mag[0])) >> 1; @@ -289,28 +225,6 @@ static int bma255_channel_get(const struct device *dev, val->val2 = (accel - val->val1) * 1000000; break; } - // radians/second - case SENSOR_CHAN_GYRO_X: - { - float rate = data->gyro_x * 2000.0 / 32767; - val->val1 = rate; - val->val2 = (rate - val->val1) * 1000000; - break; - } - case SENSOR_CHAN_GYRO_Y: - { - float rate = data->gyro_y * 2000.0 / 32767; - val->val1 = rate; - val->val2 = (rate - val->val1) * 1000000; - break; - } - case SENSOR_CHAN_GYRO_Z: - { - float rate = data->gyro_z * 2000.0 / 32767; - val->val1 = rate; - val->val2 = (rate - val->val1) * 1000000; - break; - } // Gauss case SENSOR_CHAN_MAGN_X: { @@ -394,21 +308,6 @@ static int bma255_init(const struct device *dev) uint8_t hbw = (1 << 7); // data_high_bw (read filtered data) and enable lsb/msb shadowing i2c_burst_write_dt(&config->bus, ACCEL_REG_ACCD_HBW, &hbw, sizeof(hbw)); - struct i2c_dt_spec gyro_bus = config->bus; - gyro_bus.addr = 0x68; - uint8_t gyro_chip_id; - ret = i2c_burst_read_dt(&gyro_bus, GYRO_REG_CHIP_ID, &gyro_chip_id, sizeof(gyro_chip_id)); - if (ret < 0) - { - LOG_ERR("Failed to read gyro chip ID register!"); - return ret; - } - const uint8_t GYRO_CHIP_ID = 0x0f; - if (gyro_chip_id != GYRO_CHIP_ID) - { - LOG_ERR("Gyro chip ID read from %s incorrect. Read 0x%02X, expected 0x%02X", dev->name, gyro_chip_id, GYRO_CHIP_ID); - } - struct i2c_dt_spec mag_bus = config->bus; mag_bus.addr = 0x10; diff --git a/drivers/sensor/bmg160/CMakeLists.txt b/drivers/sensor/bmg160/CMakeLists.txt new file mode 100644 index 0000000..5bb8db2 --- /dev/null +++ b/drivers/sensor/bmg160/CMakeLists.txt @@ -0,0 +1,2 @@ +zephyr_library() +zephyr_library_sources(bmg160.c) diff --git a/drivers/sensor/bmg160/Kconfig b/drivers/sensor/bmg160/Kconfig new file mode 100644 index 0000000..ba586c3 --- /dev/null +++ b/drivers/sensor/bmg160/Kconfig @@ -0,0 +1,17 @@ +config BMG160 + bool "BMG160" + default y + depends on DT_HAS_BOSCH_BMG160_ENABLED + help + Enable driver for BMG160. + +if BMG160 + +config BMG160_TRIGGER + bool "BMG160 trigger mode" + depends on BMG160 + help + Set to enable trigger mode using gpio interrupt, where + interrupts are configured to line ALERT PIN. + +endif # BMG160 diff --git a/drivers/sensor/bmg160/bmg160.c b/drivers/sensor/bmg160/bmg160.c new file mode 100644 index 0000000..f668bad --- /dev/null +++ b/drivers/sensor/bmg160/bmg160.c @@ -0,0 +1,189 @@ +/* + * Copyright (c) 2021 Nordic Semiconductor ASA + * SPDX-License-Identifier: Apache-2.0 + */ + +#define DT_DRV_COMPAT bosch_bmg160 + +#include +#include +#include +#include +#include + +#include +LOG_MODULE_REGISTER(bmg160, CONFIG_SENSOR_LOG_LEVEL); + +const uint8_t GYRO_REG_CHIP_ID = 0x00; +const uint8_t GYRO_REG_RATE_X_LSB = 0x02; +const uint8_t GYRO_REG_RATE_X_MSB = 0x03; +const uint8_t GYRO_REG_RATE_Y_LSB = 0x04; +const uint8_t GYRO_REG_RATE_Y_MSB = 0x05; +const uint8_t GYRO_REG_RATE_Z_LSB = 0x06; +const uint8_t GYRO_REG_RATE_Z_MSB = 0x07; +const uint8_t GYRO_REG_INT_STATUS_0 = 0x09; +const uint8_t GYRO_REG_INT_STATUS_1 = 0x0A; +const uint8_t GYRO_REG_INT_STATUS_2 = 0x0B; +const uint8_t GYRO_REG_INT_STATUS_3 = 0x0C; +const uint8_t GYRO_REG_FIFO_STATUS = 0x0E; +const uint8_t GYRO_REG_RANGE = 0x0F; +const uint8_t GYRO_REG_BW = 0x10; +const uint8_t GYRO_REG_LPM1 = 0x11; +const uint8_t GYRO_REG_LPM2 = 0x12; +const uint8_t GYRO_REG_RATE_HBW = 0x13; +const uint8_t GYRO_REG_BGW_SOFTRESET = 0x14; +const uint8_t GYRO_REG_INT_EN_0 = 0x15; +const uint8_t GYRO_REG_INT_EN_1 = 0x16; +const uint8_t GYRO_REG_INT_MAP_0 = 0x17; +const uint8_t GYRO_REG_INT_MAP_1 = 0x18; +const uint8_t GYRO_REG_INT_MAP_2 = 0x19; +const uint8_t GYRO_REG_INT_SOURCE_1 = 0x1A; +const uint8_t GYRO_REG_INT_SOURCE_2 = 0x1B; +// const uint8_t GYRO_REG_ = 0x1C; +// const uint8_t GYRO_REG_ = 0x1E; +const uint8_t GYRO_REG_INT_RST_LATCH = 0x21; +const uint8_t GYRO_REG_HIGH_TH_X = 0x22; +const uint8_t GYRO_REG_HIGH_DUR_X = 0x23; +const uint8_t GYRO_REG_HIGH_TH_Y = 0x24; +const uint8_t GYRO_REG_HIGH_DUR_Y = 0x25; +const uint8_t GYRO_REG_HIGH_TH_Z = 0x26; +const uint8_t GYRO_REG_HIGH_DUR_Z = 0x27; +const uint8_t GYRO_REG_SOC = 0x31; +const uint8_t GYRO_REG_A_FOC = 0x32; +const uint8_t GYRO_REG_TRIM_NVM_CTRL = 0x33; +const uint8_t GYRO_REG_BGW_SPI3_WDT = 0x34; +const uint8_t GYRO_REG_OFC1 = 0x36; +const uint8_t GYRO_REG_OFC2 = 0x37; +const uint8_t GYRO_REG_OFC3 = 0x38; +const uint8_t GYRO_REG_OFC4 = 0x39; +const uint8_t GYRO_REG_TRIM_GP0 = 0x3A; +const uint8_t GYRO_REG_TRIM_GP1 = 0x3B; +const uint8_t GYRO_REG_BIST = 0x3C; +const uint8_t GYRO_REG_FIFO_CONFIG_0 = 0x3D; +const uint8_t GYRO_REG_FIFO_CONFIG_1 = 0x3E; + +struct bmg160_data +{ + int16_t gyro_x; + int16_t gyro_y; + int16_t gyro_z; + int8_t temperature; // FIXME: does gyro have die temp sensor? +}; + +struct bmg160_config +{ + struct i2c_dt_spec bus; +}; + +static int bmg160_sample_fetch(const struct device *dev, + enum sensor_channel chan) +{ + const struct bmg160_config *config = dev->config; + struct bmg160_data *data = dev->data; + int ret; + + uint8_t gyro[6]; + ret = i2c_burst_read_dt(&config->bus, GYRO_REG_RATE_X_LSB, gyro, sizeof(gyro)); + if (ret < 0) + { + LOG_ERR("Failed to read gyro registers!"); + return ret; + } + data->gyro_x = ((int16_t)sys_get_le16(&gyro[0])); + data->gyro_y = ((int16_t)sys_get_le16(&gyro[2])); + data->gyro_z = ((int16_t)sys_get_le16(&gyro[4])); + + return 0; +} + +static int bmg160_channel_get(const struct device *dev, + enum sensor_channel chan, + struct sensor_value *val) +{ + struct bmg160_data *data = dev->data; + + switch (chan) + { + // // degrees C + // case SENSOR_CHAN_DIE_TEMP: + // // 0.5K/LSB, center temperature is 23C + // val->val1 = 23 + data->temperature / 2; + // val->val2 = 0; // TODO: don't throw out LSB + // break; + // radians/second + case SENSOR_CHAN_GYRO_X: + { + float rate = data->gyro_x * 2000.0 / 32767; + val->val1 = rate; + val->val2 = (rate - val->val1) * 1000000; + break; + } + case SENSOR_CHAN_GYRO_Y: + { + float rate = data->gyro_y * 2000.0 / 32767; + val->val1 = rate; + val->val2 = (rate - val->val1) * 1000000; + break; + } + case SENSOR_CHAN_GYRO_Z: + { + float rate = data->gyro_z * 2000.0 / 32767; + val->val1 = rate; + val->val2 = (rate - val->val1) * 1000000; + break; + } + default: + return -ENOTSUP; + } + + return 0; +} + +static const struct sensor_driver_api bmg160_api = { + .sample_fetch = &bmg160_sample_fetch, + .channel_get = &bmg160_channel_get, +}; + +static int bmg160_init(const struct device *dev) +{ + const struct bmg160_config *const config = dev->config; + int ret; + + if (!device_is_ready(config->bus.bus)) + { + LOG_ERR("I2C bus %s is not ready", config->bus.bus->name); + return -ENODEV; + } + + // TODO: reset IC? + // Wait for device to reset + + uint8_t gyro_chip_id; + ret = i2c_burst_read_dt(&config->bus, GYRO_REG_CHIP_ID, &gyro_chip_id, sizeof(gyro_chip_id)); + if (ret < 0) + { + LOG_ERR("Failed to read gyro chip ID register!"); + return ret; + } + const uint8_t GYRO_CHIP_ID = 0x0f; + if (gyro_chip_id != GYRO_CHIP_ID) + { + LOG_ERR("Gyro chip ID read from %s incorrect. Read 0x%02X, expected 0x%02X", dev->name, gyro_chip_id, GYRO_CHIP_ID); + } + + return 0; +} + +#define BMG160_INIT(i) \ + static struct bmg160_data bmg160_data_##i; \ + \ + static const struct bmg160_config bmg160_config_##i = { \ + .bus = I2C_DT_SPEC_INST_GET(i), \ + }; \ + \ + SENSOR_DEVICE_DT_INST_DEFINE(i, &bmg160_init, NULL, \ + &bmg160_data_##i, \ + &bmg160_config_##i, POST_KERNEL, \ + CONFIG_SENSOR_INIT_PRIORITY, &bmg160_api); + +DT_INST_FOREACH_STATUS_OKAY(BMG160_INIT) diff --git a/drivers/sensor/bmm150/CMakeLists.txt b/drivers/sensor/bmm150/CMakeLists.txt new file mode 100644 index 0000000..d8fe8ea --- /dev/null +++ b/drivers/sensor/bmm150/CMakeLists.txt @@ -0,0 +1,2 @@ +zephyr_library() +# zephyr_library_sources(bmm150.c)