Skip to content

Commit 584e1c8

Browse files
sean-madigancarlescufi
sean-madigan
authored and
carlescufi
committed
samples: bluetooth: Update distance estimation for mode 3 and multi antenna
Mode 3 is a combination of modes 1 and 2, so extract out some of the common logic and use this for mode 3. Adjust array sizes and indexes so they can be used for multi antenna. Signed-off-by: Sean Madigan <[email protected]>
1 parent 75b2ff2 commit 584e1c8

File tree

1 file changed

+106
-72
lines changed

1 file changed

+106
-72
lines changed

‎samples/bluetooth/channel_sounding_ras_initiator/src/distance_estimation.c‎

Lines changed: 106 additions & 72 deletions
Original file line numberDiff line numberDiff line change
@@ -21,7 +21,8 @@ LOG_MODULE_DECLARE(app_main, LOG_LEVEL_INF);
2121
#define SPEED_OF_LIGHT_M_PER_S (299792458.0f)
2222
#define SPEED_OF_LIGHT_NM_PER_S (SPEED_OF_LIGHT_M_PER_S / 1000000000.0f)
2323
#define PI 3.14159265358979323846f
24-
#define MAX_NUM_SAMPLES 256
24+
#define MAX_NUM_RTT_SAMPLES 256
25+
#define MAX_NUM_IQ_SAMPLES 256 * CONFIG_BT_RAS_MAX_ANTENNA_PATHS
2526

