dm: Add support for high-precision distance calculations

Add support for high-precision distance estimation from nrf_dm.
Estimate the distance with more computation-intensive higher precision
algorithm.

Ref: WRLS-1421

Signed-off-by: Marcin Jeliński <marcin.jelinski@nordicsemi.no>

Author: maje-emb

include/dm.h

@@ -93,6 +93,11 @@ struct dm_result {
 
 			/** Best effort distance estimate. */
 			float best;
+
+#ifdef CONFIG_DM_HIGH_PRECISION_CALC
+			/* MCPD: Distance estimate based on advanced algorithms */
+			float high_precision;
+#endif
 		} mcpd;
 		struct rtt {
 			/** RTT: Distance estimate based on RTT measurement. */

subsys/dm/Kconfig

@@ -15,32 +15,41 @@ if DM_MODULE
 
 menu "nRF DM (Distance Measurement)"
 
-choice NRF_DM_TIMER
-	default NRF_DM_TIMER2
-	prompt "Timer to use for the nrf_dm library"
+choice DM_TIMER
+	default DM_TIMER2
+	prompt "Timer to use for the nrf_dm library."
 
-config NRF_DM_TIMER0
+config DM_TIMER0
 	depends on HAS_HW_NRF_TIMER0
 	bool "TIMER0"
 
-config NRF_DM_TIMER1
+config DM_TIMER1
 	depends on HAS_HW_NRF_TIMER1
 	bool "TIMER1"
 
-config NRF_DM_TIMER2
+config DM_TIMER2
 	depends on HAS_HW_NRF_TIMER2
 	bool "TIMER2"
 
-config NRF_DM_TIMER3
+config DM_TIMER3
 	depends on HAS_HW_NRF_TIMER3
 	bool "TIMER3"
 
-config NRF_DM_TIMER4
+config DM_TIMER4
 	depends on HAS_HW_NRF_TIMER4
 	bool "TIMER4"
 
 endchoice
 
+config DM_HIGH_PRECISION_CALC
+	# Due to memory limitations, functionality is not provided for nrf52832.
+	depends on !SOC_NRF52832
+	default n
+	bool "Use high-precision distance estimation"
+	help
+		"Use a more compute-intensive algorithm for the distance estimation.
+		Only works with MCPD ranging mode"
+
 config DM_TIMESLOT_RESCHEDULE
 	bool "Timeslot reschedule"
 	default n
@@ -55,6 +64,7 @@ config DM_INITIATOR_DELAY_US
 
 config DM_MIN_TIME_BETWEEN_TIMESLOTS_US
 	int "Minimum time between two timeslots"
+	default 180000 if DM_HIGH_PRECISION_CALC
 	default 8000
 	help
 	  "Minimum time between two timeslots.

subsys/dm/dm.c

@@ -37,29 +37,35 @@
 
 LOG_MODULE_REGISTER(nrf_dm, CONFIG_DM_MODULE_LOG_LEVEL);
 
-#ifdef CONFIG_NRF_DM_TIMER0
-#define NRF_DM_TIMER NRF_TIMER0
+#ifdef CONFIG_DM_TIMER0
+#define DM_TIMER NRF_TIMER0
 #endif
-#ifdef CONFIG_NRF_DM_TIMER1
-#define NRF_DM_TIMER NRF_TIMER1
+#ifdef CONFIG_DM_TIMER1
+#define DM_TIMER NRF_TIMER1
 #endif
-#ifdef CONFIG_NRF_DM_TIMER2
-#define NRF_DM_TIMER NRF_TIMER2
+#ifdef CONFIG_DM_TIMER2
+#define DM_TIMER NRF_TIMER2
 #endif
-#ifdef CONFIG_NRF_DM_TIMER3
-#define NRF_DM_TIMER NRF_TIMER3
+#ifdef CONFIG_DM_TIMER3
+#define DM_TIMER NRF_TIMER3
 #endif
-#ifdef CONFIG_NRF_DM_TIMER4
-#define NRF_DM_TIMER NRF_TIMER4
+#ifdef CONFIG_DM_TIMER4
+#define DM_TIMER NRF_TIMER4
 #endif
 
 #define PPI_CH_COUNT                 2
 
 #define MPSL_THREAD_PRIO             CONFIG_MPSL_THREAD_COOP_PRIO
-#define STACKSIZE                    CONFIG_MAIN_STACK_SIZE
-#define DM_THREAD_PRIORITY           K_HIGHEST_APPLICATION_THREAD_PRIO
+#define MPSL_THREAD_STACK_SIZE       CONFIG_MAIN_STACK_SIZE
+#define DM_THREAD_PRIORITY           0
 #define TIMESLOT_TIMEOUT_STEP_MS     120000
 
+#if CONFIG_DM_HIGH_PRECISION_CALC && !defined(NRF5340_XXAA)
+#define DM_THREAD_STACK_SIZE         2560
+#else
+#define DM_THREAD_STACK_SIZE         1536
+#endif
+
 #define DM_TIMESLOT_OVERHEAD_US      400
 #define DM_REFLECTOR_OVERHEAD_US     2000
 
@@ -176,6 +182,7 @@ static mpsl_timeslot_signal_return_param_t *mpsl_timeslot_callback(
 		dm_io_set(DM_IO_RANGING);
 
 		if (atomic_get(&timeslot_ctx.state) == TIMESLOT_STATE_EARLY_PENDING) {
+			dm_io_clear(DM_IO_RANGING);
 			return p_ret_val;
 		}
 
@@ -258,7 +265,7 @@ static void mpsl_nonpreemptible_thread(void)
 	}
 }
 
