split out gyro code
Some checks failed
Build / build (ubuntu-22.04) (push) Failing after 1m43s
Documentation / build (push) Successful in 24s

This commit is contained in:
2024-07-04 23:21:16 -06:00
parent d0fd24548c
commit 9a2419223a
6 changed files with 224 additions and 116 deletions

View File

@ -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");
}

View File

@ -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;

View File

@ -0,0 +1,2 @@
zephyr_library()
zephyr_library_sources(bmg160.c)

View File

@ -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

View File

@ -0,0 +1,189 @@
/*
* Copyright (c) 2021 Nordic Semiconductor ASA
* SPDX-License-Identifier: Apache-2.0
*/
#define DT_DRV_COMPAT bosch_bmg160
#include <zephyr/device.h>
#include <zephyr/drivers/gpio.h>
#include <zephyr/drivers/i2c.h>
#include <zephyr/drivers/sensor.h>
#include <zephyr/sys/byteorder.h>
#include <zephyr/logging/log.h>
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)

View File

@ -0,0 +1,2 @@
zephyr_library()
# zephyr_library_sources(bmm150.c)