From 0f773221fdd875c1a79264e3a03a636dad0fcf57 Mon Sep 17 00:00:00 2001 From: Yuval Peress Date: Tue, 11 Jul 2023 02:05:38 -0600 Subject: [PATCH] Implement angle sensor --- cmake/linker_script/common/common-ram.cmake | 1 + drivers/sensor/icm42688/icm42688_decoder.c | 18 +- drivers/sensor/icm42688/icm42688_rtio.c | 2 +- .../sensing/sensing,accel-based-angle.yaml | 21 ++ dts/bindings/sensing/sensing,emul.yaml | 39 +++ dts/bindings/vendor-prefixes.txt | 1 + include/zephyr/sensing/datatypes.h | 11 +- include/zephyr/sensing/sensing.h | 5 +- include/zephyr/sensing/sensor.h | 24 +- .../sensor_shell/boards/tdk_robokit1.conf | 1 + .../sensor_shell/boards/tdk_robokit1.overlay | 17 +- samples/sensor/sensor_shell/prj.conf | 8 +- scripts/build/gen_kobject_list.py | 3 +- subsys/sensing/CMakeLists.txt | 3 + subsys/sensing/Kconfig | 4 +- .../include/sensing/internal/sensing.h | 17 +- subsys/sensing/sections.ld | 3 + subsys/sensing/src/processing.c | 142 +++++++- subsys/sensing/src/sensor_arbitrate.c | 6 +- subsys/sensing/src/sensor_connections.c | 35 +- subsys/sensing/src/sensor_pipe.c | 9 +- subsys/sensing/src/shell.c | 89 ++++- subsys/sensing/src/userspace.c | 7 + subsys/sensing/vsensors/CMakeLists.txt | 2 + subsys/sensing/vsensors/Kconfig | 10 + .../vsensors/accel-based-angle/CMakeLists.txt | 2 + .../vsensors/accel-based-angle/driver.c | 321 ++++++++++++++++++ subsys/sensing/vsensors/emul/CMakeLists.txt | 3 + subsys/sensing/vsensors/emul/driver.c | 254 ++++++++++++++ .../zephyr/sensing/vsensors/emul/emul.h | 12 + tests/subsys/sensing/boards/native_posix.conf | 1 + tests/subsys/sensing/src/test_arbitration.c | 6 +- tests/subsys/sensing/src/test_connections.c | 12 +- 33 files changed, 1021 insertions(+), 68 deletions(-) create mode 100644 dts/bindings/sensing/sensing,accel-based-angle.yaml create mode 100644 dts/bindings/sensing/sensing,emul.yaml create mode 100644 subsys/sensing/sections.ld create mode 100644 subsys/sensing/vsensors/CMakeLists.txt create mode 100644 subsys/sensing/vsensors/Kconfig create mode 100644 subsys/sensing/vsensors/accel-based-angle/CMakeLists.txt create mode 100644 subsys/sensing/vsensors/accel-based-angle/driver.c create mode 100644 subsys/sensing/vsensors/emul/CMakeLists.txt create mode 100644 subsys/sensing/vsensors/emul/driver.c create mode 100644 subsys/sensing/vsensors/emul/include/zephyr/sensing/vsensors/emul/emul.h create mode 100644 tests/subsys/sensing/boards/native_posix.conf diff --git a/cmake/linker_script/common/common-ram.cmake b/cmake/linker_script/common/common-ram.cmake index b625ac9daa58bc..cee07d8d834606 100644 --- a/cmake/linker_script/common/common-ram.cmake +++ b/cmake/linker_script/common/common-ram.cmake @@ -56,6 +56,7 @@ endif() if(CONFIG_SENSING) zephyr_iterable_section(NAME sensing_sensor GROUP DATA_REGION ${XIP_ALIGN_WITH_INPUT} SUBALIGN 4) + zephyr_iterable_section(NAME sensing_connection GROUP DATA_REGION ${XIP_ALIGN_WITH_INPUT} SUBALIGN 4) endif() if(CONFIG_UART_MUX) diff --git a/drivers/sensor/icm42688/icm42688_decoder.c b/drivers/sensor/icm42688/icm42688_decoder.c index 2d10220ecef5de..037281f3e1b193 100644 --- a/drivers/sensor/icm42688/icm42688_decoder.c +++ b/drivers/sensor/icm42688/icm42688_decoder.c @@ -23,16 +23,16 @@ static int icm42688_get_shift(enum sensor_channel channel, int accel_fs, int gyr case SENSOR_CHAN_ACCEL_Z: switch (accel_fs) { case ICM42688_ACCEL_FS_2G: - *shift = 5; + *shift = 4; return 0; case ICM42688_ACCEL_FS_4G: - *shift = 6; + *shift = 5; return 0; case ICM42688_ACCEL_FS_8G: - *shift = 7; + *shift = 6; return 0; case ICM42688_ACCEL_FS_16G: - *shift = 8; + *shift = 7; return 0; default: return -EINVAL; @@ -132,12 +132,9 @@ int icm42688_convert_raw_to_q31(struct icm42688_cfg *cfg, enum sensor_channel ch default: return -ENOTSUP; } + intermediate = ((int64_t)whole * INT64_C(1000000) + fraction); - if (shift < 0) { - intermediate = intermediate * INT32_MAX * (1 << -shift) / INT64_C(1000000); - } else if (shift > 0) { - intermediate = intermediate * INT32_MAX / (((1 << shift) - 1) * INT64_C(1000000)); - } + intermediate = intermediate * ((INT64_C(1) << (31 - shift)) - 1) / INT64_C(1000000); *out = CLAMP(intermediate, INT32_MIN, INT32_MAX); return 0; @@ -191,6 +188,9 @@ static uint8_t icm42688_encode_channel(enum sensor_channel chan) BIT(icm42688_get_channel_position(SENSOR_CHAN_GYRO_Y)) | BIT(icm42688_get_channel_position(SENSOR_CHAN_GYRO_Z)); break; + case SENSOR_CHAN_ALL: + encode_bmask = 0x7f; + break; default: break; } diff --git a/drivers/sensor/icm42688/icm42688_rtio.c b/drivers/sensor/icm42688/icm42688_rtio.c index 1d70aadfd53413..ec19260fd030eb 100644 --- a/drivers/sensor/icm42688/icm42688_rtio.c +++ b/drivers/sensor/icm42688/icm42688_rtio.c @@ -203,7 +203,7 @@ static void icm42688_fifo_count_cb(struct rtio *r, const struct rtio_sqe *sqe, v read_len = pkts * packet_size; ((struct icm42688_fifo_data *)buf)->fifo_count = read_len; - __ASSERT_NO_MSG(read_len % pkt_size == 0); + __ASSERT_NO_MSG(read_len % packet_size == 0); uint8_t *read_buf = buf + sizeof(hdr); diff --git a/dts/bindings/sensing/sensing,accel-based-angle.yaml b/dts/bindings/sensing/sensing,accel-based-angle.yaml new file mode 100644 index 00000000000000..f3e223d3c7ed9e --- /dev/null +++ b/dts/bindings/sensing/sensing,accel-based-angle.yaml @@ -0,0 +1,21 @@ +# Copyright (c) 2023 Google LLC +# SPDX-License-Identifier: Apache-2.0 + +description: Accelerometer based lid angle virtual sensor + +compatible: "sensing,accel-based-angle" + +include: [sensor-device.yaml, base.yaml] + +properties: + plane0: + type: phandle + required: true + description: | + Node owning the sensing_sensor_info for the first plane's accelerometer + + plane1: + type: phandle + required: true + description: | + Node owning the sensing_sensor_info for the second plane's accelerometer diff --git a/dts/bindings/sensing/sensing,emul.yaml b/dts/bindings/sensing/sensing,emul.yaml new file mode 100644 index 00000000000000..de910a2f1c8602 --- /dev/null +++ b/dts/bindings/sensing/sensing,emul.yaml @@ -0,0 +1,39 @@ +# Copyright (c) 2023 Google LLC +# SPDX-License-Identifier: Apache-2.0 + +description: Emulated virtual sensor + +compatible: "sensing,emul" + +include: [sensor-device.yaml, base.yaml] + +properties: + + sensor-types: + type: array + required: true + description: | + One or more sensor data types that are provided by this device. For + example, a 'dev' pointing to a bmi160 may report accel/gyro/die_temp or + any subset of those types. + + rotation-matrix: + type: array + description: | + 3x3 matrix to rotation x, y, and z axes. + In order to make application logic agnostic to how the device was placed + on the board, it's possible to enter the rotation matrix which will be + applied to every sample produced by this sensor. The final orientation + will be: + * X-axis is horizontal and positive toward the right + * Y-axis is vertical and positive toward the top + * Z-axis is depth and positive toward the user + + If not provided, the rotation matrix will be the identity matrix. + Otherwise, the following will be used: + + +- -+ +- -+ +- -+ + | v1 v2 v3 | | sensor_X | = | X | + | v4 v5 v6 | * | sensor_Y | = | Y | + | v7 v8 v9 | | sensor_Z | = | Z | + +- -+ +- -+ +- -+ diff --git a/dts/bindings/vendor-prefixes.txt b/dts/bindings/vendor-prefixes.txt index c70b9b6e0537ae..4c4e25bdb5c808 100644 --- a/dts/bindings/vendor-prefixes.txt +++ b/dts/bindings/vendor-prefixes.txt @@ -531,6 +531,7 @@ segger SEGGER Microcontroller GmbH seeed Seeed Technology Co., Ltd seirobotics Shenzhen SEI Robotics Co., Ltd semtech Semtech Corporation +sensing Zephyr Sensing Subsystem sensirion Sensirion AG sensortek Sensortek Technology Corporation sff Small Form Factor Committee diff --git a/include/zephyr/sensing/datatypes.h b/include/zephyr/sensing/datatypes.h index 3549aab4bdb692..aa9ce1858eeb1d 100644 --- a/include/zephyr/sensing/datatypes.h +++ b/include/zephyr/sensing/datatypes.h @@ -64,18 +64,25 @@ struct sensing_sensor_value_header { * SENSING_SENSOR_TYPE_MOTION_GYROMETER_3D, * q31 version */ -struct sensing_sensor_value_3d_q31 { +struct sensing_sensor_three_axis_data { struct sensing_sensor_value_header header; int8_t shift; struct { uint32_t timestamp_delta; union { + q31_t values[3]; q31_t v[3]; struct { q31_t x; q31_t y; q31_t z; }; + q31_t bias[3]; + struct { + q31_t x_bias; + q31_t y_bias; + q31_t z_bias; + }; }; } readings[1]; }; @@ -98,7 +105,7 @@ struct sensing_sensor_value_uint32 { * struct sensing_sensor_value_q31 can be used by SENSING_SENSOR_TYPE_MOTION_HINGE_ANGLE sensor * q31 version */ -struct sensing_sensor_value_q31 { +struct sensing_sensor_float_data { int8_t shift; struct sensing_sensor_value_header header; struct { diff --git a/include/zephyr/sensing/sensing.h b/include/zephyr/sensing/sensing.h index f1791edb250c26..c8c296ba2dbbf6 100644 --- a/include/zephyr/sensing/sensing.h +++ b/include/zephyr/sensing/sensing.h @@ -166,7 +166,7 @@ typedef void *sensing_sensor_handle_t; * * @param buf The data buffer with sensor data. */ -typedef void (*sensing_data_event_t)(sensing_sensor_handle_t handle, const void *buf); +typedef void (*sensing_data_event_t)(sensing_sensor_handle_t handle, const void *buf, void *userdata); #include /** @@ -232,7 +232,8 @@ int sensing_get_sensors(int *num_sensors, const struct sensing_sensor_info **inf */ int sensing_open_sensor(const struct sensing_sensor_info *info, const struct sensing_callback_list *cb_list, - sensing_sensor_handle_t *handle); + sensing_sensor_handle_t *handle, + void *userdata); /** * @brief Close sensor instance. diff --git a/include/zephyr/sensing/sensor.h b/include/zephyr/sensing/sensor.h index f9403961fcf3a9..ced2455e339a7b 100644 --- a/include/zephyr/sensing/sensor.h +++ b/include/zephyr/sensing/sensor.h @@ -11,18 +11,36 @@ #include #include +struct sensing_connection { + const struct sensing_sensor_info *info; + const struct sensing_callback_list *cb_list; + void *userdata; + enum sensing_sensor_mode mode; + q31_t attributes[SENSOR_ATTR_COMMON_COUNT]; + uint32_t attribute_mask; +} __packed __aligned(4); + +#define SENSING_CONNECTION_DT_DEFINE(node_id, target_node_id, type, _cb_list) \ + SENSING_DMEM STRUCT_SECTION_ITERABLE(sensing_connection, node_id##_sensing_connection) = { \ + .info = SENSING_SENSOR_INFO_GET(target_node_id, type), \ + .cb_list = (_cb_list), \ + } + +STRUCT_SECTION_START_EXTERN(sensing_connection); +STRUCT_SECTION_END_EXTERN(sensing_connection); + #define SENSING_SENSOR_INFO_DT_NAME(node_id, type) \ _CONCAT(_CONCAT(__sensing_sensor_info_, DEVICE_DT_NAME_GET(node_id)), type) #define SENSING_SENSOR_INFO_INST_DEFINE_NAMED(node_id, name, prop, idx, _iodev) \ - IF_ENABLED(CONFIG_SENSING_SHELL, (static char node_id##_##idx##_name_buffer[5];)) \ + IF_ENABLED(CONFIG_SENSING_SHELL, (static char node_id##_##idx##_name_buffer[5];)) \ const STRUCT_SECTION_ITERABLE(sensing_sensor_info, name) = { \ .info = &SENSOR_INFO_DT_NAME(DT_PHANDLE(node_id, dev)), \ .dev = DEVICE_DT_GET(node_id), \ .type = DT_PROP_BY_IDX(node_id, prop, idx), \ .iodev = &(_iodev), \ - IF_ENABLED(CONFIG_SENSING_SHELL, (.shell_name = node_id##_##idx##_name_buffer, )) \ - }; + IF_ENABLED(CONFIG_SENSING_SHELL, \ + (.shell_name = node_id##_##idx##_name_buffer, ))}; #define SENSING_SENSOR_INFO_INST_DEFINE(node_id, prop, idx, _iodev) \ SENSING_SENSOR_INFO_INST_DEFINE_NAMED( \ diff --git a/samples/sensor/sensor_shell/boards/tdk_robokit1.conf b/samples/sensor/sensor_shell/boards/tdk_robokit1.conf index aee20b8b10049c..0d027a28490b74 100644 --- a/samples/sensor/sensor_shell/boards/tdk_robokit1.conf +++ b/samples/sensor/sensor_shell/boards/tdk_robokit1.conf @@ -1,3 +1,4 @@ # Copyright (c) 2023 Google LLC # SPDX-License-Identifier: Apache-2.0 CONFIG_SPI_RTIO=y +CONFIG_CMSIS_DSP=y diff --git a/samples/sensor/sensor_shell/boards/tdk_robokit1.overlay b/samples/sensor/sensor_shell/boards/tdk_robokit1.overlay index 45841a4c1fc40d..b7a4319004066d 100644 --- a/samples/sensor/sensor_shell/boards/tdk_robokit1.overlay +++ b/samples/sensor/sensor_shell/boards/tdk_robokit1.overlay @@ -5,10 +5,25 @@ #include / { - accelgyro: accelgyro { + lidaccel: accelgyro { compatible = "zephyr,sensing-pipe"; status = "okay"; + friendly-name = "Lid accel"; dev = <&icm42688>; sensor-types = ; }; + + baseaccel: accelemul { + compatible = "sensing,emul"; + status = "okay"; + friendly-name = "Base accel"; + sensor-types = ; + }; + + angle: angle { + compatible = "sensing,accel-based-angle"; + status = "okay"; + plane0 = <&lidaccel>; + plane1 = <&baseaccel>; + }; }; diff --git a/samples/sensor/sensor_shell/prj.conf b/samples/sensor/sensor_shell/prj.conf index 0e6ac047463ce6..8a1b8c78ae234d 100644 --- a/samples/sensor/sensor_shell/prj.conf +++ b/samples/sensor/sensor_shell/prj.conf @@ -7,9 +7,9 @@ CONFIG_SENSOR_INFO=y CONFIG_LOG=y CONFIG_RTIO_CONSUME_SEM=y -CONFIG_USERSPACE=y +# CONFIG_USERSPACE=y CONFIG_SENSING=y CONFIG_SENSING_SHELL=y -CONFIG_RTIO_LOG_LEVEL_DBG=y -CONFIG_SENSOR_LOG_LEVEL_DBG=y -CONFIG_SENSING_LOG_LEVEL_DBG=y +# CONFIG_RTIO_LOG_LEVEL_DBG=y +# CONFIG_SENSOR_LOG_LEVEL_DBG=y +# CONFIG_SENSING_LOG_LEVEL_DBG=y diff --git a/scripts/build/gen_kobject_list.py b/scripts/build/gen_kobject_list.py index 64524c67499d52..92d7aa75d705da 100755 --- a/scripts/build/gen_kobject_list.py +++ b/scripts/build/gen_kobject_list.py @@ -114,7 +114,8 @@ ("ztest_test_rule", ("CONFIG_ZTEST_NEW_API", True, False)), ("rtio", ("CONFIG_RTIO", False, False)), ("rtio_iodev", ("CONFIG_RTIO", False, False)), - ("sensor_decoder_api", ("CONFIG_SENSOR_ASYNC_API", True, False)) + ("sensor_decoder_api", ("CONFIG_SENSOR_ASYNC_API", True, False)), + ("sensing_connection", ("CONFIG_SENSING", True, False)) ]) def kobject_to_enum(kobj): diff --git a/subsys/sensing/CMakeLists.txt b/subsys/sensing/CMakeLists.txt index 1d19a3c49e4185..3c4541c22e370d 100644 --- a/subsys/sensing/CMakeLists.txt +++ b/subsys/sensing/CMakeLists.txt @@ -12,3 +12,6 @@ zephyr_library_sources_ifdef(CONFIG_SENSING_SHELL src/shell.c) zephyr_library_sources_ifdef(CONFIG_USERSPACE src/userspace.c) zephyr_library_include_directories(include) +add_subdirectory(vsensors) + +zephyr_linker_sources(DATA_SECTIONS sections.ld) diff --git a/subsys/sensing/Kconfig b/subsys/sensing/Kconfig index c73ac89183e014..2b5232108d78eb 100644 --- a/subsys/sensing/Kconfig +++ b/subsys/sensing/Kconfig @@ -16,7 +16,9 @@ module = SENSING module-str = sensing source "subsys/logging/Kconfig.template.log_config" -config SENSING_MAX_CONNECTIONS +source "subsys/sensing/vsensors/Kconfig" + +config SENSING_MAX_DYNAMIC_CONNECTIONS int "Maximum number of simultaneous open connections" default 10 help diff --git a/subsys/sensing/include/sensing/internal/sensing.h b/subsys/sensing/include/sensing/internal/sensing.h index 15a5ed9177d8e2..a3cf674bb9832a 100644 --- a/subsys/sensing/include/sensing/internal/sensing.h +++ b/subsys/sensing/include/sensing/internal/sensing.h @@ -6,23 +6,18 @@ #define ZEPHYR_SUBSYS_SENSING_INCLUDE_SENSING_INTERNAL_SENSING_H #include +#include #include #include #include -#define __SENSING_POOL_MASK_BUNDLE_COUNT \ - (DIV_ROUND_UP(DIV_ROUND_UP(CONFIG_SENSING_MAX_CONNECTIONS, 8), sizeof(uint32_t))) +#define __CONNECTION_POOL_COUNT \ + (STRUCT_SECTION_END(sensing_connection) - STRUCT_SECTION_START(sensing_connection)) -struct sensing_connection { - const struct sensing_sensor_info *info; - const struct sensing_callback_list *cb_list; - enum sensing_sensor_mode mode; - q31_t attributes[SENSOR_ATTR_COMMON_COUNT]; - uint32_t attribute_mask; -} __packed __aligned(4); +#define __SENSING_POOL_MASK_BUNDLE_COUNT \ + (DIV_ROUND_UP(DIV_ROUND_UP(__CONNECTION_POOL_COUNT, 8), sizeof(uint32_t))) extern struct sensing_connection_pool { - struct sensing_connection pool[CONFIG_SENSING_MAX_CONNECTIONS]; sys_bitarray_t *bitarray; struct sys_mutex *lock; } __sensing_connection_pool; @@ -35,7 +30,7 @@ static inline bool __sensing_is_connected(const struct sensing_sensor_info *info const struct sensing_connection *connection) { int is_set; - int connection_index = connection - __sensing_connection_pool.pool; + int connection_index = connection - STRUCT_SECTION_START(sensing_connection); int rc = sys_bitarray_test_bit(__sensing_connection_pool.bitarray, connection_index, &is_set); diff --git a/subsys/sensing/sections.ld b/subsys/sensing/sections.ld new file mode 100644 index 00000000000000..42bf0dd86957da --- /dev/null +++ b/subsys/sensing/sections.ld @@ -0,0 +1,3 @@ +#if defined(CONFIG_SENSING) + ITERABLE_SECTION_RAM(sensing_connection, 4) +#endif /* CONFIG_SENSING */ diff --git a/subsys/sensing/src/processing.c b/subsys/sensing/src/processing.c index ab001ae501219c..6f04399b82ca2b 100644 --- a/subsys/sensing/src/processing.c +++ b/subsys/sensing/src/processing.c @@ -12,6 +12,103 @@ LOG_MODULE_REGISTER(sensing_processing, CONFIG_SENSING_LOG_LEVEL); +static int decode_three_axis_data(int32_t type, struct sensing_sensor_three_axis_data *out, + const struct sensor_decoder_api *decoder, void *data) +{ + uint16_t frame_count; + int rc; + + rc = decoder->get_frame_count(data, &frame_count); + if (rc != 0) { + return rc; + } + + __ASSERT_NO_MSG(frame_count == 1); + LOG_DBG("Decoding 1 frame for 3 axis data from %p", data); + + sensor_frame_iterator_t fit = {0}; + sensor_channel_iterator_t cit = {0}; + enum sensor_channel channel; + q31_t value; + bool has_shift = false; + + while (true) { + rc = decoder->decode(data, &fit, &cit, &channel, &value, 1); + if (rc < 0) { + LOG_ERR("Failed to decode entry (%d)", rc); + return rc; + } + if (rc == 0) { + return 0; + } + switch (type) { + case SENSING_SENSOR_TYPE_MOTION_ACCELEROMETER_3D: + case SENSING_SENSOR_TYPE_MOTION_UNCALIB_ACCELEROMETER_3D: + if (channel == SENSOR_CHAN_ACCEL_X || channel == SENSOR_CHAN_ACCEL_Y || + channel == SENSOR_CHAN_ACCEL_Z) { + LOG_DBG("Got [%d] for accel type, value=0x%08x", (int)channel, value); + out->readings[0].values[channel] = value; + if (!has_shift) { + rc = decoder->get_shift(data, channel, &out->shift); + if (rc != 0) { + return rc; + } + has_shift = true; + LOG_DBG("Got shift value %d", out->shift); + } + } + break; + case SENSING_SENSOR_TYPE_MOTION_GYROMETER_3D: + if (channel == SENSOR_CHAN_GYRO_X || channel == SENSOR_CHAN_GYRO_Y || + channel == SENSOR_CHAN_GYRO_Z) { + out->readings[0].values[channel - SENSOR_CHAN_GYRO_X] = value; + if (!has_shift) { + rc = decoder->get_shift(data, channel, &out->shift); + if (rc != 0) { + return rc; + } + has_shift = true; + } + } + break; + } + } +} + +static int decode_float_data(int32_t type, struct sensing_sensor_float_data *out, + const struct sensor_decoder_api *decoder, void *data) +{ + uint16_t frame_count; + int rc; + + rc = decoder->get_frame_count(data, &frame_count); + if (rc != 0) { + return rc; + } + + __ASSERT_NO_MSG(frame_count == 1); + + sensor_frame_iterator_t fit = {0}; + sensor_channel_iterator_t cit = {0}; + enum sensor_channel channel; + bool has_shift = false; + + while (true) { + rc = decoder->decode(data, &fit, &cit, &channel, &out->readings[0].v, 1); + if (rc <= 0) { + return rc; + } + if (type == SENSING_SENSOR_TYPE_MOTION_HINGE_ANGLE && + channel == SENSOR_CHAN_ROTATION && !has_shift) { + rc = decoder->get_shift(data, channel, &out->shift); + if (rc != 0) { + return rc; + } + has_shift = true; + } + } +} + static void processing_task(void *a, void *b, void *c) { struct sensing_sensor_info *info; @@ -53,9 +150,46 @@ static void processing_task(void *a, void *b, void *c) continue; } + const struct sensor_decoder_api *decoder = NULL; + + rc = sensor_get_decoder(info->dev, &decoder); + if (rc != 0) { + LOG_ERR("Failed to get decoder"); + goto end; + } + + union { + struct sensing_sensor_three_axis_data three_axis_data; + struct sensing_sensor_float_data float_data; + } decoded_data; + + switch (info->type) { + case SENSING_SENSOR_TYPE_MOTION_ACCELEROMETER_3D: + case SENSING_SENSOR_TYPE_MOTION_UNCALIB_ACCELEROMETER_3D: + case SENSING_SENSOR_TYPE_MOTION_GYROMETER_3D: + rc = decode_three_axis_data(info->type, &decoded_data.three_axis_data, + decoder, data); + if (rc != 0) { + LOG_ERR("Failed to decode"); + goto end; + } + break; + case SENSING_SENSOR_TYPE_MOTION_HINGE_ANGLE: + rc = decode_float_data(info->type, &decoded_data.float_data, decoder, data); + if (rc != 0) { + LOG_ERR("Failed to decode"); + goto end; + } + break; + default: + LOG_ERR("Sensor type not supported"); + goto end; + } + sys_mutex_lock(__sensing_connection_pool.lock, K_FOREVER); - for (int i = 0; i < ARRAY_SIZE(__sensing_connection_pool.pool); ++i) { - struct sensing_connection *connection = &__sensing_connection_pool.pool[i]; + for (int i = 0; i < __CONNECTION_POOL_COUNT; ++i) { + struct sensing_connection *connection = + &STRUCT_SECTION_START(sensing_connection)[i]; if (!__sensing_is_connected(info, connection)) { continue; } @@ -64,11 +198,11 @@ static void processing_task(void *a, void *b, void *c) if (cb == NULL) { continue; } - /* TODO we actually need to decode the data first before this for loop */ - cb(connection, data); + cb(connection, &decoded_data, connection->userdata); } sys_mutex_unlock(__sensing_connection_pool.lock); + end: /* Release the memory */ rtio_release_buffer(&sensing_rtio_ctx, data, data_len); } diff --git a/subsys/sensing/src/sensor_arbitrate.c b/subsys/sensing/src/sensor_arbitrate.c index fae5276bef1e66..630d918e10fd5f 100644 --- a/subsys/sensing/src/sensor_arbitrate.c +++ b/subsys/sensing/src/sensor_arbitrate.c @@ -47,11 +47,11 @@ static void set_arbitrated_value(const struct device *dev, int32_t type, static int arbitrate_sensor_attribute(const struct sensing_sensor_info *info, enum sensor_attribute attribute) { - const struct sensing_connection *connections = __sensing_connection_pool.pool; + const struct sensing_connection *connections = STRUCT_SECTION_START(sensing_connection); int connection_count = 0; - q31_t value; + q31_t value = 0; - for (int i = 0; i < CONFIG_SENSING_MAX_CONNECTIONS; ++i) { + for (int i = 0; i < __CONNECTION_POOL_COUNT; ++i) { if (!__sensing_is_connected(info, &connections[i])) { continue; } diff --git a/subsys/sensing/src/sensor_connections.c b/subsys/sensing/src/sensor_connections.c index 71134e8581f699..9fdb2b5f56da22 100644 --- a/subsys/sensing/src/sensor_connections.c +++ b/subsys/sensing/src/sensor_connections.c @@ -12,16 +12,15 @@ LOG_MODULE_REGISTER(sensing_connect, CONFIG_SENSING_LOG_LEVEL); #define __HANDLE_TO_CONNECTION(name, handle) \ struct sensing_connection *name = handle; \ - __ASSERT_NO_MSG((uintptr_t)name >= (uintptr_t)__sensing_connection_pool.pool); \ - __ASSERT_NO_MSG((uintptr_t)name < (uintptr_t)__sensing_connection_pool.pool + \ - sizeof(__sensing_connection_pool.pool)) + __ASSERT_NO_MSG((uintptr_t)name >= (uintptr_t)STRUCT_SECTION_START(sensing_connection)); \ + __ASSERT_NO_MSG((uintptr_t)name < (uintptr_t)STRUCT_SECTION_END(sensing_connection)) SENSING_DMEM SYS_MUTEX_DEFINE(connection_lock); -SENSING_DMEM static uint32_t bitarray_bundles[__SENSING_POOL_MASK_BUNDLE_COUNT] = {0}; +SENSING_DMEM static uint32_t bitarray_bundles[4] = {0}; SENSING_DMEM static sys_bitarray_t bitarray = { - .num_bits = CONFIG_SENSING_MAX_CONNECTIONS, - .num_bundles = __SENSING_POOL_MASK_BUNDLE_COUNT, + .num_bits = ARRAY_SIZE(bitarray_bundles) * 32, + .num_bundles = ARRAY_SIZE(bitarray_bundles), .bundles = bitarray_bundles, }; @@ -30,16 +29,18 @@ SENSING_DMEM struct sensing_connection_pool __sensing_connection_pool = { .lock = &connection_lock, }; +STRUCT_SECTION_ITERABLE_ARRAY(sensing_connection, dynamic_connections, + CONFIG_SENSING_MAX_DYNAMIC_CONNECTIONS); + #define __lock sys_mutex_lock(__sensing_connection_pool.lock, K_FOREVER) #define __unlock sys_mutex_unlock(__sensing_connection_pool.lock) -RTIO_DEFINE_WITH_MEMPOOL(sensing_rtio_ctx, CONFIG_SENSING_MAX_CONNECTIONS, - CONFIG_SENSING_MAX_CONNECTIONS, CONFIG_SENSING_RTIO_BLOCK_COUNT, +RTIO_DEFINE_WITH_MEMPOOL(sensing_rtio_ctx, 32, 32, CONFIG_SENSING_RTIO_BLOCK_COUNT, CONFIG_SENSING_RTIO_BLOCK_SIZE, 4); int sensing_open_sensor(const struct sensing_sensor_info *info, const struct sensing_callback_list *cb_list, - sensing_sensor_handle_t *handle) + sensing_sensor_handle_t *handle, void *userdata) { struct sensing_connection *connection; size_t offset; @@ -50,19 +51,22 @@ int sensing_open_sensor(const struct sensing_sensor_info *info, __ASSERT_NO_MSG(handle != NULL); __lock; + bitarray.num_bits = __CONNECTION_POOL_COUNT; // TODO this should only happen once rc = sys_bitarray_alloc(__sensing_connection_pool.bitarray, 1, &offset); if (rc != 0) { __unlock; return rc; } - connection = &__sensing_connection_pool.pool[offset]; + LOG_DBG("Got offset %d/%d", (int)offset, (int)__CONNECTION_POOL_COUNT); + connection = &STRUCT_SECTION_START(sensing_connection)[offset]; LOG_DBG("Connection opened @ %p (size=%d) for info @ %p", connection, (int)sizeof(struct sensing_connection), info); memset(connection, 0, sizeof(struct sensing_connection)); connection->info = info; connection->cb_list = cb_list; + connection->userdata = userdata; *handle = connection; __unlock; return 0; @@ -76,9 +80,9 @@ int sensing_close_sensor(sensing_sensor_handle_t handle) __lock; if (__sensing_is_connected(NULL, connection)) { LOG_DBG("Releasing connection at %p/%d", handle, - connection - __sensing_connection_pool.pool); + connection - STRUCT_SECTION_START(sensing_connection)); rc = sys_bitarray_free(__sensing_connection_pool.bitarray, 1, - connection - __sensing_connection_pool.pool); + connection - STRUCT_SECTION_START(sensing_connection)); if (rc == 0) { __sensing_arbitrate(); } else { @@ -116,6 +120,9 @@ int sensing_set_attributes(sensing_sensor_handle_t handle, enum sensing_sensor_m rc = sensor_read(connection->info->iodev, &sensing_rtio_ctx, (void *)connection->info); break; + case SENSING_SENSOR_MODE_DONE: + rc = 0; + break; default: rc = -EINVAL; break; @@ -136,7 +143,9 @@ void sensing_reset_connections(void) __lock; sys_bitarray_clear_region(__sensing_connection_pool.bitarray, __sensing_connection_pool.bitarray->num_bits, 0); - memset(__sensing_connection_pool.pool, 0, sizeof(__sensing_connection_pool.pool)); + memset(STRUCT_SECTION_START(sensing_connection), 0, + (uintptr_t)STRUCT_SECTION_END(sensing_connection) - + (uintptr_t)STRUCT_SECTION_START(sensing_connection)); __sensing_arbitrate(); __unlock; } diff --git a/subsys/sensing/src/sensor_pipe.c b/subsys/sensing/src/sensor_pipe.c index c3b264bfbb0922..5977066a251722 100644 --- a/subsys/sensing/src/sensor_pipe.c +++ b/subsys/sensing/src/sensor_pipe.c @@ -51,10 +51,17 @@ static int submit(const struct device *sensor, struct rtio_iodev_sqe *sqe) return rc; } +static int get_decoder(const struct device *sensor, const struct sensor_decoder_api **api) +{ + const struct sensor_pipe_config *cfg = sensor->config; + + return sensor_get_decoder(cfg->parent_info->dev, api); +} + SENSING_DMEM static const struct sensor_driver_api sensor_pipe_api = { .attr_set = attribute_set, .attr_get = NULL, - .get_decoder = NULL, + .get_decoder = get_decoder, .submit = submit, }; diff --git a/subsys/sensing/src/shell.c b/subsys/sensing/src/shell.c index 8070abfcf0d9c1..6c7f08d09d79a5 100644 --- a/subsys/sensing/src/shell.c +++ b/subsys/sensing/src/shell.c @@ -76,6 +76,7 @@ static const char *get_sensor_type_string(int32_t type) switch (type) { SENSOR_TYPE_TO_STRING(SENSING_SENSOR_TYPE_MOTION_ACCELEROMETER_3D); SENSOR_TYPE_TO_STRING(SENSING_SENSOR_TYPE_MOTION_GYROMETER_3D); + SENSOR_TYPE_TO_STRING(SENSING_SENSOR_TYPE_MOTION_HINGE_ANGLE); default: return "UNKNOWN"; } @@ -125,14 +126,92 @@ struct shell_cmd_connection { sensing_sensor_handle_t handle; bool is_used; char shell_name[5]; + const struct shell *owning_shell; }; -static struct shell_cmd_connection open_connections[CONFIG_SENSING_MAX_CONNECTIONS]; +static struct shell_cmd_connection open_connections[CONFIG_SENSING_MAX_DYNAMIC_CONNECTIONS]; -static void sensing_shell_on_data_event(sensing_sensor_handle_t handle, const void *data) +static void sensing_shell_print_three_axis_data(const struct shell *sh, + const struct sensing_sensor_info *info, + const struct sensing_sensor_three_axis_data *data) { + int64_t intermediate[] = { + (int64_t)data->readings[0].values[0], + (int64_t)data->readings[0].values[1], + (int64_t)data->readings[0].values[2], + }; + +// shell_info(sh, +// "%s: shift=%d, mask=0x%016" PRIx64 " (" +// "0x%08" PRIx32 "/0x%016" PRIx64 ", " +// "0x%08" PRIx32 "/0x%016" PRIx64 ", " +// "0x%08" PRIx32 "/0x%016" PRIx64 ")", +// get_sensor_type_string(info->type), data->shift, GENMASK64(63, 31), +// data->readings[0].values[0], intermediate[0], data->readings[0].values[1], +// intermediate[1], data->readings[0].values[2], intermediate[2]); + + intermediate[0] = (data->shift >= 0) ? (intermediate[0] << data->shift) + : (intermediate[0] >> -data->shift); + intermediate[1] = (data->shift >= 0) ? (intermediate[1] << data->shift) + : (intermediate[1] >> -data->shift); + intermediate[2] = (data->shift >= 0) ? (intermediate[2] << data->shift) + : (intermediate[2] >> -data->shift); + // shell_info(sh, + // "%s: %" PRIi32 ".%06" PRIu32 ", %" PRIi32 ".%06" PRIu32 ", %" PRIi32 + // ".%06" PRIu32, + // get_sensor_type_string(info->type), + // (int32_t)FIELD_GET(GENMASK64(63, 31), intermediate[0]), + // (uint32_t)((uint64_t)FIELD_GET(GENMASK64(30, 0), intermediate[0]) * + // INT64_C(1000000) / INT32_MAX), + // (int32_t)FIELD_GET(GENMASK64(63, 31), intermediate[1]), + // (uint32_t)((uint64_t)FIELD_GET(GENMASK64(30, 0), intermediate[1]) * + // INT64_C(1000000) / INT32_MAX), + // (int32_t)FIELD_GET(GENMASK64(63, 31), intermediate[2]), + // (uint32_t)((uint64_t)FIELD_GET(GENMASK64(30, 0), intermediate[2]) * + // INT64_C(1000000) / INT32_MAX)); + shell_info(sh, + "%s %" PRIi32 ".%06" PRIu32 ", %" PRIi32 ".%06" PRIu32 ", %" PRIi32 + ".%06" PRIu32, + get_sensor_type_string(info->type), (int32_t)(intermediate[0] / INT32_MAX), + (uint32_t)((intermediate[0] % INT32_MAX) * INT64_C(1000000) / INT32_MAX), + (int32_t)(intermediate[1] / INT32_MAX), + (uint32_t)((intermediate[1] % INT32_MAX) * INT64_C(1000000) / INT32_MAX), + (int32_t)(intermediate[2] / INT32_MAX), + (uint32_t)((intermediate[2] % INT32_MAX) * INT64_C(1000000) / INT32_MAX)); +} + +static void sensing_shell_print_float_data(const struct shell *sh, + const struct sensing_sensor_info *info, + const struct sensing_sensor_float_data *data) +{ + int64_t intermediate = (int64_t)data->readings[0].v; + + intermediate = + (data->shift >= 0) ? (intermediate << data->shift) : (intermediate >> -data->shift); + + shell_info(sh, "%s: %" PRIi32 ".%06" PRIu32, get_sensor_type_string(info->type), + (int32_t)(intermediate / INT32_MAX), + (uint32_t)((intermediate % INT32_MAX) * INT64_C(1000000) / INT32_MAX)); +} + +static void sensing_shell_on_data_event(sensing_sensor_handle_t handle, const void *data, + void *userdata) +{ + struct shell_cmd_connection *connection = userdata; const struct sensing_sensor_info *info = sensing_get_sensor_info(handle); - printk("Got data for '%s' at %p\n", get_sensor_type_string(info->type), data); + switch (info->type) { + case SENSING_SENSOR_TYPE_MOTION_ACCELEROMETER_3D: + case SENSING_SENSOR_TYPE_MOTION_UNCALIB_ACCELEROMETER_3D: + case SENSING_SENSOR_TYPE_MOTION_GYROMETER_3D: + sensing_shell_print_three_axis_data(connection->owning_shell, info, data); + break; + case SENSING_SENSOR_TYPE_MOTION_HINGE_ANGLE: + sensing_shell_print_float_data(connection->owning_shell, info, data); + break; + default: + shell_info(connection->owning_shell, "Got data for '%s' at %p", + get_sensor_type_string(info->type), data); + } } static const struct sensing_callback_list callback_list = { @@ -189,7 +268,8 @@ static int cmd_open_connection(const struct shell *sh, size_t argc, char **argv) } rc = sensing_open_sensor(&sensors[sensor_index], &callback_list, - &open_connections[connection_idx].handle); + &open_connections[connection_idx].handle, + &open_connections[connection_idx]); if (rc != 0) { shell_error(sh, "Failed to open connection"); @@ -197,6 +277,7 @@ static int cmd_open_connection(const struct shell *sh, size_t argc, char **argv) } open_connections[connection_idx].is_used = true; + open_connections[connection_idx].owning_shell = sh; shell_print(sh, "New connection [%d] to sensor [%ld] created", connection_idx, sensor_index); diff --git a/subsys/sensing/src/userspace.c b/subsys/sensing/src/userspace.c index 486814808093b1..44b1b4f3de1606 100644 --- a/subsys/sensing/src/userspace.c +++ b/subsys/sensing/src/userspace.c @@ -7,6 +7,8 @@ #include #include +#include "sensing/internal/sensing.h" + LOG_MODULE_REGISTER(sensing_userspace, CONFIG_SENSING_LOG_LEVEL); K_APPMEM_PARTITION_DEFINE(sensing_mem_partition); @@ -15,6 +17,11 @@ static int sensing_mem_init(void) { int rc; + for (struct sensing_connection *ptr = STRUCT_SECTION_START(sensing_connection); + ptr < STRUCT_SECTION_END(sensing_connection); ++ptr) { + k_object_access_all_grant(ptr); + } + rc = k_mem_domain_add_partition(&k_mem_domain_default, &sensing_mem_partition); if (rc != 0) { LOG_ERR("Failed to add sensing memory partition to the default domain"); diff --git a/subsys/sensing/vsensors/CMakeLists.txt b/subsys/sensing/vsensors/CMakeLists.txt new file mode 100644 index 00000000000000..177f66e9f85d2d --- /dev/null +++ b/subsys/sensing/vsensors/CMakeLists.txt @@ -0,0 +1,2 @@ +add_subdirectory_ifdef(CONFIG_SENSING_ACCEL_BASED_ANGLE accel-based-angle) +add_subdirectory_ifdef(CONFIG_SENSING_EMUL emul) diff --git a/subsys/sensing/vsensors/Kconfig b/subsys/sensing/vsensors/Kconfig new file mode 100644 index 00000000000000..e3284fb8bcbdb5 --- /dev/null +++ b/subsys/sensing/vsensors/Kconfig @@ -0,0 +1,10 @@ +config SENSING_ACCEL_BASED_ANGLE + bool + default y + depends on DT_HAS_SENSING_ACCEL_BASED_ANGLE_ENABLED + select DSP + +config SENSING_EMUL + bool + default y + depends on DT_HAS_SENSING_EMUL_ENABLED diff --git a/subsys/sensing/vsensors/accel-based-angle/CMakeLists.txt b/subsys/sensing/vsensors/accel-based-angle/CMakeLists.txt new file mode 100644 index 00000000000000..95cef32bf7b5de --- /dev/null +++ b/subsys/sensing/vsensors/accel-based-angle/CMakeLists.txt @@ -0,0 +1,2 @@ +zephyr_library() +zephyr_library_sources(driver.c) diff --git a/subsys/sensing/vsensors/accel-based-angle/driver.c b/subsys/sensing/vsensors/accel-based-angle/driver.c new file mode 100644 index 00000000000000..d91709618e29f2 --- /dev/null +++ b/subsys/sensing/vsensors/accel-based-angle/driver.c @@ -0,0 +1,321 @@ + +#include +#include +#include +#include +#include +#include +#include +#include + +LOG_MODULE_REGISTER(accel_based_angle, CONFIG_SENSING_LOG_LEVEL); + +#define DT_DRV_COMPAT sensing_accel_based_angle +struct drv_config { + const struct sensing_sensor_info *plane0; + const struct sensing_sensor_info *plane1; +}; + +struct drv_data { + sensing_sensor_handle_t plane0; + sensing_sensor_handle_t plane1; + struct sensing_sensor_three_axis_data plane0_latest_sample; + struct sensing_sensor_three_axis_data plane1_latest_sample; + bool plane0_has_sample; + bool plane1_has_sample; + + struct rtio_iodev_sqe *pending_read; +}; + +static int attribute_set(const struct device *dev, enum sensor_channel chan, + enum sensor_attribute attr, const struct sensor_value *val) +{ + struct drv_data *data = dev->data; + struct sensing_sensor_attribute attribute = { + .attribute = attr, + .value = 0, + .shift = 0, + }; + int rc; + + rc = sensing_set_attributes(data->plane0, SENSING_SENSOR_MODE_DONE, &attribute, 1); + if (rc != 0) { + LOG_ERR("Failed to set plane0 attribute"); + return rc; + } + rc = sensing_set_attributes(data->plane1, SENSING_SENSOR_MODE_DONE, &attribute, 1); + if (rc != 0) { + LOG_ERR("Failed to set plane1 attribute"); + return rc; + } + return rc; +} + +static int submit(const struct device *sensor, struct rtio_iodev_sqe *sqe) +{ + struct drv_data *data = sensor->data; + const struct sensor_read_config *read_cfg = sqe->sqe.iodev->data; + int rc; + + if (read_cfg->is_streaming) { + LOG_ERR("Streaming is not yet supported"); + rtio_iodev_sqe_err(sqe, -ENOTSUP); + return -ENOTSUP; + } + + rc = sensing_set_attributes(data->plane0, SENSING_SENSOR_MODE_ONE_SHOT, NULL, 0); + rc |= sensing_set_attributes(data->plane1, SENSING_SENSOR_MODE_ONE_SHOT, NULL, 0); + + if (rc != 0) { + rtio_iodev_sqe_err(sqe, rc); + LOG_ERR("Failed to initiate read"); + } else { + data->pending_read = sqe; + } + return 0; +} + +struct encoded_data { + uint64_t timestamp_ns; + int8_t shift; + q31_t val; +} __packed; + +static int decoder_get_frame_count(const uint8_t *buffer, uint16_t *frame_count) +{ + ARG_UNUSED(buffer); + *frame_count = 1; + return 0; +} + +static int decoder_get_timestamp(const uint8_t *buffer, uint64_t *timestamp_ns) +{ + *timestamp_ns = ((struct encoded_data *)buffer)->timestamp_ns; + return 0; +} + +static bool decoder_has_trigger(const uint8_t *buffer, enum sensor_trigger_type trigger) +{ + ARG_UNUSED(buffer); + ARG_UNUSED(trigger); + return false; +} + +static int decoder_get_shift(const uint8_t *buffer, enum sensor_channel channel_type, int8_t *shift) +{ + ARG_UNUSED(buffer); + + if (channel_type != SENSOR_CHAN_ROTATION) { + return -EINVAL; + } + *shift = ((struct encoded_data *)buffer)->shift; + return 0; +} + +static int decoder_decode(const uint8_t *buffer, sensor_frame_iterator_t *fit, + sensor_channel_iterator_t *cit, enum sensor_channel *channels, + q31_t *values, uint8_t max_count) +{ + if (*fit != 0) { + return 0; + } + if (*cit > 0) { + return -EINVAL; + } + + values[0] = ((struct encoded_data *)buffer)->val; + channels[0] = SENSOR_CHAN_ROTATION; + *fit = 1; + *cit = 0; + return 1; +} + +SENSING_DMEM static const struct sensor_decoder_api decoder_api = { + .get_frame_count = decoder_get_frame_count, + .get_timestamp = decoder_get_timestamp, + .has_trigger = decoder_has_trigger, + .get_shift = decoder_get_shift, + .decode = decoder_decode, +}; + +static int get_decoder(const struct device *dev, const struct sensor_decoder_api **api) +{ + ARG_UNUSED(dev); + *api = &decoder_api; + return 0; +} + +SENSING_DMEM static const struct sensor_driver_api angle_api = { + .attr_set = attribute_set, + .attr_get = NULL, + .get_decoder = get_decoder, + .submit = submit, +}; + +static void on_data_event(sensing_sensor_handle_t handle, const void *buf, void *userdata) +{ + const struct device *dev = userdata; + struct drv_data *data = dev->data; + + if (handle == data->plane0) { + LOG_INF("Got data for plane0"); + memcpy(&data->plane0_latest_sample, buf, + sizeof(struct sensing_sensor_three_axis_data)); + data->plane0_has_sample = true; + } else if (handle == data->plane1) { + LOG_INF("Got data for plane1"); + memcpy(&data->plane1_latest_sample, buf, + sizeof(struct sensing_sensor_three_axis_data)); + data->plane1_has_sample = true; + } + + if (!data->plane0_has_sample || !data->plane1_has_sample) { + return; + } + + /* We have 2 samples */ + LOG_DBG("plane0(0x%08" PRIx32 ", 0x%08" PRIx32 ", 0x%08" PRIx32 ")", + data->plane0_latest_sample.readings[0].x, data->plane0_latest_sample.readings[0].y, + data->plane0_latest_sample.readings[0].z); + LOG_DBG("plane1(0x%08" PRIx32 ", 0x%08" PRIx32 ", 0x%08" PRIx32 ")", + data->plane1_latest_sample.readings[0].x, data->plane1_latest_sample.readings[0].y, + data->plane1_latest_sample.readings[0].z); + int8_t shift = MAX(data->plane0_latest_sample.shift, data->plane1_latest_sample.shift); + + /* Check that they both use the same shift */ + if (data->plane0_latest_sample.shift != data->plane1_latest_sample.shift) { + + if (data->plane0_latest_sample.shift != shift) { + int8_t new_shift = data->plane0_latest_sample.shift - shift; + data->plane0_latest_sample.readings[0].x = + (new_shift < 0) + ? (data->plane0_latest_sample.readings[0].x >> -new_shift) + : (data->plane0_latest_sample.readings[0].x << new_shift); + data->plane0_latest_sample.readings[0].y = + (new_shift < 0) + ? (data->plane0_latest_sample.readings[0].y >> -new_shift) + : (data->plane0_latest_sample.readings[0].y << new_shift); + data->plane0_latest_sample.readings[0].z = + (new_shift < 0) + ? (data->plane0_latest_sample.readings[0].z >> -new_shift) + : (data->plane0_latest_sample.readings[0].z << new_shift); + } + + if (data->plane1_latest_sample.shift != shift) { + int8_t new_shift = data->plane1_latest_sample.shift - shift; + data->plane1_latest_sample.readings[0].x = + (new_shift < 0) + ? (data->plane1_latest_sample.readings[0].x >> -new_shift) + : (data->plane1_latest_sample.readings[0].x << new_shift); + data->plane1_latest_sample.readings[0].y = + (new_shift < 0) + ? (data->plane1_latest_sample.readings[0].y >> -new_shift) + : (data->plane1_latest_sample.readings[0].y << new_shift); + data->plane1_latest_sample.readings[0].z = + (new_shift < 0) + ? (data->plane1_latest_sample.readings[0].z >> -new_shift) + : (data->plane1_latest_sample.readings[0].z << new_shift); + } + } + + LOG_DBG("plane0(0x%08" PRIx32 ", 0x%08" PRIx32 ", 0x%08" PRIx32 ") (%d, %d, %d)", + data->plane0_latest_sample.readings[0].x, data->plane0_latest_sample.readings[0].y, + data->plane0_latest_sample.readings[0].z, data->plane0_latest_sample.readings[0].x, + data->plane0_latest_sample.readings[0].y, data->plane0_latest_sample.readings[0].z); + LOG_DBG("plane1(0x%08" PRIx32 ", 0x%08" PRIx32 ", 0x%08" PRIx32 ") (%d, %d, %d)", + data->plane1_latest_sample.readings[0].x, data->plane1_latest_sample.readings[0].y, + data->plane1_latest_sample.readings[0].z, data->plane1_latest_sample.readings[0].x, + data->plane1_latest_sample.readings[0].y, data->plane1_latest_sample.readings[0].z); + + /* + * 5.27 x 5.27 = 10.54 + * 10.54 >> 14 = 24.40 + */ + q63_t result; + int32_t integer_part; + + zdsp_dot_prod_q31(data->plane0_latest_sample.readings[0].values, + data->plane1_latest_sample.readings[0].values, 3, &result); + integer_part = (int32_t)FIELD_GET(GENMASK64(63, 48 - shift * 2), result); + + LOG_DBG("Dot product is 0x%016" PRIx64 " %d.%06u shift=%d", result, integer_part, + (uint32_t)(FIELD_GET(GENMASK64(47 - shift * 2, 0), result) * INT64_C(1000000) / + ((INT64_C(1) << (48 - shift * 2)) - 1)), + shift); + + data->plane0_has_sample = false; + data->plane1_has_sample = false; + + uint8_t *out_buf; + uint32_t buf_len; + int rc = rtio_sqe_rx_buf(data->pending_read, sizeof(struct encoded_data), + sizeof(struct encoded_data), &out_buf, &buf_len); + if (rc != 0) { + rtio_iodev_sqe_err(data->pending_read, rc); + data->pending_read = NULL; + return; + } + + struct encoded_data *edata = (struct encoded_data *)out_buf; + edata->timestamp_ns = MAX(data->plane0_latest_sample.header.base_timestamp, + data->plane1_latest_sample.header.base_timestamp); + edata->shift = ilog2(llabs(integer_part)) + 1; + int8_t extra_shift = (47 - shift * 2) - (31 - edata->shift) + 1; + edata->val = FIELD_GET(GENMASK(31, 0), result >> extra_shift); + LOG_DBG("shift=%d, val=0x%08" PRIx32, edata->shift, edata->val); + rtio_iodev_sqe_ok(data->pending_read, 0); + data->pending_read = NULL; +} + +static const struct sensing_callback_list cb_list = { + .on_data_event = on_data_event, +}; + +static int init(const struct device *dev) +{ + const struct drv_config *cfg = dev->config; + struct drv_data *data = dev->data; + int rc; + + rc = sensing_open_sensor(cfg->plane0, &cb_list, &data->plane0, (void *)dev); + if (rc != 0) { + LOG_ERR("Failed to open connection to plane0"); + return rc; + } + + rc = sensing_open_sensor(cfg->plane1, &cb_list, &data->plane1, (void *)dev); + if (rc != 0) { + sensing_close_sensor(data->plane0); + LOG_ERR("Failed to open connection to plane1"); + return rc; + } + + return 0; +} + +#define DECLARE_IODEV(node_id, inst) \ + SENSOR_DT_READ_IODEV(read_iodev_##inst, node_id, SENSOR_CHAN_ALL); \ + IF_ENABLED(CONFIG_SENSING_SHELL, (static char name_buffer_##inst[5];)) \ + static struct sensor_info sensor_info_##inst = SENSOR_INFO_INITIALIZER( \ + DEVICE_DT_GET(node_id), DT_NODE_VENDOR_OR(node_id, NULL), \ + DT_NODE_MODEL_OR(node_id, NULL), DT_PROP_OR(node_id, friendly_name, NULL)); \ + const STRUCT_SECTION_ITERABLE(sensing_sensor_info, sensing_sensor_info_##inst) = { \ + .info = &sensor_info_##inst, \ + .dev = DEVICE_DT_GET(node_id), \ + .type = SENSING_SENSOR_TYPE_MOTION_HINGE_ANGLE, \ + .iodev = &read_iodev_##inst, \ + IF_ENABLED(CONFIG_SENSING_SHELL, (.shell_name = name_buffer_##inst, ))} + +#define DRV_INIT(inst) \ + static const struct drv_config cfg_##inst = { \ + .plane0 = SENSING_SENSOR_INFO_GET(DT_INST_PHANDLE(inst, plane0), \ + SENSING_SENSOR_TYPE_MOTION_ACCELEROMETER_3D), \ + .plane1 = SENSING_SENSOR_INFO_GET(DT_INST_PHANDLE(inst, plane1), \ + SENSING_SENSOR_TYPE_MOTION_ACCELEROMETER_3D), \ + }; \ + static struct drv_data data_##inst = {0}; \ + DECLARE_IODEV(DT_DRV_INST(inst), inst); \ + DEVICE_DT_INST_DEFINE(inst, init, NULL, &data_##inst, &cfg_##inst, APPLICATION, 10, \ + &angle_api); + +DT_INST_FOREACH_STATUS_OKAY(DRV_INIT) diff --git a/subsys/sensing/vsensors/emul/CMakeLists.txt b/subsys/sensing/vsensors/emul/CMakeLists.txt new file mode 100644 index 00000000000000..47bab4b38b063d --- /dev/null +++ b/subsys/sensing/vsensors/emul/CMakeLists.txt @@ -0,0 +1,3 @@ +zephyr_library() +zephyr_library_sources(driver.c) +zephyr_include_directories(include) diff --git a/subsys/sensing/vsensors/emul/driver.c b/subsys/sensing/vsensors/emul/driver.c new file mode 100644 index 00000000000000..b4ea50bbe6956e --- /dev/null +++ b/subsys/sensing/vsensors/emul/driver.c @@ -0,0 +1,254 @@ + +#include +#include +#include +#include +#include +#include +#include + +LOG_MODULE_REGISTER(sensing_emul, CONFIG_SENSING_LOG_LEVEL); + +#define DT_DRV_COMPAT sensing_emul +struct drv_config { + const struct sensing_sensor_info **info; + size_t info_count; +}; + +struct drv_data { +}; + +static inline bool is_channel_supported(const struct drv_config *cfg, enum sensor_channel channel) +{ + int32_t required_type; + + switch (channel) { + case SENSOR_CHAN_ACCEL_X: + case SENSOR_CHAN_ACCEL_Y: + case SENSOR_CHAN_ACCEL_Z: + case SENSOR_CHAN_ACCEL_XYZ: + required_type = SENSING_SENSOR_TYPE_MOTION_ACCELEROMETER_3D; + break; + default: + return false; + } + + for (size_t i = 0; i < cfg->info_count; ++i) { + if (cfg->info[i]->type == required_type) { + return true; + } + } + return false; +} + +static int attribute_set(const struct device *dev, enum sensor_channel chan, + enum sensor_attribute attr, const struct sensor_value *val) +{ + return 0; +} + +struct raw_data { + uint64_t timestamp_ns; + uint8_t count; + enum sensor_channel channels[0]; +} __packed; + +static int submit(const struct device *sensor, struct rtio_iodev_sqe *sqe) +{ + const struct drv_config *cfg = sensor->config; + const struct sensor_read_config *read_cfg = sqe->sqe.iodev->data; + uint8_t *buf; + uint32_t buf_len; + uint8_t channel_count = 0; + int rc; + + if (read_cfg->is_streaming) { + rtio_iodev_sqe_err(sqe, -ENOTSUP); + return 0; + } + + /* For now just assume the user wants to read all the channels */ + for (size_t i = 0; i < cfg->info_count; ++i) { + switch (cfg->info[i]->type) { + case SENSING_SENSOR_TYPE_MOTION_ACCELEROMETER_3D: + case SENSING_SENSOR_TYPE_MOTION_UNCALIB_ACCELEROMETER_3D: + case SENSING_SENSOR_TYPE_MOTION_GYROMETER_3D: + channel_count += 3; + break; + case SENSING_SENSOR_TYPE_MOTION_HINGE_ANGLE: + channel_count += 1; + break; + default: + rtio_iodev_sqe_err(sqe, -ENOTSUP); + return 0; + } + } + + const uint32_t desired_size = + sizeof(struct raw_data) + channel_count * sizeof(enum sensor_channel); + + rc = rtio_sqe_rx_buf(sqe, desired_size, desired_size, &buf, &buf_len); + if (rc != 0) { + rtio_iodev_sqe_err(sqe, rc); + return 0; + } + + ((struct raw_data *)buf)->timestamp_ns = k_ticks_to_ns_floor64(k_uptime_ticks()); + ((struct raw_data *)buf)->count = channel_count; + buf += sizeof(struct raw_data); + + int count = 0; + for (size_t i = 0; i < cfg->info_count; ++i) { + switch (cfg->info[i]->type) { + case SENSING_SENSOR_TYPE_MOTION_ACCELEROMETER_3D: + case SENSING_SENSOR_TYPE_MOTION_UNCALIB_ACCELEROMETER_3D: + ((enum sensor_channel *)buf)[count++] = SENSOR_CHAN_ACCEL_X; + ((enum sensor_channel *)buf)[count++] = SENSOR_CHAN_ACCEL_Y; + ((enum sensor_channel *)buf)[count++] = SENSOR_CHAN_ACCEL_Z; + break; + case SENSING_SENSOR_TYPE_MOTION_GYROMETER_3D: + ((enum sensor_channel *)buf)[count++] = SENSOR_CHAN_GYRO_X; + ((enum sensor_channel *)buf)[count++] = SENSOR_CHAN_GYRO_Y; + ((enum sensor_channel *)buf)[count++] = SENSOR_CHAN_GYRO_Z; + break; + case SENSING_SENSOR_TYPE_MOTION_HINGE_ANGLE: + ((enum sensor_channel *)buf)[count++] = SENSOR_CHAN_ROTATION; + break; + default: + rtio_iodev_sqe_err(sqe, -ENOTSUP); + return 0; + } + } + + rtio_iodev_sqe_ok(sqe, 0); + return 0; +} + +static int decoder_get_frame_count(const uint8_t *buffer, uint16_t *frame_count) +{ + ARG_UNUSED(buffer); + *frame_count = 1; + return 0; +} + +static int decoder_get_timestamp(const uint8_t *buffer, uint64_t *timestamp_ns) +{ + *timestamp_ns = ((uint64_t *)buffer)[0]; + return 0; +} + +static bool decoder_has_trigger(const uint8_t *buffer, enum sensor_trigger_type trigger) +{ + ARG_UNUSED(buffer); + ARG_UNUSED(trigger); + return false; +} + +static int decoder_get_shift(const uint8_t *buffer, enum sensor_channel channel_type, int8_t *shift) +{ + ARG_UNUSED(buffer); + ARG_UNUSED(channel_type); + *shift = 0; + return 0; +} + +static int decoder_decode(const uint8_t *buffer, sensor_frame_iterator_t *fit, + sensor_channel_iterator_t *cit, enum sensor_channel *channels, + q31_t *values, uint8_t max_count) +{ + if (*fit != 0) { + return 0; + } + + const struct raw_data *data = (const struct raw_data *)buffer; + int count = 0; + + if (*cit >= data->count) { + return -EINVAL; + } + + while (*cit < data->count && count < max_count) { + channels[count] = data->channels[*cit]; + values[count++] = INT32_MAX; + *cit += 1; + } + + if (*cit == data->count) { + *fit += 1; + *cit = 0; + } + return count; +} + +SENSING_DMEM static const struct sensor_decoder_api decoder_api = { + .get_frame_count = decoder_get_frame_count, + .get_timestamp = decoder_get_timestamp, + .has_trigger = decoder_has_trigger, + .get_shift = decoder_get_shift, + .decode = decoder_decode, +}; + +static int get_decoder(const struct device *dev, const struct sensor_decoder_api **api) +{ + ARG_UNUSED(dev); + *api = &decoder_api; + return 0; +} + +SENSING_DMEM static const struct sensor_driver_api emul_api = { + .attr_set = attribute_set, + .attr_get = NULL, + .get_decoder = get_decoder, + .submit = submit, +}; + +static int init(const struct device *dev) +{ + // const struct drv_config *cfg = dev->config; + // struct drv_data *data = dev->data; + + return 0; +} + +#define EMUL_SENSOR_INFO_NAME(node_id, type) \ + _CONCAT(_CONCAT(__emul_sensor_info, DEVICE_DT_NAME_GET(node_id)), type) + +#define DECLARE_SENSOR_INFO_NAME(node_id, name, prop, idx, _iodev, _info) \ + IF_ENABLED(CONFIG_SENSING_SHELL, (static char node_id##_##idx##_name_buffer[5];)) \ + const STRUCT_SECTION_ITERABLE(sensing_sensor_info, name) = { \ + .info = &(_info), \ + .dev = DEVICE_DT_GET(node_id), \ + .type = DT_PROP_BY_IDX(node_id, prop, idx), \ + .iodev = &(_iodev), \ + IF_ENABLED(CONFIG_SENSING_SHELL, (.shell_name = node_id##_##idx##_name_buffer, ))} + +#define DECLARE_SENSOR_INFO(node_id, prop, idx, _iodev, _info) \ + DECLARE_SENSOR_INFO_NAME( \ + node_id, SENSING_SENSOR_INFO_DT_NAME(node_id, DT_PROP_BY_IDX(node_id, prop, idx)), \ + prop, idx, _iodev, _info) + +#define DECLARE_IODEV(node_id, inst) \ + SENSOR_DT_READ_IODEV(emul_read_iodev_##inst, node_id, SENSOR_CHAN_ALL); \ + static struct sensor_info sensor_info_##inst = SENSOR_INFO_INITIALIZER( \ + DEVICE_DT_GET(node_id), DT_NODE_VENDOR_OR(node_id, NULL), \ + DT_NODE_MODEL_OR(node_id, NULL), DT_PROP_OR(node_id, friendly_name, NULL)); \ + DT_FOREACH_PROP_ELEM_VARGS(node_id, sensor_types, DECLARE_SENSOR_INFO, \ + emul_read_iodev_##inst, sensor_info_##inst) + +#define SENSOR_INFO_ITEM(node_id, prop, idx) \ + &SENSING_SENSOR_INFO_DT_NAME(node_id, DT_PROP_BY_IDX(node_id, prop, idx)) + +#define DRV_INIT(inst) \ + DECLARE_IODEV(DT_DRV_INST(inst), inst); \ + static const struct sensing_sensor_info *info_array_##inst[DT_INST_PROP_LEN( \ + inst, sensor_types)] = { \ + DT_INST_FOREACH_PROP_ELEM_SEP(inst, sensor_types, SENSOR_INFO_ITEM, (, ))}; \ + static const struct drv_config cfg_##inst = { \ + .info = (const struct sensing_sensor_info **)&info_array_##inst, \ + .info_count = DT_INST_PROP_LEN(inst, sensor_types), \ + }; \ + static struct drv_data data_##inst = {}; \ + DEVICE_DT_INST_DEFINE(inst, init, NULL, &data_##inst, &cfg_##inst, APPLICATION, 10, \ + &emul_api); + +DT_INST_FOREACH_STATUS_OKAY(DRV_INIT) diff --git a/subsys/sensing/vsensors/emul/include/zephyr/sensing/vsensors/emul/emul.h b/subsys/sensing/vsensors/emul/include/zephyr/sensing/vsensors/emul/emul.h new file mode 100644 index 00000000000000..ac61faf389875f --- /dev/null +++ b/subsys/sensing/vsensors/emul/include/zephyr/sensing/vsensors/emul/emul.h @@ -0,0 +1,12 @@ +// +// Created by peress on 07/07/23. +// + +#ifndef ZEPHYR_SUBSYS_SENSING_VSENSORS_EMUL_INCLUDE_ZEPHYR_SENSING_VSENSORS_EMUL_EMUL_H +#define ZEPHYR_SUBSYS_SENSING_VSENSORS_EMUL_INCLUDE_ZEPHYR_SENSING_VSENSORS_EMUL_EMUL_H + +#include + +void vsensors_emul_set(const struct device *dev); + +#endif // ZEPHYR_SUBSYS_SENSING_VSENSORS_EMUL_INCLUDE_ZEPHYR_SENSING_VSENSORS_EMUL_EMUL_H diff --git a/tests/subsys/sensing/boards/native_posix.conf b/tests/subsys/sensing/boards/native_posix.conf new file mode 100644 index 00000000000000..2b3af7b4476885 --- /dev/null +++ b/tests/subsys/sensing/boards/native_posix.conf @@ -0,0 +1 @@ +CONFIG_SPI_RTIO=y diff --git a/tests/subsys/sensing/src/test_arbitration.c b/tests/subsys/sensing/src/test_arbitration.c index b7ca5fd5587cea..32ed409f6f41b4 100644 --- a/tests/subsys/sensing/src/test_arbitration.c +++ b/tests/subsys/sensing/src/test_arbitration.c @@ -30,7 +30,7 @@ ZTEST(sensing, test_single_connection_arbitration) .value = FIELD_PREP(GENMASK(31, 24), 100), .shift = 8, }; - zassert_ok(sensing_set_attributes(handle, &attribute, 1)); + zassert_ok(sensing_set_attributes(handle, SENSING_SENSOR_MODE_DONE, &attribute, 1)); uint8_t reg_val; icm42688_emul_get_reg(icm42688, REG_ACCEL_CONFIG0, ®_val, 1); @@ -59,10 +59,10 @@ ZTEST(sensing, test_double_connection_arbitration) .value = FIELD_PREP(GENMASK(31, 23), 100), .shift = 9, }; - zassert_ok(sensing_set_attributes(handles[0], &attribute, 1)); + zassert_ok(sensing_set_attributes(handles[0], SENSING_SENSOR_MODE_DONE, &attribute, 1)); attribute.value = FIELD_PREP(GENMASK(31, 23), 200); - zassert_ok(sensing_set_attributes(handles[1], &attribute, 1)); + zassert_ok(sensing_set_attributes(handles[1], SENSING_SENSOR_MODE_DONE, &attribute, 1)); icm42688_emul_get_reg(icm42688, REG_ACCEL_CONFIG0, ®_val, 1); zassert_equal(0b0111, FIELD_GET(MASK_ACCEL_ODR, reg_val), "ACCEL_CONFIG0=0x%02x", diff --git a/tests/subsys/sensing/src/test_connections.c b/tests/subsys/sensing/src/test_connections.c index f79c309bc7cf20..5437a4355b0e19 100644 --- a/tests/subsys/sensing/src/test_connections.c +++ b/tests/subsys/sensing/src/test_connections.c @@ -13,24 +13,26 @@ ZTEST(sensing, test_open_connections_limit) const struct sensing_sensor_info *sensor = SENSING_SENSOR_INFO_GET( DT_NODELABEL(accelgyro), SENSING_SENSOR_TYPE_MOTION_ACCELEROMETER_3D); const struct sensing_callback_list cb_list; - sensing_sensor_handle_t handles[CONFIG_SENSING_MAX_CONNECTIONS + 1]; + sensing_sensor_handle_t handles[CONFIG_SENSING_MAX_DYNAMIC_CONNECTIONS + 1]; zassert_not_null(sensor); /* Allocate all the connection */ - for (int i = 0; i < CONFIG_SENSING_MAX_CONNECTIONS; ++i) { + for (int i = 0; i < CONFIG_SENSING_MAX_DYNAMIC_CONNECTIONS; ++i) { zassert_ok(sensing_open_sensor(sensor, &cb_list, &handles[i])); } /* Try to over allocate */ - zassert_equal(-ENOSPC, sensing_open_sensor(sensor, &cb_list, - &handles[CONFIG_SENSING_MAX_CONNECTIONS])); + zassert_equal(-ENOSPC, + sensing_open_sensor(sensor, &cb_list, + &handles[CONFIG_SENSING_MAX_DYNAMIC_CONNECTIONS])); /* Free one connection */ zassert_ok(sensing_close_sensor(handles[0])); /* Allocate one */ - zassert_ok(sensing_open_sensor(sensor, &cb_list, &handles[CONFIG_SENSING_MAX_CONNECTIONS])); + zassert_ok(sensing_open_sensor(sensor, &cb_list, + &handles[CONFIG_SENSING_MAX_DYNAMIC_CONNECTIONS])); } ZTEST(sensing, test_connection_get_info)