Skip to content

Commit

Permalink
sensing: add rtio implement for sensing subsys
Browse files Browse the repository at this point in the history 
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>
  • Loading branch information
@lixuzha
lixuzha committed Dec 18, 2023
1 parent 1b5cdef commit 437501e
Show file tree
Hide file tree
Showing 6 changed files with 110 additions and 83 deletions.
23 changes: 19 additions & 4 deletions include/zephyr/sensing/sensing_sensor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -153,13 +153,28 @@ extern struct sensing_sensor SENSING_SENSOR_SOURCE_NAME(idx, node); \
(,), node, cb_list_ptr) \
};

#define SENSING_SENSOR_IODEV_NAME(node, idx) \
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))
Expand Down
17 changes: 17 additions & 0 deletions subsys/sensing/sensing_sensor.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
101 changes: 43 additions & 58 deletions subsys/sensing/sensor/phy_3d_sensor/phy_3d_sensor.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -102,18 +102,21 @@ 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) {
case SENSING_SENSOR_TYPE_MOTION_ACCELEROMETER_3D:
data->custom = &custom_accel;
for (i = 0; i < cfg->sensor_num; i++) {
switch (cfg->sensor_types[i]) {
case SENSING_SENSOR_TYPE_MOTION_ACCELEROMETER_3D:
data->customs[i] = &custom_accel;
break;
case SENSING_SENSOR_TYPE_MOTION_GYROMETER_3D:
data->custom = &custom_gyro;
case SENSING_SENSOR_TYPE_MOTION_GYROMETER_3D:
data->customs[i] = &custom_gyro;
break;
default:
LOG_ERR("phy_3d_sensor doesn't support sensor type %d",
cfg->sensor_type);
return -ENOTSUP;
default:
LOG_ERR("phy_3d_sensor doesn't support sensor type %d",
cfg->sensor_types[i]);
return -ENOTSUP;
}
}

LOG_INF("%s: Underlying device: %s", dev->name, cfg->hw_dev->name);
Expand All @@ -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;
} else {
LOG_ERR("%s: set sensitivity: invalid index: %d",
dev->name, index);
return -EINVAL;
}

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;
}
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 {
return sensor_attr_set(cfg->hw_dev, chan, SENSOR_ATTR_HYSTERESIS, val);
}

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,
Expand All @@ -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) {
Expand All @@ -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;
}
Expand All @@ -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;
Expand All @@ -201,27 +178,26 @@ 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);
return ret;
}

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,
Expand All @@ -238,25 +214,34 @@ 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),
.sensitivity_count = PHY_3D_SENSOR_CHANNEL_NUM,
.version.value = SENSING_SENSOR_VERSION(0, 8, 0, 0),
};

#define SENSING_PHY_3D_SENSOR_DT_DEFINE(_inst) \
static struct phy_3d_sensor_data _CONCAT(data, _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),\
}; \
SENSING_SENSORS_DT_INST_DEFINE(_inst, &phy_3d_sensor_reg, NULL, \
&phy_3d_sensor_init, NULL, \
&_CONCAT(data, _inst), &_CONCAT(cfg, _inst), \
POST_KERNEL, CONFIG_SENSOR_INIT_PRIORITY, \
#define SENSING_PHY_3D_SENSOR_DT_DEFINE(_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_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, \
&_CONCAT(data, _inst), &_CONCAT(cfg, _inst), \
POST_KERNEL, CONFIG_SENSOR_INIT_PRIORITY, \
&phy_3d_sensor_api);

DT_INST_FOREACH_STATUS_OKAY(SENSING_PHY_3D_SENSOR_DT_DEFINE);
6 changes: 4 additions & 2 deletions subsys/sensing/sensor/phy_3d_sensor/phy_3d_sensor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 changes: 27 additions & 8 deletions subsys/sensing/sensor_mgmt.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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;

Expand All @@ -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);
Expand Down Expand Up @@ -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);
}

Expand Down Expand Up @@ -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;

Expand All @@ -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;
}

Expand Down
11 changes: 0 additions & 11 deletions subsys/sensing/sensor_mgmt.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down

0 comments on commit 437501e

Please sign in to comment.