subsys/sensing/sensor/phy_3d_sensor/phy_3d_sensor.c

/*
 * Copyright (c) 2023 Intel Corporation.
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include "zephyr/devicetree.h"
#define DT_DRV_COMPAT zephyr_sensing_phy_3d_sensor

#include <stdlib.h>
#include <zephyr/drivers/sensor.h>
#include <zephyr/logging/log.h>
#include <zephyr/rtio/rtio.h>
#include <zephyr/sys_clock.h>
#include <zephyr/sensing/sensing.h>
#include <zephyr/sensing/sensing_sensor.h>
#include <zephyr/sys/util.h>

#include "phy_3d_sensor.h"

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 int phy_3d_sensor_init(const struct device *dev)
{
	const struct phy_3d_sensor_config *cfg = dev->config;
	struct phy_3d_sensor_data *data = dev->data;
	int i;

	for (i = 0; i < cfg->sensor_num; i++) {
		switch (cfg->sensor_types[i]) {
		case SENSING_SENSOR_TYPE_MOTION_ACCELEROMETER_3D:
			data->customs[i] = &custom_accel;
		break;
		case SENSING_SENSOR_TYPE_MOTION_GYROMETER_3D:
			data->customs[i] = &custom_gyro;
		break;
		default:
			LOG_ERR("phy_3d_sensor doesn't support sensor type %d",
			cfg->sensor_types[i]);
			return -ENOTSUP;
		}
	}

	LOG_INF("%s: Underlying device: %s", dev->name, cfg->hw_dev->name);

	return 0;
}

static int phy_3d_sensor_attr_set_hyst(const struct device *dev,
		enum sensor_channel chan,
		const struct sensor_value *val)
{
	const struct phy_3d_sensor_config *cfg = dev->config;

	if (chan == SENSOR_CHAN_ACCEL_XYZ || chan == SENSOR_CHAN_GYRO_XYZ) {
		return sensor_attr_set(cfg->hw_dev, chan, SENSOR_ATTR_SLOPE_TH, val);
	} else {
		return sensor_attr_set(cfg->hw_dev, chan, SENSOR_ATTR_HYSTERESIS, val);
	}
}

static int phy_3d_sensor_attr_set(const struct device *dev,
		enum sensor_channel chan,
		enum sensor_attribute attr,
		const struct sensor_value *val)
{
	const struct phy_3d_sensor_config *cfg = dev->config;
	int ret = 0;

	switch (attr) {
	case SENSOR_ATTR_HYSTERESIS:
		ret = phy_3d_sensor_attr_set_hyst(dev, chan, val);
		break;

	default:
		ret = sensor_attr_set(cfg->hw_dev, chan, attr, val);
		break;
	}

	LOG_INF("%s:%s attr:%d ret:%d", __func__, dev->name, attr, ret);
	return ret;
}

static int phy_3d_sensor_submit(const struct device *dev,
		struct rtio_iodev_sqe *sqe)
{
	struct sensing_submit_config *config = (struct sensing_submit_config *)sqe->sqe.iodev->data;
	const struct phy_3d_sensor_config *cfg = dev->config;
	struct phy_3d_sensor_data *data = dev->data;
	const struct phy_3d_sensor_custom *custom = data->customs[config->info_index];
	struct sensing_sensor_value_3d_q31 *sample;
	struct sensor_value value[PHY_3D_SENSOR_CHANNEL_NUM];
	uint32_t buffer_len;
	int i, ret;

	ret = rtio_sqe_rx_buf(sqe, sizeof(*sample), sizeof(*sample),
			(uint8_t **)&sample, &buffer_len);
	if (ret) {
		rtio_iodev_sqe_err(sqe, ret);
		return ret;
	}

	ret = sensor_sample_fetch_chan(cfg->hw_dev, custom->chan_all);
	if (ret) {
		LOG_ERR("%s: sample fetch failed: %d", dev->name, ret);
		rtio_iodev_sqe_err(sqe, ret);
		return ret;
	}

	ret = sensor_channel_get(cfg->hw_dev, custom->chan_all, value);
	if (ret) {
		LOG_ERR("%s: channel get failed: %d", dev->name, ret);
		rtio_iodev_sqe_err(sqe, ret);
		return ret;
	}

	for (i = 0; i < ARRAY_SIZE(value); ++i) {
		sample->readings[0].v[i] = custom->sensor_value_to_q31(&value[i]);
	}

	sample->header.reading_count = 1;
	sample->shift = 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);

	rtio_iodev_sqe_ok(sqe, 0);
	return 0;
}

static const struct sensor_driver_api phy_3d_sensor_api = {
	.attr_set = phy_3d_sensor_attr_set,
	.submit = phy_3d_sensor_submit,
};

/* To be removed */
static const struct sensing_sensor_register_info phy_3d_sensor_reg = {
	.flags = SENSING_SENSOR_FLAG_REPORT_ON_CHANGE,
	.sample_size = sizeof(struct sensing_sensor_value_3d_q31),
	.sensitivity_count = PHY_3D_SENSOR_CHANNEL_NUM,
	.version.value = SENSING_SENSOR_VERSION(0, 8, 0, 0),
};

#define SENSING_PHY_3D_SENSOR_DT_DEFINE(_inst)						\
	static struct rtio_iodev_sqe _CONCAT(sqes, _inst)[DT_INST_PROP_LEN(_inst,	\
			sensor_types)];							\
	static const struct phy_3d_sensor_custom *_CONCAT(customs, _inst)		\
			[DT_INST_PROP_LEN(_inst, sensor_types)];			\
	static struct phy_3d_sensor_data _CONCAT(data, _inst) = {			\
		.sqes = _CONCAT(sqes, _inst),						\
		.customs = _CONCAT(customs, _inst),					\
	};										\
	static const struct phy_3d_sensor_config _CONCAT(cfg, _inst) = {		\
		.hw_dev = DEVICE_DT_GET(						\
				DT_PHANDLE(DT_DRV_INST(_inst),				\
				underlying_device)),					\
		.sensor_num = DT_INST_PROP_LEN(_inst, sensor_types),			\
		.sensor_types = DT_PROP(DT_DRV_INST(_inst), sensor_types),		\
	};										\
	SENSING_SENSORS_DT_INST_DEFINE(_inst, &phy_3d_sensor_reg, NULL,			\
		&phy_3d_sensor_init, NULL,						\
		&_CONCAT(data, _inst), &_CONCAT(cfg, _inst),				\
		POST_KERNEL, CONFIG_SENSOR_INIT_PRIORITY,				\
		&phy_3d_sensor_api);

DT_INST_FOREACH_STATUS_OKAY(SENSING_PHY_3D_SENSOR_DT_DEFINE);