-static void process_data(const nrf_dm_report_t *data)
+static void process_data(const nrf_dm_report_t *data, float high_precision_estimate)
 {
 	if (!data) {
 		result.status = false;
@@ -287,6 +294,9 @@ static void process_data(const nrf_dm_report_t *data)
 		result.dist_estimates.mcpd.best = data->distance_estimates.mcpd.best;
 		result.dist_estimates.mcpd.rssi_openspace =
 						      data->distance_estimates.mcpd.rssi_openspace;
+#ifdef CONFIG_DM_HIGH_PRECISION_CALC
+		result.dist_estimates.mcpd.high_precision = high_precision_estimate;
+#endif
 	}
 }
 
@@ -350,11 +360,6 @@ static void dm_start_ranging(void)
 	timeslot_queue_remove_first();
 
 	uint32_t distance = time_distance_get(timeslot_ctx.last_start, req->start_time);
-	uint32_t distance_now = time_distance_get(timeslot_ctx.last_start, time_now());
-
-	if (distance_now > distance) {
-		goto out;
-	}
 
 	atomic_set(&timeslot_ctx.state, TIMESLOT_STATE_PENDING);
 	err = timeslot_request(TICKS_TO_US(distance));
@@ -378,7 +383,7 @@ static void dm_reschedule(void)
 			timeslot_len_us = timeslot_ctx.curr_req.timeslot_length_us;
 
 			err = timeslot_queue_append(&timeslot_ctx.curr_req.dm_req,
-					   timeslot_ctx.last_start, window_len_us, timeslot_len_us);
+					   time_now(), window_len_us, timeslot_len_us);
 			if (err) {
 				LOG_DBG("Timeslot allocator failed (err %d)", err);
 			}
@@ -400,11 +405,17 @@ static void calculation(void)
 		}
 	} else {
 		static nrf_dm_report_t report;
+		float high_precision_estimate = 0;
 
 		nrf_dm_populate_report(&report);
 		nrf_dm_calc(&report);
-		process_data(&report);
 
+#ifdef CONFIG_DM_HIGH_PRECISION_CALC
+		if (report.ranging_mode == NRF_DM_RANGING_MODE_MCPD) {
+			high_precision_estimate = nrf_dm_high_precision_calc(&report);
+		}
+#endif
+		process_data(&report, high_precision_estimate);
 		if (dm_context.cb->data_ready != NULL) {
 			dm_context.cb->data_ready(&result);
 		}
@@ -506,7 +517,7 @@ int dm_init(struct dm_init_param *init_param)
 		ppi_ch[i] = (uint8_t)channel;
 	}
 
-	nrf_dm_init(&ppi_conf, &ant_conf, NRF_DM_TIMER);
+	nrf_dm_init(&ppi_conf, &ant_conf, DM_TIMER);
 
 	ver = nrf_dm_version_string_get();
 	LOG_DBG("Initialized NRF_DM version %s", ver);
@@ -522,7 +533,8 @@ int dm_init(struct dm_init_param *init_param)
 	return 0;
 }
 
-K_THREAD_DEFINE(dm_thread_id, STACKSIZE, dm_thread, NULL, NULL, NULL, DM_THREAD_PRIORITY, 0, 0);
+K_THREAD_DEFINE(dm_thread_id, DM_THREAD_STACK_SIZE,
+		dm_thread, NULL, NULL, NULL, DM_THREAD_PRIORITY, 0, 0);
 
