fb60aab245
In order to bring consistency in-tree, migrate all drivers to the new prefix <zephyr/...>. Note that the conversion has been scripted, refer to #45388 for more details. Signed-off-by: Gerard Marull-Paretas <gerard.marull@nordicsemi.no>
707 lines
17 KiB
C
707 lines
17 KiB
C
/*
|
|
* Copyright (c) 2022 Esco Medical ApS
|
|
* Copyright (c) 2020 TDK Invensense
|
|
*
|
|
* SPDX-License-Identifier: Apache-2.0
|
|
*/
|
|
|
|
#define DT_DRV_COMPAT invensense_icm42670
|
|
|
|
#include <zephyr/drivers/sensor.h>
|
|
#include <zephyr/drivers/spi.h>
|
|
#include <zephyr/sys/byteorder.h>
|
|
#include "icm42670.h"
|
|
#include "icm42670_reg.h"
|
|
#include "icm42670_spi.h"
|
|
#include "icm42670_trigger.h"
|
|
|
|
#include <zephyr/logging/log.h>
|
|
LOG_MODULE_REGISTER(ICM42670, CONFIG_SENSOR_LOG_LEVEL);
|
|
|
|
/*
|
|
* Gyro FS to scaling factor mapping.
|
|
* See datasheet section 3.1 for details
|
|
*/
|
|
static const uint16_t icm42670_gyro_sensitivity_x10[] = {
|
|
164, /* BIT_GYRO_UI_FS_2000 */
|
|
328, /* BIT_GYRO_UI_FS_1000 */
|
|
655, /* BIT_GYRO_UI_FS_500 */
|
|
1310, /* BIT_GYRO_UI_FS_250 */
|
|
};
|
|
|
|
static int icm42670_set_accel_fs(const struct device *dev, uint16_t fs)
|
|
{
|
|
const struct icm42670_config *cfg = dev->config;
|
|
struct icm42670_data *data = dev->data;
|
|
uint8_t temp;
|
|
|
|
if ((fs > 16) || (fs < 2)) {
|
|
LOG_ERR("Unsupported range");
|
|
return -ENOTSUP;
|
|
}
|
|
|
|
if (fs > 8) {
|
|
temp = BIT_ACCEL_UI_FS_16;
|
|
} else if (fs > 4) {
|
|
temp = BIT_ACCEL_UI_FS_8;
|
|
} else if (fs > 2) {
|
|
temp = BIT_ACCEL_UI_FS_4;
|
|
} else {
|
|
temp = BIT_ACCEL_UI_FS_2;
|
|
}
|
|
|
|
data->accel_sensitivity_shift = MIN_ACCEL_SENS_SHIFT + temp;
|
|
|
|
return icm42670_spi_update_register(&cfg->spi, REG_ACCEL_CONFIG0,
|
|
(uint8_t)MASK_ACCEL_UI_FS_SEL, temp);
|
|
}
|
|
|
|
static int icm42670_set_gyro_fs(const struct device *dev, uint16_t fs)
|
|
{
|
|
const struct icm42670_config *cfg = dev->config;
|
|
struct icm42670_data *data = dev->data;
|
|
uint8_t temp;
|
|
|
|
if ((fs > 2000) || (fs < 250)) {
|
|
LOG_ERR("Unsupported range");
|
|
return -ENOTSUP;
|
|
}
|
|
|
|
if (fs > 1000) {
|
|
temp = BIT_GYRO_UI_FS_2000;
|
|
} else if (fs > 500) {
|
|
temp = BIT_GYRO_UI_FS_1000;
|
|
} else if (fs > 250) {
|
|
temp = BIT_GYRO_UI_FS_500;
|
|
} else {
|
|
temp = BIT_GYRO_UI_FS_250;
|
|
}
|
|
|
|
data->gyro_sensitivity_x10 = icm42670_gyro_sensitivity_x10[temp];
|
|
|
|
return icm42670_spi_update_register(&cfg->spi, REG_GYRO_CONFIG0,
|
|
(uint8_t)MASK_GYRO_UI_FS_SEL, temp);
|
|
}
|
|
|
|
static int icm42670_set_accel_odr(const struct device *dev, uint16_t rate)
|
|
{
|
|
const struct icm42670_config *cfg = dev->config;
|
|
uint8_t temp;
|
|
|
|
if ((rate > 1600) || (rate < 1)) {
|
|
LOG_ERR("Unsupported frequency");
|
|
return -ENOTSUP;
|
|
}
|
|
|
|
if (rate > 800) {
|
|
temp = BIT_ACCEL_ODR_1600;
|
|
} else if (rate > 400) {
|
|
temp = BIT_ACCEL_ODR_800;
|
|
} else if (rate > 200) {
|
|
temp = BIT_ACCEL_ODR_400;
|
|
} else if (rate > 100) {
|
|
temp = BIT_ACCEL_ODR_200;
|
|
} else if (rate > 50) {
|
|
temp = BIT_ACCEL_ODR_100;
|
|
} else if (rate > 25) {
|
|
temp = BIT_ACCEL_ODR_50;
|
|
} else if (rate > 12) {
|
|
temp = BIT_ACCEL_ODR_25;
|
|
} else if (rate > 6) {
|
|
temp = BIT_ACCEL_ODR_12;
|
|
} else if (rate > 3) {
|
|
temp = BIT_ACCEL_ODR_6;
|
|
} else if (rate > 1) {
|
|
temp = BIT_ACCEL_ODR_3;
|
|
} else {
|
|
temp = BIT_ACCEL_ODR_1;
|
|
}
|
|
|
|
return icm42670_spi_update_register(&cfg->spi, REG_ACCEL_CONFIG0, (uint8_t)MASK_ACCEL_ODR,
|
|
temp);
|
|
}
|
|
|
|
static int icm42670_set_gyro_odr(const struct device *dev, uint16_t rate)
|
|
{
|
|
const struct icm42670_config *cfg = dev->config;
|
|
uint8_t temp;
|
|
|
|
if ((rate > 1600) || (rate < 12)) {
|
|
LOG_ERR("Unsupported frequency");
|
|
return -ENOTSUP;
|
|
}
|
|
|
|
if (rate > 800) {
|
|
temp = BIT_GYRO_ODR_1600;
|
|
} else if (rate > 400) {
|
|
temp = BIT_GYRO_ODR_800;
|
|
} else if (rate > 200) {
|
|
temp = BIT_GYRO_ODR_400;
|
|
} else if (rate > 100) {
|
|
temp = BIT_GYRO_ODR_200;
|
|
} else if (rate > 50) {
|
|
temp = BIT_GYRO_ODR_100;
|
|
} else if (rate > 25) {
|
|
temp = BIT_GYRO_ODR_50;
|
|
} else if (rate > 12) {
|
|
temp = BIT_GYRO_ODR_25;
|
|
} else {
|
|
temp = BIT_GYRO_ODR_12;
|
|
}
|
|
|
|
return icm42670_spi_update_register(&cfg->spi, REG_GYRO_CONFIG0, (uint8_t)MASK_GYRO_ODR,
|
|
temp);
|
|
}
|
|
|
|
static int icm42670_enable_mclk(const struct device *dev)
|
|
{
|
|
const struct icm42670_config *cfg = dev->config;
|
|
|
|
/* switch on MCLK by setting the IDLE bit */
|
|
int res = icm42670_spi_single_write(&cfg->spi, REG_PWR_MGMT0, BIT_IDLE);
|
|
|
|
if (res) {
|
|
return res;
|
|
}
|
|
|
|
/* wait for the MCLK to stabilize by polling MCLK_RDY register */
|
|
for (int i = 0; i < MCLK_POLL_ATTEMPTS; i++) {
|
|
uint8_t value = 0;
|
|
|
|
k_usleep(MCLK_POLL_INTERVAL_US);
|
|
res = icm42670_spi_read(&cfg->spi, REG_MCLK_RDY, &value, 1);
|
|
|
|
if (res) {
|
|
return res;
|
|
}
|
|
|
|
if (FIELD_GET(BIT_MCLK_RDY, value)) {
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
return -EIO;
|
|
}
|
|
|
|
static int icm42670_sensor_init(const struct device *dev)
|
|
{
|
|
int res;
|
|
uint8_t value;
|
|
const struct icm42670_config *cfg = dev->config;
|
|
|
|
/* start up time for register read/write after POR is 1ms and supply ramp time is 3ms */
|
|
k_msleep(3);
|
|
|
|
/* perform a soft reset to ensure a clean slate, reset bit will auto-clear */
|
|
res = icm42670_spi_single_write(&cfg->spi, REG_SIGNAL_PATH_RESET, BIT_SOFT_RESET);
|
|
|
|
if (res) {
|
|
LOG_ERR("write REG_SIGNAL_PATH_RESET failed");
|
|
return res;
|
|
}
|
|
|
|
/* wait for soft reset to take effect */
|
|
k_msleep(SOFT_RESET_TIME_MS);
|
|
|
|
/* force SPI-4w hardware configuration (so that next read is correct) */
|
|
res = icm42670_spi_single_write(&cfg->spi, REG_DEVICE_CONFIG, BIT_SPI_AP_4WIRE);
|
|
|
|
if (res) {
|
|
return res;
|
|
}
|
|
|
|
/* always use internal RC oscillator */
|
|
res = icm42670_spi_single_write(&cfg->spi, REG_INTF_CONFIG1,
|
|
(uint8_t)FIELD_PREP(MASK_CLKSEL, BIT_CLKSEL_INT_RC));
|
|
|
|
if (res) {
|
|
return res;
|
|
}
|
|
|
|
/* clear reset done int flag */
|
|
res = icm42670_spi_read(&cfg->spi, REG_INT_STATUS, &value, 1);
|
|
|
|
if (res) {
|
|
return res;
|
|
}
|
|
|
|
if (FIELD_GET(BIT_STATUS_RESET_DONE_INT, value) != 1) {
|
|
LOG_ERR("unexpected RESET_DONE_INT value, %i", value);
|
|
return -EINVAL;
|
|
}
|
|
|
|
/* enable the master clock to ensure proper operation */
|
|
res = icm42670_enable_mclk(dev);
|
|
|
|
if (res) {
|
|
return res;
|
|
}
|
|
|
|
res = icm42670_spi_read(&cfg->spi, REG_WHO_AM_I, &value, 1);
|
|
|
|
if (res) {
|
|
return res;
|
|
}
|
|
|
|
if (value != WHO_AM_I_ICM42670) {
|
|
LOG_ERR("invalid WHO_AM_I value, was %i but expected %i", value, WHO_AM_I_ICM42670);
|
|
return -EINVAL;
|
|
}
|
|
|
|
LOG_DBG("device id: 0x%02X", value);
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int icm42670_turn_on_sensor(const struct device *dev)
|
|
{
|
|
struct icm42670_data *data = dev->data;
|
|
const struct icm42670_config *cfg = dev->config;
|
|
uint8_t value;
|
|
int res;
|
|
|
|
value = FIELD_PREP(MASK_ACCEL_MODE, BIT_ACCEL_MODE_LNM) |
|
|
FIELD_PREP(MASK_GYRO_MODE, BIT_GYRO_MODE_LNM);
|
|
|
|
res = icm42670_spi_update_register(&cfg->spi, REG_PWR_MGMT0,
|
|
(uint8_t)(MASK_ACCEL_MODE | MASK_GYRO_MODE), value);
|
|
|
|
if (res) {
|
|
return res;
|
|
}
|
|
|
|
res = icm42670_set_accel_fs(dev, data->accel_fs);
|
|
|
|
if (res) {
|
|
return res;
|
|
}
|
|
|
|
res = icm42670_set_accel_odr(dev, data->accel_hz);
|
|
|
|
if (res) {
|
|
return res;
|
|
}
|
|
|
|
res = icm42670_set_gyro_fs(dev, data->gyro_fs);
|
|
|
|
if (res) {
|
|
return res;
|
|
}
|
|
|
|
res = icm42670_set_gyro_odr(dev, data->gyro_hz);
|
|
|
|
if (res) {
|
|
return res;
|
|
}
|
|
|
|
/*
|
|
* Accelerometer sensor need at least 10ms startup time
|
|
* Gyroscope sensor need at least 30ms startup time
|
|
*/
|
|
k_msleep(100);
|
|
|
|
return 0;
|
|
}
|
|
|
|
static void icm42670_convert_accel(struct sensor_value *val, int16_t raw_val,
|
|
uint16_t sensitivity_shift)
|
|
{
|
|
/* see datasheet section 3.2 for details */
|
|
int64_t conv_val = ((int64_t)raw_val * SENSOR_G) >> sensitivity_shift;
|
|
|
|
val->val1 = conv_val / 1000000LL;
|
|
val->val2 = conv_val % 1000000LL;
|
|
}
|
|
|
|
static void icm42670_convert_gyro(struct sensor_value *val, int16_t raw_val,
|
|
uint16_t sensitivity_x10)
|
|
{
|
|
/* see datasheet section 3.1 for details */
|
|
int64_t conv_val = ((int64_t)raw_val * SENSOR_PI * 10) / (sensitivity_x10 * 180LL);
|
|
|
|
val->val1 = conv_val / 1000000LL;
|
|
val->val2 = conv_val % 1000000LL;
|
|
}
|
|
|
|
static inline void icm42670_convert_temp(struct sensor_value *val, int16_t raw_val)
|
|
{
|
|
/* see datasheet section 15.9 for details */
|
|
val->val1 = (((int64_t)raw_val * 100) / 12800) + 25;
|
|
val->val2 = ((((int64_t)raw_val * 100) % 12800) * 1000000) / 12800;
|
|
|
|
if (val->val2 < 0) {
|
|
val->val1--;
|
|
val->val2 += 1000000;
|
|
} else if (val->val2 >= 1000000) {
|
|
val->val1++;
|
|
val->val2 -= 1000000;
|
|
}
|
|
}
|
|
|
|
static int icm42670_channel_get(const struct device *dev, enum sensor_channel chan,
|
|
struct sensor_value *val)
|
|
{
|
|
int res = 0;
|
|
const struct icm42670_data *data = dev->data;
|
|
|
|
icm42670_lock(dev);
|
|
|
|
switch (chan) {
|
|
case SENSOR_CHAN_ACCEL_XYZ:
|
|
icm42670_convert_accel(&val[0], data->accel_x, data->accel_sensitivity_shift);
|
|
icm42670_convert_accel(&val[1], data->accel_y, data->accel_sensitivity_shift);
|
|
icm42670_convert_accel(&val[2], data->accel_z, data->accel_sensitivity_shift);
|
|
break;
|
|
case SENSOR_CHAN_ACCEL_X:
|
|
icm42670_convert_accel(val, data->accel_x, data->accel_sensitivity_shift);
|
|
break;
|
|
case SENSOR_CHAN_ACCEL_Y:
|
|
icm42670_convert_accel(val, data->accel_y, data->accel_sensitivity_shift);
|
|
break;
|
|
case SENSOR_CHAN_ACCEL_Z:
|
|
icm42670_convert_accel(val, data->accel_z, data->accel_sensitivity_shift);
|
|
break;
|
|
case SENSOR_CHAN_GYRO_XYZ:
|
|
icm42670_convert_gyro(&val[0], data->gyro_x, data->gyro_sensitivity_x10);
|
|
icm42670_convert_gyro(&val[1], data->gyro_y, data->gyro_sensitivity_x10);
|
|
icm42670_convert_gyro(&val[2], data->gyro_z, data->gyro_sensitivity_x10);
|
|
break;
|
|
case SENSOR_CHAN_GYRO_X:
|
|
icm42670_convert_gyro(val, data->gyro_x, data->gyro_sensitivity_x10);
|
|
break;
|
|
case SENSOR_CHAN_GYRO_Y:
|
|
icm42670_convert_gyro(val, data->gyro_y, data->gyro_sensitivity_x10);
|
|
break;
|
|
case SENSOR_CHAN_GYRO_Z:
|
|
icm42670_convert_gyro(val, data->gyro_z, data->gyro_sensitivity_x10);
|
|
break;
|
|
case SENSOR_CHAN_DIE_TEMP:
|
|
icm42670_convert_temp(val, data->temp);
|
|
break;
|
|
default:
|
|
res = -ENOTSUP;
|
|
break;
|
|
}
|
|
|
|
icm42670_unlock(dev);
|
|
|
|
return res;
|
|
}
|
|
|
|
static int icm42670_sample_fetch_accel(const struct device *dev)
|
|
{
|
|
const struct icm42670_config *cfg = dev->config;
|
|
struct icm42670_data *data = dev->data;
|
|
uint8_t buffer[ACCEL_DATA_SIZE];
|
|
|
|
int res = icm42670_spi_read(&cfg->spi, REG_ACCEL_DATA_X1, buffer, ACCEL_DATA_SIZE);
|
|
|
|
if (res) {
|
|
return res;
|
|
}
|
|
|
|
data->accel_x = (int16_t)sys_get_be16(&buffer[0]);
|
|
data->accel_y = (int16_t)sys_get_be16(&buffer[2]);
|
|
data->accel_z = (int16_t)sys_get_be16(&buffer[4]);
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int icm42670_sample_fetch_gyro(const struct device *dev)
|
|
{
|
|
const struct icm42670_config *cfg = dev->config;
|
|
struct icm42670_data *data = dev->data;
|
|
uint8_t buffer[GYRO_DATA_SIZE];
|
|
|
|
int res = icm42670_spi_read(&cfg->spi, REG_GYRO_DATA_X1, buffer, GYRO_DATA_SIZE);
|
|
|
|
if (res) {
|
|
return res;
|
|
}
|
|
|
|
data->gyro_x = (int16_t)sys_get_be16(&buffer[0]);
|
|
data->gyro_y = (int16_t)sys_get_be16(&buffer[2]);
|
|
data->gyro_z = (int16_t)sys_get_be16(&buffer[4]);
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int icm42670_sample_fetch_temp(const struct device *dev)
|
|
{
|
|
const struct icm42670_config *cfg = dev->config;
|
|
struct icm42670_data *data = dev->data;
|
|
uint8_t buffer[TEMP_DATA_SIZE];
|
|
|
|
int res = icm42670_spi_read(&cfg->spi, REG_TEMP_DATA1, buffer, TEMP_DATA_SIZE);
|
|
|
|
if (res) {
|
|
return res;
|
|
}
|
|
|
|
data->temp = (int16_t)sys_get_be16(&buffer[0]);
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int icm42670_sample_fetch(const struct device *dev, enum sensor_channel chan)
|
|
{
|
|
uint8_t status;
|
|
const struct icm42670_config *cfg = dev->config;
|
|
|
|
icm42670_lock(dev);
|
|
|
|
int res = icm42670_spi_read(&cfg->spi, REG_INT_STATUS_DRDY, &status, 1);
|
|
|
|
if (res) {
|
|
goto cleanup;
|
|
}
|
|
|
|
if (!FIELD_GET(BIT_INT_STATUS_DATA_DRDY, status)) {
|
|
res = -EBUSY;
|
|
goto cleanup;
|
|
}
|
|
|
|
switch (chan) {
|
|
case SENSOR_CHAN_ALL:
|
|
res |= icm42670_sample_fetch_accel(dev);
|
|
res |= icm42670_sample_fetch_gyro(dev);
|
|
res |= icm42670_sample_fetch_temp(dev);
|
|
break;
|
|
case SENSOR_CHAN_ACCEL_XYZ:
|
|
case SENSOR_CHAN_ACCEL_X:
|
|
case SENSOR_CHAN_ACCEL_Y:
|
|
case SENSOR_CHAN_ACCEL_Z:
|
|
res = icm42670_sample_fetch_accel(dev);
|
|
break;
|
|
case SENSOR_CHAN_GYRO_XYZ:
|
|
case SENSOR_CHAN_GYRO_X:
|
|
case SENSOR_CHAN_GYRO_Y:
|
|
case SENSOR_CHAN_GYRO_Z:
|
|
res = icm42670_sample_fetch_gyro(dev);
|
|
break;
|
|
case SENSOR_CHAN_DIE_TEMP:
|
|
res = icm42670_sample_fetch_temp(dev);
|
|
break;
|
|
default:
|
|
res = -ENOTSUP;
|
|
break;
|
|
}
|
|
|
|
cleanup:
|
|
icm42670_unlock(dev);
|
|
return res;
|
|
}
|
|
|
|
static int icm42670_attr_set(const struct device *dev, enum sensor_channel chan,
|
|
enum sensor_attribute attr, const struct sensor_value *val)
|
|
{
|
|
int res = 0;
|
|
struct icm42670_data *data = dev->data;
|
|
|
|
__ASSERT_NO_MSG(val != NULL);
|
|
|
|
icm42670_lock(dev);
|
|
|
|
switch (chan) {
|
|
case SENSOR_CHAN_ACCEL_X:
|
|
case SENSOR_CHAN_ACCEL_Y:
|
|
case SENSOR_CHAN_ACCEL_Z:
|
|
case SENSOR_CHAN_ACCEL_XYZ:
|
|
if (attr == SENSOR_ATTR_SAMPLING_FREQUENCY) {
|
|
res = icm42670_set_accel_odr(dev, data->accel_hz);
|
|
|
|
if (res) {
|
|
LOG_ERR("Incorrect sampling value");
|
|
} else {
|
|
data->accel_hz = val->val1;
|
|
}
|
|
} else if (attr == SENSOR_ATTR_FULL_SCALE) {
|
|
res = icm42670_set_accel_fs(dev, data->accel_fs);
|
|
|
|
if (res) {
|
|
LOG_ERR("Incorrect fullscale value");
|
|
} else {
|
|
data->accel_fs = val->val1;
|
|
}
|
|
} else {
|
|
LOG_ERR("Unsupported attribute");
|
|
res = -ENOTSUP;
|
|
}
|
|
break;
|
|
|
|
case SENSOR_CHAN_GYRO_X:
|
|
case SENSOR_CHAN_GYRO_Y:
|
|
case SENSOR_CHAN_GYRO_Z:
|
|
case SENSOR_CHAN_GYRO_XYZ:
|
|
if (attr == SENSOR_ATTR_SAMPLING_FREQUENCY) {
|
|
res = icm42670_set_gyro_odr(dev, data->gyro_hz);
|
|
|
|
if (res) {
|
|
LOG_ERR("Incorrect sampling value");
|
|
} else {
|
|
data->gyro_hz = val->val1;
|
|
}
|
|
} else if (attr == SENSOR_ATTR_FULL_SCALE) {
|
|
res = icm42670_set_gyro_fs(dev, data->gyro_fs);
|
|
|
|
if (res) {
|
|
LOG_ERR("Incorrect fullscale value");
|
|
} else {
|
|
data->gyro_fs = val->val1;
|
|
}
|
|
} else {
|
|
LOG_ERR("Unsupported attribute");
|
|
res = -EINVAL;
|
|
}
|
|
break;
|
|
|
|
default:
|
|
LOG_ERR("Unsupported channel");
|
|
res = -EINVAL;
|
|
break;
|
|
}
|
|
|
|
icm42670_unlock(dev);
|
|
|
|
return res;
|
|
}
|
|
|
|
static int icm42670_attr_get(const struct device *dev, enum sensor_channel chan,
|
|
enum sensor_attribute attr, struct sensor_value *val)
|
|
{
|
|
const struct icm42670_data *data = dev->data;
|
|
int res = 0;
|
|
|
|
__ASSERT_NO_MSG(val != NULL);
|
|
|
|
icm42670_lock(dev);
|
|
|
|
switch (chan) {
|
|
case SENSOR_CHAN_ACCEL_X:
|
|
case SENSOR_CHAN_ACCEL_Y:
|
|
case SENSOR_CHAN_ACCEL_Z:
|
|
case SENSOR_CHAN_ACCEL_XYZ:
|
|
if (attr == SENSOR_ATTR_SAMPLING_FREQUENCY) {
|
|
val->val1 = data->accel_hz;
|
|
} else if (attr == SENSOR_ATTR_FULL_SCALE) {
|
|
val->val1 = data->accel_fs;
|
|
} else {
|
|
LOG_ERR("Unsupported attribute");
|
|
res = -EINVAL;
|
|
}
|
|
break;
|
|
|
|
case SENSOR_CHAN_GYRO_X:
|
|
case SENSOR_CHAN_GYRO_Y:
|
|
case SENSOR_CHAN_GYRO_Z:
|
|
case SENSOR_CHAN_GYRO_XYZ:
|
|
if (attr == SENSOR_ATTR_SAMPLING_FREQUENCY) {
|
|
val->val1 = data->gyro_hz;
|
|
} else if (attr == SENSOR_ATTR_FULL_SCALE) {
|
|
val->val1 = data->gyro_fs;
|
|
} else {
|
|
LOG_ERR("Unsupported attribute");
|
|
res = -EINVAL;
|
|
}
|
|
break;
|
|
|
|
default:
|
|
LOG_ERR("Unsupported channel");
|
|
res = -EINVAL;
|
|
break;
|
|
}
|
|
|
|
icm42670_unlock(dev);
|
|
|
|
return res;
|
|
}
|
|
|
|
static int icm42670_init(const struct device *dev)
|
|
{
|
|
struct icm42670_data *data = dev->data;
|
|
const struct icm42670_config *cfg = dev->config;
|
|
|
|
if (!spi_is_ready(&cfg->spi)) {
|
|
LOG_ERR("SPI bus is not ready");
|
|
return -ENODEV;
|
|
}
|
|
|
|
data->accel_x = 0;
|
|
data->accel_y = 0;
|
|
data->accel_z = 0;
|
|
data->gyro_x = 0;
|
|
data->gyro_y = 0;
|
|
data->gyro_z = 0;
|
|
data->temp = 0;
|
|
|
|
if (icm42670_sensor_init(dev)) {
|
|
LOG_ERR("could not initialize sensor");
|
|
return -EIO;
|
|
}
|
|
|
|
#ifdef CONFIG_ICM42670_TRIGGER
|
|
if (icm42670_trigger_init(dev)) {
|
|
LOG_ERR("Failed to initialize interrupts.");
|
|
return -EIO;
|
|
}
|
|
#endif
|
|
|
|
int res = icm42670_turn_on_sensor(dev);
|
|
|
|
#ifdef CONFIG_ICM42670_TRIGGER
|
|
if (icm42670_trigger_enable_interrupt(dev)) {
|
|
LOG_ERR("Failed to enable interrupts");
|
|
return -EIO;
|
|
}
|
|
#endif
|
|
|
|
return res;
|
|
}
|
|
|
|
#ifndef CONFIG_ICM42670_TRIGGER
|
|
|
|
void icm42670_lock(const struct device *dev)
|
|
{
|
|
ARG_UNUSED(dev);
|
|
}
|
|
|
|
void icm42670_unlock(const struct device *dev)
|
|
{
|
|
ARG_UNUSED(dev);
|
|
}
|
|
|
|
#endif
|
|
|
|
static const struct sensor_driver_api icm42670_driver_api = {
|
|
#ifdef CONFIG_ICM42670_TRIGGER
|
|
.trigger_set = icm42670_trigger_set,
|
|
#endif
|
|
.sample_fetch = icm42670_sample_fetch,
|
|
.channel_get = icm42670_channel_get,
|
|
.attr_set = icm42670_attr_set,
|
|
.attr_get = icm42670_attr_get,
|
|
};
|
|
|
|
/* device defaults to spi mode 0/3 support */
|
|
#define ICM42670_SPI_CFG \
|
|
SPI_OP_MODE_MASTER | SPI_MODE_CPOL | SPI_MODE_CPHA | SPI_WORD_SET(8) | SPI_TRANSFER_MSB
|
|
|
|
#define ICM42670_INIT(inst) \
|
|
static struct icm42670_data icm42670_driver_##inst = { \
|
|
.accel_hz = DT_INST_PROP(inst, accel_hz), \
|
|
.accel_fs = DT_INST_PROP(inst, accel_fs), \
|
|
.gyro_hz = DT_INST_PROP(inst, gyro_hz), \
|
|
.gyro_fs = DT_INST_PROP(inst, gyro_fs), \
|
|
}; \
|
|
\
|
|
static const struct icm42670_config icm42670_cfg_##inst = { \
|
|
.spi = SPI_DT_SPEC_INST_GET(inst, ICM42670_SPI_CFG, 0U), \
|
|
.gpio_int = GPIO_DT_SPEC_INST_GET_OR(inst, int_gpios, { 0 }), \
|
|
}; \
|
|
\
|
|
DEVICE_DT_INST_DEFINE(inst, icm42670_init, NULL, &icm42670_driver_##inst, \
|
|
&icm42670_cfg_##inst, POST_KERNEL, CONFIG_SENSOR_INIT_PRIORITY, \
|
|
&icm42670_driver_api);
|
|
|
|
DT_INST_FOREACH_STATUS_OKAY(ICM42670_INIT)
|