diff --git a/dts/bindings/sensor/zephyr,sensing-hinge-angle.yaml b/dts/bindings/sensor/zephyr,sensing-hinge-angle.yaml new file mode 100644 index 00000000000000..4d4e4415677b96 --- /dev/null +++ b/dts/bindings/sensor/zephyr,sensing-hinge-angle.yaml @@ -0,0 +1,10 @@ +# Copyright (c) 2023, Intel Corporation +# SPDX-License-Identifier: Apache-2.0 +# + +description: Sensing subsystem hinge angle sensor bindings. + +compatible: "zephyr,sensing-hinge-angle" + +# Common sensor subsystem sensor properties. +include: ["zephyr,sensing-sensor.yaml"] diff --git a/dts/bindings/sensor/zephyr,senss-phy-3d-sensor.yaml b/dts/bindings/sensor/zephyr,sensing-phy-3d-sensor.yaml similarity index 100% rename from dts/bindings/sensor/zephyr,senss-phy-3d-sensor.yaml rename to dts/bindings/sensor/zephyr,sensing-phy-3d-sensor.yaml diff --git a/include/zephyr/drivers/sensor.h b/include/zephyr/drivers/sensor.h index 9a74a536949963..04eae1bb01a8af 100644 --- a/include/zephyr/drivers/sensor.h +++ b/include/zephyr/drivers/sensor.h @@ -1227,6 +1227,46 @@ static inline int64_t sensor_value_to_micro(const struct sensor_value *val) return ((int64_t)val->val1 * 1000000) + val->val2; } +/** + * @brief Helper function for converting integer milli units to struct sensor_value. + * + * @param val A pointer to a sensor_value struct. + * @param milli The converted value. + * @return 0 if successful, negative errno code if failure. + */ +static inline int sensor_value_from_milli(struct sensor_value *val, int64_t milli) +{ + if (milli < ((int64_t)INT32_MIN - 1) * 1000LL || + milli > ((int64_t)INT32_MAX + 1) * 1000LL) { + return -ERANGE; + } + + val->val1 = (int32_t)(milli / 1000); + val->val2 = (int32_t)(milli % 1000) * 1000; + + return 0; +} + +/** + * @brief Helper function for converting integer micro units to struct sensor_value. + * + * @param val A pointer to a sensor_value struct. + * @param micro The converted value. + * @return 0 if successful, negative errno code if failure. + */ +static inline int sensor_value_from_micro(struct sensor_value *val, int64_t micro) +{ + if (micro < ((int64_t)INT32_MIN - 1) * 1000000LL || + micro > ((int64_t)INT32_MAX + 1) * 1000000LL) { + return -ERANGE; + } + + val->val1 = (int32_t)(micro / 1000000LL); + val->val2 = (int32_t)(micro % 1000000LL); + + return 0; +} + /** * @} */ diff --git a/include/zephyr/sensing/sensing.h b/include/zephyr/sensing/sensing.h index 3b11d1b410ca4a..0a58c2ccb09104 100644 --- a/include/zephyr/sensing/sensing.h +++ b/include/zephyr/sensing/sensing.h @@ -72,6 +72,11 @@ struct sensing_sensor_version { */ #define SENSING_SENSOR_FLAG_REPORT_ON_CHANGE BIT(1) +/** + * @brief SENSING_SENSITIVITY_INDEX_ALL indicating sensitivity of each data field should be set + * + */ +#define SENSING_SENSITIVITY_INDEX_ALL -1 /** * @brief Sensing subsystem sensor state. @@ -151,8 +156,12 @@ struct sensing_callback_list { * */ struct sensing_sensor_config { + enum sensing_sensor_attribute attri; + + /** \ref SENSING_SENSITIVITY_INDEX_ALL */ int8_t data_field; + union { uint32_t interval; uint32_t sensitivity; diff --git a/include/zephyr/sensing/sensing_datatypes.h b/include/zephyr/sensing/sensing_datatypes.h index 3549aab4bdb692..a5abcc3c8f826e 100644 --- a/include/zephyr/sensing/sensing_datatypes.h +++ b/include/zephyr/sensing/sensing_datatypes.h @@ -99,8 +99,8 @@ struct sensing_sensor_value_uint32 { * q31 version */ struct sensing_sensor_value_q31 { - int8_t shift; struct sensing_sensor_value_header header; + int8_t shift; struct { uint32_t timestamp_delta; q31_t v; diff --git a/include/zephyr/sensing/sensing_sensor.h b/include/zephyr/sensing/sensing_sensor.h index e7716a317d8e81..caf8c7f931841c 100644 --- a/include/zephyr/sensing/sensing_sensor.h +++ b/include/zephyr/sensing/sensing_sensor.h @@ -313,7 +313,7 @@ typedef int (*sensing_sensor_read_sample_t)( * * @param dev The sensor instance device structure. * - * @param reporter The reporter handle who delivered this sensor data + * @param reporter_handle The reporter handle who delivered this sensor data * * @param buf The buffer stored the reporter's sensor data. * @@ -324,7 +324,7 @@ typedef int (*sensing_sensor_read_sample_t)( */ typedef int (*sensing_sensor_process_t)( const struct device *dev, - int reporter, + const sensing_sensor_handle_t reporter_handle, void *buf, int size); /** diff --git a/samples/subsys/sensing/simple/boards/native_posix.overlay b/samples/subsys/sensing/simple/boards/native_posix.overlay index d11630cd5d98af..190eae86efc73a 100644 --- a/samples/subsys/sensing/simple/boards/native_posix.overlay +++ b/samples/subsys/sensing/simple/boards/native_posix.overlay @@ -41,5 +41,14 @@ minimal-interval = <625>; underlying-device = <&bmi160_spi>; }; + + hinge_angle: hinge-angle { + compatible = "zephyr,sensing-hinge-angle"; + status = "okay"; + sensor-type = <0x20B>; + friendly-name = "Hinge Angle Sensor"; + reporters = <&base_accel &lid_accel>; + minimal-interval = <100000>; + }; }; }; diff --git a/samples/subsys/sensing/simple/src/main.c b/samples/subsys/sensing/simple/src/main.c index bc1b3bb8b916da..fde105993c3b35 100644 --- a/samples/subsys/sensing/simple/src/main.c +++ b/samples/subsys/sensing/simple/src/main.c @@ -5,6 +5,7 @@ */ #include +#include #include #include #include @@ -24,6 +25,15 @@ static void acc_data_event_callback(sensing_sensor_handle_t handle, const void * sample->readings[0].z); } + +static void hinge_angle_data_event_callback(sensing_sensor_handle_t handle, const void *buf) +{ + const struct sensing_sensor_info *info = sensing_get_sensor_info(handle); + struct sensing_sensor_value_q31 *sample = (struct sensing_sensor_value_q31 *)buf; + + LOG_INF("handle:%p, Sensor:%s data:(v:%d)", handle, info->name, sample->readings[0].v); +} + void main(void) { const struct sensing_callback_list base_acc_cb_list = { @@ -32,9 +42,17 @@ void main(void) const struct sensing_callback_list lid_acc_cb_list = { .on_data_event = &acc_data_event_callback, }; + const struct sensing_callback_list hinge_angle_cb_list = { + .on_data_event = &hinge_angle_data_event_callback, + }; const struct sensing_sensor_info *info; sensing_sensor_handle_t base_acc; sensing_sensor_handle_t lid_acc; + sensing_sensor_handle_t hinge_angle; + struct sensing_sensor_config base_acc_config; + struct sensing_sensor_config lid_acc_config; + struct sensing_sensor_config hinge_angle_config; + const struct sensing_sensor_info *tmp_sensor_info; int ret, i, num = 0; ret = sensing_get_sensors(&num, &info); @@ -44,7 +62,7 @@ void main(void) } for (i = 0; i < num; ++i) { - LOG_INF("Sensor %d: name: %s friendly_name: %s type: %d", + LOG_INF("Sensor %d: name: %s friendly_name: %s, type: %d", i, info[i].name, info[i].friendly_name, @@ -63,17 +81,113 @@ void main(void) &lid_acc_cb_list, &lid_acc); if (ret) { - LOG_ERR("sensing_open_sensor, type:0x%x index:1 error:%d", + LOG_ERR("sensing_open_sensor_by_dt, type:0x%x index:1 error:%d", SENSING_SENSOR_TYPE_MOTION_ACCELEROMETER_3D, ret); } + ret = sensing_open_sensor_by_dt(DEVICE_DT_GET(DT_NODELABEL(hinge_angle)), + &hinge_angle_cb_list, + &hinge_angle); + if (ret) { + LOG_ERR("sensing_open_sensor_by_type, type:0x%x index:0 error:%d", + SENSING_SENSOR_TYPE_MOTION_HINGE_ANGLE, ret); + } + + /* set base acc, lid acc, hinge sensor interval */ + base_acc_config.attri = SENSING_SENSOR_ATTRIBUTE_INTERVAL; + base_acc_config.interval = 100 * USEC_PER_MSEC; + ret = sensing_set_config(base_acc, &base_acc_config, 1); + if (ret) { + LOG_ERR("base_acc sensing_set_interval error:%d\n", ret); + } + + lid_acc_config.attri = SENSING_SENSOR_ATTRIBUTE_INTERVAL; + lid_acc_config.interval = 100 * USEC_PER_MSEC; + ret = sensing_set_config(lid_acc, &lid_acc_config, 1); + if (ret) { + LOG_ERR("lid_acc sensing_set_interval error:%d\n", ret); + } + + tmp_sensor_info = sensing_get_sensor_info(hinge_angle); + hinge_angle_config.attri = SENSING_SENSOR_ATTRIBUTE_INTERVAL; + hinge_angle_config.interval = tmp_sensor_info->minimal_interval; + ret = sensing_set_config(hinge_angle, &hinge_angle_config, 1); + if (ret) { + LOG_ERR("hinge_angle sensing_set_interval error:%d\n", ret); + } + + memset(&base_acc_config, 0x00, sizeof(struct sensing_sensor_config)); + memset(&lid_acc_config, 0x00, sizeof(struct sensing_sensor_config)); + memset(&hinge_angle_config, 0x00, sizeof(struct sensing_sensor_config)); + + /* get base acc, lid acc, hinge sensor interval */ + base_acc_config.attri = SENSING_SENSOR_ATTRIBUTE_INTERVAL; + ret = sensing_get_config(base_acc, &base_acc_config, 1); + if (ret) { + LOG_ERR("base_acc sensing_get_interval error:%d\n", ret); + } + + lid_acc_config.attri = SENSING_SENSOR_ATTRIBUTE_INTERVAL; + ret = sensing_get_config(lid_acc, &lid_acc_config, 1); + if (ret) { + LOG_ERR("lid_acc sensing_get_interval error:%d\n", ret); + } - ret = sensing_close_sensor(&base_acc); + hinge_angle_config.attri = SENSING_SENSOR_ATTRIBUTE_INTERVAL; + ret = sensing_get_config(hinge_angle, &hinge_angle_config, 1); if (ret) { - LOG_ERR("sensing_close_sensor:%p error:%d", base_acc, ret); + LOG_ERR("hinge_angle sensing_get_interval error:%d\n", ret); } - ret = sensing_close_sensor(&lid_acc); + /* set base acc, lid acc, hinge sensor sensitivity */ + base_acc_config.attri = SENSING_SENSOR_ATTRIBUTE_SENSITIVITY; + base_acc_config.data_field = SENSING_SENSITIVITY_INDEX_ALL; + base_acc_config.sensitivity = 0; + ret = sensing_set_config(base_acc, &base_acc_config, 1); if (ret) { - LOG_ERR("sensing_close_sensor:%p error:%d", lid_acc, ret); + LOG_ERR("base_acc sensing_set_sensitivity error:%d\n", ret); } + + lid_acc_config.attri = SENSING_SENSOR_ATTRIBUTE_SENSITIVITY; + lid_acc_config.data_field = SENSING_SENSITIVITY_INDEX_ALL; + lid_acc_config.sensitivity = 0; + ret = sensing_set_config(lid_acc, &lid_acc_config, 1); + if (ret) { + LOG_ERR("lid_acc sensing_set_sensitivity error:%d\n", ret); + } + + hinge_angle_config.attri = SENSING_SENSOR_ATTRIBUTE_SENSITIVITY; + hinge_angle_config.data_field = SENSING_SENSITIVITY_INDEX_ALL; + hinge_angle_config.sensitivity = 1; + ret = sensing_set_config(hinge_angle, &hinge_angle_config, 1); + if (ret) { + LOG_ERR("hinge_angle sensing_set_sensitivity error:%d\n", ret); + } + + memset(&base_acc_config, 0x00, sizeof(struct sensing_sensor_config)); + memset(&lid_acc_config, 0x00, sizeof(struct sensing_sensor_config)); + memset(&hinge_angle_config, 0x00, sizeof(struct sensing_sensor_config)); + + /* get base acc, lid acc, hinge sensor sensitivity */ + base_acc_config.attri = SENSING_SENSOR_ATTRIBUTE_SENSITIVITY; + base_acc_config.data_field = SENSING_SENSITIVITY_INDEX_ALL; + ret = sensing_get_config(base_acc, &base_acc_config, 1); + if (ret) { + LOG_ERR("base_acc sensing_get_sensitivity error:%d\n", ret); + } + + lid_acc_config.attri = SENSING_SENSOR_ATTRIBUTE_SENSITIVITY; + lid_acc_config.data_field = SENSING_SENSITIVITY_INDEX_ALL; + ret = sensing_get_config(lid_acc, &lid_acc_config, 1); + if (ret) { + LOG_ERR("lid_acc sensing_get_sensitivity error:%d\n", ret); + } + + hinge_angle_config.attri = SENSING_SENSOR_ATTRIBUTE_SENSITIVITY; + hinge_angle_config.data_field = SENSING_SENSITIVITY_INDEX_ALL; + ret = sensing_get_config(hinge_angle, &hinge_angle_config, 1); + if (ret) { + LOG_ERR("hinge_angle sensing_get_sensitivity error:%d\n", ret); + } + + k_sleep(K_MSEC(10)); } diff --git a/subsys/sensing/CMakeLists.txt b/subsys/sensing/CMakeLists.txt index 59aa28514b03fe..2d205c286a06ed 100644 --- a/subsys/sensing/CMakeLists.txt +++ b/subsys/sensing/CMakeLists.txt @@ -5,8 +5,10 @@ zephyr_library_include_directories(include) zephyr_library_sources( sensor_mgmt.c + runtime.c sensing.c sensing_sensor.c ) add_subdirectory_ifdef(CONFIG_SENSING_SENSOR_PHY_3D_SENSOR sensor/phy_3d_sensor) +add_subdirectory_ifdef(CONFIG_SENSING_SENSOR_HINGE_ANGLE sensor/hinge_angle) diff --git a/subsys/sensing/Kconfig b/subsys/sensing/Kconfig index 9101db074457a5..33c63ba5de88cc 100644 --- a/subsys/sensing/Kconfig +++ b/subsys/sensing/Kconfig @@ -5,6 +5,7 @@ config SENSING bool "Sensing Subsystem" default y depends on DT_HAS_ZEPHYR_SENSING_ENABLED + select RING_BUFFER help Enable Sensing Subsystem. @@ -24,6 +25,67 @@ config SENSING_MAX_SENSITIVITY_COUNT So, maximum sensitivity count is needed for sensors Typical values are 6 +config SENSING_RING_BUF_SIZE + int "ring buf size to store sensor data" + depends on SENSING + default 16384 + help + This is the ring buffer to store sensor data that + will be posted to application + Typical values are 16 * 1024 + +config SENSING_MAX_SENSOR_DATA_SIZE + int "maximum sensor data size" + depends on SENSING + default 64 + help + This is the maximum sensor data size sensing subsystem could support + when sensor is posting data to client, sensing subsystem would + get no more than SENSING_MAX_SENSOR_DATA_SIZE sensor count from ring buffer + Typical values are 64 + +config SENSING_RUNTIME_THREAD_STACK_SIZE + int "stack size for sensing subsystem runtime thread" + depends on SENSING + default 4096 + help + This is the stack size for sensing subsystem runtime thread + Typical values are 4096 + +config SENSING_DISPATCH_THREAD_STACK_SIZE + int "stack size for sensor dispatch thread" + depends on SENSING + default 1024 + help + This is the stack size for sensor dispatch thread + Typical values are 1024 + +config SENSING_RUNTIME_THREAD_PRIORITY + int "priority for sensing subsystem runtime thread" + depends on SENSING + default 9 + help + This is the thread priority for sensor subsystem runtime thread + Ring buffer data is stored by runtime thread, and then give semaphore + to notify dispatch thread, runtime thread priority should lower than + dispatch thread priority to ensure dispatch thread could fetch data as + soon as runtime thread give semaphore. Take for example, if runtime + priority is higher than dispatch thread, and runtime running in full + loading with no sleep, then dispatch thread has no change to fetch + data, then ring buf will always be put into data until overflow. + Typical values are 9 + +config SENSING_DISPATCH_THREAD_PRIORITY + int "priority for sensor dispatch thread" + depends on SENSING + default 8 + help + This is the thread priority for sensing subsystem dispatch thread + Ring buffer data should be fetched ASAP, so Dispatch + thread priority should be higher than runtime thread + Typical values are 8 + source "subsys/sensing/sensor/phy_3d_sensor/Kconfig" +source "subsys/sensing/sensor/hinge_angle/Kconfig" endif # SENSING diff --git a/subsys/sensing/runtime.c b/subsys/sensing/runtime.c new file mode 100644 index 00000000000000..76d49c9a88a6a7 --- /dev/null +++ b/subsys/sensing/runtime.c @@ -0,0 +1,473 @@ +/* + * Copyright (c) 2022 Intel Corporation. + * + * SPDX-License-Identifier: Apache-2.0 + */ +#include +#include +#include +#include +#include "sensor_mgmt.h" + +LOG_MODULE_DECLARE(sensing, CONFIG_SENSING_LOG_LEVEL); + + +static int fetch_data_and_dispatch(struct sensing_context *ctx) +{ + struct sensing_connection *conn = NULL; + uint8_t buf[CONFIG_SENSING_MAX_SENSOR_DATA_SIZE]; + uint32_t wanted_size = sizeof(sensing_sensor_handle_t); + uint32_t ret_size, rd_size = 0; + uint16_t sample_size = 0; + int ret = 0; + + while ((ret_size = ring_buf_get(&ctx->sensor_ring_buf, buf + rd_size, wanted_size)) > 0) { + rd_size += ret_size; + + if (rd_size == sizeof(sensing_sensor_handle_t)) { + /* read sensing_sensor_handle_t handle first */ + conn = (struct sensing_connection *)(*(int32_t *)buf); + if (!conn || !conn->source) { + LOG_ERR("fetch data and dispatch, connection or reporter is NULL"); + ret = -EINVAL; + break; + } + sample_size = conn->source->sample_size; + + __ASSERT(sample_size + sizeof(sensing_sensor_handle_t) + <= CONFIG_SENSING_MAX_SENSOR_DATA_SIZE, + "invalid sample size:%d", sample_size); + /* get sample_size from connection, then read sensor data from ring buf */ + wanted_size = sample_size; + } else if (rd_size == sizeof(sensing_sensor_handle_t) + wanted_size) { + /* read next sample header */ + wanted_size = sizeof(sensing_sensor_handle_t); + rd_size = 0; + if (!conn->data_evt_cb) { + LOG_WRN("sensor:%s event callback not registered", + conn->source->dev->name); + continue; + } + conn->data_evt_cb(conn, + (const void *)(buf + sizeof(sensing_sensor_handle_t))); + } else { + LOG_ERR("fetch data and dispatch, invalid ret_size:%d, rd_size:%d", + ret_size, rd_size); + ret = -EINVAL; + } + } + + if (ret_size == 0 && wanted_size != sizeof(sensing_sensor_handle_t)) { + LOG_ERR("fetch data and dispatch, ret_size:%d, wanted_size:%d not expected:%d", + ret_size, wanted_size, sizeof(sensing_sensor_handle_t)); + ret = -EINVAL; + __ASSERT(wanted_size, "wanted_size:%d", wanted_size); + } + + return ret; +} + + +static void add_data_to_sensor_ring_buf(struct sensing_context *ctx, + struct sensing_sensor *sensor, + sensing_sensor_handle_t handle) +{ + uint8_t data[CONFIG_SENSING_MAX_SENSOR_DATA_SIZE]; + uint32_t size; + + if (ring_buf_space_get(&ctx->sensor_ring_buf) < sizeof(void *) + sensor->sample_size) { + LOG_WRN("ring buffer will overflow, ignore the coming data"); + return; + } + __ASSERT(sizeof(struct sensing_connection *) + sensor->sample_size + <= CONFIG_SENSING_MAX_SENSOR_DATA_SIZE, + "sample_size:%d is too large, should enlarge SENSING_MAX_SENSOR_DATA_SIZE:%d", + sensor->sample_size, CONFIG_SENSING_MAX_SENSOR_DATA_SIZE); + + memcpy(data, &handle, sizeof(sensing_sensor_handle_t)); + memcpy(data + sizeof(handle), sensor->data_buf, sensor->sample_size); + size = ring_buf_put(&ctx->sensor_ring_buf, data, sizeof(handle) + sensor->sample_size); + __ASSERT(size == sizeof(handle) + sensor->sample_size, + "sample size:%d put to ring buf is not expected: %d", + size, sizeof(handle) + sensor->sample_size); +} + +/* check whether sensor need to poll data or not, if polling data is needed, update execute time + * when time arriving + */ +static bool sensor_need_poll(struct sensing_sensor *sensor, uint64_t cur_us) +{ + bool poll = false; + + /* sensor is not in polling mode or sensor interval still not set yet, + * no need to poll, return directly + */ + if (sensor->mode != SENSOR_TRIGGER_MODE_POLLING || sensor->interval == 0) { + LOG_INF("sensor %s not in polling mode:%d or sensor interval:%d not opened yet", + sensor->dev->name, sensor->mode, sensor->interval); + sensor->next_exec_time = EXEC_TIME_OFF; + return false; + } + + /* sensor is in polling mode, first time execute, will poll data at next interval */ + if (sensor->next_exec_time == EXEC_TIME_INIT) { + LOG_INF("sensor:%s first time exe, cur time:%lld, interval:%d(us)", + sensor->dev->name, cur_us, sensor->interval); + sensor->next_exec_time = cur_us + sensor->interval; + return false; + } + + /* execute time arrived, execute this polling data, meanwhile calculate next execute time */ + if (sensor->next_exec_time <= cur_us) { + poll = true; + sensor->next_exec_time += sensor->interval; + } + + LOG_DBG("sensor:%s need poll:%d, cur:%llu, next_exec_time:%llu, mode:%d", + sensor->dev->name, poll, cur_us, sensor->next_exec_time, sensor->mode); + + return poll; +} + +/* check whether sensor needs to be executed/processed */ +static bool sensor_need_exec(struct sensing_sensor *sensor, uint64_t cur_us) +{ + LOG_DBG("sensor:%s need to execute, next_exec_time:%lld, sensor_mode:%d, interval:%d", + sensor->dev->name, sensor->next_exec_time, sensor->mode, sensor->interval); + + if (!is_sensor_opened(sensor)) { + return false; + } + if (sensor_need_poll(sensor, cur_us) || + is_sensor_data_ready(sensor) || + sensor_has_new_data(sensor)) { + return true; + } + + return false; +} + +static int virtual_sensor_process_data(struct sensing_sensor *sensor) +{ + const struct sensing_sensor_api *sensor_api; + struct sensing_connection *conn; + int ret = 0, i; + + __ASSERT(sensor && sensor->dev, "virtual sensor proc, sensor or sensor device is NULL"); + sensor_api = sensor->dev->api; + __ASSERT(sensor_api, "virtual sensor proc, sensor device sensor_api is NULL"); + + /* enumerate each connection, and call process data for each connection, + * after data processing, clear new_data_arrive flag + */ + for (i = 0; i < sensor->reporter_num; i++) { + conn = &sensor->conns[i]; + if (!conn->new_data_arrive) { + continue; + } + LOG_DBG("virtual sensor proc data, index:%d, sensor:%s, sample_size:%d", + i, sensor->dev->name, sensor->sample_size); + + ret |= sensor_api->process(sensor->dev, conn, conn->data, sensor->sample_size); + conn->new_data_arrive = false; + } + + return ret; +} + +static int process_streaming_data(struct sensing_sensor *sensor, uint64_t cur_us) +{ + const struct sensing_sensor_api *sensor_api; + uint64_t next_time; + uint64_t *sample_time; + int ret = 0; + + __ASSERT(sensor && sensor->dev, "process streaming data, sensor or sensor device is NULL"); + + sample_time = &((struct sensing_sensor_value_header *)sensor->data_buf)->base_timestamp; + /* sample time 0 is for first sample, + * update sample time according to current time + */ + next_time = (*sample_time == 0 ? cur_us : MIN(cur_us, *sample_time + sensor->interval)); + + LOG_DBG("proc stream data, sensor:%s, cur:%lld, sample_time:%lld, ri:%d(us), next:%lld", + sensor->dev->name, cur_us, *sample_time, sensor->interval, next_time); + + sensor_api = sensor->dev->api; + __ASSERT(sensor_api, "proc stream data, sensor device sensor_api is NULL"); + ret = sensor_api->read_sample(sensor->dev, sensor->data_buf, sensor->sample_size); + if (ret) { + return ret; + } + /* update data sample time */ + *sample_time = next_time; + + return 0; +} + +static int physical_sensor_process_data(struct sensing_sensor *sensor, uint64_t cur_us) +{ + int ret = 0; + + ret = process_streaming_data(sensor, cur_us); + if (ret) + return ret; + /* TODO: process fifo data*/ + + return 0; +} + +static int sensor_process_data(struct sensing_sensor *sensor, uint64_t cur_us) +{ + __ASSERT(sensor, "sensor process data, sensing_sensor is NULL"); + + if (is_phy_sensor(sensor)) { + return physical_sensor_process_data(sensor, cur_us); + } else { + return virtual_sensor_process_data(sensor); + } + + return 0; +} + +/* check whether it is right time for client to consume this sample */ +static bool sensor_test_consume_time(struct sensing_sensor *sensor, + struct sensing_connection *conn, + uint64_t cur_time) +{ + uint64_t ts = ((struct sensing_sensor_value_header *)sensor->data_buf)->base_timestamp; + + LOG_DBG("sensor:%s next_consume_time:%lld sample_time:%lld, cur_time:%lld", + sensor->dev->name, conn->next_consume_time, ts, cur_time); + + if (conn->next_consume_time <= ts) + return true; + + LOG_DBG("sensor:%s data not ready, next_consume_time:%lld sample_time:%lld, cur_time:%lld", + sensor->dev->name, conn->next_consume_time, ts, cur_time); + + return false; +} + +static void update_client_consume_time(struct sensing_sensor *sensor, + struct sensing_connection *conn) +{ + uint32_t interval = conn->interval; + uint64_t ts = ((struct sensing_sensor_value_header *)sensor->data_buf)->base_timestamp; + + LOG_DBG("update time, sensor:%s, next_consume:%lld, interval:%d, sample_time:%lld", + sensor->dev->name, conn->next_consume_time, interval, ts); + + if (conn->next_consume_time == 0 || conn->next_consume_time + interval < ts) { + /* three cases next consume time start counting from last sample time: + * 1) first sample arrived, next_consume_time is set to 0 + * 2) samples dropped + * 3) data ready mode is also processed this way to avoid error accumulation + */ + conn->next_consume_time = ts + interval; + } else { + /* regular flow */ + conn->next_consume_time += interval; + } +} + +static int sensor_sensitivity_test(struct sensing_sensor *sensor, + struct sensing_connection *conn) +{ + const struct sensing_sensor_api *sensor_api; + void *last_sample = conn->data; + void *cur_sample = sensor->data_buf; + int ret = 0, i; + + __ASSERT(sensor && sensor->dev, "sensor sensitivity test, sensor or sensor device is NULL"); + sensor_api = sensor->dev->api; + __ASSERT(sensor_api, "sensor sensitivity test, sensor device sensor_api is NULL"); + + if (!sensor_api->sensitivity_test) { + LOG_ERR("sensor:%s not register sensitivity callback", sensor->dev->name); + return -ENODEV; + } + for (i = 0; i < sensor->sensitivity_count; i++) { + ret |= sensor_api->sensitivity_test(sensor->dev, i, sensor->sensitivity[i], + last_sample, sensor->sample_size, cur_sample, sensor->sample_size); + } + LOG_INF("sensor:%s sensitivity test, ret:%d", sensor->dev->name, ret); + + return ret; +} + +/* check whether new sample could pass sensitivity test, sample will be sent to client if passed */ +static bool sensor_test_sensitivity(struct sensing_sensor *sensor, struct sensing_connection *conn) +{ + int ret = 0; + + /* always send the first sample to client */ + if (conn->next_consume_time == EXEC_TIME_INIT) { + return true; + } + + /* skip checking if sensitivity equals to 0 */ + if (!is_filtering_sensitivity(&sensor->sensitivity[0])) { + return true; + } + + /* call sensor sensitivity_test, ret: + * < 0: means call sensor_sensitivity_test() failed + * 0: means sample delta less than sensitivity threshold + * 1: means sample data over sensitivity threshold + */ + ret = sensor_sensitivity_test(sensor, conn); + if (ret) { + return false; + } + + return true; +} + +/* send data to clients based on interval and sensitivity */ +static int send_data_to_clients(struct sensing_context *ctx, + struct sensing_sensor *sensor, + uint64_t cur_us) +{ + struct sensing_sensor *client; + struct sensing_connection *conn; + bool sensitivity_pass = false; + + for_each_client_conn(sensor, conn) { + client = conn->sink; + LOG_DBG("sensor:%s send data to client:%p", conn->source->dev->name, conn); + + if (!is_client_request_data(conn)) { + continue; + } + /* sensor_test_consume_time(), check whether time is ready or not: + * true: it's time for client consuming the data + * false: client time not arrived yet, not consume the data + */ + if (!sensor_test_consume_time(sensor, conn, cur_us)) { + continue; + } + + /* sensor_test_sensitivity(), check sensitivity threshold passing or not: + * true: sensitivity checking pass, could post the data + * false: sensitivity checking not pass, return + */ + sensitivity_pass = sensor_test_sensitivity(sensor, conn); + + update_client_consume_time(sensor, conn); + + if (!sensitivity_pass) { + continue; + } + + conn->new_data_arrive = true; + /* copy sensor data to connection data buf + * 1) connection data will be used as last sample in next cycle sensitivity test + * 2) connection data will be passed to client in its process() callback + */ + memcpy(conn->data, sensor->data_buf, sensor->sample_size); + + if (conn->sink) { + /* pass the sensor mode to its client */ + client->mode = sensor->mode; + /* if client switch to polling mode, reset next_execute_time */ + if (client->mode == SENSOR_TRIGGER_MODE_POLLING && + client->next_exec_time == EXEC_TIME_OFF) { + client->next_exec_time = EXEC_TIME_INIT; + } + } else { + add_data_to_sensor_ring_buf(ctx, sensor, conn); + ctx->data_to_ring_buf = true; + } + } + + /* notify dispatch thread to dispatch data to application */ + if (ctx->data_to_ring_buf) { + k_sem_give(&ctx->dispatch_sem); + ctx->data_to_ring_buf = false; + } + + return 0; +} + +static uint64_t calc_next_poll_time(struct sensing_context *ctx) +{ + struct sensing_sensor *sensor; + uint64_t next_poll = EXEC_TIME_OFF; + int i; + + for_each_sensor(ctx, i, sensor) { + if (!is_sensor_state_ready(sensor)) + continue; + if (!is_sensor_opened(sensor)) + continue; + if (sensor->next_exec_time == EXEC_TIME_OFF) { + continue; + } + next_poll = MIN(next_poll, sensor->next_exec_time); + } + + return next_poll; +} + +static int calc_sleep_time(struct sensing_context *ctx, uint64_t cur_us) +{ + uint64_t next_poll_time; + uint32_t sleep_time; + + next_poll_time = calc_next_poll_time(ctx); + if (next_poll_time == EXEC_TIME_OFF) { + /* no sampling requested, sleep forever */ + sleep_time = UINT32_MAX; + } else if (next_poll_time <= cur_us) { + /* next polling time is no more than current time, no sleep at all */ + sleep_time = 0; + } else { + sleep_time = (uint32_t)((next_poll_time - cur_us) / USEC_PER_MSEC); + } + + LOG_DBG("calc sleep time, next:%lld, cur:%lld, sleep_time:%d(ms)", + next_poll_time, cur_us, sleep_time); + + return sleep_time; +} + +int loop_sensors(struct sensing_context *ctx) +{ + struct sensing_sensor *sensor; + uint64_t cur_us; + int i = 0, ret = 0; + + cur_us = get_us(); + LOG_DBG("loop sensors, cur_us:%lld(us)", cur_us); + + for_each_sensor(ctx, i, sensor) { + if (!sensor_need_exec(sensor, cur_us)) { + continue; + } + ret = sensor_process_data(sensor, cur_us); + if (ret) { + LOG_ERR("sensor:%s processed error:%d", sensor->dev->name, ret); + } + ret = send_data_to_clients(ctx, sensor, cur_us); + if (ret) { + LOG_ERR("sensor:%s send data to client error:%d", sensor->dev->name, ret); + } + } + + return calc_sleep_time(ctx, cur_us); +} + + +void sensing_dispatch_thread(void *p1, void *p2, void *p3) +{ + struct sensing_context *ctx = p1; + + LOG_INF("sensing dispatch thread start..."); + + do { + k_sem_take(&ctx->dispatch_sem, K_FOREVER); + + fetch_data_and_dispatch(ctx); + } while (1); +} diff --git a/subsys/sensing/sensing.c b/subsys/sensing/sensing.c index 6074925437923c..9b5469b5c7e56a 100644 --- a/subsys/sensing/sensing.c +++ b/subsys/sensing/sensing.c @@ -19,7 +19,7 @@ int sensing_open_sensor(const struct sensing_sensor_info *sensor_info, { int ret = 0; - if (handle == NULL) { + if (sensor_info == NULL || handle == NULL) { return -ENODEV; } @@ -64,6 +64,10 @@ int sensing_open_sensor_by_dt(const struct device *dev, /* sensing_close_sensor is normally called by applications: hid, chre, zephyr main, etc */ int sensing_close_sensor(sensing_sensor_handle_t *handle) { + if (handle == NULL) { + return -ENODEV; + } + return close_sensor((struct sensing_connection **)handle); } @@ -74,6 +78,10 @@ int sensing_set_config(sensing_sensor_handle_t handle, struct sensing_sensor_config *cfg; int i, ret = 0; + if (handle == NULL || configs == NULL) { + return -ENODEV; + } + if (count <= 0 || count > SENSING_SENSOR_ATTRIBUTE_MAX) { LOG_ERR("invalid config count:%d", count); return -EINVAL; @@ -110,6 +118,10 @@ int sensing_get_config(sensing_sensor_handle_t handle, struct sensing_sensor_config *cfg; int i, ret = 0; + if (handle == NULL || configs == NULL) { + return -ENODEV; + } + if (count <= 0 || count > SENSING_SENSOR_ATTRIBUTE_MAX) { LOG_ERR("invalid config count:%d", count); return -EINVAL; diff --git a/subsys/sensing/sensing_sensor.c b/subsys/sensing/sensing_sensor.c index c305cbb35c4fac..ee9744dfffab4d 100644 --- a/subsys/sensing/sensing_sensor.c +++ b/subsys/sensing/sensing_sensor.c @@ -8,18 +8,44 @@ #include #include #include - #include +#include "sensor_mgmt.h" + LOG_MODULE_DECLARE(sensing, CONFIG_SENSING_LOG_LEVEL); int sensing_sensor_notify_data_ready(const struct device *dev) { - return -ENOTSUP; + struct sensing_sensor *sensor = get_sensor_by_dev(dev); + struct sensing_context *ctx = get_sensing_ctx(); + + __ASSERT(sensor, "sensing sensor notify data ready, sensing_sensor is NULL"); + + LOG_INF("sensor:%s notify data ready, sensor_mode:%d", sensor->dev->name, sensor->mode); + + if (sensor->mode != SENSOR_TRIGGER_MODE_DATA_READY) { + LOG_ERR("sensor:%s not in data ready mode", sensor->dev->name); + return -EINVAL; + } + + atomic_set_bit(&sensor->flag, SENSOR_DATA_READY_BIT); + + atomic_set_bit(&ctx->event_flag, EVENT_DATA_READY); + k_sem_give(&ctx->event_sem); + + return 0; } int sensing_sensor_set_data_ready(const struct device *dev, bool data_ready) { - return -ENOTSUP; + struct sensing_sensor *sensor = get_sensor_by_dev(dev); + + __ASSERT(sensor, "sensing sensor set data ready, sensing_sensor is NULL"); + + sensor->mode = data_ready ? SENSOR_TRIGGER_MODE_DATA_READY : SENSOR_TRIGGER_MODE_POLLING; + LOG_INF("set data ready, sensor:%s, data_ready:%d, trigger_mode:%d", + sensor->dev->name, data_ready, sensor->mode); + + return 0; } int sensing_sensor_post_data(const struct device *dev, void *buf, int size) diff --git a/subsys/sensing/sensor/hinge_angle/CMakeLists.txt b/subsys/sensing/sensor/hinge_angle/CMakeLists.txt new file mode 100644 index 00000000000000..48d791eba804a9 --- /dev/null +++ b/subsys/sensing/sensor/hinge_angle/CMakeLists.txt @@ -0,0 +1,3 @@ +# SPDX-License-Identifier: Apache-2.0 + +zephyr_library_sources(hinge_angle.c) diff --git a/subsys/sensing/sensor/hinge_angle/Kconfig b/subsys/sensing/sensor/hinge_angle/Kconfig new file mode 100644 index 00000000000000..465ab2fb1489d3 --- /dev/null +++ b/subsys/sensing/sensor/hinge_angle/Kconfig @@ -0,0 +1,9 @@ +# Copyright (c) 2023 Intel Corporation +# SPDX-License-Identifier: Apache-2.0 + +config SENSING_SENSOR_HINGE_ANGLE + bool "Sensing hinge angle sensor" + default y + depends on DT_HAS_ZEPHYR_SENSING_HINGE_ANGLE_ENABLED + help + Enable sensing hinge angle sensor. diff --git a/subsys/sensing/sensor/hinge_angle/hinge_angle.c b/subsys/sensing/sensor/hinge_angle/hinge_angle.c new file mode 100644 index 00000000000000..656de9fc334f2a --- /dev/null +++ b/subsys/sensing/sensor/hinge_angle/hinge_angle.c @@ -0,0 +1,151 @@ +/* + * Copyright (c) 2023 Intel Corporation. + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include +#include +#include + +LOG_MODULE_REGISTER(hinge_angle, CONFIG_SENSING_LOG_LEVEL); + +#define HINGE_ANGLE_ACC_INTERVAL_US 100000 + +static struct sensing_sensor_register_info hinge_reg = { + .flags = SENSING_SENSOR_FLAG_REPORT_ON_CHANGE, + .sample_size = sizeof(struct sensing_sensor_value_q31), + .sensitivity_count = 1, + .version.value = SENSING_SENSOR_VERSION(1, 0, 0, 0), +}; + +struct hinge_angle_context { + uint32_t interval; + uint32_t sensitivity; + sensing_sensor_handle_t base_acc_handle; + sensing_sensor_handle_t lid_acc_handle; + void *algo_handle; +}; + +static int hinge_init(const struct device *dev, + const struct sensing_sensor_info *info, const sensing_sensor_handle_t *reporter_handles, + int32_t reporters_count) +{ + + int32_t i; + struct hinge_angle_context *ctx = sensing_sensor_get_ctx_data(dev); + const struct sensing_sensor_info *rpt_info = NULL; + + LOG_INF("[%s] name: %s", __func__, dev->name); + + ctx->base_acc_handle = NULL; + ctx->lid_acc_handle = NULL; + + for (i = 0; i < reporters_count; i++) { + rpt_info = sensing_get_sensor_info(reporter_handles[i]); + + LOG_INF("[%s] reporter_handles[%d] %p, type 0x%x", + __func__, i, reporter_handles[i], rpt_info->type); + + if (rpt_info->type == + SENSING_SENSOR_TYPE_MOTION_ACCELEROMETER_3D) { + if (strcmp(rpt_info->name, "base-accel") == 0) { + ctx->base_acc_handle = reporter_handles[i]; + continue; + } else if (strcmp(rpt_info->name, "lid-accel") == 0) { + ctx->lid_acc_handle = reporter_handles[i]; + continue; + } + } + + LOG_WRN("[%s] unused reporter_handles[%d] %d, type 0x%x", + __func__, i, reporter_handles[i], rpt_info->type); + } + + return 0; +} + +static int hinge_set_interval(const struct device *dev, uint32_t value) +{ + + int ret; + + struct sensing_sensor_config base_acc_config; + struct sensing_sensor_config lid_acc_config; + struct hinge_angle_context *ctx = sensing_sensor_get_ctx_data(dev); + uint32_t acc_interval = value ? HINGE_ANGLE_ACC_INTERVAL_US : 0; + + base_acc_config.attri = SENSING_SENSOR_ATTRIBUTE_INTERVAL; + base_acc_config.interval = HINGE_ANGLE_ACC_INTERVAL_US; + ret = sensing_set_config(ctx->base_acc_handle, &base_acc_config, 1); + if (ret) { + LOG_ERR("base_acc sensing_set_interval error:%d\n", ret); + } + + lid_acc_config.attri = SENSING_SENSOR_ATTRIBUTE_INTERVAL; + lid_acc_config.interval = HINGE_ANGLE_ACC_INTERVAL_US; + ret = sensing_set_config(ctx->lid_acc_handle, &lid_acc_config, 1); + if (ret) { + LOG_ERR("lid_acc sensing_set_interval error:%d\n", ret); + } + + ctx->interval = value; + LOG_INF("[%s] name: %s, value %d acc_interval %d", __func__, dev->name, + value, acc_interval); + + + return 0; +} + +static int hinge_get_interval(const struct device *dev, uint32_t *value) +{ + return 0; +} + +static int hinge_set_sensitivity(const struct device *dev, int index, + uint32_t value) +{ + return 0; +} + +static int hinge_get_sensitivity(const struct device *dev, int index, + uint32_t *value) +{ + return 0; +} + +static int hinge_process(const struct device *dev, + const sensing_sensor_handle_t reporter_handle, + void *buf, + int32_t size) +{ + return 0; +} + +static int hinge_sensitivity_test(const struct device *dev, int index, + uint32_t sensitivity, void *last_sample_buf, + int last_sample_size, void *current_sample_buf, + int current_sample_size) +{ + return 0; +} + +static const struct sensing_sensor_api hinge_api = { + .init = hinge_init, + .get_interval = hinge_get_interval, + .set_interval = hinge_set_interval, + .get_sensitivity = hinge_get_sensitivity, + .set_sensitivity = hinge_set_sensitivity, + .process = hinge_process, + .sensitivity_test = hinge_sensitivity_test, +}; + +#define DT_DRV_COMPAT zephyr_sensing_hinge_angle +#define SENSING_HINGE_ANGLE_DT_DEFINE(_inst) \ + static struct hinge_angle_context _CONCAT(hinge_ctx, _inst) = { 0 }; \ + SENSING_SENSOR_DT_DEFINE(DT_DRV_INST(_inst), &hinge_reg, \ + &_CONCAT(hinge_ctx, _inst), &hinge_api); + +DT_INST_FOREACH_STATUS_OKAY(SENSING_HINGE_ANGLE_DT_DEFINE); diff --git a/subsys/sensing/sensor/phy_3d_sensor/phy_3d_sensor.c b/subsys/sensing/sensor/phy_3d_sensor/phy_3d_sensor.c index 6ee2131bc3bdb0..6891c29505de45 100644 --- a/subsys/sensing/sensor/phy_3d_sensor/phy_3d_sensor.c +++ b/subsys/sensing/sensor/phy_3d_sensor/phy_3d_sensor.c @@ -18,23 +18,214 @@ LOG_MODULE_REGISTER(phy_3d_sensor, CONFIG_SENSING_LOG_LEVEL); +#define SENSING_ACCEL_Q31_SHIFT 6 +#define SENSING_GYRO_Q31_SHIFT 15 + +static int64_t shifted_q31_to_scaled_int64(q31_t q, int8_t shift, int64_t scale) +{ + int64_t scaled_value; + int64_t shifted_value; + + shifted_value = (int64_t)q << shift; + shifted_value = llabs(shifted_value); + + scaled_value = + FIELD_GET(GENMASK64(31 + shift, 31), shifted_value) * scale + + (FIELD_GET(GENMASK64(30, 0), shifted_value) * scale / BIT(31)); + + if (q < 0) { + scaled_value = -scaled_value; + } + + return scaled_value; +} + +static q31_t scaled_int64_to_shifted_q31(int64_t val, int64_t scale, + int8_t shift) +{ + return (q31_t)((val * BIT(31 - shift) / scale)); +} + +static q31_t accel_sensor_value_to_q31(struct sensor_value *val) +{ + int64_t micro_ms2 = sensor_value_to_micro(val); + int64_t micro_g = micro_ms2 * 1000000LL / SENSOR_G; + + return scaled_int64_to_shifted_q31(micro_g, 1000000LL, + SENSING_ACCEL_Q31_SHIFT); +} + +static void accel_q31_to_sensor_value(q31_t q, struct sensor_value *val) +{ + int64_t micro_g = shifted_q31_to_scaled_int64(q, + SENSING_ACCEL_Q31_SHIFT, 1000000LL); + int64_t micro_ms2 = micro_g * SENSOR_G / 1000000LL; + + sensor_value_from_micro(val, micro_ms2); +} + +static const struct phy_3d_sensor_custom custom_accel = { + .chan_all = SENSOR_CHAN_ACCEL_XYZ, + .shift = SENSING_ACCEL_Q31_SHIFT, + .q31_to_sensor_value = accel_q31_to_sensor_value, + .sensor_value_to_q31 = accel_sensor_value_to_q31, +}; + +static q31_t gyro_sensor_value_to_q31(struct sensor_value *val) +{ + int64_t micro_rad = (int64_t)sensor_value_to_micro(val); + int64_t micro_deg = micro_rad * 180000000LL / SENSOR_PI; + + return scaled_int64_to_shifted_q31(micro_deg, 1000000LL, + SENSING_GYRO_Q31_SHIFT); +} + +static void gyro_q31_to_sensor_value(q31_t q, struct sensor_value *val) +{ + int64_t micro_deg = shifted_q31_to_scaled_int64(q, + SENSING_GYRO_Q31_SHIFT, 1000000LL); + int64_t micro_rad = micro_deg * SENSOR_PI / 180000000LL; + + sensor_value_from_micro(val, micro_rad); +} + +static const struct phy_3d_sensor_custom custom_gyro = { + .chan_all = SENSOR_CHAN_GYRO_XYZ, + .shift = SENSING_GYRO_Q31_SHIFT, + .q31_to_sensor_value = gyro_q31_to_sensor_value, + .sensor_value_to_q31 = gyro_sensor_value_to_q31, +}; + +static void phy_3d_sensor_data_ready_handler(const struct device *dev, + const struct sensor_trigger *trig) +{ + struct phy_3d_sensor_context *ctx = CONTAINER_OF(trig, + struct phy_3d_sensor_context, + trig); + int ret; + + LOG_DBG("%s: trigger type:%d", dev->name, trig->type); + + ret = sensor_sample_fetch_chan(ctx->hw_dev, ctx->custom->chan_all); + if (ret) { + LOG_ERR("%s: sample fetch failed: %d", dev->name, ret); + return; + } + + sensing_sensor_notify_data_ready(ctx->dev); +} + +static int phy_3d_sensor_enable_data_ready(struct phy_3d_sensor_context *ctx, + bool enable) +{ + sensor_trigger_handler_t handler; + int ret = 0; + + ctx->trig.type = SENSOR_TRIG_DATA_READY; + ctx->trig.chan = ctx->custom->chan_all; + + if (enable != ctx->data_ready_enabled) { + handler = enable ? phy_3d_sensor_data_ready_handler : NULL; + + ret = sensing_sensor_set_data_ready(ctx->dev, enable); + if (!ret) { + if (!sensor_trigger_set(ctx->hw_dev, &ctx->trig, + handler)) { + ctx->data_ready_enabled = enable; + } else { + sensing_sensor_set_data_ready(ctx->dev, + !enable); + LOG_INF("%s: set data ready trigger not successfully", + ctx->dev->name); + } + } + } + + LOG_DBG("%s: trigger data ready enabled:%d", ctx->dev->name, + ctx->data_ready_enabled); + + return ret; +} + static int phy_3d_sensor_init(const struct device *dev, const struct sensing_sensor_info *info, const sensing_sensor_handle_t *reporter_handles, int reporters_count) { - return 0; -} + struct phy_3d_sensor_context *ctx; + int ret; -static int phy_3d_sensor_deinit(const struct device *dev) -{ - return 0; + ARG_UNUSED(reporter_handles); + ARG_UNUSED(reporters_count); + + ctx = sensing_sensor_get_ctx_data(dev); + ctx->dev = dev; + + switch (ctx->sensor_type) { + case SENSING_SENSOR_TYPE_MOTION_ACCELEROMETER_3D: + ctx->custom = &custom_accel; + break; + case SENSING_SENSOR_TYPE_MOTION_GYROMETER_3D: + ctx->custom = &custom_gyro; + break; + default: + LOG_ERR("phy_3d_sensor doesn't support sensor type %d", + ctx->sensor_type); + return -ENOTSUP; + } + + LOG_INF("%s: Underlying device: %s", dev->name, ctx->hw_dev->name); + + /* Check underlying device supports data ready or not */ + ret = phy_3d_sensor_enable_data_ready(ctx, true); + ctx->data_ready_support = ctx->data_ready_enabled; + if (!ret && ctx->data_ready_enabled) { + ret = phy_3d_sensor_enable_data_ready(ctx, false); + } + + return ret; } static int phy_3d_sensor_read_sample(const struct device *dev, void *buf, int size) { - return 0; + struct phy_3d_sensor_context *ctx; + struct sensing_sensor_value_3d_q31 *sample = buf; + struct sensor_value value[PHY_3D_SENSOR_CHANNEL_NUM]; + int i, ret; + + ctx = sensing_sensor_get_ctx_data(dev); + + if (!ctx->data_ready_enabled) { + ret = sensor_sample_fetch_chan(ctx->hw_dev, + ctx->custom->chan_all); + if (ret) { + LOG_ERR("%s: sample fetch failed: %d", dev->name, ret); + return ret; + } + } + + ret = sensor_channel_get(ctx->hw_dev, ctx->custom->chan_all, value); + if (ret) { + LOG_ERR("%s: channel get failed: %d", dev->name, ret); + return ret; + } + + for (i = 0; i < ARRAY_SIZE(value); ++i) { + sample->readings[0].v[i] = + ctx->custom->sensor_value_to_q31(&value[i]); + } + + sample->header.reading_count = 1; + sample->shift = ctx->custom->shift; + + LOG_DBG("%s: Sample data:\t x: %d, y: %d, z: %d", + dev->name, + sample->readings[0].x, + sample->readings[0].y, + sample->readings[0].z); + + return ret; } static int phy_3d_sensor_sensitivity_test(const struct device *dev, @@ -42,39 +233,196 @@ static int phy_3d_sensor_sensitivity_test(const struct device *dev, void *last_sample_buf, int last_sample_size, void *current_sample_buf, int current_sample_size) { - return 0; + struct sensing_sensor_value_3d_q31 *last = last_sample_buf; + struct sensing_sensor_value_3d_q31 *curr = current_sample_buf; + q31_t sensi = sensitivity; + struct phy_3d_sensor_context *ctx; + int reached = 0; + int i; + + ctx = sensing_sensor_get_ctx_data(dev); + if (index >= 0 && index < ARRAY_SIZE(ctx->sensitivities)) { + reached = abs(curr->readings[0].v[index] + - last->readings[0].v[index]) + >= sensi; + } else if (index == SENSING_SENSITIVITY_INDEX_ALL) { + for (i = 0; i < ARRAY_SIZE(ctx->sensitivities); ++i) { + reached |= abs(curr->readings[0].v[i] + - last->readings[0].v[i]) + >= sensi; + } + } else { + LOG_ERR("%s: test sensitivity: invalid index: %d", dev->name, + index); + return -EINVAL; + } + + return !reached; } static int phy_3d_sensor_set_interval(const struct device *dev, uint32_t value) { + struct phy_3d_sensor_context *ctx; + struct sensor_value odr; + int ret; + + LOG_DBG("%s: set report interval %u us", dev->name, value); + + ctx = sensing_sensor_get_ctx_data(dev); + + if (ctx->interval == value) { + return 0; + } + + if (value) { + if (ctx->data_ready_support) { + phy_3d_sensor_enable_data_ready(ctx, true); + } + + odr.val1 = USEC_PER_SEC / value; + odr.val2 = (uint64_t)USEC_PER_SEC * 1000000 / value % 1000000; + + ret = sensor_attr_set(ctx->hw_dev, ctx->custom->chan_all, + SENSOR_ATTR_SAMPLING_FREQUENCY, &odr); + if (ret) { + LOG_ERR("%s: Cannot set sampling frequency %d.%06d Hz. ret:%d", + dev->name, odr.val1, odr.val2, ret); + } else { + LOG_DBG("%s: Set sampling frequency %d.%06d Hz.", + dev->name, odr.val1, odr.val2); + } + } else { + if (ctx->data_ready_support) { + phy_3d_sensor_enable_data_ready(ctx, false); + } + } + + ctx->interval = value; + return 0; } static int phy_3d_sensor_get_interval(const struct device *dev, uint32_t *value) { + struct phy_3d_sensor_context *ctx; + struct sensor_value odr; + uint64_t micro_freq; + + ctx = sensing_sensor_get_ctx_data(dev); + + if (!sensor_attr_get(ctx->hw_dev, ctx->custom->chan_all, + SENSOR_ATTR_SAMPLING_FREQUENCY, &odr)) { + micro_freq = sensor_value_to_micro(&odr); + if (micro_freq) { + *value = (uint32_t)(USEC_PER_SEC * 1000000 / micro_freq); + } else { + *value = 0; + } + } else { + *value = ctx->interval; + } + + LOG_DBG("%s: get report interval %u us", dev->name, *value); + return 0; } -static int phy_3d_sensor_set_sensitivity(const struct device *dev, - int index, uint32_t value) +static int phy_3d_sensor_set_slope(struct phy_3d_sensor_context *ctx, + enum sensor_channel chan, uint32_t value) { - return 0; + struct sensor_value attr_value; + enum sensor_attribute attr; + int ret = 0; + + ctx->custom->q31_to_sensor_value((q31_t)value, &attr_value); + attr = SENSOR_ATTR_SLOPE_TH; + + ret = sensor_attr_set(ctx->hw_dev, chan, attr, &attr_value); + if (!ret) { + /* set slope duration */ + attr_value.val1 = PHY_3D_SENSOR_SLOPE_DURATION; + attr_value.val2 = 0; + attr = SENSOR_ATTR_SLOPE_DUR; + + ret = sensor_attr_set(ctx->hw_dev, chan, attr, &attr_value); + if (!ret) { + ctx->trig.type = SENSOR_TRIG_DELTA; + ctx->trig.chan = chan; + + if (value) { + ret = sensor_trigger_set(ctx->hw_dev, + &ctx->trig, + phy_3d_sensor_data_ready_handler); + } else { + ret = sensor_trigger_set(ctx->hw_dev, + &ctx->trig, + NULL); + } + } + } + + if (ret) { + LOG_DBG("%s: set slope failed! attr:%d chan:%d ret:%d", + ctx->hw_dev->name, attr, chan, ret); + } + + return ret; } -static int phy_3d_sensor_get_sensitivity(const struct device *dev, - int index, uint32_t *value) +static int phy_3d_sensor_set_sensitivity(const struct device *dev, + int index, uint32_t value) { + struct phy_3d_sensor_context *ctx; + uint32_t sensi; + bool enabled; + int ret = 0; + int i; + + ctx = sensing_sensor_get_ctx_data(dev); + + if (index >= 0 && index < ARRAY_SIZE(ctx->sensitivities)) { + ctx->sensitivities[index] = value; + } else if (index == SENSING_SENSITIVITY_INDEX_ALL) { + for (i = 0; i < ARRAY_SIZE(ctx->sensitivities); ++i) { + ctx->sensitivities[i] = value; + } + } else { + LOG_ERR("%s: set sensitivity: invalid index: %d", + dev->name, index); + return -EINVAL; + } + + LOG_DBG("%s: set sensitivity index: %d value: %d", dev->name, + index, value); + + sensi = ctx->sensitivities[0]; + for (i = 1; i < ARRAY_SIZE(ctx->sensitivities); ++i) { + sensi = MIN(ctx->sensitivities[i], sensi); + } + + /* Disable data ready before enable any-motion */ + enabled = ctx->data_ready_enabled; + if (ctx->data_ready_support && enabled) { + phy_3d_sensor_enable_data_ready(ctx, false); + } + + ret = phy_3d_sensor_set_slope(ctx, ctx->custom->chan_all, sensi); + if (ret) { + /* Try to enable data ready if enable any-motion failed */ + if (ctx->data_ready_support && enabled) { + phy_3d_sensor_enable_data_ready(ctx, true); + } + } + return 0; } static const struct sensing_sensor_api phy_3d_sensor_api = { .init = phy_3d_sensor_init, - .deinit = phy_3d_sensor_deinit, .set_interval = phy_3d_sensor_set_interval, .get_interval = phy_3d_sensor_get_interval, .set_sensitivity = phy_3d_sensor_set_sensitivity, - .get_sensitivity = phy_3d_sensor_get_sensitivity, .read_sample = phy_3d_sensor_read_sample, .sensitivity_test = phy_3d_sensor_sensitivity_test, }; diff --git a/subsys/sensing/sensor/phy_3d_sensor/phy_3d_sensor.h b/subsys/sensing/sensor/phy_3d_sensor/phy_3d_sensor.h index 966a6f088c33b9..6b3a18c5028622 100644 --- a/subsys/sensing/sensor/phy_3d_sensor/phy_3d_sensor.h +++ b/subsys/sensing/sensor/phy_3d_sensor/phy_3d_sensor.h @@ -8,13 +8,28 @@ #define ZEPHYR_INCLUDE_SPHY_3D_SENSOR_H_ #include +#include #define PHY_3D_SENSOR_CHANNEL_NUM 3 +#define PHY_3D_SENSOR_SLOPE_DURATION 2 + +struct phy_3d_sensor_custom { + const enum sensor_channel chan_all; + const int8_t shift; + void (*q31_to_sensor_value)(q31_t q31, struct sensor_value *val); + q31_t (*sensor_value_to_q31)(struct sensor_value *val); +}; struct phy_3d_sensor_context { const struct device *dev; const struct device *hw_dev; const int32_t sensor_type; + const struct phy_3d_sensor_custom *custom; + struct sensor_trigger trig; + bool data_ready_enabled; + bool data_ready_support; + uint32_t interval; + uint32_t sensitivities[PHY_3D_SENSOR_CHANNEL_NUM]; }; #endif diff --git a/subsys/sensing/sensor_mgmt.c b/subsys/sensing/sensor_mgmt.c index 9ecde551a02a5c..201ce589cb8f85 100644 --- a/subsys/sensing/sensor_mgmt.c +++ b/subsys/sensing/sensor_mgmt.c @@ -6,9 +6,7 @@ #include #include #include -#include #include -#include #include #include #include "sensor_mgmt.h" @@ -26,22 +24,230 @@ LOG_MODULE_REGISTER(sensing, CONFIG_SENSING_LOG_LEVEL); DT_FOREACH_CHILD_STATUS_OKAY(DT_DRV_INST(0), SENSING_SENSOR_INFO_DEFINE) DT_FOREACH_CHILD_STATUS_OKAY(DT_DRV_INST(0), SENSING_SENSOR_DEFINE) +K_THREAD_STACK_DEFINE(runtime_stack, CONFIG_SENSING_RUNTIME_THREAD_STACK_SIZE); +K_THREAD_STACK_DEFINE(dispatch_stack, CONFIG_SENSING_DISPATCH_THREAD_STACK_SIZE); -/** - * @struct sensing_context - * @brief sensing subsystem context to include global variables - */ -struct sensing_context { - bool sensing_initialized; - int sensor_num; - struct sensing_sensor *sensors[SENSING_SENSOR_NUM]; -}; + +struct sensing_sensor *sensors[SENSING_SENSOR_NUM]; static struct sensing_context sensing_ctx = { .sensor_num = SENSING_SENSOR_NUM, }; +/* sensor_later_config including arbitrate/set interval/sensitivity + */ +static uint32_t arbitrate_interval(struct sensing_sensor *sensor) +{ + struct sensing_connection *conn; + uint32_t min_interval = UINT32_MAX; + uint32_t interval; + + /* search from all clients, arbitrate the interval */ + for_each_client_conn(sensor, conn) { + LOG_INF("arbitrate interval, sensor:%s for each conn:%p, interval:%d(us)", + sensor->dev->name, conn, conn->interval); + if (!is_client_request_data(conn)) { + continue; + } + if (conn->interval < min_interval) { + min_interval = conn->interval; + } + } + /* min_interval == UINT32_MAX means sensor is not opened by any clients, + * then interval should be 0 + */ + interval = (min_interval == UINT32_MAX ? 0 : min_interval); + + if (interval == 0) { + /* sensor is closed by all clients, reset next_exec_time as EXEC_TIME_OFF + * open -> close: next_exec_time = EXEC_TIME_OFF + */ + sensor->next_exec_time = EXEC_TIME_OFF; + } else { + /* sensor is still closed last time, set next_exec_time as EXEC_TIME_INIT + * close -> open: next_exec_time = EXEC_TIME_INIT + */ + if (sensor->next_exec_time == EXEC_TIME_OFF) { + sensor->next_exec_time = EXEC_TIME_INIT; + } + } + LOG_DBG("arbitrate interval, sensor:%s, interval:%d(us), next_exec_time:%lld", + sensor->dev->name, interval, sensor->next_exec_time); + + return interval; +} + +static int set_arbitrate_interval(struct sensing_sensor *sensor, uint32_t interval) +{ + const struct sensing_sensor_api *sensor_api; + + __ASSERT(sensor && sensor->dev, "set arbitrate interval, sensor or sensor device is NULL"); + sensor_api = sensor->dev->api; + __ASSERT(sensor_api, "set arbitrate interval, sensor device sensor_api is NULL"); + + sensor->interval = interval; + /* reset sensor next_exec_time and sample timestamp as soon as sensor interval is changed */ + sensor->next_exec_time = interval > 0 ? EXEC_TIME_INIT : EXEC_TIME_OFF; + + LOG_DBG("set arbitrate interval:%d, sensor:%s, next_exec_time:%lld", + interval, sensor->dev->name, sensor->next_exec_time); + + ((struct sensing_sensor_value_header *)sensor->data_buf)->base_timestamp = 0; + + if (!sensor_api->set_interval) { + LOG_ERR("sensor:%s set_interval callback is not set yet", sensor->dev->name); + return -ENODEV; + } + + return sensor_api->set_interval(sensor->dev, interval); +} + +static int config_interval(struct sensing_sensor *sensor) +{ + uint32_t interval = arbitrate_interval(sensor); + + LOG_INF("config interval, sensor:%s, interval:%d", sensor->dev->name, interval); + + return set_arbitrate_interval(sensor, interval); +} + +static uint32_t arbitrate_sensitivity(struct sensing_sensor *sensor, int index) +{ + struct sensing_connection *conn; + uint32_t min_sensitivity = UINT32_MAX; + + /* search from all clients, arbitrate the sensitivity */ + for_each_client_conn(sensor, conn) { + LOG_DBG("arbitrate sensitivity, sensor:%s for each conn:%p, idx:%d, sens:%d", + sensor->dev->name, conn, index, + conn->sensitivity[index]); + if (!is_client_request_data(conn)) { + continue; + } + if (conn->sensitivity[index] < min_sensitivity) { + min_sensitivity = conn->sensitivity[index]; + } + } + LOG_DBG("arbitrate sensitivity, sensor:%s, min_sensitivity:%d", + sensor->dev->name, min_sensitivity); + + /* min_sensitivity == UINT32_MAX means no client is requesting to open sensor, + * by any client, in this case, return sensitivity 0 + */ + return (min_sensitivity == UINT32_MAX ? 0 : min_sensitivity); +} + +static int set_arbitrate_sensitivity(struct sensing_sensor *sensor, int index, uint32_t sensitivity) +{ + const struct sensing_sensor_api *sensor_api; + + __ASSERT(sensor && sensor->dev, "arbitrate sensitivity, sensor or sensor device is NULL"); + sensor_api = sensor->dev->api; + __ASSERT(sensor_api, "arbitrate sensitivity, sensor device sensor_api is NULL"); + + /* update sensor sensitivity */ + sensor->sensitivity[index] = sensitivity; + + if (!sensor_api->set_sensitivity) { + LOG_WRN("sensor:%s set_sensitivity callback is not set", sensor->dev->name); + /* sensor driver may not set sensitivity callback, no need to return error here */ + return 0; + } + + return sensor_api->set_sensitivity(sensor->dev, index, sensitivity); +} + +static int config_sensitivity(struct sensing_sensor *sensor, int index) +{ + uint32_t sensitivity = arbitrate_sensitivity(sensor, index); + + LOG_INF("config sensitivity, sensor:%s, index:%d, sensitivity:%d", + sensor->dev->name, index, sensitivity); + + return set_arbitrate_sensitivity(sensor, index, sensitivity); +} + +static int config_sensor(struct sensing_sensor *sensor) +{ + int ret; + int i = 0; + + ret = config_interval(sensor); + if (ret) { + LOG_WRN("sensor:%s config interval error", sensor->dev->name); + } + + for (i = 0; i < sensor->sensitivity_count; i++) { + ret = config_sensitivity(sensor, i); + if (ret) { + LOG_WRN("sensor:%s config sensitivity index:%d error", + sensor->dev->name, i); + } + } + + return ret; +} + +static void sensor_later_config(struct sensing_context *ctx) +{ + struct sensing_sensor *sensor; + int i = 0; + + LOG_INF("sensor later config begin..."); + + for_each_sensor_reverse(ctx, i, sensor) { + if (atomic_test_and_clear_bit(&sensor->flag, SENSOR_LATER_CFG_BIT)) { + LOG_INF("sensor later config, index:%d, sensor:%s", + i, sensor->dev->name); + config_sensor(sensor); + } + } +} + +static void sensing_runtime_thread(void *p1, void *p2, void *p3) +{ + struct sensing_context *ctx = p1; + int sleep_time = UINT32_MAX; + int ret; + + LOG_INF("sensing runtime thread start..."); + + do { + sleep_time = loop_sensors(ctx); + + LOG_INF("sensing runtime thread, sleep_time:%d(ms)", sleep_time); + + ret = k_sem_take(&ctx->event_sem, calc_timeout(sleep_time)); + if (!ret) { + if (atomic_test_and_clear_bit(&ctx->event_flag, EVENT_CONFIG_READY)) { + LOG_INF("runtime thread triggered by EVENT_CONFIG_READY"); + sensor_later_config(ctx); + } + if (atomic_test_and_clear_bit(&ctx->event_flag, EVENT_DATA_READY)) { + LOG_INF("runtime thread triggered by EVENT_DATA_READY"); + } + } + } while (1); +} + +static void save_config_and_notify(struct sensing_sensor *sensor) +{ + struct sensing_context *ctx = &sensing_ctx; + + __ASSERT(sensor, "save config and notify, sensing_sensor not be NULL"); + + LOG_INF("save config and notify, sensor:%s", sensor->dev->name); + + /* remember sensor_later_config bit to sensor */ + atomic_set_bit(&sensor->flag, SENSOR_LATER_CFG_BIT); + + /*remember event config ready and notify sensing_runtime_thread */ + atomic_set_bit(&ctx->event_flag, EVENT_CONFIG_READY); + + k_sem_give(&ctx->event_sem); +} + static int set_sensor_state(struct sensing_sensor *sensor, enum sensing_sensor_state state) { __ASSERT(sensor, "set sensor state, sensing_sensor is NULL"); @@ -93,8 +299,8 @@ static int init_sensor(struct sensing_sensor *sensor, int conns_num) init_connection(conn, reporter, sensor); - LOG_DBG("init sensor, reporter:%s, client:%s, connection:%d", - reporter->dev->name, sensor->dev->name, i); + LOG_INF("init sensor, reporter:%s, client:%s, connection:%d(%p)", + reporter->dev->name, sensor->dev->name, i, conn); tmp_conns[i] = conn; } @@ -200,13 +406,15 @@ static int sensing_init(void) return 0; } - STRUCT_SECTION_FOREACH(sensing_sensor, tmp_sensor) { - ret = pre_init_sensor(tmp_sensor); + for (i = 0; i < ctx->sensor_num; i++) { + STRUCT_SECTION_GET(sensing_sensor, i, &sensor); + ret = pre_init_sensor(sensor); if (ret) { LOG_ERR("sensing init, pre init sensor error"); } - ctx->sensors[i++] = tmp_sensor; + sensors[i] = sensor; } + ctx->sensors = sensors; for_each_sensor(ctx, i, sensor) { ret = init_sensor(sensor, sensor->reporter_num); @@ -218,9 +426,36 @@ static int sensing_init(void) if (ret) { LOG_ERR("set sensor:%s state:%d error", sensor->dev->name, state); } - LOG_INF("sensing init, sensor:%s state:%d", sensor->dev->name, sensor->state); + LOG_INF("sensing init, sensor:%s, state:%d", sensor->dev->name, sensor->state); + } + + k_sem_init(&ctx->event_sem, 0, 1); + k_sem_init(&ctx->dispatch_sem, 0, 1); + + /* sensing subsystem runtime thread: sensor scheduling and sensor data processing */ + ctx->runtime_id = k_thread_create(&ctx->runtime_thread, runtime_stack, + CONFIG_SENSING_RUNTIME_THREAD_STACK_SIZE, + (k_thread_entry_t) sensing_runtime_thread, ctx, NULL, NULL, + CONFIG_SENSING_RUNTIME_THREAD_PRIORITY, 0, K_NO_WAIT); + if (!ctx->runtime_id) { + LOG_ERR("create sensing runtime thread error"); + return -EAGAIN; } + /* sensor dispatch thread: get sensor data from senss and dispatch data */ + ctx->dispatch_id = k_thread_create(&ctx->dispatch_thread, dispatch_stack, + CONFIG_SENSING_DISPATCH_THREAD_STACK_SIZE, + (k_thread_entry_t) sensing_dispatch_thread, ctx, NULL, NULL, + CONFIG_SENSING_DISPATCH_THREAD_PRIORITY, 0, K_NO_WAIT); + if (!ctx->dispatch_id) { + LOG_ERR("create dispatch thread error"); + return -EAGAIN; + } + + ring_buf_init(&ctx->sensor_ring_buf, sizeof(ctx->buf), ctx->buf); + + ctx->sensing_initialized = true; + return ret; } @@ -258,11 +493,15 @@ int close_sensor(struct sensing_connection **conn) __ASSERT(!tmp_conn->sink, "sensor derived from device tree cannot be closed"); + __ASSERT(tmp_conn->source, "reporter should not be NULL"); + sys_slist_find_and_remove(&tmp_conn->source->client_list, &tmp_conn->snode); *conn = NULL; free(*conn); + save_config_and_notify(tmp_conn->source); + return 0; } @@ -287,22 +526,95 @@ int sensing_register_callback(struct sensing_connection *conn, int set_interval(struct sensing_connection *conn, uint32_t interval) { - return -ENOTSUP; + LOG_INF("set interval, sensor:%s, interval:%u(us)", conn->source->dev->name, interval); + + __ASSERT(conn && conn->source, "set interval, connection or reporter not be NULL"); + + if (interval > 0 && interval < conn->source->info->minimal_interval) { + LOG_ERR("interval:%d(us) should no less than min interval:%d(us)", + interval, conn->source->info->minimal_interval); + return -EINVAL; + } + + conn->interval = interval; + conn->next_consume_time = EXEC_TIME_INIT; + + LOG_INF("set interval, sensor:%s, conn:%p, interval:%d", + conn->source->dev->name, conn, interval); + + save_config_and_notify(conn->source); + + return 0; } int get_interval(struct sensing_connection *conn, uint32_t *interval) { - return -ENOTSUP; + __ASSERT(conn, "get interval, connection not be NULL"); + *interval = conn->interval; + + LOG_INF("get interval, sensor:%s, interval:%u(us)", conn->source->dev->name, *interval); + + return 0; } int set_sensitivity(struct sensing_connection *conn, int8_t index, uint32_t sensitivity) { - return -ENOTSUP; + int i; + + __ASSERT(conn && conn->source, "set sensitivity, connection or reporter not be NULL"); + + LOG_INF("set sensitivity, sensor:%s, index:%d, sensitivity:%d, count:%d", + conn->source->dev->name, index, + sensitivity, conn->source->sensitivity_count); + + if (index < SENSING_SENSITIVITY_INDEX_ALL || index >= conn->source->sensitivity_count) { + LOG_ERR("sensor:%s sensitivity index:%d invalid", conn->source->dev->name, index); + return -EINVAL; + } + + if (index == SENSING_SENSITIVITY_INDEX_ALL) { + for (i = 0; i < conn->source->sensitivity_count; i++) { + conn->sensitivity[i] = sensitivity; + } + } else { + conn->sensitivity[index] = sensitivity; + } + + return 0; } int get_sensitivity(struct sensing_connection *conn, int8_t index, uint32_t *sensitivity) { - return -ENOTSUP; + int i = 0; + + __ASSERT(conn && conn->source, "get sensitivity, connection or reporter not be NULL"); + + *sensitivity = UINT32_MAX; + + if (index < SENSING_SENSITIVITY_INDEX_ALL || index >= conn->source->sensitivity_count) { + LOG_ERR("sensor:%s sensitivity index:%d invalid", conn->source->dev->name, index); + return -EINVAL; + } + + if (index == SENSING_SENSITIVITY_INDEX_ALL) { + /* each sensitivity index value should be same for global sensitivity */ + for (i = 1; i < conn->source->sensitivity_count; i++) { + if (conn->sensitivity[i] != conn->sensitivity[0]) { + LOG_ERR("sensitivity[%d]:%d should be same as sensitivity:%d", + i, conn->sensitivity[i], conn->sensitivity[0]); + return -EINVAL; + } + } + *sensitivity = conn->sensitivity[0]; + } else { + *sensitivity = conn->sensitivity[index]; + } + + LOG_INF("get_sensitivity, sensor:%s, index:%d, sensitivity:%d, count:%d", + conn->source->dev->name, index, + *sensitivity, conn->source->sensitivity_count); + + return 0; } int sensing_get_sensors(int *sensor_nums, const struct sensing_sensor_info **info) @@ -319,5 +631,9 @@ int sensing_get_sensors(int *sensor_nums, const struct sensing_sensor_info **inf return 0; } +struct sensing_context *get_sensing_ctx(void) +{ + return &sensing_ctx; +} SYS_INIT(sensing_init, APPLICATION, CONFIG_APPLICATION_INIT_PRIORITY); diff --git a/subsys/sensing/sensor_mgmt.h b/subsys/sensing/sensor_mgmt.h index 38219305304d6e..5b89b2360cb0ae 100644 --- a/subsys/sensing/sensor_mgmt.h +++ b/subsys/sensing/sensor_mgmt.h @@ -11,6 +11,8 @@ #include #include #include +#include +#include #ifdef __cplusplus extern "C" { @@ -57,11 +59,30 @@ extern "C" { #define for_each_sensor(ctx, i, sensor) \ for (i = 0; i < ctx->sensor_num && (sensor = ctx->sensors[i]) != NULL; i++) +#define for_each_sensor_reverse(ctx, i, sensor) \ + for (i = ctx->sensor_num - 1; i >= 0 && (sensor = ctx->sensors[i]) != NULL; i--) + +#define for_each_client_conn(sensor, client) \ + SYS_SLIST_FOR_EACH_CONTAINER(&sensor->client_list, client, snode) + +#define EXEC_TIME_INIT 0 +#define EXEC_TIME_OFF UINT64_MAX + enum sensor_trigger_mode { SENSOR_TRIGGER_MODE_POLLING = 1, SENSOR_TRIGGER_MODE_DATA_READY = 2, }; +enum { + EVENT_CONFIG_READY, + EVENT_DATA_READY, +}; + +enum { + SENSOR_LATER_CFG_BIT, + SENSOR_DATA_READY_BIT, +}; + /** * @struct sensing_connection information * @brief sensing_connection indicates connection from reporter(source) to client(sink) @@ -75,6 +96,9 @@ struct sensing_connection { /* copy sensor data to connection data buf from reporter */ void *data; /* client(sink) next consume time */ + uint64_t next_consume_time; + /* when new data arrive, set flag to true, after data processing, clear the flag */ + bool new_data_arrive; sys_snode_t snode; /* post data to application */ sensing_data_event_t data_evt_cb; @@ -101,12 +125,34 @@ struct sensing_sensor { enum sensing_sensor_state state; enum sensor_trigger_mode mode; /* runtime info */ + atomic_t flag; + uint64_t next_exec_time; uint16_t sample_size; void *data_buf; struct sensing_connection *conns; const struct device *reporters[]; }; +/** + * @struct sensing_context + * @brief sensing subsystem context to include global variables + */ +struct sensing_context { + bool sensing_initialized; + int sensor_num; + struct sensing_sensor **sensors; + struct k_thread runtime_thread; + struct k_thread dispatch_thread; + k_tid_t runtime_id; + k_tid_t dispatch_id; + struct k_sem event_sem; + struct k_sem dispatch_sem; + atomic_t event_flag; + bool data_to_ring_buf; + struct ring_buf sensor_ring_buf; + uint8_t buf[CONFIG_SENSING_RING_BUF_SIZE]; +}; + int open_sensor(struct sensing_sensor *sensor, struct sensing_connection **conn); int close_sensor(struct sensing_connection **conn); int sensing_register_callback(struct sensing_connection *conn, @@ -115,6 +161,9 @@ int set_interval(struct sensing_connection *conn, uint32_t interval); int get_interval(struct sensing_connection *con, uint32_t *sensitivity); int set_sensitivity(struct sensing_connection *conn, int8_t index, uint32_t interval); int get_sensitivity(struct sensing_connection *con, int8_t index, uint32_t *sensitivity); +int loop_sensors(struct sensing_context *ctx); +void sensing_dispatch_thread(void *p1, void *p2, void *p3); +struct sensing_context *get_sensing_ctx(void); static inline bool is_phy_sensor(struct sensing_sensor *sensor) @@ -154,6 +203,71 @@ static inline const struct sensing_sensor_info *get_sensor_info(struct sensing_c return conn->source->info; } +/* check if client has requested data from reporter */ +static inline bool is_client_request_data(struct sensing_connection *conn) +{ + return conn->interval != 0; +} + +static inline uint64_t get_us(void) +{ + return k_cycle_get_64() * USEC_PER_SEC / CONFIG_SYS_CLOCK_HW_CYCLES_PER_SEC; +} + +static inline bool is_sensor_opened(struct sensing_sensor *sensor) +{ + return sensor->interval != 0; +} + +static inline bool is_sensor_state_ready(struct sensing_sensor *sensor) +{ + return (sensor->state == SENSING_SENSOR_STATE_READY); +} + +/* sensor not in polling mode, meanwhile data ready arrived from physical sensor */ +static inline bool is_sensor_data_ready(struct sensing_sensor *sensor) +{ + return is_phy_sensor(sensor) && + sensor->mode == SENSOR_TRIGGER_MODE_DATA_READY && + atomic_test_and_clear_bit(&sensor->flag, SENSOR_DATA_READY_BIT); +} + +/* when reporter post data to its client, new_data_arrive flag will be set, + * indicates sensor has new data arriving + */ +static inline bool sensor_has_new_data(const struct sensing_sensor *sensor) +{ + for (int i = 0; i < sensor->reporter_num; i++) { + if (sensor->conns[i].new_data_arrive) + return true; + } + + return false; +} + +/* this function is used to decide whether filtering sensitivity checking + * for example: filter sensitivity checking if sensitivity value is 0. + */ +static inline bool is_filtering_sensitivity(int *sensitivity) +{ + bool filtering = false; + + __ASSERT(sensitivity, "sensitivity should not be NULL"); + for (int i = 0; i < CONFIG_SENSING_MAX_SENSITIVITY_COUNT; i++) { + if (sensitivity[i] != 0) { + filtering = true; + break; + } + } + + return filtering; +} + +static inline k_timeout_t calc_timeout(int sleep_time) +{ + return (sleep_time == UINT32_MAX ? K_FOREVER : K_MSEC(sleep_time)); +} + /** * @} */