diff --git a/CODEOWNERS b/CODEOWNERS index 35a62f07f935..a78c22ebf753 100644 --- a/CODEOWNERS +++ b/CODEOWNERS @@ -413,6 +413,7 @@ /samples/bluetooth/central_nfc_pairing/ @nrfconnect/ncs-si-muffin /samples/bluetooth/central_smp_client/ @nrfconnect/ncs-si-muffin /samples/bluetooth/central_uart/ @nrfconnect/ncs-si-muffin +/samples/bluetooth/channel_sounding_ras_initiator/ @nrfconnect/ncs-dragoon /samples/bluetooth/conn_time_sync/ @nrfconnect/ncs-dragoon /samples/bluetooth/direction_finding_central/ @nrfconnect/ncs-dragoon /samples/bluetooth/direction_finding_connectionless_rx/ @nrfconnect/ncs-dragoon diff --git a/samples/bluetooth/channel_sounding_ras_initiator/CMakeLists.txt b/samples/bluetooth/channel_sounding_ras_initiator/CMakeLists.txt new file mode 100644 index 000000000000..590c689328c7 --- /dev/null +++ b/samples/bluetooth/channel_sounding_ras_initiator/CMakeLists.txt @@ -0,0 +1,17 @@ +# +# Copyright (c) 2024 Nordic Semiconductor +# +# SPDX-License-Identifier: LicenseRef-Nordic-5-Clause +# + +cmake_minimum_required(VERSION 3.20.0) + +find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) +project(NONE) + +FILE(GLOB app_sources src/*.c) +# NORDIC SDK APP START +target_sources(app PRIVATE + ${app_sources} +) +# NORDIC SDK APP END diff --git a/samples/bluetooth/channel_sounding_ras_initiator/README.rst b/samples/bluetooth/channel_sounding_ras_initiator/README.rst new file mode 100644 index 000000000000..675933bcf446 --- /dev/null +++ b/samples/bluetooth/channel_sounding_ras_initiator/README.rst @@ -0,0 +1,98 @@ +.. _channel_sounding_ras_reflector: + +Bluetooth: Channel Sounding Reflector with Ranging Responder +############################################################ + +.. contents:: + :local: + :depth: 2 + +This sample demonstrates how to use the ranging service to request ranging data from a server. +It also provides a basic distance estimation algorithm to show how channel sounding can be used to estimate distance between two devices. + +Requirements +************ + +The sample supports the following development kits: + +.. table-from-sample-yaml:: + +The sample also requires a device running a Channel Sounding Reflector with Ranging Responder to connect to. + +Overview +******** + +The sample demonstrates a basic Bluetooth® Low Energy Central role functionality that acts as a GATT Ranging Requestor client and configures the Channel Sounding initiator role. +Regular Channel Sounding procedures are set up, local subevent data is stored and peer ranging data is fetched. + +A basic distance estimation algorithm is included in the sample. +The mathematical representations described in [#phase_and_amplitude]_ and [#rtt_packets]_ are used as the basis for this algorithm. + +User interface +************** + +The sample does not require user input and will scan for a device advertising with the GATT Ranging Service UUID. +The first LED on the development kit will be lit when a connection has been established. + +Building and running +******************** +.. |sample path| replace:: :file:`samples/bluetooth/channel_sounding_ras_initiator` + +.. include:: /includes/build_and_run.txt + +Testing +======= + +After programming the sample to your development kit, you can test it by connecting to another development kit with a Channel Sounding Reflector role with Ranging Responder. + +1. |connect_terminal_specific| +#. Reset the kit. +#. Program the other kit with the Channel Sounding Reflector with Ranging Responder sample. +#. Wait until the scanner detects the Peripheral. + In the terminal window, check for information similar to the following:: + + Filters matched. Address: XX:XX:XX:XX:XX:XX (random) connectable: 1 + Connecting + Connected to XX:XX:XX:XX:XX:XX (random) (err 0x00) + Security changed: XX:XX:XX:XX:XX:XX (random) level 2 + MTU exchange success (498) + The discovery procedure succeeded + CS capability exchange completed. + CS config creation complete. ID: 0 + CS security enabled. + CS procedures enabled. + Subevent result callback 0 + Ranging data ready 0 + Ranging data get completed for ranging counter 0 + Estimated distance to reflector: + - Round-Trip Timing method: X.XXXXX meters (derived from X samples) + - Phase-Based Ranging method: X.XXXXX meters (derived from X samples) + +Dependencies +************ + +This sample uses the following |NCS| libraries: + +* :ref:`dk_buttons_and_leds_readme` +* :file:`include/bluetooth/gatt_dm.h` +* :file:`include/bluetooth/services/ras.h` + +This sample uses the following Zephyr libraries: + +* :file:`include/sys/printk.h` +* :file:`include/zephyr/types.h` +* :ref:`zephyr:kernel_api`: + + * :file:`include/kernel.h` + +* :ref:`zephyr:bluetooth_api`: + +* :file:`include/bluetooth/bluetooth.h` +* :file:`include/bluetooth/conn.h` +* :file:`include/bluetooth/cs.h` + +References +********** + +.. [#phase_and_amplitude] `Bluetooth Core Specification v. 6.0: Vol. 1, Part A, 9.2 `_ +.. [#rtt_packets] `Bluetooth Core Specification v. 6.0: Vol. 1, Part A, 9.3 `_ diff --git a/samples/bluetooth/channel_sounding_ras_initiator/prj.conf b/samples/bluetooth/channel_sounding_ras_initiator/prj.conf new file mode 100644 index 000000000000..bd646707e839 --- /dev/null +++ b/samples/bluetooth/channel_sounding_ras_initiator/prj.conf @@ -0,0 +1,38 @@ +# +# Copyright (c) 2024 Nordic Semiconductor ASA +# +# SPDX-License-Identifier: LicenseRef-Nordic-5-Clause +# + +CONFIG_NCS_SAMPLES_DEFAULTS=y +CONFIG_DK_LIBRARY=y + +CONFIG_BT=y +CONFIG_BT_SMP=y +CONFIG_BT_CENTRAL=y +CONFIG_BT_MAX_CONN=1 +CONFIG_BT_BONDABLE=n + +CONFIG_BT_GATT_CLIENT=y +CONFIG_BT_GATT_DYNAMIC_DB=y + +CONFIG_BT_CHANNEL_SOUNDING=y +CONFIG_BT_RAS=y +CONFIG_BT_RAS_RREQ=y + +CONFIG_BT_SCAN=y +CONFIG_BT_SCAN_FILTER_ENABLE=y +CONFIG_BT_SCAN_UUID_CNT=1 + +# The Ranging Profile recommends a MTU of at least 247 octets. +CONFIG_BT_L2CAP_TX_MTU=498 +CONFIG_BT_BUF_ACL_TX_SIZE=502 +CONFIG_BT_BUF_ACL_RX_SIZE=502 +CONFIG_BT_ATT_PREPARE_COUNT=3 + +CONFIG_BT_CTLR_PHY_2M=y +CONFIG_BT_CTLR_DATA_LENGTH_MAX=251 + +CONFIG_HEAP_MEM_POOL_SIZE=1024 +CONFIG_MAIN_STACK_SIZE=4096 +CONFIG_CBPRINTF_FP_SUPPORT=y diff --git a/samples/bluetooth/channel_sounding_ras_initiator/sample.yaml b/samples/bluetooth/channel_sounding_ras_initiator/sample.yaml new file mode 100644 index 000000000000..1751f5cd9cc8 --- /dev/null +++ b/samples/bluetooth/channel_sounding_ras_initiator/sample.yaml @@ -0,0 +1,12 @@ +sample: + description: Bluetooth Low Energy Channel Sounding Initiator with Ranging Service Requestor + name: Bluetooth LE Channel Sounding Initiator with RREQ +tests: + sample.bluetooth.channel_sounding_ras_initiator: + sysbuild: true + build_only: true + integration_platforms: + - nrf54l15dk/nrf54l15/cpuapp + platform_allow: + - nrf54l15dk/nrf54l15/cpuapp + tags: bluetooth ci_build sysbuild diff --git a/samples/bluetooth/channel_sounding_ras_initiator/src/distance_estimation.c b/samples/bluetooth/channel_sounding_ras_initiator/src/distance_estimation.c new file mode 100644 index 000000000000..bc20c5fa33cf --- /dev/null +++ b/samples/bluetooth/channel_sounding_ras_initiator/src/distance_estimation.c @@ -0,0 +1,336 @@ +/* + * Copyright (c) 2024 Nordic Semiconductor ASA + * + * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause + */ + +/** @file + * @brief Channel Sounding distance estimation for Ranging Requestor + */ + +#include "zephyr/bluetooth/hci_types.h" +#include +#include +#include + +#define CS_FREQUENCY_MHZ(ch) (2402u + 1u * (ch)) +#define CS_FREQUENCY_HZ(ch) (CS_FREQUENCY_MHZ(ch) * 1000000.0f) +#define SPEED_OF_LIGHT_M_PER_S (299792458.0f) +#define SPEED_OF_LIGHT_NM_PER_S (SPEED_OF_LIGHT_M_PER_S / 1000000000.0f) +#define PI 3.14159265358979323846f +#define MAX_NUM_SAMPLES 256 + +struct iq_sample_and_channel { + bool failed; + uint8_t channel; + uint8_t antenna_permutation; + struct bt_le_cs_iq_sample local_iq_sample; + struct bt_le_cs_iq_sample peer_iq_sample; +}; + +struct rtt_timing { + bool failed; + int16_t toa_tod_initiator; + int16_t tod_toa_reflector; +}; + +static struct iq_sample_and_channel mode_2_data[MAX_NUM_SAMPLES]; +static struct rtt_timing mode_1_data[MAX_NUM_SAMPLES]; + +struct processing_context { + uint8_t mode_1_data_index; + uint8_t mode_2_data_index; + uint8_t n_ap; + enum bt_conn_le_cs_role role; +}; + +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) +{ + *z_out_real = z_a_real * z_b_real - z_a_imag * z_b_imag; + *z_out_imag = z_a_real * z_b_imag + z_a_imag * z_b_real; +} + +static float linear_regression(float *x_values, float *y_values, uint8_t n_samples) +{ + if (n_samples == 0) { + return 0.0; + } + + /* Estimates b in y = a + b x */ + + float y_mean = 0.0; + float x_mean = 0.0; + + for (uint8_t i = 0; i < n_samples; i++) { + y_mean += (y_values[i] - y_mean) / (i + 1); + x_mean += (x_values[i] - x_mean) / (i + 1); + } + + float b_est_upper = 0.0; + float b_est_lower = 0.0; + + for (uint8_t i = 0; i < n_samples; i++) { + b_est_upper += (x_values[i] - x_mean) * (y_values[i] - y_mean); + b_est_lower += (x_values[i] - x_mean) * (x_values[i] - x_mean); + } + + return b_est_upper / b_est_lower; +} + +static void bubblesort_2(float *array1, float *array2, uint16_t len) +{ + bool swapped; + float temp; + + for (uint16_t i = 0; i < len - 1; i++) { + swapped = false; + for (uint16_t j = 0; j < len - i - 1; j++) { + if (array1[j] > array1[j + 1]) { + temp = array1[j]; + array1[j] = array1[j + 1]; + array1[j + 1] = temp; + temp = array2[j]; + array2[j] = array2[j + 1]; + array2[j + 1] = temp; + swapped = true; + } + } + + if (!swapped) { + break; + } + } +} + +static float estimate_distance_using_phase_slope(struct iq_sample_and_channel *data, uint8_t len) +{ + int32_t combined_i; + int32_t combined_q; + uint16_t num_angles = 0; + static float theta[MAX_NUM_SAMPLES]; + static float frequencies[MAX_NUM_SAMPLES]; + + for (uint8_t i = 0; i < len; i++) { + if (!data[i].failed) { + 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); + + 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++; + } + } + + if (num_angles < 2) { + return 0.0; + } + + /* Sort phases by tone frequency */ + bubblesort_2(frequencies, theta, num_angles); + + /* One-dimensional phase unwrapping */ + for (uint8_t i = 1; i < num_angles; i++) { + float difference = theta[i] - theta[i - 1]; + + if (difference > PI) { + for (uint8_t j = i; j < num_angles; j++) { + theta[j] -= 2.0f * PI; + } + } else if (difference < -PI) { + for (uint8_t j = i; j < num_angles; j++) { + theta[j] += 2.0f * PI; + } + } + } + + float phase_slope = linear_regression(frequencies, theta, num_angles); + + float distance = -phase_slope * (SPEED_OF_LIGHT_M_PER_S / (4 * PI)); + + return distance / 1000000.0f; /* Scale to meters. */ +} + +static float estimate_distance_using_time_of_flight(uint8_t n_samples) +{ + float tof; + float tof_mean = 0.0; + + /* Cumulative Moving Average */ + for (uint8_t i = 0; i < n_samples; i++) { + if (!mode_1_data[i].failed) { + tof = (mode_1_data[i].toa_tod_initiator - + mode_1_data[i].tod_toa_reflector) / + 2; + tof_mean += (tof - tof_mean) / (i + 1); + } + } + + float tof_mean_ns = tof_mean / 2.0f; + + return tof_mean_ns * SPEED_OF_LIGHT_NM_PER_S; +} + +static bool process_local_step_data(struct bt_le_cs_subevent_step *step, void *user_data) +{ + struct processing_context *context = (struct processing_context *)user_data; + + if (step->mode == BT_CONN_LE_CS_MAIN_MODE_2) { + struct bt_hci_le_cs_step_data_mode_2 *step_data = + (struct bt_hci_le_cs_step_data_mode_2 *)step->data; + + for (uint8_t i = 0; i < (context->n_ap + 1); i++) { + if (step_data->tone_info[i].extension_indicator != + BT_HCI_LE_CS_NOT_TONE_EXT_SLOT) { + continue; + } + + mode_2_data[context->mode_2_data_index].channel = step->channel; + mode_2_data[context->mode_2_data_index].antenna_permutation = + step_data->antenna_permutation_index; + mode_2_data[context->mode_2_data_index].local_iq_sample = + bt_le_cs_parse_pct(step_data->tone_info[i].phase_correction_term); + if (step_data->tone_info[i].quality_indicator == + BT_HCI_LE_CS_TONE_QUALITY_LOW || + step_data->tone_info[i].quality_indicator == + BT_HCI_LE_CS_TONE_QUALITY_UNAVAILABLE) { + mode_2_data[context->mode_2_data_index].failed = true; + } + + context->mode_2_data_index++; + } + } else if (step->mode == BT_HCI_OP_LE_CS_MAIN_MODE_1) { + struct bt_hci_le_cs_step_data_mode_1 *step_data = + (struct bt_hci_le_cs_step_data_mode_1 *)step->data; + + if (step_data->packet_quality_aa_check != + BT_HCI_LE_CS_PACKET_QUALITY_AA_CHECK_SUCCESSFUL || + step_data->packet_rssi == BT_HCI_LE_CS_PACKET_RSSI_NOT_AVAILABLE || + step_data->tod_toa_reflector == BT_HCI_LE_CS_TIME_DIFFERENCE_NOT_AVAILABLE) { + mode_1_data[context->mode_1_data_index].failed = true; + } + + if (context->role == BT_CONN_LE_CS_ROLE_INITIATOR) { + mode_1_data[context->mode_1_data_index].toa_tod_initiator = + step_data->toa_tod_initiator; + } else if (context->role == BT_CONN_LE_CS_ROLE_REFLECTOR) { + mode_1_data[context->mode_1_data_index].tod_toa_reflector = + step_data->tod_toa_reflector; + } + + context->mode_1_data_index++; + } + + return true; +} + +static bool process_peer_step_data(struct ras_rd_cs_subevent_step *step, uint16_t *step_data_length, + void *user_data) +{ + struct processing_context *context = (struct processing_context *)user_data; + uint16_t step_data_len = 0; + + if (step->mode == BT_CONN_LE_CS_MAIN_MODE_2) { + struct bt_hci_le_cs_step_data_mode_2 *step_data = + (struct bt_hci_le_cs_step_data_mode_2 *)step->data; + step_data_len = + sizeof(struct bt_hci_le_cs_step_data_mode_2) + + (sizeof(struct bt_hci_le_cs_step_data_tone_info) * (context->n_ap + 1)); + + for (uint8_t i = 0; i < (context->n_ap + 1); i++) { + if (step_data->tone_info[i].extension_indicator != + BT_HCI_LE_CS_NOT_TONE_EXT_SLOT) { + continue; + } + + mode_2_data[context->mode_2_data_index].peer_iq_sample = + bt_le_cs_parse_pct(step_data->tone_info[i].phase_correction_term); + if (step_data->tone_info[i].quality_indicator == + BT_HCI_LE_CS_TONE_QUALITY_LOW || + step_data->tone_info[i].quality_indicator == + BT_HCI_LE_CS_TONE_QUALITY_UNAVAILABLE) { + mode_2_data[context->mode_2_data_index].failed = true; + } + + context->mode_2_data_index++; + } + } else if (step->mode == BT_HCI_OP_LE_CS_MAIN_MODE_1) { + struct bt_hci_le_cs_step_data_mode_1 *step_data = + (struct bt_hci_le_cs_step_data_mode_1 *)step->data; + step_data_len = sizeof(struct bt_hci_le_cs_step_data_mode_1); + + if (step_data->packet_quality_aa_check != + BT_HCI_LE_CS_PACKET_QUALITY_AA_CHECK_SUCCESSFUL || + step_data->packet_rssi == BT_HCI_LE_CS_PACKET_RSSI_NOT_AVAILABLE || + step_data->tod_toa_reflector == BT_HCI_LE_CS_TIME_DIFFERENCE_NOT_AVAILABLE) { + mode_1_data[context->mode_1_data_index].failed = true; + } + + if (context->role == BT_CONN_LE_CS_ROLE_INITIATOR) { + mode_1_data[context->mode_1_data_index].tod_toa_reflector = + step_data->tod_toa_reflector; + } else if (context->role == BT_CONN_LE_CS_ROLE_REFLECTOR) { + mode_1_data[context->mode_1_data_index].toa_tod_initiator = + step_data->toa_tod_initiator; + } + + context->mode_1_data_index++; + } else if (step->mode == 0) { + if (context->role == BT_CONN_LE_CS_ROLE_INITIATOR) { + step_data_len = sizeof(struct bt_hci_le_cs_step_data_mode_0_reflector); + } else { + step_data_len = sizeof(struct bt_hci_le_cs_step_data_mode_0_initiator); + } + } + + *step_data_length = step_data_len; + + return true; +} + +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) +{ + struct processing_context context = { + .mode_1_data_index = 0, + .mode_2_data_index = 0, + .n_ap = n_ap, + .role = role, + }; + + memset(mode_1_data, 0, sizeof(mode_1_data)); + memset(mode_2_data, 0, sizeof(mode_2_data)); + + bt_le_cs_step_data_parse(local_steps, process_local_step_data, &context); + + context.mode_1_data_index = 0; + context.mode_2_data_index = 0; + + struct ras_ranging_header *rd_header = + net_buf_simple_pull_mem(peer_steps, sizeof(struct ras_ranging_header)); + (void)rd_header; + + bt_ras_rreq_rd_subevent_data_parse(peer_steps, NULL, process_peer_step_data, &context); + + float phase_slope_based_distance = + estimate_distance_using_phase_slope(mode_2_data, context.mode_2_data_index); + + float rtt_based_distance = + estimate_distance_using_time_of_flight(context.mode_1_data_index); + + if (rtt_based_distance == 0.0f && phase_slope_based_distance == 0.0f) { + printk("A reliable distance estimate could not be computed.\n"); + } else { + printk("Estimated distance to reflector:\n"); + } + + if (rtt_based_distance != 0.0f) { + printk("- Round-Trip Timing method: %f meters (derived from %d samples)\n", + (double)rtt_based_distance, context.mode_1_data_index); + } + if (phase_slope_based_distance != 0.0f) { + printk("- Phase-Based Ranging method: %f meters (derived from %d samples)\n", + (double)phase_slope_based_distance, context.mode_2_data_index); + } +} diff --git a/samples/bluetooth/channel_sounding_ras_initiator/src/distance_estimation.h b/samples/bluetooth/channel_sounding_ras_initiator/src/distance_estimation.h new file mode 100644 index 000000000000..11c9a888def0 --- /dev/null +++ b/samples/bluetooth/channel_sounding_ras_initiator/src/distance_estimation.h @@ -0,0 +1,11 @@ +/* + * Copyright (c) 2024 Nordic Semiconductor ASA + * + * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause + */ + +#include +#include + +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); diff --git a/samples/bluetooth/channel_sounding_ras_initiator/src/main.c b/samples/bluetooth/channel_sounding_ras_initiator/src/main.c new file mode 100644 index 000000000000..19498eee8549 --- /dev/null +++ b/samples/bluetooth/channel_sounding_ras_initiator/src/main.c @@ -0,0 +1,569 @@ +/* + * Copyright (c) 2024 Nordic Semiconductor ASA + * + * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause + */ + +/** @file + * @brief Channel Sounding Initiator with Ranging Requestor sample + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "distance_estimation.h" + +#include + +#define CON_STATUS_LED DK_LED1 + +#define CS_CONFIG_ID 0 +#define NUM_MODE_0_STEPS 1 +#define PROCEDURE_COUNTER_NONE (-1) + +static K_SEM_DEFINE(sem_remote_capabilities_obtained, 0, 1); +static K_SEM_DEFINE(sem_config_created, 0, 1); +static K_SEM_DEFINE(sem_cs_security_enabled, 0, 1); +static K_SEM_DEFINE(sem_procedure_done, 0, 1); +static K_SEM_DEFINE(sem_connected, 0, 1); +static K_SEM_DEFINE(sem_rd_ready, 0, 1); +static K_SEM_DEFINE(sem_discovery_done, 0, 1); +static K_SEM_DEFINE(sem_mtu_exchange_done, 0, 1); +static K_SEM_DEFINE(sem_rd_complete, 0, 1); +static K_SEM_DEFINE(sem_security, 0, 1); + +static struct bt_conn *connection; +static uint8_t n_ap; +static uint8_t latest_num_steps_reported; +NET_BUF_SIMPLE_DEFINE_STATIC(latest_local_steps, 5500); +NET_BUF_SIMPLE_DEFINE_STATIC(latest_peer_steps, 5500); +static int32_t most_recent_peer_ranging_counter = PROCEDURE_COUNTER_NONE; +static int32_t most_recent_local_ranging_counter = PROCEDURE_COUNTER_NONE; +static int32_t dropped_ranging_counter = PROCEDURE_COUNTER_NONE; +static bool distance_estimation_in_progress; +static bool reconnecting; + +static void subevent_result_cb(struct bt_conn *conn, struct bt_conn_le_cs_subevent_result *result) +{ + printk("Subevent result callback %d\n", result->header.procedure_counter); + + if (distance_estimation_in_progress) { + printk("New procedure data received whilst estimating previous distance, drop this " + "procedure.\n"); + dropped_ranging_counter = result->header.procedure_counter; + return; + } + + if (dropped_ranging_counter == result->header.procedure_counter) { + return; + } + + dropped_ranging_counter = PROCEDURE_COUNTER_NONE; + latest_num_steps_reported = result->header.num_steps_reported; + n_ap = result->header.num_antenna_paths; + + if (result->step_data_buf) { + if (result->step_data_buf->len <= net_buf_simple_tailroom(&latest_local_steps)) { + uint16_t len = result->step_data_buf->len; + uint8_t *step_data = net_buf_simple_pull_mem(result->step_data_buf, len); + + net_buf_simple_add_mem(&latest_local_steps, step_data, len); + } else { + printk("Not enough memory to store step data. (%d > %d)\n", + latest_local_steps.len + result->step_data_buf->len, + latest_local_steps.size); + latest_num_steps_reported = 0; + } + } + + if (result->header.procedure_done_status == BT_CONN_LE_CS_PROCEDURE_COMPLETE) { + most_recent_local_ranging_counter = result->header.procedure_counter; + distance_estimation_in_progress = true; + k_sem_give(&sem_procedure_done); + } else if (result->header.procedure_done_status == BT_CONN_LE_CS_PROCEDURE_ABORTED) { + net_buf_simple_reset(&latest_local_steps); + } +} + +void ranging_data_get_complete_cb(struct bt_conn *conn, uint16_t ranging_counter, int err) +{ + ARG_UNUSED(conn); + + if (err) { + printk("Error when getting ranging data with ranging counter %d (err %d)\n", + ranging_counter, err); + return; + } + + printk("Ranging data get completed for ranging counter %d\n", ranging_counter); + k_sem_give(&sem_rd_complete); +} + +static void ranging_data_ready_cb(struct bt_conn *conn, uint16_t ranging_counter) +{ + printk("Ranging data ready %i\n", ranging_counter); + most_recent_peer_ranging_counter = ranging_counter; + k_sem_give(&sem_rd_ready); +} + +static void ranging_data_overwritten_cb(struct bt_conn *conn, uint16_t ranging_counter) +{ + printk("Ranging data overwritten %i\n", ranging_counter); +} + +static void mtu_exchange_cb(struct bt_conn *conn, uint8_t err, + struct bt_gatt_exchange_params *params) +{ + if (err) { + printk("MTU exchange failed (err %d)\n", err); + return; + } + + printk("MTU exchange success (%u)\n", bt_gatt_get_mtu(conn)); + k_sem_give(&sem_mtu_exchange_done); +} + +static void discovery_completed_cb(struct bt_gatt_dm *dm, void *context) +{ + int err; + + printk("The discovery procedure succeeded\n"); + + struct bt_conn *conn = bt_gatt_dm_conn_get(dm); + + bt_gatt_dm_data_print(dm); + + err = bt_ras_rreq_alloc_and_assign_handles(dm, conn); + if (err) { + printk("RAS RREQ alloc init failed (err %d)\n", err); + } + + err = bt_gatt_dm_data_release(dm); + if (err) { + printk("Could not release the discovery data (err %d)\n", err); + } + + k_sem_give(&sem_discovery_done); +} + +static void discovery_service_not_found_cb(struct bt_conn *conn, void *context) +{ + printk("The service could not be found during the discovery, disconnecting\n"); + bt_conn_disconnect(connection, BT_HCI_ERR_REMOTE_USER_TERM_CONN); +} + +static void discovery_error_found_cb(struct bt_conn *conn, int err, void *context) +{ + printk("The discovery procedure failed (err %d)\n", err); + bt_conn_disconnect(connection, BT_HCI_ERR_REMOTE_USER_TERM_CONN); +} + +static struct bt_gatt_dm_cb discovery_cb = { + .completed = discovery_completed_cb, + .service_not_found = discovery_service_not_found_cb, + .error_found = discovery_error_found_cb, +}; + +static void security_changed(struct bt_conn *conn, bt_security_t level, enum bt_security_err err) +{ + char addr[BT_ADDR_LE_STR_LEN]; + + bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr)); + + if (err) { + printk("Security failed: %s level %u err %d %s\n", addr, level, err, + bt_security_err_to_str(err)); + return; + } + + printk("Security changed: %s level %u\n", addr, level); + k_sem_give(&sem_security); +} + +static bool le_param_req(struct bt_conn *conn, struct bt_le_conn_param *param) +{ + /* Ignore peer parameter preferences. */ + return false; +} + +static void connected_cb(struct bt_conn *conn, uint8_t err) +{ + char addr[BT_ADDR_LE_STR_LEN]; + + (void)bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr)); + printk("Connected to %s (err 0x%02X)\n", addr, err); + + if (err) { + bt_conn_unref(conn); + connection = NULL; + } + + connection = bt_conn_ref(conn); + + k_sem_give(&sem_connected); + + dk_set_led_on(CON_STATUS_LED); +} + +static void disconnected_cb(struct bt_conn *conn, uint8_t reason) +{ + int err; + + printk("Disconnected (reason 0x%02X)\n", reason); + + bt_conn_unref(conn); + connection = NULL; + reconnecting = true; + dk_set_led_off(CON_STATUS_LED); + + err = bt_scan_start(BT_SCAN_TYPE_SCAN_PASSIVE); + if (err) { + printk("Restarting scanning failed (err %i)\n", err); + } + + printk("Scanner restarted\n"); +} + +static void remote_capabilities_cb(struct bt_conn *conn, struct bt_conn_le_cs_capabilities *params) +{ + ARG_UNUSED(params); + printk("CS capability exchange completed.\n"); + k_sem_give(&sem_remote_capabilities_obtained); +} + +static void config_created_cb(struct bt_conn *conn, struct bt_conn_le_cs_config *config) +{ + printk("CS config creation complete. ID: %d\n", config->id); + k_sem_give(&sem_config_created); +} + +static void security_enabled_cb(struct bt_conn *conn) +{ + printk("CS security enabled.\n"); + k_sem_give(&sem_cs_security_enabled); +} + +static void procedure_enabled_cb(struct bt_conn *conn, + struct bt_conn_le_cs_procedure_enable_complete *params) +{ + if (params->state == 1) { + printk("CS procedures enabled.\n"); + } else { + printk("CS procedures disabled.\n"); + } +} + +static void scan_filter_match(struct bt_scan_device_info *device_info, + struct bt_scan_filter_match *filter_match, bool connectable) +{ + char addr[BT_ADDR_LE_STR_LEN]; + + bt_addr_le_to_str(device_info->recv_info->addr, addr, sizeof(addr)); + + printk("Filters matched. Address: %s connectable: %d\n", addr, connectable); +} + +static void scan_connecting_error(struct bt_scan_device_info *device_info) +{ + int err; + + printk("Connecting failed, restarting scanning\n"); + + err = bt_scan_start(BT_SCAN_TYPE_SCAN_PASSIVE); + if (err) { + printk("Failed to restart scanning (err %i)\n", err); + return; + } +} + +static void scan_connecting(struct bt_scan_device_info *device_info, struct bt_conn *conn) +{ + printk("Connecting\n"); +} + +BT_SCAN_CB_INIT(scan_cb, scan_filter_match, NULL, scan_connecting_error, scan_connecting); + +static int scan_init(void) +{ + int err; + + struct bt_scan_init_param param = { + .scan_param = NULL, .conn_param = BT_LE_CONN_PARAM_DEFAULT, .connect_if_match = 1}; + + bt_scan_init(¶m); + bt_scan_cb_register(&scan_cb); + + err = bt_scan_filter_add(BT_SCAN_FILTER_TYPE_UUID, BT_UUID_RANGING_SERVICE); + if (err) { + printk("Scanning filters cannot be set (err %d)\n", err); + return err; + } + + err = bt_scan_filter_enable(BT_SCAN_UUID_FILTER, false); + if (err) { + printk("Filters cannot be turned on (err %d)\n", err); + return err; + } + + return 0; +} + +BT_CONN_CB_DEFINE(conn_cb) = { + .connected = connected_cb, + .disconnected = disconnected_cb, + .le_param_req = le_param_req, + .security_changed = security_changed, + .le_cs_remote_capabilities_available = remote_capabilities_cb, + .le_cs_config_created = config_created_cb, + .le_cs_security_enabled = security_enabled_cb, + .le_cs_procedure_enabled = procedure_enabled_cb, + .le_cs_subevent_data_available = subevent_result_cb, +}; + +int main(void) +{ + int err; + + printk("Starting Channel Sounding Initiator Demo\n"); + + dk_leds_init(); + + err = bt_enable(NULL); + if (err) { + printk("Bluetooth init failed (err %d)\n", err); + return 0; + } + + err = scan_init(); + if (err) { + printk("Scan init failed (err %d)\n", err); + return 0; + } + + err = bt_scan_start(BT_SCAN_TYPE_SCAN_PASSIVE); + if (err) { + printk("Scanning failed to start (err %i)\n", err); + return 0; + } + + while (true) { + err = k_sem_take(&sem_connected, K_FOREVER); + __ASSERT_NO_MSG(!err); + + reconnecting = false; + + err = bt_conn_set_security(connection, BT_SECURITY_L2); + if (err) { + printk("Failed to encrypt connection (err %d)\n", err); + return 0; + } + + err = k_sem_take(&sem_security, K_SECONDS(5)); + if (err) { + printk("Timeout waiting for pairing (err %d)\n", err); + return 0; + } + + static struct bt_gatt_exchange_params mtu_exchange_params = { + .func = mtu_exchange_cb}; + + err = bt_gatt_exchange_mtu(connection, &mtu_exchange_params); + if (err) { + printk("MTU exchange failed (err %d)\n", err); + } + + err = k_sem_take(&sem_mtu_exchange_done, K_SECONDS(5)); + if (err) { + printk("Timeout waiting for mtu exchange (err %d)\n", err); + return 0; + } + + err = bt_gatt_dm_start(connection, BT_UUID_RANGING_SERVICE, &discovery_cb, NULL); + if (err) { + printk("Discovery failed (err %d)\n", err); + return 0; + } + + err = k_sem_take(&sem_discovery_done, K_SECONDS(5)); + if (err) { + printk("Timeout waiting for gatt discovery (err %d)\n", err); + return 0; + } + + const struct bt_le_cs_set_default_settings_param default_settings = { + .enable_initiator_role = true, + .enable_reflector_role = false, + .cs_sync_antenna_selection = BT_LE_CS_ANTENNA_SELECTION_OPT_REPETITIVE, + .max_tx_power = BT_HCI_OP_LE_CS_MAX_MAX_TX_POWER, + }; + + err = bt_le_cs_set_default_settings(connection, &default_settings); + if (err) { + printk("Failed to configure default CS settings (err %d)\n", err); + return 0; + } + + err = bt_ras_rreq_rd_overwritten_subscribe(connection, ranging_data_overwritten_cb); + if (err) { + printk("RAS RREQ ranging data overwritten subscribe failed (err %d)\n", + err); + return 0; + } + + err = bt_ras_rreq_rd_ready_subscribe(connection, ranging_data_ready_cb); + if (err) { + printk("RAS RREQ ranging data ready subscribe failed (err %d)\n", err); + return 0; + } + + err = bt_ras_rreq_on_demand_rd_subscribe(connection); + if (err) { + printk("RAS RREQ On-demand ranging data subscribe failed (err %d)\n", err); + return 0; + } + + err = bt_ras_rreq_cp_subscribe(connection); + if (err) { + printk("RAS RREQ CP subscribe failed (err %d)\n", err); + return 0; + } + + err = bt_le_cs_read_remote_supported_capabilities(connection); + if (err) { + printk("Failed to exchange CS capabilities (err %d)\n", err); + return 0; + } + + k_sem_take(&sem_remote_capabilities_obtained, K_SECONDS(5)); + if (err) { + printk("Timeout waiting for CS capabilities (err %d)\n", err); + return 0; + } + + struct bt_le_cs_create_config_params config_params = { + .id = CS_CONFIG_ID, + .main_mode_type = BT_CONN_LE_CS_MAIN_MODE_2, + .sub_mode_type = BT_CONN_LE_CS_SUB_MODE_1, + .min_main_mode_steps = 2, + .max_main_mode_steps = 50, + .main_mode_repetition = 0, + .mode_0_steps = NUM_MODE_0_STEPS, + .role = BT_CONN_LE_CS_ROLE_INITIATOR, + .rtt_type = BT_CONN_LE_CS_RTT_TYPE_AA_ONLY, + .cs_sync_phy = BT_CONN_LE_CS_SYNC_1M_PHY, + .channel_map_repetition = 10, + .channel_selection_type = BT_CONN_LE_CS_CHSEL_TYPE_3B, + .ch3c_shape = BT_CONN_LE_CS_CH3C_SHAPE_HAT, + .ch3c_jump = 2, + }; + + bt_le_cs_set_valid_chmap_bits(config_params.channel_map); + + err = bt_le_cs_create_config(connection, &config_params, + BT_LE_CS_CREATE_CONFIG_CONTEXT_LOCAL_AND_REMOTE); + if (err) { + printk("Failed to create CS config (err %d)\n", err); + return 0; + } + + k_sem_take(&sem_config_created, K_SECONDS(5)); + if (err) { + printk("Timeout waiting for CS config (err %d)\n", err); + return 0; + } + + err = bt_le_cs_security_enable(connection); + if (err) { + printk("Failed to start CS Security (err %d)\n", err); + return 0; + } + + k_sem_take(&sem_cs_security_enabled, K_SECONDS(5)); + if (err) { + printk("Timeout waiting CS Security (err %d)\n", err); + return 0; + } + + const struct bt_le_cs_set_procedure_parameters_param procedure_params = { + .config_id = CS_CONFIG_ID, + .max_procedure_len = 200, + .min_procedure_interval = 100, + .max_procedure_interval = 100, + .max_procedure_count = 0, + .min_subevent_len = 120000, + .max_subevent_len = 120000, + .tone_antenna_config_selection = + BT_LE_CS_TONE_ANTENNA_CONFIGURATION_INDEX_ONE, + .phy = BT_LE_CS_PROCEDURE_PHY_1M, + .tx_power_delta = 0x80, + .preferred_peer_antenna = BT_LE_CS_PROCEDURE_PREFERRED_PEER_ANTENNA_1, + .snr_control_initiator = BT_LE_CS_INITIATOR_SNR_CONTROL_NOT_USED, + .snr_control_reflector = BT_LE_CS_REFLECTOR_SNR_CONTROL_NOT_USED, + }; + + err = bt_le_cs_set_procedure_parameters(connection, &procedure_params); + if (err) { + printk("Failed to set procedure parameters (err %d)\n", err); + return 0; + } + + struct bt_le_cs_procedure_enable_param params = { + .config_id = CS_CONFIG_ID, + .enable = 1, + }; + + err = bt_le_cs_procedure_enable(connection, ¶ms); + if (err) { + printk("Failed to enable CS procedures (err %d)\n", err); + return 0; + } + + while (!reconnecting) { + k_sem_take(&sem_procedure_done, K_FOREVER); + distance_estimation_in_progress = true; + + err = k_sem_take(&sem_rd_ready, K_SECONDS(1)); + if (err) { + printk("Timeout waiting for ranging data ready (err %d)\n", err); + goto retry; + } + + if (most_recent_peer_ranging_counter != most_recent_local_ranging_counter) { + printk("Mismatch of local and peer ranging counters (%d != %d)\n", + most_recent_peer_ranging_counter, + most_recent_local_ranging_counter); + goto retry; + } + + err = bt_ras_rreq_cp_get_ranging_data(connection, &latest_peer_steps, + most_recent_peer_ranging_counter, + ranging_data_get_complete_cb); + if (err) { + printk("Get ranging data failed (err %d\n)", err); + goto retry; + } + + err = k_sem_take(&sem_rd_complete, K_SECONDS(5)); + if (err) { + printk("Timeout waiting for ranging data complete (err %d)\n", err); + goto retry; + } + + estimate_distance(&latest_local_steps, &latest_peer_steps, n_ap, + BT_CONN_LE_CS_ROLE_INITIATOR); + +retry: + net_buf_simple_reset(&latest_local_steps); + net_buf_simple_reset(&latest_peer_steps); + distance_estimation_in_progress = false; + } + } + + return 0; +}