-K_THREAD_DEFINE(mpsl_nonpreemptible_thread_id, STACKSIZE,
+K_THREAD_DEFINE(mpsl_nonpreemptible_thread_id, MPSL_THREAD_STACK_SIZE,
 		mpsl_nonpreemptible_thread, NULL, NULL, NULL, K_PRIO_COOP(MPSL_THREAD_PRIO), 0, 0);

subsys/dm/rpc/client/dm_rpc_client.c

@@ -69,7 +69,8 @@ int dm_init(struct dm_init_param *init_param)
 	return result;
 }
 
-static void process_data(const struct dm_rpc_process_data *data, struct dm_result *result)
+static void process_data(const struct dm_rpc_process_data *data, struct dm_result *result,
+			 float high_precision_estimate)
 {
 	if (!data || !result) {
 		return;
@@ -99,6 +100,9 @@ static void process_data(const struct dm_rpc_process_data *data, struct dm_resul
 		result->dist_estimates.mcpd.best = data->report.distance_estimates.mcpd.best;
 		result->dist_estimates.mcpd.rssi_openspace =
 						data->report.distance_estimates.mcpd.rssi_openspace;
+#ifdef CONFIG_DM__HIGH_PRECISION_CALC
+		result->dist_estimates.mcpd.high_precision = high_precision_estimate;
+#endif
 	}
 }
 
@@ -133,12 +137,18 @@ static void data_handler(struct k_work *work)
 {
 	int ret;
 	enum dm_rpc_cmd cmd;
-	struct dm_result result;
+	struct dm_result result = {0};
+	float high_precision_estimate = 0;
 
 	struct dm_rpc_process_data *dm_data = (struct dm_rpc_process_data *)recv_data;
 
 	nrf_dm_calc(&dm_data->report);
-	process_data(dm_data, &result);
+#ifdef CONFIG_DM_HIGH_PRECISION_CALC
+	if (dm_data->report.ranging_mode == NRF_DM_RANGING_MODE_MCPD) {
+		high_precision_estimate = nrf_dm_high_precision_calc(&dm_data->report);
+	}
+#endif
+	process_data(dm_data, &result, high_precision_estimate);
 	if (init_param_cb->data_ready) {
 		init_param_cb->data_ready(&result);
 	}
@@ -159,17 +169,17 @@ static int ipc_init(const struct device *dev)
 	ARG_UNUSED(dev);
 
 	int err;
-	const struct device *ipc0_instance;
+	const struct device *ipc_instance;
 
-	ipc0_instance = DEVICE_DT_GET(DT_NODELABEL(ipc0));
+	ipc_instance = DEVICE_DT_GET(DT_NODELABEL(ipc1));
 
-	err = ipc_service_open_instance(ipc0_instance);
+	err = ipc_service_open_instance(ipc_instance);
 	if ((err < 0) && (err != -EALREADY)) {
 		LOG_ERR("IPC service instance initialization failed with err: %d", err);
 		return err;
 	}
 
-	err = ipc_service_register_endpoint(ipc0_instance, &ep, &ep_cfg);
+	err = ipc_service_register_endpoint(ipc_instance, &ep, &ep_cfg);
 	if (err < 0) {
 		LOG_ERR("Registering endpoint failed with err: %d", err);
 		return err;

subsys/dm/rpc/host/dm_rpc_host.c

@@ -128,17 +128,17 @@ static int ipc_init(const struct device *dev)
 	ARG_UNUSED(dev);
 
 	int err;
-	const struct device *ipc0_instance;
+	const struct device *ipc_instance;
 
-	ipc0_instance = DEVICE_DT_GET(DT_NODELABEL(ipc0));
+	ipc_instance = DEVICE_DT_GET(DT_NODELABEL(ipc1));
 
-	err = ipc_service_open_instance(ipc0_instance);
+	err = ipc_service_open_instance(ipc_instance);
 	if ((err < 0) && (err != -EALREADY)) {
 		LOG_ERR("IPC service instance initialization failed with err: %d", err);
 		return err;
 	}
 
-	err = ipc_service_register_endpoint(ipc0_instance, &ep, &ep_cfg);
+	err = ipc_service_register_endpoint(ipc_instance, &ep, &ep_cfg);
 	if (err < 0) {
 		LOG_ERR("Registering endpoint failed with err: %d", err);
 		return err;