Multiple fixes in cs_de

CODEOWNERS

@@ -775,6 +775,7 @@
 /subsys/bluetooth/                        @nrfconnect/ncs-dragoon
 /subsys/bluetooth/adv_prov/               @nrfconnect/ncs-si-bluebagel
 /subsys/bluetooth/controller/             @nrfconnect/ncs-dragoon
+/subsys/bluetooth/cs_de/                  @nrfconnect/ncs-dragoon
 /subsys/bluetooth/host_extensions/        @nrfconnect/ncs-dragoon
 /subsys/bluetooth/mesh/                   @nrfconnect/ncs-paladin
 /subsys/bluetooth/rpc/                    @nrfconnect/ncs-si-muffin @nrfconnect/ncs-protocols-serialization
@@ -910,6 +911,7 @@
 /tests/subsys/audio/audio_module_template/ @nrfconnect/ncs-audio
 /tests/subsys/audio_module/               @nrfconnect/ncs-audio
 /tests/subsys/bluetooth/controller/        @nrfconnect/ncs-dragoon
+/tests/subsys/bluetooth/cs_de/            @nrfconnect/ncs-dragoon
 /tests/subsys/bluetooth/gatt_dm/          @nrfconnect/ncs-si-muffin
 /tests/subsys/bluetooth/enocean/          @nrfconnect/ncs-paladin
 /tests/subsys/bluetooth/fast_pair/        @nrfconnect/ncs-si-bluebagel

scripts/ci/tags.yaml

@@ -888,6 +888,11 @@ ci_tests_subsys_bluetooth_controller:
     - nrf/subsys/bluetooth/controller/
     - nrf/tests/subsys/bluetooth/controller/
 
+ci_tests_subsys_bluetooth_cs_de:
+  files:
+    - nrf/subsys/bluetooth/cs_de/
+    - nrf/tests/subsys/bluetooth/cs_de/
+
 ci_tests_subsys_mpsl:
   files:
     - nrf/subsys/mpsl/

subsys/bluetooth/cs_de/Kconfig

@@ -23,14 +23,9 @@ module = BT_CS_DE
 module-str = CS_DE
 source "$(ZEPHYR_BASE)/subsys/logging/Kconfig.template.log_config"
 
-config BT_CS_DE_NFFT_SIZE
-	int
-	default 512
-	default 512 if BT_CS_DE_512_NFFT
-	default 1024 if BT_CS_DE_1024_NFFT
-	default 2048 if BT_CS_DE_2048_NFFT
-	help
-	  Internal config. Not intended for use.
+choice BT_CS_DE_NFFT_SIZE_SELECTION
+prompt "FFT size used in the CS_DE IFFT algorithm"
+	default BT_CS_DE_512_NFFT
 
 config BT_CS_DE_512_NFFT
 	bool "Use NFFT with 512 samples."
@@ -40,5 +35,13 @@ config BT_CS_DE_1024_NFFT
 
 config BT_CS_DE_2048_NFFT
 	bool "Use NFFT with 2048 samples."
+endchoice
 
+config BT_CS_DE_NFFT_SIZE
+	int
+	default 512 if BT_CS_DE_512_NFFT
+	default 1024 if BT_CS_DE_1024_NFFT
+	default 2048 if BT_CS_DE_2048_NFFT
+	help
+	  Internal config. Not intended for use.
 endif # BT_CS_DE

subsys/bluetooth/cs_de/cs_de.c

@@ -161,9 +161,26 @@ static int32_t calculate_left_null_compensation_of_peak(int32_t peak_index,
 	return compensated_peak_index;
 }
 
