patrick/zephyr
1
0
Fork 0
Code Issues Pull requests Actions 8 Packages Projects Releases Wiki Activity

sensing: add rtio implement for sensing subsys

Browse source 
1. add function sensing_iodev_submit and defined struct
   sensing_submit_config
2. fix the issue of phy_3d_sensor

Signed-off-by: Zhang Lixu <lixu.zhang@intel.com>
This commit is contained in:
Zhang Lixu 2023-12-11 16:56:10 +08:00 committed by Carles Cufí
parent da5118001e
commit 5058355c1b
6 changed files with 109 additions and 82 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

21
include/zephyr/sensing/sensing_sensor.h
View file

@ -153,13 +153,28 @@ extern struct sensing_sensor SENSING_SENSOR_SOURCE_NAME(idx, node); \
(,), node, cb_list_ptr) \
};
struct sensing_submit_config {
enum sensor_channel chan;
const int info_index;
const bool is_streaming;
};
extern const struct rtio_iodev_api __sensing_iodev_api;
#define SENSING_SUBMIT_CFG_NAME(node, idx) \
_CONCAT(_CONCAT(__sensing_submit_cfg_, idx), DEVICE_DT_NAME_GET(node))
#define SENSING_SENSOR_IODEV_NAME(node, idx) \
_CONCAT(_CONCAT(__sensing_iodev_, idx), DEVICE_DT_NAME_GET(node))
#define SENSING_SENSOR_IODEV_DEFINE(node, idx) \
COND_CODE_1(DT_PROP(node, stream_mode), \
(SENSOR_DT_STREAM_IODEV(SENSING_SENSOR_IODEV_NAME(node, idx), node)), \
(SENSOR_DT_READ_IODEV(SENSING_SENSOR_IODEV_NAME(node, idx), node)));
static struct sensing_submit_config SENSING_SUBMIT_CFG_NAME(node, idx) = { \
.is_streaming = DT_PROP(node, stream_mode), \
.info_index = idx, \
}; \
RTIO_IODEV_DEFINE(SENSING_SENSOR_IODEV_NAME(node, idx), \
&__sensing_iodev_api, \
&SENSING_SUBMIT_CFG_NAME(node, idx));
#define SENSING_SENSOR_NAME(node, idx) \
_CONCAT(_CONCAT(__sensing_sensor_, idx), DEVICE_DT_NAME_GET(node))

17
subsys/sensing/sensing_sensor.c
View file

@ -13,6 +13,23 @@
LOG_MODULE_DECLARE(sensing, CONFIG_SENSING_LOG_LEVEL);
static void sensing_iodev_submit(struct rtio_iodev_sqe *iodev_sqe)
{
struct sensing_sensor *sensor = (struct sensing_sensor *)iodev_sqe->sqe.userdata;
const struct device *dev = sensor->dev;
const struct sensor_driver_api *api = dev->api;
if (api->submit != NULL) {
api->submit(dev, iodev_sqe);
} else {
rtio_iodev_sqe_err(iodev_sqe, -ENOTSUP);
}
}
const struct rtio_iodev_api __sensing_iodev_api = {
.submit = sensing_iodev_submit,
};
int sensing_sensor_get_reporters(const struct device *dev, int type,
sensing_sensor_handle_t *reporter_handles,
int max_handles)

69
subsys/sensing/sensor/phy_3d_sensor/phy_3d_sensor.c
View file

