Implement angle sensor

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)

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;
 	}

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);
 

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

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 |
+      +-        -+   +-        -+   +- -+

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

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 {

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 <zephyr/rtio/rtio.h>
 /**
@@ -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.

include/zephyr/sensing/sensor.h

@@ -11,18 +11,36 @@
 #include <zephyr/drivers/sensor.h>
 #include <zephyr/sensing/sensing.h>
 
+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(                                                     \

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

samples/sensor/sensor_shell/boards/tdk_robokit1.overlay

@@ -5,10 +5,25 @@
  #include <zephyr/sensing/sensor_types.h>
 
 / {
-	accelgyro: accelgyro {
+	lidaccel: accelgyro {
 		compatible = "zephyr,sensing-pipe";
 		status = "okay";
+		friendly-name = "Lid accel";
 		dev = <&icm42688>;
 		sensor-types = <SENSING_SENSOR_TYPE_MOTION_ACCELEROMETER_3D SENSING_SENSOR_TYPE_MOTION_GYROMETER_3D>;
 	};
+
+	baseaccel: accelemul {
+		compatible = "sensing,emul";
+		status = "okay";
+		friendly-name = "Base accel";
+		sensor-types = <SENSING_SENSOR_TYPE_MOTION_ACCELEROMETER_3D>;
+	};
+
+	angle: angle {
+		compatible = "sensing,accel-based-angle";
+		status = "okay";
+		plane0 = <&lidaccel>;
+		plane1 = <&baseaccel>;
+	};
 };

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

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):

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)

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

subsys/sensing/include/sensing/internal/sensing.h

@@ -6,23 +6,18 @@
 #define ZEPHYR_SUBSYS_SENSING_INCLUDE_SENSING_INTERNAL_SENSING_H
 
 #include <zephyr/drivers/sensor.h>
+#include <zephyr/sensing/sensor.h>
 #include <zephyr/sensing/sensing.h>
 #include <zephyr/dsp/types.h>
 #include <zephyr/sys/mutex.h>
 
-#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);
 

subsys/sensing/sections.ld

@@ -0,0 +1,3 @@
+#if defined(CONFIG_SENSING)
+	ITERABLE_SECTION_RAM(sensing_connection, 4)
+#endif /* CONFIG_SENSING */