You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
317 lines
8.2 KiB
317 lines
8.2 KiB
/* |
|
* SPDX-License-Identifier: Apache-2.0 |
|
* |
|
* Copyright (c) 2023 Linumiz |
|
*/ |
|
|
|
#define DT_DRV_COMPAT memsic_mc3419 |
|
|
|
#include <zephyr/init.h> |
|
#include <zephyr/drivers/sensor.h> |
|
#include <zephyr/sys/byteorder.h> |
|
#include <zephyr/kernel.h> |
|
#include <zephyr/logging/log.h> |
|
#include "mc3419.h" |
|
|
|
LOG_MODULE_REGISTER(MC3419, CONFIG_SENSOR_LOG_LEVEL); |
|
|
|
static const uint16_t mc3419_accel_sense_map[] = {1, 2, 4, 8, 6}; |
|
static struct mc3419_odr_map odr_map_table[] = { |
|
{25}, {50}, {62, 500}, {100}, |
|
{125}, {250}, {500}, {1000} |
|
}; |
|
|
|
static int mc3419_get_odr_value(uint16_t freq, uint16_t m_freq) |
|
{ |
|
for (int i = 0; i < ARRAY_SIZE(odr_map_table); i++) { |
|
if (odr_map_table[i].freq == freq && |
|
odr_map_table[i].mfreq == m_freq) { |
|
return i; |
|
} |
|
} |
|
|
|
return -EINVAL; |
|
} |
|
|
|
static inline int mc3419_set_op_mode(const struct mc3419_config *cfg, |
|
enum mc3419_op_mode mode) |
|
{ |
|
return i2c_reg_write_byte_dt(&cfg->i2c, MC3419_REG_OP_MODE, mode); |
|
} |
|
|
|
static int mc3419_sample_fetch(const struct device *dev, |
|
enum sensor_channel chan) |
|
{ |
|
int ret = 0; |
|
const struct mc3419_config *cfg = dev->config; |
|
struct mc3419_driver_data *data = dev->data; |
|
|
|
k_sem_take(&data->sem, K_FOREVER); |
|
ret = i2c_burst_read_dt(&cfg->i2c, MC3419_REG_XOUT_L, |
|
(uint8_t *)data->samples, |
|
MC3419_SAMPLE_READ_SIZE); |
|
k_sem_give(&data->sem); |
|
return ret; |
|
} |
|
|
|
static int mc3419_to_sensor_value(double sensitivity, int16_t *raw_data, |
|
struct sensor_value *val) |
|
{ |
|
double value = sys_le16_to_cpu(*raw_data); |
|
|
|
value *= sensitivity * SENSOR_GRAVITY_DOUBLE / 1000; |
|
|
|
return sensor_value_from_double(val, value); |
|
} |
|
|
|
static int mc3419_channel_get(const struct device *dev, |
|
enum sensor_channel chan, |
|
struct sensor_value *val) |
|
{ |
|
int ret = 0; |
|
struct mc3419_driver_data *data = dev->data; |
|
|
|
k_sem_take(&data->sem, K_FOREVER); |
|
switch (chan) { |
|
case SENSOR_CHAN_ACCEL_X: |
|
ret = mc3419_to_sensor_value(data->sensitivity, &data->samples[0], val); |
|
break; |
|
case SENSOR_CHAN_ACCEL_Y: |
|
ret = mc3419_to_sensor_value(data->sensitivity, &data->samples[1], val); |
|
break; |
|
case SENSOR_CHAN_ACCEL_Z: |
|
ret = mc3419_to_sensor_value(data->sensitivity, &data->samples[2], val); |
|
break; |
|
case SENSOR_CHAN_ACCEL_XYZ: |
|
ret = mc3419_to_sensor_value(data->sensitivity, &data->samples[0], &val[0]); |
|
ret |= mc3419_to_sensor_value(data->sensitivity, &data->samples[1], &val[1]); |
|
ret |= mc3419_to_sensor_value(data->sensitivity, &data->samples[2], &val[2]); |
|
break; |
|
default: |
|
LOG_ERR("Unsupported channel"); |
|
ret = -ENOTSUP; |
|
} |
|
|
|
k_sem_give(&data->sem); |
|
return ret; |
|
} |
|
|
|
static int mc3419_set_accel_range(const struct device *dev, uint8_t range) |
|
{ |
|
int ret = 0; |
|
const struct mc3419_config *cfg = dev->config; |
|
struct mc3419_driver_data *data = dev->data; |
|
|
|
if (range >= MC3419_ACCL_RANGE_END) { |
|
LOG_ERR("Accel resolution is out of range"); |
|
return -EINVAL; |
|
} |
|
|
|
ret = i2c_reg_update_byte_dt(&cfg->i2c, MC3419_REG_RANGE_SELECT_CTRL, |
|
MC3419_RANGE_MASK, range << 4); |
|
if (ret < 0) { |
|
LOG_ERR("Failed to set resolution (%d)", ret); |
|
return ret; |
|
} |
|
|
|
data->sensitivity = (double)(mc3419_accel_sense_map[range] * |
|
SENSOR_GRAIN_VALUE); |
|
|
|
return 0; |
|
} |
|
|
|
static int mc3419_set_odr(const struct device *dev, |
|
const struct sensor_value *val) |
|
{ |
|
int ret = 0; |
|
int data_rate = 0; |
|
const struct mc3419_config *cfg = dev->config; |
|
|
|
ret = mc3419_get_odr_value(val->val1, val->val2); |
|
if (ret < 0) { |
|
LOG_ERR("Failed to get odr value from odr map (%d)", ret); |
|
return ret; |
|
} |
|
|
|
data_rate = MC3419_BASE_ODR_VAL + ret; |
|
|
|
ret = i2c_reg_write_byte_dt(&cfg->i2c, MC3419_REG_SAMPLE_RATE, |
|
data_rate); |
|
if (ret < 0) { |
|
LOG_ERR("Failed to set ODR (%d)", ret); |
|
return ret; |
|
} |
|
|
|
LOG_DBG("Set ODR Rate to 0x%x", data_rate); |
|
ret = i2c_reg_write_byte_dt(&cfg->i2c, MC3419_REG_SAMPLE_RATE_2, cfg->decimation_rate); |
|
if (ret < 0) { |
|
LOG_ERR("Failed to set decimation rate (%d)", ret); |
|
return ret; |
|
} |
|
|
|
return 0; |
|
} |
|
|
|
#if defined(CONFIG_MC3419_TRIGGER) |
|
static int mc3419_set_anymotion_threshold(const struct device *dev, |
|
const struct sensor_value *val) |
|
{ |
|
int ret = 0; |
|
const struct mc3419_config *cfg = dev->config; |
|
uint8_t buf[3] = {0}; |
|
|
|
if (val->val1 > MC3419_ANY_MOTION_THRESH_MAX) { |
|
return -EINVAL; |
|
} |
|
|
|
buf[0] = MC3419_REG_ANY_MOTION_THRES; |
|
sys_put_le16((uint16_t)val->val1, &buf[1]); |
|
|
|
ret = i2c_write_dt(&cfg->i2c, buf, sizeof(buf)); |
|
if (ret < 0) { |
|
LOG_ERR("Failed to set anymotion threshold (%d)", ret); |
|
return ret; |
|
} |
|
|
|
return 0; |
|
} |
|
|
|
static int mc3419_trigger_set(const struct device *dev, |
|
const struct sensor_trigger *trig, |
|
sensor_trigger_handler_t handler) |
|
{ |
|
int ret = 0; |
|
const struct mc3419_config *cfg = dev->config; |
|
struct mc3419_driver_data *data = dev->data; |
|
|
|
k_sem_take(&data->sem, K_FOREVER); |
|
ret = mc3419_set_op_mode(cfg, MC3419_MODE_STANDBY); |
|
if (ret < 0) { |
|
goto exit; |
|
} |
|
|
|
ret = mc3419_configure_trigger(dev, trig, handler); |
|
if (ret < 0) { |
|
LOG_ERR("Failed to set trigger (%d)", ret); |
|
} |
|
|
|
exit: |
|
mc3419_set_op_mode(cfg, MC3419_MODE_WAKE); |
|
|
|
k_sem_give(&data->sem); |
|
return ret; |
|
} |
|
#endif |
|
|
|
static int mc3419_attr_set(const struct device *dev, |
|
enum sensor_channel chan, |
|
enum sensor_attribute attr, |
|
const struct sensor_value *val) |
|
{ |
|
int ret = 0; |
|
struct mc3419_driver_data *data = dev->data; |
|
|
|
if (chan != SENSOR_CHAN_ACCEL_X && |
|
chan != SENSOR_CHAN_ACCEL_Y && |
|
chan != SENSOR_CHAN_ACCEL_Z && |
|
chan != SENSOR_CHAN_ACCEL_XYZ) { |
|
LOG_ERR("Not supported on this channel."); |
|
return -ENOTSUP; |
|
} |
|
|
|
k_sem_take(&data->sem, K_FOREVER); |
|
ret = mc3419_set_op_mode(dev->config, MC3419_MODE_STANDBY); |
|
if (ret < 0) { |
|
goto exit; |
|
} |
|
|
|
switch (attr) { |
|
case SENSOR_ATTR_FULL_SCALE: |
|
ret = mc3419_set_accel_range(dev, val->val1); |
|
break; |
|
case SENSOR_ATTR_SAMPLING_FREQUENCY: |
|
ret = mc3419_set_odr(dev, val); |
|
break; |
|
#if defined(CONFIG_MC3419_TRIGGER) |
|
case SENSOR_ATTR_SLOPE_TH: |
|
ret = mc3419_set_anymotion_threshold(dev, val); |
|
break; |
|
#endif |
|
default: |
|
LOG_ERR("ACCEL attribute is not supported"); |
|
ret = -EINVAL; |
|
} |
|
|
|
exit: |
|
mc3419_set_op_mode(dev->config, MC3419_MODE_WAKE); |
|
|
|
k_sem_give(&data->sem); |
|
return ret; |
|
} |
|
|
|
static int mc3419_init(const struct device *dev) |
|
{ |
|
int ret = 0; |
|
struct mc3419_driver_data *data = dev->data; |
|
const struct mc3419_config *cfg = dev->config; |
|
|
|
if (!(i2c_is_ready_dt(&cfg->i2c))) { |
|
LOG_ERR("Bus device is not ready"); |
|
return -ENODEV; |
|
} |
|
|
|
k_sem_init(&data->sem, 1, 1); |
|
|
|
#if defined(CONFIG_MC3419_TRIGGER) |
|
ret = mc3419_trigger_init(dev); |
|
if (ret < 0) { |
|
LOG_ERR("Could not initialize interrupts"); |
|
return ret; |
|
} |
|
#endif |
|
|
|
ret = i2c_reg_update_byte_dt(&cfg->i2c, MC3419_REG_RANGE_SELECT_CTRL, MC3419_LPF_MASK, |
|
cfg->lpf_fc_sel); |
|
if (ret < 0) { |
|
LOG_ERR("Failed to configure LPF (%d)", ret); |
|
return ret; |
|
} |
|
/* Leave the sensor in default power on state, will be |
|
* enabled by configure attr or setting trigger. |
|
*/ |
|
|
|
LOG_INF("MC3419 Initialized"); |
|
|
|
return ret; |
|
} |
|
|
|
static DEVICE_API(sensor, mc3419_api) = { |
|
.attr_set = mc3419_attr_set, |
|
#if defined(CONFIG_MC3419_TRIGGER) |
|
.trigger_set = mc3419_trigger_set, |
|
#endif |
|
.sample_fetch = mc3419_sample_fetch, |
|
.channel_get = mc3419_channel_get, |
|
}; |
|
|
|
#if defined(CONFIG_MC3419_TRIGGER) |
|
#define MC3419_CFG_IRQ(idx) \ |
|
.int_gpio = GPIO_DT_SPEC_INST_GET_OR(idx, int_gpios, { 0 }), \ |
|
.int_cfg = DT_INST_PROP(idx, int_pin2), |
|
#else |
|
#define MC3419_CFG_IRQ(idx) |
|
#endif |
|
|
|
#define MC3419_DEFINE(idx) \ |
|
\ |
|
static const struct mc3419_config mc3419_config_##idx = { \ |
|
.i2c = I2C_DT_SPEC_INST_GET(idx), \ |
|
.lpf_fc_sel = DT_INST_PROP(idx, lpf_fc_sel), \ |
|
.decimation_rate = DT_INST_PROP(idx, decimation_rate), \ |
|
MC3419_CFG_IRQ(idx)}; \ |
|
static struct mc3419_driver_data mc3419_data_##idx; \ |
|
SENSOR_DEVICE_DT_INST_DEFINE(idx, mc3419_init, NULL, &mc3419_data_##idx, \ |
|
&mc3419_config_##idx, POST_KERNEL, \ |
|
CONFIG_SENSOR_INIT_PRIORITY, &mc3419_api); |
|
|
|
DT_INST_FOREACH_STATUS_OKAY(MC3419_DEFINE)
|
|
|