-
-static void calculate_dist_ifft(float *dist, float iq_tones_comb[2 * CONFIG_BT_CS_DE_NFFT_SIZE])
+static void calculate_ifft_mag(float iq_tones_comb[2 * CONFIG_BT_CS_DE_NFFT_SIZE])
 {
+	/* This function calculates the magnitude of the IFFT of the input IQ values.
+	 * Note that the result is written back to the input array.
+	 *
+	 * To find the IFFT this the function uses FFT functions provided by the CMSIS-DSP library.
+	 * To calculate the IFFT using FFT the following steps can be used:
+	 *  1. Complex conjugate the input.
+	 *  2. Perform the FFT.
+	 *  3. Complex conjugate the output.
+	 * Since we are interested in the magnitude of the IFFT, we can skip the last step
+	 * and directly calculate the magnitude of the output of step 2.
+	 */
+
+	/* Complex conjugate the input. */
+	for (uint32_t i = 0; i < NUM_CHANNELS; i++) {
+		iq_tones_comb[i * 2 + 1] = -iq_tones_comb[i * 2 + 1];
+	}
+
+/* Perform the FFT. */
 #if CONFIG_BT_CS_DE_NFFT_SIZE == 512
 	arm_cfft_f32(&arm_cfft_sR_f32_len512, iq_tones_comb, 0, 1);
 #elif CONFIG_BT_CS_DE_NFFT_SIZE == 1024
@@ -174,28 +191,29 @@ static void calculate_dist_ifft(float *dist, float iq_tones_comb[2 * CONFIG_BT_C
 #error
 #endif
 
-	/* Compute the magnitude of iq_tones_comb[0:2*CONFIG_BT_CS_DE_NFFT_SIZE - 1], store output
-	 * in iq_tones_comb[0:CONFIG_BT_CS_DE_NFFT_SIZE - 1]
+	/* Compute the magnitude of complex values in
+	 * iq_tones_comb[0:2*CONFIG_BT_CS_DE_NFFT_SIZE - 1]
+	 * and scale by 1/CONFIG_BT_CS_DE_NFFT_SIZE.
+	 * Store output in iq_tones_comb[0:CONFIG_BT_CS_DE_NFFT_SIZE - 1]
 	 */
 	for (uint32_t n = 0; n < CONFIG_BT_CS_DE_NFFT_SIZE; n++) {
-		float realIn = iq_tones_comb[2 * n];
-		float imagIn = iq_tones_comb[(2 * n) + 1];
+		float realIn = iq_tones_comb[2 * n] / CONFIG_BT_CS_DE_NFFT_SIZE;
+		float imagIn = iq_tones_comb[(2 * n) + 1] / CONFIG_BT_CS_DE_NFFT_SIZE;
 
 		arm_sqrt_f32((realIn * realIn) + (imagIn * imagIn), &iq_tones_comb[n]);
 	}
-	/* Reverse the elements in iq_tones_comb[0:CONFIG_BT_CS_DE_NFFT_SIZE-1] */
-	for (uint32_t n = 0; n < CONFIG_BT_CS_DE_NFFT_SIZE / 2; n++) {
-		float temp = iq_tones_comb[n];
-
-		iq_tones_comb[n] = iq_tones_comb[CONFIG_BT_CS_DE_NFFT_SIZE - 1 - n];
-		iq_tones_comb[CONFIG_BT_CS_DE_NFFT_SIZE - 1 - n] = temp;
-	}
+}
 
-	/* The iq_tones_comb array now contains the ifft_mag in the indices
-	 * [0:CONFIG_BT_CS_DE_NFFT_SIZE-1]
+static uint32_t find_ifft_peak_index(float ifft_mag[2 * CONFIG_BT_CS_DE_NFFT_SIZE])
+{
+	/* This function tries to find the peak index of the input IFFT magnitude.
+	 *
+	 * The function uses the following approach:
+	 *  1. Find the index of the strongest peak,
+	 *     corresponding to the maximum value in the IFFT magnitude.
+	 *  2. Search for strong peaks closer than the max peak.
+	 *  3. When applicable: Compensate peak based on left null location.
 	 */
-	float *ifft_mag = iq_tones_comb;
-
 	uint32_t ifft_mag_max_index;
 	float ifft_mag_max;
 
@@ -231,7 +249,18 @@ static void calculate_dist_ifft(float *dist, float iq_tones_comb[2 * CONFIG_BT_C
 			calculate_left_null_compensation_of_peak(shortest_path_idx, ifft_mag);
 	}
 
-	*dist = calculate_ifft_peak_index_to_distance(compensated_peak_index, ifft_mag);
+	return compensated_peak_index;
+}
+
+static void calculate_dist_ifft(float *dist, float iq_tones_comb[2 * CONFIG_BT_CS_DE_NFFT_SIZE])
+{
+	calculate_ifft_mag(iq_tones_comb);
+
+	float *ifft_mag = iq_tones_comb;
+
+	uint32_t ifft_peak_index = find_ifft_peak_index(ifft_mag);
+
+	*dist = calculate_ifft_peak_index_to_distance(ifft_peak_index, ifft_mag);
 }
 
 static void calculate_dist_rtt(cs_de_report_t *p_report)
@@ -240,7 +269,7 @@ static void calculate_dist_rtt(cs_de_report_t *p_report)
 		float rtt_avg_measured_ns =
 			(p_report->rtt_accumulated_half_ns * 0.5f) / p_report->rtt_count;
 		float tof_ns = rtt_avg_measured_ns / 2.0f;
-		float rtt_distance_m = tof_ns * (SPEED_OF_LIGHT_M_PER_S / 1e9f);
+		float rtt_distance_m = fmaxf(tof_ns * (SPEED_OF_LIGHT_M_PER_S / 1e9f), 0.0f);
 
 		for (uint8_t ap = 0; ap < p_report->n_ap; ap++) {
 			if (rtt_distance_m >= 0.0f) {

tests/subsys/bluetooth/cs_de/CMakeLists.txt

@@ -0,0 +1,15 @@
+#
+# Copyright (c) 2025 Nordic Semiconductor ASA
+#
+# SPDX-License-Identifier: LicenseRef-Nordic-5-Clause
+#
+
+cmake_minimum_required(VERSION 3.20.0)
+
+find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE})
+project(cs_de_test)
+
+# Generate runner for the test
+test_runner_generate(src/cs_de_test.c)
+# Add test source file
+target_sources(app PRIVATE src/cs_de_test.c)

tests/subsys/bluetooth/cs_de/prj.conf

@@ -0,0 +1,31 @@
+#
+# Copyright (c) 2025 Nordic Semiconductor ASA
+#
+# SPDX-License-Identifier: LicenseRef-Nordic-5-Clause
+#
+
+# Enable Unity testing framework
+CONFIG_UNITY=y
+
+# Enable Bluetooth support
+CONFIG_BT=y
+CONFIG_BT_HCI=y
+CONFIG_BT_CENTRAL=y
+
+# Enable Bluetooth Channel Sounding
+CONFIG_BT_CHANNEL_SOUNDING=y
+
+# Enable CS Distance Estimation
+CONFIG_BT_CS_DE=y
+CONFIG_BT_CS_DE_LOG_LEVEL_INF=y
+
+# Enable RAS service
+CONFIG_BT_RAS=y
+CONFIG_BT_RAS_RREQ=y
+CONFIG_BT_RAS_MAX_ANTENNA_PATHS=4
+# Enable FPU support for float operations
+CONFIG_FPU=y
+
+# Increase stack sizes for floating point operations
+CONFIG_MAIN_STACK_SIZE=4096
+CONFIG_SYSTEM_WORKQUEUE_STACK_SIZE=2048

tests/subsys/bluetooth/cs_de/src/cs_de_test.c

@@ -0,0 +1,107 @@
+/*
+ * Copyright (c) 2025 Nordic Semiconductor ASA
+ *
+ * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause
+ */
+
+#include <unity.h>
+#include <stdbool.h>
+#include <string.h>
+#include <math.h>
+
+#include <bluetooth/cs_de.h>
+
+#define NUM_CHANNELS	       (75)
+#define CHANNEL_SPACING_HZ     (1e6f)
+#define PI		       (3.14159265358979f)
+#define SPEED_OF_LIGHT_M_PER_S (299792458.0f)
+
+/* The unity_main is not declared in any header file. It is only defined in the generated test
+ * runner because of ncs' unity configuration. It is therefore declared here to avoid a compiler
+ * warning.
+ */
+extern int unity_main(void);
+
+/* Generate ideal IQ data for a given distance in meters.*/
+static void generate_ideal_iq_data(float distance, cs_de_iq_tones_t *iq_tones)
+{
+	float rotation_per_channel =
+		2 * PI * CHANNEL_SPACING_HZ * distance / SPEED_OF_LIGHT_M_PER_S;
+	for (int i = 0; i < NUM_CHANNELS; i++) {
+		iq_tones->i_local[i] = 100 * cosf(-rotation_per_channel * i);
+		iq_tones->q_local[i] = 100 * sinf(-rotation_per_channel * i);
+		iq_tones->i_remote[i] = 100 * cosf(-rotation_per_channel * i);
+		iq_tones->q_remote[i] = 100 * sinf(-rotation_per_channel * i);
+	}
+}
+
+void test_cs_de_calc_empty_report(void)
+{
+	cs_de_report_t test_report;
+	cs_de_quality_t result;
+
+	/* Setup: Initialize report with no antenna paths */
+	test_report.n_ap = 0;
+
+	/* Execute */
+	result = cs_de_calc(&test_report);
+
+	/* Verify: Should return DO_NOT_USE quality */
+	TEST_ASSERT_EQUAL(CS_DE_QUALITY_DO_NOT_USE, result);
+}
+
+void test_cs_de_calc_bad_tone_quality(void)
+{
+	cs_de_report_t test_report;
+	cs_de_quality_t result;
+	/* Setup: Initialize report with 1 antenna path but bad tone quality */
+	test_report.n_ap = 1;
+	test_report.tone_quality[0] = CS_DE_TONE_QUALITY_BAD;
+
+	/* Execute */
+	result = cs_de_calc(&test_report);
+
+	/* Verify: Should return DO_NOT_USE quality due to bad tone quality */
+	TEST_ASSERT_EQUAL(CS_DE_QUALITY_DO_NOT_USE, result);
+}
+
+void test_cs_de_calc_with_ideal_iq_data(void)
+{
+	for (uint8_t n_ap = 1; n_ap <= CONFIG_BT_RAS_MAX_ANTENNA_PATHS; n_ap++) {
+		for (float distance = 0.0f; distance < 74.5f; distance += 0.1f) {
+			cs_de_report_t test_report;
+			cs_de_quality_t result;
+
+			test_report.n_ap = n_ap;
+
+			for (uint8_t ap = 0; ap < n_ap; ap++) {
+				test_report.tone_quality[ap] = CS_DE_TONE_QUALITY_OK;
+				generate_ideal_iq_data(distance, &test_report.iq_tones[ap]);
+			}
+
+			result = cs_de_calc(&test_report);
+			TEST_ASSERT_TRUE(result == CS_DE_QUALITY_OK);
+
+			for (uint8_t ap = 0; ap < n_ap; ap++) {
+				/* Verify that the estimated distance is within 1 cm of the distance
+				 * used to generate the ideal IQ data.
+				 */
+				TEST_ASSERT_FLOAT_WITHIN(0.01f, distance,
+							 test_report.distance_estimates[ap].ifft);
+				TEST_ASSERT_FLOAT_WITHIN(
+					0.01f, distance,
+					test_report.distance_estimates[ap].phase_slope);
+				TEST_ASSERT_EQUAL_FLOAT(test_report.distance_estimates[ap].ifft,
+							test_report.distance_estimates[ap].best);
+			}
+		}
+	}
+}
+
+/* Main test entry point */
+int main(void)
+{
+	(void)unity_main();
+
+	return 0;
+}

tests/subsys/bluetooth/cs_de/testcase.yaml

@@ -0,0 +1,9 @@
+tests:
+  subsys.bluetooth.cs_de:
+    platform_allow:
+      - native_sim
+    integration_platforms:
+      - native_sim
+    tags:
+      - unittest
+      - ci_tests_subsys_bluetooth_cs_de