Commit b73eafcc authored by Zhang Lixu's avatar Zhang Lixu Committed by Carles Cufi
Browse files

sensing: implement set/get interval and sensitivity 

1. set/get interval sensitivity is implmented in sensor management module.
2. add the runtime thread to handle the sensor configuration.

Signed-off-by: default avatarZhang Lixu <lixu.zhang@intel.com>
parent 46eb27cf

subsys/sensing/Kconfig

+39 −0
Changes for subsys/sensing/Kconfig: 39 added lines, 0 removed lines.
Original line number Diff line number Diff line
@@ -18,6 +18,22 @@ module = SENSING 
module-str = sensing

source "subsys/logging/Kconfig.template.log_config"



config SENSING_RTIO_SQE_NUM

	int "Number of the sensing RTIO SQE"

	default 32



config SENSING_RTIO_CQE_NUM

	int "Number of the sensing RTIO SQE"

	default 32



config SENSING_RTIO_BLOCK_SIZE

	int "Block size of the RTIO context"

	default 64



config SENSING_RTIO_BLOCK_COUNT

	int "Number of memory blocks of the RTIO context"

	default 32



config SENSING_MAX_SENSITIVITY_COUNT

	int "maximum sensitivity count one sensor could support"

	depends on SENSING
@@ -28,6 +44,29 @@ config SENSING_MAX_SENSITIVITY_COUNT 
	  So, maximum sensitivity count is needed for sensors

	  Typical values are 6



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_RUNTIME_THREAD_PRIORITY

	int "priority for sensing subsystem runtime thread"

	depends on SENSING

	default 7

	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



source "subsys/sensing/sensor/phy_3d_sensor/Kconfig"



endif # SENSING

subsys/sensing/sensor_mgmt.c

+303 −4
Changes for subsys/sensing/sensor_mgmt.c: 303 added lines, 4 removed lines.
Original line number Diff line number Diff line
@@ -5,8 +5,10 @@ 
 */

#include <zephyr/sys/__assert.h>

#include <zephyr/devicetree.h>

#include <zephyr/drivers/sensor.h>

#include <zephyr/init.h>

#include <zephyr/kernel.h>

#include <zephyr/rtio/rtio.h>

#include <zephyr/sensing/sensing.h>

#include <zephyr/sensing/sensing_sensor.h>

#include <zephyr/logging/log.h>
@@ -22,6 +24,216 @@ LOG_MODULE_REGISTER(sensing, CONFIG_SENSING_LOG_LEVEL); 


static struct sensing_context sensing_ctx = {

};

RTIO_DEFINE_WITH_MEMPOOL(sensing_rtio_ctx, CONFIG_SENSING_RTIO_SQE_NUM,

		CONFIG_SENSING_RTIO_CQE_NUM,

		CONFIG_SENSING_RTIO_BLOCK_COUNT,

		CONFIG_SENSING_RTIO_BLOCK_SIZE, 4);



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



	LOG_DBG("arbitrate interval, sensor:%s, interval:%d(us)",

			sensor->dev->name, interval);



	return interval;

}



static int set_arbitrate_interval(struct sensing_sensor *sensor, uint32_t interval)

{

	struct sensor_read_config *config = sensor->iodev->data;

	struct sensor_value odr = {0};

	int ret;



	__ASSERT(sensor && sensor->dev, "set arbitrate interval, sensor or sensor device is NULL");



	LOG_INF("set arbitrate interval:%d, sensor:%s, is_streaming:%d",

			interval, sensor->dev->name, config->is_streaming);



	if (interval) {

		odr.val1 = USEC_PER_SEC / interval;

		odr.val2 = (uint64_t)USEC_PER_SEC * 1000000 / interval % 1000000;

	}



	/* The SENSOR_CHAN_MAX should be overridden by sensing sensor driver */

	ret = sensor_attr_set(sensor->dev, SENSOR_CHAN_MAX,

			SENSOR_ATTR_SAMPLING_FREQUENCY, &odr);

	if (ret) {

		LOG_ERR("%s set attr freq failed:%d", sensor->dev->name, ret);

		return ret;

	}



	if (sensor->interval) {

		if (config->is_streaming) {

			rtio_sqe_cancel(sensor->stream_sqe);

		} else {

			k_timer_stop(&sensor->timer);

		}

	}



	if (interval) {

		if (config->is_streaming) {

			ret = sensor_stream(sensor->iodev, &sensing_rtio_ctx,

					sensor, &sensor->stream_sqe);

		} else {

			k_timer_start(&sensor->timer, K_USEC(interval),

					K_USEC(interval));

		}

	}



	sensor->interval = interval;



	return ret;

}



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)

