1 /*
2 * Copyright (c) 2024 Nordic Semiconductor ASA
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <math.h>
8 #include <zephyr/bluetooth/cs.h>
9 #include "distance_estimation.h"
10
11 #define CS_FREQUENCY_MHZ(ch) (2402u + 1u * (ch))
12 #define CS_FREQUENCY_HZ(ch) (CS_FREQUENCY_MHZ(ch) * 1000000.0f)
13 #define SPEED_OF_LIGHT_M_PER_S (299792458.0f)
14 #define SPEED_OF_LIGHT_NM_PER_S (SPEED_OF_LIGHT_M_PER_S / 1000000000.0f)
15 #define PI 3.14159265358979323846f
16 #define MAX_NUM_SAMPLES 256
17
18 struct iq_sample_and_channel {
19 bool failed;
20 uint8_t channel;
21 uint8_t antenna_permutation;
22 struct bt_le_cs_iq_sample local_iq_sample;
23 struct bt_le_cs_iq_sample peer_iq_sample;
24 };
25
26 struct rtt_timing {
27 bool failed;
28 int16_t toa_tod_initiator;
29 int16_t tod_toa_reflector;
30 };
31
32 static struct iq_sample_and_channel mode_2_data[MAX_NUM_SAMPLES];
33 static struct rtt_timing mode_1_data[MAX_NUM_SAMPLES];
34
35 struct processing_context {
36 bool local_steps;
37 uint8_t mode_1_data_index;
38 uint8_t mode_2_data_index;
39 uint8_t n_ap;
40 enum bt_conn_le_cs_role role;
41 };
42
calc_complex_product(int32_t z_a_real,int32_t z_a_imag,int32_t z_b_real,int32_t z_b_imag,int32_t * z_out_real,int32_t * z_out_imag)43 static void calc_complex_product(int32_t z_a_real, int32_t z_a_imag, int32_t z_b_real,
44 int32_t z_b_imag, int32_t *z_out_real, int32_t *z_out_imag)
45 {
46 *z_out_real = z_a_real * z_b_real - z_a_imag * z_b_imag;
47 *z_out_imag = z_a_real * z_b_imag + z_a_imag * z_b_real;
48 }
49
linear_regression(float * x_values,float * y_values,uint8_t n_samples)50 static float linear_regression(float *x_values, float *y_values, uint8_t n_samples)
51 {
52 if (n_samples == 0) {
53 return 0.0;
54 }
55
56 /* Estimates b in y = a + b x */
57
58 float y_mean = 0.0;
59 float x_mean = 0.0;
60
61 for (uint8_t i = 0; i < n_samples; i++) {
62 y_mean += (y_values[i] - y_mean) / (i + 1);
63 x_mean += (x_values[i] - x_mean) / (i + 1);
64 }
65
66 float b_est_upper = 0.0;
67 float b_est_lower = 0.0;
68
69 for (uint8_t i = 0; i < n_samples; i++) {
70 b_est_upper += (x_values[i] - x_mean) * (y_values[i] - y_mean);
71 b_est_lower += (x_values[i] - x_mean) * (x_values[i] - x_mean);
72 }
73
74 return b_est_upper / b_est_lower;
75 }
76
bubblesort_2(float * array1,float * array2,uint16_t len)77 static void bubblesort_2(float *array1, float *array2, uint16_t len)
78 {
79 bool swapped;
80 float temp;
81
82 for (uint16_t i = 0; i < len - 1; i++) {
83 swapped = false;
84 for (uint16_t j = 0; j < len - i - 1; j++) {
85 if (array1[j] > array1[j + 1]) {
86 temp = array1[j];
87 array1[j] = array1[j + 1];
88 array1[j + 1] = temp;
89 temp = array2[j];
90 array2[j] = array2[j + 1];
91 array2[j + 1] = temp;
92 swapped = true;
93 }
94 }
95
96 if (!swapped) {
97 break;
98 }
99 }
100 }
101
estimate_distance_using_phase_slope(struct iq_sample_and_channel * data,uint8_t len)102 static float estimate_distance_using_phase_slope(struct iq_sample_and_channel *data, uint8_t len)
103 {
104 int32_t combined_i;
105 int32_t combined_q;
106 uint16_t num_angles = 0;
107 static float theta[MAX_NUM_SAMPLES];
108 static float frequencies[MAX_NUM_SAMPLES];
109
110 for (uint8_t i = 0; i < len; i++) {
111 if (!data[i].failed) {
112 calc_complex_product(data[i].local_iq_sample.i, data[i].local_iq_sample.q,
113 data[i].peer_iq_sample.i, data[i].peer_iq_sample.q,
114 &combined_i, &combined_q);
115
116 theta[num_angles] = atan2(1.0 * combined_q, 1.0 * combined_i);
117 frequencies[num_angles] = 1.0 * CS_FREQUENCY_MHZ(data[i].channel);
118 num_angles++;
119 }
120 }
121
122 if (num_angles < 2) {
123 return 0.0;
124 }
125
126 /* Sort phases by tone frequency */
127 bubblesort_2(frequencies, theta, num_angles);
128
129 /* One-dimensional phase unwrapping */
130 for (uint8_t i = 1; i < num_angles; i++) {
131 float difference = theta[i] - theta[i - 1];
132
133 if (difference > PI) {
134 for (uint8_t j = i; j < num_angles; j++) {
135 theta[j] -= 2.0f * PI;
136 }
137 } else if (difference < -PI) {
138 for (uint8_t j = i; j < num_angles; j++) {
139 theta[j] += 2.0f * PI;
140 }
141 }
142 }
143
144 float phase_slope = linear_regression(frequencies, theta, num_angles);
145
146 float distance = -phase_slope * (SPEED_OF_LIGHT_M_PER_S / (4 * PI));
147
148 return distance / 1000000.0f; /* Scale to meters. */
149 }
150
estimate_distance_using_time_of_flight(uint8_t n_samples)151 static float estimate_distance_using_time_of_flight(uint8_t n_samples)
152 {
153 float tof;
154 float tof_mean = 0.0;
155
156 /* Cumulative Moving Average */
157 for (uint8_t i = 0; i < n_samples; i++) {
158 if (!mode_1_data[i].failed) {
159 tof = (mode_1_data[i].toa_tod_initiator -
160 mode_1_data[i].tod_toa_reflector) /
161 2;
162 tof_mean += (tof - tof_mean) / (i + 1);
163 }
164 }
165
166 float tof_mean_ns = tof_mean / 2.0f;
167
168 return tof_mean_ns * SPEED_OF_LIGHT_NM_PER_S;
169 }
170
process_step_data(struct bt_le_cs_subevent_step * step,void * user_data)171 static bool process_step_data(struct bt_le_cs_subevent_step *step, void *user_data)
172 {
173 struct processing_context *context = (struct processing_context *)user_data;
174
175 if (step->mode == BT_CONN_LE_CS_MAIN_MODE_2) {
176 struct bt_hci_le_cs_step_data_mode_2 *step_data =
177 (struct bt_hci_le_cs_step_data_mode_2 *)step->data;
178
179 if (context->local_steps) {
180 for (uint8_t i = 0; i < (context->n_ap + 1); i++) {
181 if (step_data->tone_info[i].extension_indicator !=
182 BT_HCI_LE_CS_NOT_TONE_EXT_SLOT) {
183 continue;
184 }
185
186 mode_2_data[context->mode_2_data_index].channel = step->channel;
187 mode_2_data[context->mode_2_data_index].antenna_permutation =
188 step_data->antenna_permutation_index;
189 mode_2_data[context->mode_2_data_index].local_iq_sample =
190 bt_le_cs_parse_pct(
191 step_data->tone_info[i].phase_correction_term);
192 if (step_data->tone_info[i].quality_indicator ==
193 BT_HCI_LE_CS_TONE_QUALITY_LOW ||
194 step_data->tone_info[i].quality_indicator ==
195 BT_HCI_LE_CS_TONE_QUALITY_UNAVAILABLE) {
196 mode_2_data[context->mode_2_data_index].failed = true;
197 }
198
199 context->mode_2_data_index++;
200 }
201 } else {
202 for (uint8_t i = 0; i < (context->n_ap + 1); i++) {
203 if (step_data->tone_info[i].extension_indicator !=
204 BT_HCI_LE_CS_NOT_TONE_EXT_SLOT) {
205 continue;
206 }
207
208 mode_2_data[context->mode_2_data_index].peer_iq_sample =
209 bt_le_cs_parse_pct(
210 step_data->tone_info[i].phase_correction_term);
211 if (step_data->tone_info[i].quality_indicator ==
212 BT_HCI_LE_CS_TONE_QUALITY_LOW ||
213 step_data->tone_info[i].quality_indicator ==
214 BT_HCI_LE_CS_TONE_QUALITY_UNAVAILABLE) {
215 mode_2_data[context->mode_2_data_index].failed = true;
216 }
217
218 context->mode_2_data_index++;
219 }
220 }
221 } else if (step->mode == BT_HCI_OP_LE_CS_MAIN_MODE_1) {
222 struct bt_hci_le_cs_step_data_mode_1 *step_data =
223 (struct bt_hci_le_cs_step_data_mode_1 *)step->data;
224
225 if (step_data->packet_quality_aa_check !=
226 BT_HCI_LE_CS_PACKET_QUALITY_AA_CHECK_SUCCESSFUL ||
227 step_data->packet_rssi == BT_HCI_LE_CS_PACKET_RSSI_NOT_AVAILABLE ||
228 step_data->tod_toa_reflector == BT_HCI_LE_CS_TIME_DIFFERENCE_NOT_AVAILABLE) {
229 mode_1_data[context->mode_1_data_index].failed = true;
230 }
231
232 if (context->local_steps) {
233 if (context->role == BT_CONN_LE_CS_ROLE_INITIATOR) {
234 mode_1_data[context->mode_1_data_index].toa_tod_initiator =
235 step_data->toa_tod_initiator;
236 } else if (context->role == BT_CONN_LE_CS_ROLE_REFLECTOR) {
237 mode_1_data[context->mode_1_data_index].tod_toa_reflector =
238 step_data->tod_toa_reflector;
239 }
240 } else {
241 if (context->role == BT_CONN_LE_CS_ROLE_INITIATOR) {
242 mode_1_data[context->mode_1_data_index].tod_toa_reflector =
243 step_data->tod_toa_reflector;
244 } else if (context->role == BT_CONN_LE_CS_ROLE_REFLECTOR) {
245 mode_1_data[context->mode_1_data_index].toa_tod_initiator =
246 step_data->toa_tod_initiator;
247 }
248 }
249
250 context->mode_1_data_index++;
251 }
252
253 return true;
254 }
255
estimate_distance(uint8_t * local_steps,uint16_t local_steps_len,uint8_t * peer_steps,uint16_t peer_steps_len,uint8_t n_ap,enum bt_conn_le_cs_role role)256 void estimate_distance(uint8_t *local_steps, uint16_t local_steps_len, uint8_t *peer_steps,
257 uint16_t peer_steps_len, uint8_t n_ap, enum bt_conn_le_cs_role role)
258 {
259 struct net_buf_simple buf;
260
261 struct processing_context context = {
262 .local_steps = true,
263 .mode_1_data_index = 0,
264 .mode_2_data_index = 0,
265 .n_ap = n_ap,
266 .role = role,
267 };
268
269 memset(mode_1_data, 0, sizeof(mode_1_data));
270 memset(mode_2_data, 0, sizeof(mode_2_data));
271
272 net_buf_simple_init_with_data(&buf, local_steps, local_steps_len);
273
274 bt_le_cs_step_data_parse(&buf, process_step_data, &context);
275
276 context.mode_1_data_index = 0;
277 context.mode_2_data_index = 0;
278 context.local_steps = false;
279
280 net_buf_simple_init_with_data(&buf, peer_steps, peer_steps_len);
281
282 bt_le_cs_step_data_parse(&buf, process_step_data, &context);
283
284 float phase_slope_based_distance =
285 estimate_distance_using_phase_slope(mode_2_data, context.mode_2_data_index);
286
287 float rtt_based_distance =
288 estimate_distance_using_time_of_flight(context.mode_1_data_index);
289
290 if (rtt_based_distance == 0.0f && phase_slope_based_distance == 0.0f) {
291 printk("A reliable distance estimate could not be computed.\n");
292 } else {
293 printk("Estimated distance to reflector:\n");
294 }
295
296 if (rtt_based_distance != 0.0f) {
297 printk("- Round-Trip Timing method: %f meters (derived from %d samples)\n",
298 (double)rtt_based_distance, context.mode_1_data_index);
299 }
300 if (phase_slope_based_distance != 0.0f) {
301 printk("- Phase-Based Ranging method: %f meters (derived from %d samples)\n",
302 (double)phase_slope_based_distance, context.mode_2_data_index);
303 }
304 }
305
