sensing: implement sleep time calculation for sensor loop

implement sleep time calculation in loop sensors

Signed-off-by: Guangfu Hu <[email protected]>

subsys/sensing/runtime.c

@@ -307,6 +307,47 @@ static int send_data_to_clients(struct sensing_context *ctx,
 	return 0;
 }
 
+static uint64_t calc_next_poll_time(struct sensing_context *ctx)
+{
+	struct sensing_sensor *sensor;
+	uint64_t next_poll = EXEC_TIME_OFF;
+	int i;
+
+	for_each_sensor(ctx, i, sensor) {
+		if (!is_sensor_state_ready(sensor))
+			continue;
+		if (!is_sensor_opened(sensor))
+			continue;
+		if (sensor->next_exec_time == EXEC_TIME_OFF) {
+			continue;
+		}
+		next_poll = MIN(next_poll, sensor->next_exec_time);
+	}
+
+	return next_poll;
+}
+
+static int calc_sleep_time(struct sensing_context *ctx, uint64_t cur_us)
+{
+	uint64_t next_poll_time;
+	uint32_t sleep_time;
+
+	next_poll_time = calc_next_poll_time(ctx);
+	if (next_poll_time == EXEC_TIME_OFF) {
+		/* no sampling request. sleep forever */
+		sleep_time = UINT32_MAX;
+	} else if (next_poll_time <= cur_us) {
+		sleep_time = 0;
+	} else {
+		sleep_time = (uint32_t)((next_poll_time - cur_us) / USEC_PER_MSEC);
+	}
+
+	LOG_DBG("calc sleep time, next:%lld, cur:%lld, sleep_time:%d(ms)",
+		next_poll_time, cur_us, sleep_time);
+
+	return sleep_time;
+}
+
 int loop_sensors(struct sensing_context *ctx)
 {
 	struct sensing_sensor *sensor;
@@ -330,5 +371,5 @@ int loop_sensors(struct sensing_context *ctx)
 		}
 	}
 
-	return ret;
+	return calc_sleep_time(ctx, cur_us);
 }

subsys/sensing/sensing_sensor.c

@@ -29,8 +29,8 @@ int sensing_sensor_notify_data_ready(const struct device *dev)
 
 	atomic_set_bit(&sensor->flag, SENSOR_DATA_READY_BIT);
 
-	atomic_set_bit(&ctx->runtime_event_flag, EVENT_DATA_READY);
-	k_sem_give(&ctx->runtime_event_sem);
+	atomic_set_bit(&ctx->event_flag, EVENT_DATA_READY);
+	k_sem_give(&ctx->event_sem);
 
 	return 0;
 }

subsys/sensing/sensor_mgmt.c

@@ -207,26 +207,25 @@ static void sensor_later_config(struct sensing_context *ctx)
 static void sensing_runtime_thread(void *p1, void *p2, void *p3)
 {
 	struct sensing_context *ctx = p1;
-	int sleep_time = 100;
-	k_timeout_t timeout;
+	int sleep_time = UINT32_MAX;
 	int ret;
 
 	LOG_INF("sensing runtime thread start...");
 
 	do {
-		loop_sensors(ctx);
+		sleep_time = loop_sensors(ctx);
 
-		timeout = (sleep_time == UINT32_MAX ? K_FOREVER : K_MSEC(sleep_time));
-
-		ret = k_sem_take(&ctx->runtime_event_sem, timeout);
+		ret = k_sem_take(&ctx->event_sem, calc_timeout(sleep_time));
 		if (!ret) {
-			if (atomic_test_and_clear_bit(&ctx->runtime_event_flag, EVENT_CONFIG_READY)) {
+			if (atomic_test_and_clear_bit(&ctx->event_flag, EVENT_CONFIG_READY)) {
 				LOG_INF("runtime thread triggered by event_config ready");
 				sensor_later_config(ctx);
 			}
+			if (atomic_test_and_clear_bit(&ctx->event_flag, EVENT_DATA_READY)) {
+				LOG_INF("runtime thread, event_data ready");
+			}
 		}
 	} while (1);
-
 }
 
 static void save_config_and_notify(struct sensing_sensor *sensor)
@@ -241,9 +240,9 @@ static void save_config_and_notify(struct sensing_sensor *sensor)
 	atomic_set_bit(&sensor->flag, SENSOR_LATER_CFG_BIT);
 
 	/*remember event config ready and notify sensing_runtime_thread */
-	atomic_set_bit(&ctx->runtime_event_flag, EVENT_CONFIG_READY);
+	atomic_set_bit(&ctx->event_flag, EVENT_CONFIG_READY);
 
-	k_sem_give(&ctx->runtime_event_sem);
+	k_sem_give(&ctx->event_sem);
 }
 
 static int set_sensor_state(struct sensing_sensor *sensor, enum sensing_sensor_state state)
@@ -427,7 +426,7 @@ static int sensing_init(void)
 		LOG_INF("sensing init, sensor:%s, state:%d", sensor->dev->name, sensor->state);
 	}
 
-	k_sem_init(&ctx->runtime_event_sem, 0, 1);
+	k_sem_init(&ctx->event_sem, 0, 1);
 
 	ctx->sensing_initialized = true;
 
@@ -441,8 +440,6 @@ static int sensing_init(void)
 		return -EAGAIN;
 	}
 
-	LOG_INF("%s(%d), runtime_id:%p", __func__, __LINE__, ctx->runtime_id);
-
 	return ret;
 }
 
@@ -610,7 +607,6 @@ int sensing_get_sensors(int *sensor_nums, const struct sensing_sensor_info **inf
 	return 0;
 }
 
-
 struct sensing_context *get_sensing_ctx(void)
 {
 	return &sensing_ctx;

subsys/sensing/sensor_mgmt.h

@@ -142,8 +142,8 @@ struct sensing_context {
 	struct sensing_sensor **sensors;
 	struct k_thread runtime_thread;
 	k_tid_t runtime_id;
-	struct k_sem runtime_event_sem;
-	atomic_t runtime_event_flag;
+	struct k_sem event_sem;
+	atomic_t event_flag;
 	bool data_to_ring_buf;
 };
 
@@ -212,6 +212,11 @@ static inline bool is_sensor_opened(struct sensing_sensor *sensor)
 	return sensor->interval != 0;
 }
 
+static inline bool is_sensor_state_ready(struct sensing_sensor *sensor)
+{
+	return (sensor->state == SENSING_SENSOR_STATE_READY);
+}
+
 /* sensor not in polling mode, meanwhile data ready arrived from physical sensor */
 static inline bool is_sensor_data_ready(struct sensing_sensor *sensor)
 {
@@ -251,6 +256,11 @@ static inline bool is_filtering_sensitivity(int *sensitivity)
 	return filtering;
 }
 
+static inline k_timeout_t calc_timeout(int sleep_time)
+{
+	return (sleep_time == UINT32_MAX ? K_FOREVER : K_MSEC(sleep_time));
+}
+
 /**
  * @}
  */