{

	struct sensor_value threshold = {.val1 = sensitivity};



	/* update sensor sensitivity */

	sensor->sensitivity[index] = sensitivity;



	/* The SENSOR_CHAN_PRIV_START should be overridden by sensing sensor

	 * driver, SENSOR_ATTR_HYSTERESIS can be overridden for different

	 * sensora type.

	 */

	return sensor_attr_set(sensor->dev, SENSOR_CHAN_PRIV_START + index,

			SENSOR_ATTR_HYSTERESIS, &threshold);

}



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:%d",

					sensor->dev->name, i, ret);

		}

	}



	return ret;

}



static void sensor_later_config(void)

{

	LOG_INF("sensor later config begin...");



	for_each_sensor_reverse(sensor) {

		if (atomic_test_and_clear_bit(&sensor->flag, SENSOR_LATER_CFG_BIT)) {

			LOG_INF("sensor later config, sensor:%s",

				sensor->dev->name);

			config_sensor(sensor);

		}

	}

}



static void sensing_runtime_thread(void *p1, void *p2, void *p3)

{

	struct sensing_context *ctx = p1;

	int ret;



	LOG_INF("sensing runtime thread start...");



	do {

		ret = k_sem_take(&ctx->event_sem, K_FOREVER);

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

			}

		}

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

{
@@ -52,6 +264,15 @@ static void init_connection(struct sensing_connection *conn, 
	sys_slist_append(&conn->source->client_list, &conn->snode);

}



static void sensing_sensor_polling_timer(struct k_timer *timer_id)

{

	struct sensing_sensor *sensor = CONTAINER_OF(timer_id,

			struct sensing_sensor, timer);



	/* TODO: move it into sensing_runtime_thread */

	sensor_read(sensor->iodev, &sensing_rtio_ctx, sensor);

}



static int init_sensor(struct sensing_sensor *sensor)

{

	struct sensing_connection *conn;
@@ -59,6 +280,7 @@ static int init_sensor(struct sensing_sensor *sensor) 


	__ASSERT(sensor && sensor->dev, "init sensor, sensor or sensor device is NULL");



	k_timer_init(&sensor->timer, sensing_sensor_polling_timer, NULL);

	sys_slist_init(&sensor->client_list);



	for (i = 0; i < sensor->reporter_num; i++) {
@@ -138,6 +360,8 @@ int close_sensor(struct sensing_connection **conn) 


	sys_slist_find_and_remove(&tmp_conn->source->client_list, &tmp_conn->snode);



	save_config_and_notify(tmp_conn->source);



	free(*conn);

	*conn = NULL;
@@ -165,22 +389,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)
@@ -197,6 +494,8 @@ int sensing_get_sensors(int *sensor_nums, const struct sensing_sensor_info **inf 
	return 0;

}



K_THREAD_DEFINE(sensing_runtime, CONFIG_SENSING_RUNTIME_THREAD_STACK_SIZE, sensing_runtime_thread,

		&sensing_ctx, NULL, NULL, CONFIG_SENSING_RUNTIME_THREAD_PRIORITY, 0, 0);



DEVICE_DT_INST_DEFINE(0, sensing_init, NULL, &sensing_ctx, NULL,		\

		POST_KERNEL, CONFIG_SENSOR_INIT_PRIORITY, NULL);

CRA Git | Maintained and supported by SUSTech CRA and CCSE