Implement angle sensor

Author: Yuval Peress

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