2627
struct iq_sample_and_channel {
2728
bool failed;
@@ -37,12 +38,12 @@ struct rtt_timing {
3738
int16_t tod_toa_reflector;
3839
};
3940

40-
static struct iq_sample_and_channel mode_2_data[MAX_NUM_SAMPLES];
41-
static struct rtt_timing mode_1_data[MAX_NUM_SAMPLES];
41+
static struct iq_sample_and_channel iq_sample_channel_data[MAX_NUM_IQ_SAMPLES];
42+
static struct rtt_timing rtt_timing_data[MAX_NUM_RTT_SAMPLES];
4243

4344
struct processing_context {
44-
uint8_t mode_1_data_index;
45-
uint8_t mode_2_data_index;
45+
uint16_t rtt_timing_data_index;
46+
uint16_t iq_sample_channel_data_index;
4647
uint8_t n_ap;
4748
enum bt_conn_le_cs_role role;
4849
};
@@ -111,8 +112,8 @@ static float estimate_distance_using_phase_slope(struct iq_sample_and_channel *d
111112
int32_t combined_i;
112113
int32_t combined_q;
113114
uint16_t num_angles = 0;
114-
static float theta[MAX_NUM_SAMPLES];
115-
static float frequencies[MAX_NUM_SAMPLES];
115+
static float theta[MAX_NUM_IQ_SAMPLES];
116+
static float frequencies[MAX_NUM_IQ_SAMPLES];
116117

117118
for (uint8_t i = 0; i < len; i++) {
118119
if (!data[i].failed) {
@@ -162,9 +163,9 @@ static float estimate_distance_using_time_of_flight(uint8_t n_samples)
162163

163164
/* Cumulative Moving Average */
164165
for (uint8_t i = 0; i < n_samples; i++) {
165-
if (!mode_1_data[i].failed) {
166-
tof = (mode_1_data[i].toa_tod_initiator -
167-
mode_1_data[i].tod_toa_reflector) /
166+
if (!rtt_timing_data[i].failed) {
167+
tof = (rtt_timing_data[i].toa_tod_initiator -
168+
rtt_timing_data[i].tod_toa_reflector) /
168169
2;
169170
tof_mean += (tof - tof_mean) / (i + 1);
170171
}
@@ -175,6 +176,76 @@ static float estimate_distance_using_time_of_flight(uint8_t n_samples)
175176
return tof_mean_ns * SPEED_OF_LIGHT_NM_PER_S;
176177
}
177178

179+
static void process_tone_info_data(struct processing_context *context,
180+
struct bt_hci_le_cs_step_data_tone_info local_tone_info[],
181+
struct bt_hci_le_cs_step_data_tone_info peer_tone_info[],
182+
uint8_t channel, uint8_t antenna_permutation_index)
183+
{
184+
for (uint8_t i = 0; i < (context->n_ap + 1); i++) {
185+
if (local_tone_info[i].extension_indicator != BT_HCI_LE_CS_NOT_TONE_EXT_SLOT ||
186+
peer_tone_info[i].extension_indicator != BT_HCI_LE_CS_NOT_TONE_EXT_SLOT) {
187+
continue;
188+
}
189+
190+
if (context->iq_sample_channel_data_index >= MAX_NUM_IQ_SAMPLES) {
191+
LOG_WRN("More IQ samples than size of iq_sample_channel_data array");
192+
return;
193+
}
194+
195+
iq_sample_channel_data[context->iq_sample_channel_data_index].channel = channel;
196+
iq_sample_channel_data[context->iq_sample_channel_data_index].antenna_permutation =
197+
antenna_permutation_index;
198+
iq_sample_channel_data[context->iq_sample_channel_data_index].local_iq_sample =
199+
bt_le_cs_parse_pct(local_tone_info[i].phase_correction_term);
200+
iq_sample_channel_data[context->iq_sample_channel_data_index].peer_iq_sample =
201+
bt_le_cs_parse_pct(peer_tone_info[i].phase_correction_term);
202+
203+
if (local_tone_info[i].quality_indicator == BT_HCI_LE_CS_TONE_QUALITY_LOW ||
204+
local_tone_info[i].quality_indicator == BT_HCI_LE_CS_TONE_QUALITY_UNAVAILABLE ||
205+
peer_tone_info[i].quality_indicator == BT_HCI_LE_CS_TONE_QUALITY_LOW ||
206+
peer_tone_info[i].quality_indicator == BT_HCI_LE_CS_TONE_QUALITY_UNAVAILABLE) {
207+
iq_sample_channel_data[context->iq_sample_channel_data_index].failed = true;
208+
}
209+
210+
context->iq_sample_channel_data_index++;
211+
}
212+
}
213+
214+
static void process_rtt_timing_data(struct processing_context *context,
215+
struct bt_hci_le_cs_step_data_mode_1 *local_rtt_data,
216+
struct bt_hci_le_cs_step_data_mode_1 *peer_rtt_data)
217+
{
218+
if (context->rtt_timing_data_index >= MAX_NUM_RTT_SAMPLES) {
219+
LOG_WRN("More RTT samples processed than size of rtt_timing_data array");
220+
return;
221+
}
222+
223+
if (local_rtt_data->packet_quality_aa_check !=
224+
BT_HCI_LE_CS_PACKET_QUALITY_AA_CHECK_SUCCESSFUL ||
225+
local_rtt_data->packet_rssi == BT_HCI_LE_CS_PACKET_RSSI_NOT_AVAILABLE ||
226+
local_rtt_data->tod_toa_reflector == BT_HCI_LE_CS_TIME_DIFFERENCE_NOT_AVAILABLE ||
227+
peer_rtt_data->packet_quality_aa_check !=
228+
BT_HCI_LE_CS_PACKET_QUALITY_AA_CHECK_SUCCESSFUL ||
229+
peer_rtt_data->packet_rssi == BT_HCI_LE_CS_PACKET_RSSI_NOT_AVAILABLE ||
230+
peer_rtt_data->tod_toa_reflector == BT_HCI_LE_CS_TIME_DIFFERENCE_NOT_AVAILABLE) {
231+
rtt_timing_data[context->rtt_timing_data_index].failed = true;
232+
}
233+
234+
if (context->role == BT_CONN_LE_CS_ROLE_INITIATOR) {
235+
rtt_timing_data[context->rtt_timing_data_index].toa_tod_initiator =
236+
local_rtt_data->toa_tod_initiator;
237+
rtt_timing_data[context->rtt_timing_data_index].tod_toa_reflector =
238+
peer_rtt_data->tod_toa_reflector;
239+
} else if (context->role == BT_CONN_LE_CS_ROLE_REFLECTOR) {
240+
rtt_timing_data[context->rtt_timing_data_index].tod_toa_reflector =
241+
local_rtt_data->tod_toa_reflector;
242+
rtt_timing_data[context->rtt_timing_data_index].toa_tod_initiator =
243+
peer_rtt_data->toa_tod_initiator;
244+
}
245+
246+
context->rtt_timing_data_index++;
247+
}
248+
178249
static bool process_step_data(struct bt_le_cs_subevent_step *local_step,
179250
struct bt_le_cs_subevent_step *peer_step, void *user_data)
180251
{
@@ -186,68 +257,31 @@ static bool process_step_data(struct bt_le_cs_subevent_step *local_step,
186257
struct bt_hci_le_cs_step_data_mode_2 *peer_step_data =
187258
(struct bt_hci_le_cs_step_data_mode_2 *)peer_step->data;
188259

189-
for (uint8_t i = 0; i < (context->n_ap + 1); i++) {
190-
if (local_step_data->tone_info[i].extension_indicator !=
191-
BT_HCI_LE_CS_NOT_TONE_EXT_SLOT ||
192-
peer_step_data->tone_info[i].extension_indicator !=
193-
BT_HCI_LE_CS_NOT_TONE_EXT_SLOT) {
194-
continue;
195-
}
260+
process_tone_info_data(context, local_step_data->tone_info,
261+
peer_step_data->tone_info, local_step->channel,
262+
local_step_data->antenna_permutation_index);
196263

197-
mode_2_data[context->mode_2_data_index].channel = local_step->channel;
198-
mode_2_data[context->mode_2_data_index].antenna_permutation =
199-
local_step_data->antenna_permutation_index;
200-
mode_2_data[context->mode_2_data_index].local_iq_sample =
201-
bt_le_cs_parse_pct(
202-
local_step_data->tone_info[i].phase_correction_term);
203-
mode_2_data[context->mode_2_data_index].peer_iq_sample = bt_le_cs_parse_pct(
204-
peer_step_data->tone_info[i].phase_correction_term);
205-
206-
if (local_step_data->tone_info[i].quality_indicator ==
207-
BT_HCI_LE_CS_TONE_QUALITY_LOW ||
208-
local_step_data->tone_info[i].quality_indicator ==
209-
BT_HCI_LE_CS_TONE_QUALITY_UNAVAILABLE ||
210-
peer_step_data->tone_info[i].quality_indicator ==
211-
BT_HCI_LE_CS_TONE_QUALITY_LOW ||
212-
peer_step_data->tone_info[i].quality_indicator ==
213-
BT_HCI_LE_CS_TONE_QUALITY_UNAVAILABLE) {
214-
mode_2_data[context->mode_2_data_index].failed = true;
215-
}
216-
217-
context->mode_2_data_index++;
218-
}
219264
} else if (local_step->mode == BT_HCI_OP_LE_CS_MAIN_MODE_1) {
220265
struct bt_hci_le_cs_step_data_mode_1 *local_step_data =
221266
(struct bt_hci_le_cs_step_data_mode_1 *)local_step->data;
222267
struct bt_hci_le_cs_step_data_mode_1 *peer_step_data =
223268
(struct bt_hci_le_cs_step_data_mode_1 *)peer_step->data;
224269

225-
if (local_step_data->packet_quality_aa_check !=
226-
BT_HCI_LE_CS_PACKET_QUALITY_AA_CHECK_SUCCESSFUL ||
227-
local_step_data->packet_rssi == BT_HCI_LE_CS_PACKET_RSSI_NOT_AVAILABLE ||
228-
local_step_data->tod_toa_reflector ==
229-
BT_HCI_LE_CS_TIME_DIFFERENCE_NOT_AVAILABLE ||
230-
peer_step_data->packet_quality_aa_check !=
231-
BT_HCI_LE_CS_PACKET_QUALITY_AA_CHECK_SUCCESSFUL ||
232-
peer_step_data->packet_rssi == BT_HCI_LE_CS_PACKET_RSSI_NOT_AVAILABLE ||
233-
peer_step_data->tod_toa_reflector ==
234-
BT_HCI_LE_CS_TIME_DIFFERENCE_NOT_AVAILABLE) {
235-
mode_1_data[context->mode_1_data_index].failed = true;
236-
}
270+
process_rtt_timing_data(context, local_step_data, peer_step_data);
237271

238-
if (context->role == BT_CONN_LE_CS_ROLE_INITIATOR) {
239-
mode_1_data[context->mode_1_data_index].toa_tod_initiator =
240-
local_step_data->toa_tod_initiator;
241-
mode_1_data[context->mode_1_data_index].tod_toa_reflector =
242-
peer_step_data->tod_toa_reflector;
243-
} else if (context->role == BT_CONN_LE_CS_ROLE_REFLECTOR) {
244-
mode_1_data[context->mode_1_data_index].tod_toa_reflector =
245-
local_step_data->tod_toa_reflector;
246-
mode_1_data[context->mode_1_data_index].toa_tod_initiator =
247-
peer_step_data->toa_tod_initiator;
248-
}
272+
} else if (local_step->mode == BT_HCI_OP_LE_CS_MAIN_MODE_3) {
273+
struct bt_hci_le_cs_step_data_mode_3 *local_step_data =
274+
(struct bt_hci_le_cs_step_data_mode_3 *)local_step->data;
275+
struct bt_hci_le_cs_step_data_mode_3 *peer_step_data =
276+
(struct bt_hci_le_cs_step_data_mode_3 *)peer_step->data;
277+
278+
process_rtt_timing_data(context,
279+
(struct bt_hci_le_cs_step_data_mode_1 *)local_step_data,
280+
(struct bt_hci_le_cs_step_data_mode_1 *)peer_step_data);
249281

250-
context->mode_1_data_index++;
282+
process_tone_info_data(context, local_step_data->tone_info,
283+
peer_step_data->tone_info, local_step->channel,
284+
local_step_data->antenna_permutation_index);
251285
}
252286

253287
return true;
@@ -257,23 +291,23 @@ void estimate_distance(struct net_buf_simple *local_steps, struct net_buf_simple
257291
uint8_t n_ap, enum bt_conn_le_cs_role role)
258292
{
259293
struct processing_context context = {
260-
.mode_1_data_index = 0,
261-
.mode_2_data_index = 0,
294+
.rtt_timing_data_index = 0,
295+
.iq_sample_channel_data_index = 0,
262296
.n_ap = n_ap,
263297
.role = role,
264298
};
265299

266-
memset(mode_1_data, 0, sizeof(mode_1_data));
267-
memset(mode_2_data, 0, sizeof(mode_2_data));
300+
memset(rtt_timing_data, 0, sizeof(rtt_timing_data));
301+
memset(iq_sample_channel_data, 0, sizeof(iq_sample_channel_data));
268302

269303
bt_ras_rreq_rd_subevent_data_parse(peer_steps, local_steps, context.role, NULL,
270304
process_step_data, &context);
271305

272-
float phase_slope_based_distance =
273-
estimate_distance_using_phase_slope(mode_2_data, context.mode_2_data_index);
306+
float phase_slope_based_distance = estimate_distance_using_phase_slope(
307+
iq_sample_channel_data, context.iq_sample_channel_data_index);
274308

275309
float rtt_based_distance =
276-
estimate_distance_using_time_of_flight(context.mode_1_data_index);
310+
estimate_distance_using_time_of_flight(context.rtt_timing_data_index);
277311

278312
if (rtt_based_distance == 0.0f && phase_slope_based_distance == 0.0f) {
279313
LOG_INF("A reliable distance estimate could not be computed.");
@@ -283,10 +317,10 @@ void estimate_distance(struct net_buf_simple *local_steps, struct net_buf_simple
283317

284318
if (rtt_based_distance != 0.0f) {
285319
LOG_INF("- Round-Trip Timing method: %f meters (derived from %d samples)",
286-
(double)rtt_based_distance, context.mode_1_data_index);
320+
(double)rtt_based_distance, context.rtt_timing_data_index);
287321
}
288322
if (phase_slope_based_distance != 0.0f) {
289323
LOG_INF("- Phase-Based Ranging method: %f meters (derived from %d samples)",
290-
(double)phase_slope_based_distance, context.mode_2_data_index);
324+
(double)phase_slope_based_distance, context.iq_sample_channel_data_index);
291325
}
292326
}

0 commit comments

Comments
 (0)