bluetooth: cs_de: Fix issue in IFFT calculation

This commit fixes an issue with how cs_de implemented IFFT, the issue
caused a 1 bin offset in the resulting IFFT magnitude, which resulted
in a constant offset of distance estimations by 1 bin.

The issue was how IFFT was implemented using the FFT, the result of FFT
should be reversed but this was done in an incorrect way (the 0th
element should be fixed) whereas all other elements should be reversed
order.

The fix however chooses a different approach to find the IFFT
using the FFT:
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 skip step 3. and directly calculate
the magnitude of the output of step 2.

The new approach is tested and verified to be equal to the
output of numpy IFFT:
`np.abs(np.fft.ifft(iq_tones_comb, n=512))`

Signed-off-by: Erik Sandgren <[email protected]>

Author: eriksandgren

subsys/bluetooth/cs_de/cs_de.c

@@ -164,6 +164,12 @@ static int32_t calculate_left_null_compensation_of_peak(int32_t peak_index,
 
 static void calculate_dist_ifft(float *dist, float iq_tones_comb[2 * CONFIG_BT_CS_DE_NFFT_SIZE])
 {
+
+	/* 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];
+	}
+
 #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
@@ -178,18 +184,11 @@ static void calculate_dist_ifft(float *dist, float iq_tones_comb[2 * CONFIG_BT_C
 	 * 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]