@ -102,19 +102,22 @@ static int phy_3d_sensor_init(const struct device *dev)
{
const struct phy_3d_sensor_config *cfg = dev->config;
struct phy_3d_sensor_data *data = dev->data;
int i;
switch (cfg->sensor_type) {
for (i = 0; i < cfg->sensor_num; i++) {
switch (cfg->sensor_types[i]) {
case SENSING_SENSOR_TYPE_MOTION_ACCELEROMETER_3D:
data->custom = &custom_accel;
data->customs[i] = &custom_accel;
break;
case SENSING_SENSOR_TYPE_MOTION_GYROMETER_3D:
data->custom = &custom_gyro;
data->customs[i] = &custom_gyro;
break;
default:
LOG_ERR("phy_3d_sensor doesn't support sensor type %d",
cfg->sensor_type);
cfg->sensor_types[i]);
return -ENOTSUP;
}
}
LOG_INF("%s: Underlying device: %s", dev->name, cfg->hw_dev->name);
@ -126,38 +129,12 @@ static int phy_3d_sensor_attr_set_hyst(const struct device *dev,
const struct sensor_value *val)
{
const struct phy_3d_sensor_config *cfg = dev->config;
struct phy_3d_sensor_data *data = dev->data;
int index = chan - SENSOR_CHAN_PRIV_START;
int i, min = 0;
if (index >= 0 && index < ARRAY_SIZE(data->sensitivities)) {
data->sensitivities[index] = *val;
} else if (index == SENSING_SENSITIVITY_INDEX_ALL) {
for (i = 0; i < ARRAY_SIZE(data->sensitivities); ++i) {
data->sensitivities[i] = *val;
}
min = ARRAY_SIZE(data->sensitivities) - 1;
if (chan == SENSOR_CHAN_ACCEL_XYZ || chan == SENSOR_CHAN_GYRO_XYZ) {
return sensor_attr_set(cfg->hw_dev, chan, SENSOR_ATTR_SLOPE_TH, val);
} else {
LOG_ERR("%s: set sensitivity: invalid index: %d",
dev->name, index);
return -EINVAL;
return sensor_attr_set(cfg->hw_dev, chan, SENSOR_ATTR_HYSTERESIS, val);
}
for (i = min + 1; i < ARRAY_SIZE(data->sensitivities); ++i) {
if ((data->sensitivities[i].val1 <
data->sensitivities[min].val1) ||
(data->sensitivities[i].val1 ==
data->sensitivities[min].val1 &&
data->sensitivities[i].val2 <
data->sensitivities[min].val2)) {
min = i;
}
}
chan = data->custom->chan_all;
return sensor_attr_set(cfg->hw_dev, chan, SENSOR_ATTR_SLOPE_TH,
&data->sensitivities[min]);
}
static int phy_3d_sensor_attr_set(const struct device *dev,
@ -166,7 +143,6 @@ static int phy_3d_sensor_attr_set(const struct device *dev,
const struct sensor_value *val)
{
const struct phy_3d_sensor_config *cfg = dev->config;
struct phy_3d_sensor_data *data = dev->data;
int ret = 0;
switch (attr) {
@ -175,7 +151,6 @@ static int phy_3d_sensor_attr_set(const struct device *dev,
break;
default:
chan = data->custom->chan_all;
ret = sensor_attr_set(cfg->hw_dev, chan, attr, val);
break;
}
@ -187,8 +162,10 @@ static int phy_3d_sensor_attr_set(const struct device *dev,
static int phy_3d_sensor_submit(const struct device *dev,
struct rtio_iodev_sqe *sqe)
{
struct sensing_submit_config *config = (struct sensing_submit_config *)sqe->sqe.iodev->data;
const struct phy_3d_sensor_config *cfg = dev->config;
struct phy_3d_sensor_data *data = dev->data;
const struct phy_3d_sensor_custom *custom = data->customs[config->info_index];
struct sensing_sensor_value_3d_q31 *sample;
struct sensor_value value[PHY_3D_SENSOR_CHANNEL_NUM];
uint32_t buffer_len;
@ -201,14 +178,14 @@ static int phy_3d_sensor_submit(const struct device *dev,
return ret;
}
ret = sensor_sample_fetch_chan(cfg->hw_dev, data->custom->chan_all);
ret = sensor_sample_fetch_chan(cfg->hw_dev, custom->chan_all);
if (ret) {
LOG_ERR("%s: sample fetch failed: %d", dev->name, ret);
rtio_iodev_sqe_err(sqe, ret);
return ret;
}
ret = sensor_channel_get(cfg->hw_dev, data->custom->chan_all, value);
ret = sensor_channel_get(cfg->hw_dev, custom->chan_all, value);
if (ret) {
LOG_ERR("%s: channel get failed: %d", dev->name, ret);
rtio_iodev_sqe_err(sqe, ret);
@ -216,12 +193,11 @@ static int phy_3d_sensor_submit(const struct device *dev,
}
for (i = 0; i < ARRAY_SIZE(value); ++i) {
sample->readings[0].v[i] =
data->custom->sensor_value_to_q31(&value[i]);
sample->readings[0].v[i] = custom->sensor_value_to_q31(&value[i]);
}
sample->header.reading_count = 1;
sample->shift = data->custom->shift;
sample->shift = custom->shift;
LOG_DBG("%s: Sample data:\t x: %d, y: %d, z: %d",
dev->name,
@ -238,6 +214,7 @@ static const struct sensor_driver_api phy_3d_sensor_api = {
.submit = phy_3d_sensor_submit,
};
/* To be removed */
static const struct sensing_sensor_register_info phy_3d_sensor_reg = {
.flags = SENSING_SENSOR_FLAG_REPORT_ON_CHANGE,
.sample_size = sizeof(struct sensing_sensor_value_3d_q31),
@ -246,12 +223,20 @@ static const struct sensing_sensor_register_info phy_3d_sensor_reg = {
};
#define SENSING_PHY_3D_SENSOR_DT_DEFINE(_inst) \
static struct phy_3d_sensor_data _CONCAT(data, _inst); \
static struct rtio_iodev_sqe _CONCAT(sqes, _inst)[DT_INST_PROP_LEN(_inst, \
sensor_types)]; \
static const struct phy_3d_sensor_custom *_CONCAT(customs, _inst) \
[DT_INST_PROP_LEN(_inst, sensor_types)]; \
static struct phy_3d_sensor_data _CONCAT(data, _inst) = { \
.sqes = _CONCAT(sqes, _inst), \
.customs = _CONCAT(customs, _inst), \
}; \
static const struct phy_3d_sensor_config _CONCAT(cfg, _inst) = { \
.hw_dev = DEVICE_DT_GET( \
DT_PHANDLE(DT_DRV_INST(_inst), \
underlying_device)), \
.sensor_type = DT_PROP_BY_IDX(DT_DRV_INST(_inst), sensor_types, 0),\
.sensor_num = DT_INST_PROP_LEN(_inst, sensor_types), \
.sensor_types = DT_PROP(DT_DRV_INST(_inst), sensor_types), \
}; \
SENSING_SENSORS_DT_INST_DEFINE(_inst, &phy_3d_sensor_reg, NULL, \
&phy_3d_sensor_init, NULL, \

6
subsys/sensing/sensor/phy_3d_sensor/phy_3d_sensor.h
View file

@ -20,13 +20,15 @@ struct phy_3d_sensor_custom {
};
struct phy_3d_sensor_data {
const struct phy_3d_sensor_custom *custom;
struct sensor_value sensitivities[PHY_3D_SENSOR_CHANNEL_NUM];
const struct phy_3d_sensor_custom **customs;
struct rtio_iodev_sqe *sqes;
};
struct phy_3d_sensor_config {
const struct device *hw_dev;
const int32_t sensor_type;
const int sensor_num;
const int32_t sensor_types[];
};
#endif

35
subsys/sensing/sensor_mgmt.c
View file

@ -29,6 +29,20 @@ RTIO_DEFINE_WITH_MEMPOOL(sensing_rtio_ctx, CONFIG_SENSING_RTIO_SQE_NUM,
CONFIG_SENSING_RTIO_BLOCK_COUNT,
CONFIG_SENSING_RTIO_BLOCK_SIZE, 4);
static enum sensor_channel sensing_sensor_type_to_chan(const int32_t type)
{
switch (type) {
case SENSING_SENSOR_TYPE_MOTION_ACCELEROMETER_3D:
return SENSOR_CHAN_ACCEL_XYZ;
case SENSING_SENSOR_TYPE_MOTION_GYROMETER_3D:
return SENSOR_CHAN_GYRO_XYZ;
default:
break;
}
return SENSOR_CHAN_PRIV_START;
}
/* sensor_later_config including arbitrate/set interval/sensitivity
*/
static uint32_t arbitrate_interval(struct sensing_sensor *sensor)
@ -61,7 +75,7 @@ static uint32_t arbitrate_interval(struct sensing_sensor *sensor)
static int set_arbitrate_interval(struct sensing_sensor *sensor, uint32_t interval)
{
struct sensor_read_config *config = sensor->iodev->data;
struct sensing_submit_config *config = sensor->iodev->data;
struct sensor_value odr = {0};
int ret;
@ -75,8 +89,7 @@ static int set_arbitrate_interval(struct sensing_sensor *sensor, uint32_t interv
odr.val2 = (uint64_t)USEC_PER_SEC * 1000000 / interval % 1000000;
}
/* The SENSOR_CHAN_MAX should be overridden by sensing sensor driver */
ret = sensor_attr_set(sensor->dev, SENSOR_CHAN_MAX,
ret = sensor_attr_set(sensor->dev, config->chan,
SENSOR_ATTR_SAMPLING_FREQUENCY, &odr);
if (ret) {
LOG_ERR("%s set attr freq failed:%d", sensor->dev->name, ret);
@ -143,16 +156,18 @@ static uint32_t arbitrate_sensitivity(struct sensing_sensor *sensor, int index)
static int set_arbitrate_sensitivity(struct sensing_sensor *sensor, int index, uint32_t sensitivity)
{
struct sensing_submit_config *config = (struct sensing_submit_config *)sensor->iodev->data;
struct sensor_value threshold = {.val1 = sensitivity};
int i;
/* update sensor sensitivity */
sensor->sensitivity[index] = sensitivity;
/* The SENSOR_CHAN_PRIV_START should be overridden by sensing sensor
* driver, SENSOR_ATTR_HYSTERESIS can be overridden for different
* sensora type.
*/
return sensor_attr_set(sensor->dev, SENSOR_CHAN_PRIV_START + index,
for (i = 0; i < sensor->sensitivity_count; i++) {
threshold.val1 = MIN(sensor->sensitivity[i], threshold.val1);
}
return sensor_attr_set(sensor->dev, config->chan,
SENSOR_ATTR_HYSTERESIS, &threshold);
}
@ -275,6 +290,7 @@ static void sensing_sensor_polling_timer(struct k_timer *timer_id)
static int init_sensor(struct sensing_sensor *sensor)
{
struct sensing_submit_config *config;
struct sensing_connection *conn;
int i;
@ -293,6 +309,9 @@ static int init_sensor(struct sensing_sensor *sensor)
conn->source->dev->name, sensor->dev->name, i, conn);
}
config = sensor->iodev->data;
config->chan = sensing_sensor_type_to_chan(sensor->info->type);
return 0;
}

11
subsys/sensing/sensor_mgmt.h
View file

@ -71,17 +71,6 @@ static inline struct sensing_sensor *get_sensor_by_dev(const struct device *dev)
return NULL;
}
static inline uint16_t get_reporter_sample_size(const struct sensing_sensor *sensor, int i)
{
const struct sensing_sensor *reporter;
__ASSERT(i < sensor->reporter_num, "dt index should less than reporter num");
reporter = sensor->conns[i].source;
return reporter->register_info->sample_size;
}
static inline struct sensing_sensor *get_reporter_sensor(struct sensing_sensor *sensor, int index)
{
if (!sensor || index >= sensor->reporter_num) {