From 6acb7075c921cf6d74d3ad93117245e0f0a90563 Mon Sep 17 00:00:00 2001 From: Ivan Iushkov Date: Tue, 18 Mar 2025 08:08:03 +0100 Subject: [PATCH] samples: bluetooth: distance measurement multiantenna improvements added better handling of multiple antenna path in Channel Sounding Initiator distance estimation algorithm. Now distance is calculated and printed separately for each antenna path Signed-off-by: Ivan Iushkov --- .../channel_sounding_ras_initiator/README.rst | 2 +- .../src/distance_estimation.c | 118 +++++++++++++++--- 2 files changed, 103 insertions(+), 17 deletions(-) diff --git a/samples/bluetooth/channel_sounding_ras_initiator/README.rst b/samples/bluetooth/channel_sounding_ras_initiator/README.rst index 8c064efc2d66..ee4c810acebd 100644 --- a/samples/bluetooth/channel_sounding_ras_initiator/README.rst +++ b/samples/bluetooth/channel_sounding_ras_initiator/README.rst @@ -66,7 +66,7 @@ After programming the sample to your development kit, you can test it by connect I: Ranging data get completed for ranging counter 0 I: Estimated distance to reflector: I: - Round-Trip Timing method: X.XXXXX meters (derived from X samples) - I: - Phase-Based Ranging method: X.XXXXX meters (derived from X samples) + I: - Phase-Based Ranging method: X.XXXXX meters (derived on antenna path A from X samples) Dependencies ************ diff --git a/samples/bluetooth/channel_sounding_ras_initiator/src/distance_estimation.c b/samples/bluetooth/channel_sounding_ras_initiator/src/distance_estimation.c index e0827cfc03d3..629baf4e5cd8 100644 --- a/samples/bluetooth/channel_sounding_ras_initiator/src/distance_estimation.c +++ b/samples/bluetooth/channel_sounding_ras_initiator/src/distance_estimation.c @@ -24,10 +24,65 @@ LOG_MODULE_DECLARE(app_main, LOG_LEVEL_INF); #define MAX_NUM_RTT_SAMPLES 256 #define MAX_NUM_IQ_SAMPLES 256 * CONFIG_BT_RAS_MAX_ANTENNA_PATHS +#define A1 (0) +#define A2 (1) +#define A3 (2) +#define A4 (3) + +/* Bluetooth Core Specification 6.0, Table 4.13, Antenna Path Permutation for N_AP=2. + * The last element corresponds to extension slot + */ +static uint8_t antenna_path_lut_n_ap_2[2][3] = { + {A1, A2, A2}, + {A2, A1, A1}, +}; + +/* Bluetooth Core Specification 6.0, Table 4.14, Antenna Path Permutation for N_AP=3. + * The last element corresponds to extension slot + */ +static uint8_t antenna_path_lut_n_ap_3[6][4] = { + {A1, A2, A3, A3}, + {A2, A1, A3, A3}, + {A1, A3, A2, A2}, + {A3, A1, A2, A2}, + {A3, A2, A1, A1}, + {A2, A3, A1, A1}, +}; + +/* Bluetooth Core Specification 6.0, Table 4.15, Antenna Path Permutation for N_AP=4. + * The last element corresponds to extension slot + */ +static uint8_t antenna_path_lut_n_ap_4[24][5] = { + {A1, A2, A3, A4, A4}, + {A2, A1, A3, A4, A4}, + {A1, A3, A2, A4, A4}, + {A3, A1, A2, A4, A4}, + {A3, A2, A1, A4, A4}, + {A2, A3, A1, A4, A4}, + {A1, A2, A4, A3, A3}, + {A2, A1, A4, A3, A3}, + {A1, A4, A2, A3, A3}, + {A4, A1, A2, A3, A3}, + {A4, A2, A1, A3, A3}, + {A2, A4, A1, A3, A3}, + {A1, A4, A3, A2, A2}, + {A4, A1, A3, A2, A2}, + {A1, A3, A4, A2, A2}, + {A3, A1, A4, A2, A2}, + {A3, A4, A1, A2, A2}, + {A4, A3, A1, A2, A2}, + {A4, A2, A3, A1, A1}, + {A2, A4, A3, A1, A1}, + {A4, A3, A2, A1, A1}, + {A3, A4, A2, A1, A1}, + {A3, A2, A4, A1, A1}, + {A2, A3, A4, A1, A1}, +}; + struct iq_sample_and_channel { bool failed; uint8_t channel; - uint8_t antenna_permutation; + uint8_t antenna_path; struct bt_le_cs_iq_sample local_iq_sample; struct bt_le_cs_iq_sample peer_iq_sample; }; @@ -48,6 +103,22 @@ struct processing_context { enum bt_conn_le_cs_role role; }; +static uint8_t get_antenna_path(uint8_t n_ap, + uint8_t antenna_path_permutation_index, + uint8_t antenna_index) +{ + if (n_ap == 2) { + return antenna_path_lut_n_ap_2[antenna_path_permutation_index][antenna_index]; + } + if (n_ap == 3) { + return antenna_path_lut_n_ap_3[antenna_path_permutation_index][antenna_index]; + } + if (n_ap == 4) { + return antenna_path_lut_n_ap_4[antenna_path_permutation_index][antenna_index]; + } + return 0; +} + static void 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) { @@ -107,7 +178,10 @@ static void bubblesort_2(float *array1, float *array2, uint16_t len) } } -static float estimate_distance_using_phase_slope(struct iq_sample_and_channel *data, uint8_t len) +static float estimate_distance_using_phase_slope(struct iq_sample_and_channel *data, + uint8_t len, + uint8_t ant_path, + uint8_t *samples_cnt) { int32_t combined_i; int32_t combined_q; @@ -116,7 +190,7 @@ static float estimate_distance_using_phase_slope(struct iq_sample_and_channel *d static float frequencies[MAX_NUM_IQ_SAMPLES]; for (uint8_t i = 0; i < len; i++) { - if (!data[i].failed) { + if (!data[i].failed && data[i].antenna_path == ant_path) { calc_complex_product(data[i].local_iq_sample.i, data[i].local_iq_sample.q, data[i].peer_iq_sample.i, data[i].peer_iq_sample.q, &combined_i, &combined_q); @@ -124,6 +198,7 @@ static float estimate_distance_using_phase_slope(struct iq_sample_and_channel *d theta[num_angles] = atan2(1.0 * combined_q, 1.0 * combined_i); frequencies[num_angles] = 1.0 * CS_FREQUENCY_MHZ(data[i].channel); num_angles++; + *samples_cnt += 1; } } @@ -193,8 +268,8 @@ static void process_tone_info_data(struct processing_context *context, } iq_sample_channel_data[context->iq_sample_channel_data_index].channel = channel; - iq_sample_channel_data[context->iq_sample_channel_data_index].antenna_permutation = - antenna_permutation_index; + iq_sample_channel_data[context->iq_sample_channel_data_index].antenna_path = + get_antenna_path(context->n_ap, antenna_permutation_index, i); iq_sample_channel_data[context->iq_sample_channel_data_index].local_iq_sample = bt_le_cs_parse_pct(local_tone_info[i].phase_correction_term); iq_sample_channel_data[context->iq_sample_channel_data_index].peer_iq_sample = @@ -290,6 +365,7 @@ static bool process_step_data(struct bt_le_cs_subevent_step *local_step, void estimate_distance(struct net_buf_simple *local_steps, struct net_buf_simple *peer_steps, uint8_t n_ap, enum bt_conn_le_cs_role role) { + bool distance_measurement_failed = true; struct processing_context context = { .rtt_timing_data_index = 0, .iq_sample_channel_data_index = 0, @@ -303,24 +379,34 @@ void estimate_distance(struct net_buf_simple *local_steps, struct net_buf_simple bt_ras_rreq_rd_subevent_data_parse(peer_steps, local_steps, context.role, NULL, process_step_data, &context); - float phase_slope_based_distance = estimate_distance_using_phase_slope( - iq_sample_channel_data, context.iq_sample_channel_data_index); + LOG_INF("Estimated distance to reflector:"); float rtt_based_distance = estimate_distance_using_time_of_flight(context.rtt_timing_data_index); - if (rtt_based_distance == 0.0f && phase_slope_based_distance == 0.0f) { - LOG_INF("A reliable distance estimate could not be computed."); - } else { - LOG_INF("Estimated distance to reflector:"); - } - if (rtt_based_distance != 0.0f) { LOG_INF("- Round-Trip Timing method: %f meters (derived from %d samples)", (double)rtt_based_distance, context.rtt_timing_data_index); + distance_measurement_failed = false; + } + for (int i = 0; i < n_ap; i++) { + uint8_t samples_cnt = 0; + float phase_slope_based_distance = estimate_distance_using_phase_slope( + iq_sample_channel_data, + context.iq_sample_channel_data_index, + i, + &samples_cnt); + + if (phase_slope_based_distance != 0.0f) { + LOG_INF("- Phase-Based Ranging method: %f meters " + "(derived on antenna path %d from %d samples)", + (double)phase_slope_based_distance, + i, + samples_cnt); + distance_measurement_failed = false; + } } - if (phase_slope_based_distance != 0.0f) { - LOG_INF("- Phase-Based Ranging method: %f meters (derived from %d samples)", - (double)phase_slope_based_distance, context.iq_sample_channel_data_index); + if (distance_measurement_failed) { + LOG_INF("- A reliable distance estimate could not be computed."); } }