diff --git a/samples/bluetooth/channel_sounding/README.rst b/samples/bluetooth/channel_sounding/README.rst new file mode 100644 index 00000000000..8f94a00b78c --- /dev/null +++ b/samples/bluetooth/channel_sounding/README.rst @@ -0,0 +1,133 @@ +.. zephyr:code-sample:: ble_cs + :name: Channel Sounding + :relevant-api: bt_gap bluetooth + + Use Channel Sounding functionality. + +Overview +******** + +These samples demonstrates how to use the Bluetooth Channel Sounding feature. + +The CS Test sample shows how to us the CS test command to override randomization of certain channel +sounding parameters, experiment with different configurations, or evaluate the RF medium. It can +be found under :zephyr_file:`samples/bluetooth/channel_sounding/cs_test`. + +The connected CS sample shows how to set up regular channel sounding procedures on a connection +between two devices. +It can be found under :zephyr_file:`samples/bluetooth/channel_sounding/connected_cs` + +A basic distance estimation algorithm is included in both. +The Channel Sounding feature does not mandate a specific algorithm for computing distance estimates, +but the mathematical representation described in [#phase_and_amplitude]_ and [#rtt_packets]_ is used +as a starting point for these samples. + +Distance estimation using channel sounding requires data from two devices, and for that reason +the channel sounding results in the sample are exchanged in a simple way using a GATT characteristic. +This limits the amount of data that can be processed at once to about 512 bytes from each device, +which is enough to estimate distance using a handful of RTT timings and PBR phase samples across +about 35-40 channels, assuming a single antenna path. + +Both samples will perform channel sounding procedures repeatedly and print regular distance estimates to +the console. They are designed assuming a single subevent per procedure. + +Diagrams illustrating the steps involved in setting up channel sounding procedures between two +connected devices are available in [#cs_setup_phase]_ and [#cs_start]_. + +Requirements +************ + +* Two boards with Bluetooth LE and Channel Sounding support (such as an :ref:`nRF54L15 `) +* A controller that supports the Channel Sounding feature + +Building and Running +******************** + +These samples can be found under :zephyr_file:`samples/bluetooth/channel_sounding` in +the Zephyr tree. + +See :zephyr:code-sample-category:`bluetooth` samples for details. + +These sample use two applications, so two devices need to be setup. +Flash one device with the initiator application, and another device with the +reflector application. + +The devices should perform distance estimations repeatedly every few seconds if they are close enough. + +Here is an example output from the connected CS sample: + +Reflector: + +.. code-block:: console + + *** Using Zephyr OS v3.7.99-585fbd2e318c *** + Starting Channel Sounding Demo + Connected to EC:E7:DB:66:14:86 (random) (err 0x00) + MTU exchange success (247) + Discovery: attr 0x20006a2c + UUID 87654321-4567-2389-1254-f67f9fedcba8 + Found expected UUID + CS capability exchange completed. + CS config creation complete. ID: 0 + CS security enabled. + CS procedures enabled. + + +Initiator: + +.. code-block:: console + + *** Using Zephyr OS v3.7.99-585fbd2e318c *** + Starting Channel Sounding Demo + Found device with name CS Sample, connecting... + Connected to C7:78:79:CD:16:B9 (random) (err 0x00) + MTU exchange success (247) + CS capability exchange completed. + CS config creation complete. ID: 0 + CS security enabled. + CS procedures enabled. + Estimated distance to reflector: + - Round-Trip Timing method: 2.633891 meters (derived from 7 samples) + - Phase-Based Ranging method: 0.511853 meters (derived from 38 samples) + + +Here is an example output from the CS Test sample: + +Reflector: + +.. code-block:: console + + *** Using Zephyr OS v3.7.99-585fbd2e318c *** + Starting Channel Sounding Demo + Connected to C7:78:79:CD:16:B9 (random) (err 0x00) + MTU exchange success (247) + Discovery: attr 0x20006544 + UUID 87654321-4567-2389-1254-f67f9fedcba8 + Found expected UUID + Disconnected (reason 0x13) + Re-running CS test... + + +Initiator: + +.. code-block:: console + + *** Using Zephyr OS v3.7.99-585fbd2e318c *** + Starting Channel Sounding Demo + Found device with name CS Test Sample, connecting... + Connected to EC:E7:DB:66:14:86 (random) (err 0x00) + MTU exchange success (247) + Estimated distance to reflector: + - Round-Trip Timing method: 0.374741 meters (derived from 4 samples) + - Phase-Based Ranging method: 0.588290 meters (derived from 35 samples) + Disconnected (reason 0x16) + Re-running CS test... + + +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 `_ +.. [#cs_setup_phase] `Bluetooth Core Specification v. 6.0: Vol. 6, Part D, 6.34 `_ +.. [#cs_start] `Bluetooth Core Specification v. 6.0: Vol. 6, Part D, 6.35 `_ diff --git a/samples/bluetooth/channel_sounding/connected_cs/initiator/CMakeLists.txt b/samples/bluetooth/channel_sounding/connected_cs/initiator/CMakeLists.txt new file mode 100644 index 00000000000..f64168ed874 --- /dev/null +++ b/samples/bluetooth/channel_sounding/connected_cs/initiator/CMakeLists.txt @@ -0,0 +1,11 @@ +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20.0) +find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) +project(connected_cs_initiator) + +target_sources(app PRIVATE ../../src/connected_cs_initiator.c ../../src/distance_estimation.c) + +zephyr_library_include_directories( + ${CMAKE_CURRENT_LIST_DIR}/../../include +) diff --git a/samples/bluetooth/channel_sounding/connected_cs/initiator/prj.conf b/samples/bluetooth/channel_sounding/connected_cs/initiator/prj.conf new file mode 100644 index 00000000000..2ed92de59e8 --- /dev/null +++ b/samples/bluetooth/channel_sounding/connected_cs/initiator/prj.conf @@ -0,0 +1,16 @@ +CONFIG_BT=y +CONFIG_BT_SMP=y +CONFIG_BT_CENTRAL=y +CONFIG_BT_GAP_AUTO_UPDATE_CONN_PARAMS=n +CONFIG_BT_GATT_CLIENT=y +CONFIG_BT_GATT_DYNAMIC_DB=y +CONFIG_BT_ATT_PREPARE_COUNT=3 +CONFIG_BT_CHANNEL_SOUNDING=y + +CONFIG_BT_BUF_ACL_RX_SIZE=251 +CONFIG_BT_BUF_ACL_TX_SIZE=251 +CONFIG_BT_L2CAP_TX_MTU=247 + +CONFIG_MAIN_STACK_SIZE=4096 + +CONFIG_CBPRINTF_FP_SUPPORT=y diff --git a/samples/bluetooth/channel_sounding/connected_cs/initiator/sample.yaml b/samples/bluetooth/channel_sounding/connected_cs/initiator/sample.yaml new file mode 100644 index 00000000000..05e0a484345 --- /dev/null +++ b/samples/bluetooth/channel_sounding/connected_cs/initiator/sample.yaml @@ -0,0 +1,11 @@ +sample: + name: Bluetooth Channel Sounding - Initiator +tests: + sample.bluetooth.channel_sounding.connected_cs.initiator: + harness: bluetooth + platform_allow: + - qemu_cortex_m3 + - qemu_x86 + tags: bluetooth + integration_platforms: + - qemu_cortex_m3 diff --git a/samples/bluetooth/channel_sounding/connected_cs/reflector/CMakeLists.txt b/samples/bluetooth/channel_sounding/connected_cs/reflector/CMakeLists.txt new file mode 100644 index 00000000000..6d455df7733 --- /dev/null +++ b/samples/bluetooth/channel_sounding/connected_cs/reflector/CMakeLists.txt @@ -0,0 +1,11 @@ +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20.0) +find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) +project(connected_cs_reflector) + +target_sources(app PRIVATE ../../src/connected_cs_reflector.c ../../src/distance_estimation.c) + +zephyr_library_include_directories( + ${CMAKE_CURRENT_LIST_DIR}/../../include +) diff --git a/samples/bluetooth/channel_sounding/connected_cs/reflector/prj.conf b/samples/bluetooth/channel_sounding/connected_cs/reflector/prj.conf new file mode 100644 index 00000000000..016d0de98c2 --- /dev/null +++ b/samples/bluetooth/channel_sounding/connected_cs/reflector/prj.conf @@ -0,0 +1,14 @@ +CONFIG_BT=y +CONFIG_BT_SMP=y +CONFIG_BT_PERIPHERAL=y +CONFIG_BT_GAP_AUTO_UPDATE_CONN_PARAMS=n +CONFIG_BT_GATT_CLIENT=y +CONFIG_BT_CHANNEL_SOUNDING=y + +CONFIG_BT_BUF_ACL_RX_SIZE=251 +CONFIG_BT_BUF_ACL_TX_SIZE=251 +CONFIG_BT_L2CAP_TX_MTU=247 + +CONFIG_MAIN_STACK_SIZE=4096 + +CONFIG_CBPRINTF_FP_SUPPORT=y diff --git a/samples/bluetooth/channel_sounding/connected_cs/reflector/sample.yaml b/samples/bluetooth/channel_sounding/connected_cs/reflector/sample.yaml new file mode 100644 index 00000000000..5495b7ba545 --- /dev/null +++ b/samples/bluetooth/channel_sounding/connected_cs/reflector/sample.yaml @@ -0,0 +1,11 @@ +sample: + name: Bluetooth Channel Sounding - Reflector +tests: + sample.bluetooth.channel_sounding.connected_cs.reflector: + harness: bluetooth + platform_allow: + - qemu_cortex_m3 + - qemu_x86 + tags: bluetooth + integration_platforms: + - qemu_cortex_m3 diff --git a/samples/bluetooth/channel_sounding/cs_test/initiator/CMakeLists.txt b/samples/bluetooth/channel_sounding/cs_test/initiator/CMakeLists.txt new file mode 100644 index 00000000000..6053ffaf4b9 --- /dev/null +++ b/samples/bluetooth/channel_sounding/cs_test/initiator/CMakeLists.txt @@ -0,0 +1,11 @@ +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20.0) +find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) +project(cs_test_initiator) + +target_sources(app PRIVATE ../../src/cs_test_initiator.c ../../src/distance_estimation.c) + +zephyr_library_include_directories( + ${CMAKE_CURRENT_LIST_DIR}/../../include +) diff --git a/samples/bluetooth/channel_sounding/cs_test/initiator/prj.conf b/samples/bluetooth/channel_sounding/cs_test/initiator/prj.conf new file mode 100644 index 00000000000..1e1e8d3c99d --- /dev/null +++ b/samples/bluetooth/channel_sounding/cs_test/initiator/prj.conf @@ -0,0 +1,15 @@ +CONFIG_BT=y +CONFIG_BT_CENTRAL=y +CONFIG_BT_GATT_CLIENT=y +CONFIG_BT_GATT_DYNAMIC_DB=y +CONFIG_BT_ATT_PREPARE_COUNT=3 +CONFIG_BT_CHANNEL_SOUNDING=y +CONFIG_BT_CHANNEL_SOUNDING_TEST=y + +CONFIG_BT_BUF_ACL_RX_SIZE=251 +CONFIG_BT_BUF_ACL_TX_SIZE=251 +CONFIG_BT_L2CAP_TX_MTU=247 + +CONFIG_MAIN_STACK_SIZE=4096 + +CONFIG_CBPRINTF_FP_SUPPORT=y diff --git a/samples/bluetooth/channel_sounding/cs_test/initiator/sample.yaml b/samples/bluetooth/channel_sounding/cs_test/initiator/sample.yaml new file mode 100644 index 00000000000..71d5ee81d63 --- /dev/null +++ b/samples/bluetooth/channel_sounding/cs_test/initiator/sample.yaml @@ -0,0 +1,11 @@ +sample: + name: Bluetooth Channel Sounding Test - Initiator +tests: + sample.bluetooth.channel_sounding.cs_test.initiator: + harness: bluetooth + platform_allow: + - qemu_cortex_m3 + - qemu_x86 + tags: bluetooth + integration_platforms: + - qemu_cortex_m3 diff --git a/samples/bluetooth/channel_sounding/cs_test/reflector/CMakeLists.txt b/samples/bluetooth/channel_sounding/cs_test/reflector/CMakeLists.txt new file mode 100644 index 00000000000..67304f16597 --- /dev/null +++ b/samples/bluetooth/channel_sounding/cs_test/reflector/CMakeLists.txt @@ -0,0 +1,11 @@ +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20.0) +find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) +project(cs_test_reflector) + +target_sources(app PRIVATE ../../src/cs_test_reflector.c ../../src/distance_estimation.c) + +zephyr_library_include_directories( + ${CMAKE_CURRENT_LIST_DIR}/../../include +) diff --git a/samples/bluetooth/channel_sounding/cs_test/reflector/prj.conf b/samples/bluetooth/channel_sounding/cs_test/reflector/prj.conf new file mode 100644 index 00000000000..37c3d79685f --- /dev/null +++ b/samples/bluetooth/channel_sounding/cs_test/reflector/prj.conf @@ -0,0 +1,13 @@ +CONFIG_BT=y +CONFIG_BT_PERIPHERAL=y +CONFIG_BT_GATT_CLIENT=y +CONFIG_BT_CHANNEL_SOUNDING=y +CONFIG_BT_CHANNEL_SOUNDING_TEST=y + +CONFIG_BT_BUF_ACL_RX_SIZE=251 +CONFIG_BT_BUF_ACL_TX_SIZE=251 +CONFIG_BT_L2CAP_TX_MTU=247 + +CONFIG_MAIN_STACK_SIZE=4096 + +CONFIG_CBPRINTF_FP_SUPPORT=y diff --git a/samples/bluetooth/channel_sounding/cs_test/reflector/sample.yaml b/samples/bluetooth/channel_sounding/cs_test/reflector/sample.yaml new file mode 100644 index 00000000000..ee0933a9e33 --- /dev/null +++ b/samples/bluetooth/channel_sounding/cs_test/reflector/sample.yaml @@ -0,0 +1,11 @@ +sample: + name: Bluetooth Channel Sounding Test - Reflector +tests: + sample.bluetooth.channel_sounding.cs_test.reflector: + harness: bluetooth + platform_allow: + - qemu_cortex_m3 + - qemu_x86 + tags: bluetooth + integration_platforms: + - qemu_cortex_m3 diff --git a/samples/bluetooth/channel_sounding/include/common.h b/samples/bluetooth/channel_sounding/include/common.h new file mode 100644 index 00000000000..940dd76a19e --- /dev/null +++ b/samples/bluetooth/channel_sounding/include/common.h @@ -0,0 +1,15 @@ +/* + * Copyright (c) 2024 Nordic Semiconductor ASA + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include + +#define NAME_LEN 30 +#define STEP_DATA_BUF_LEN 512 /* Maximum GATT characteristic length */ + +static struct bt_uuid_128 step_data_char_uuid = + BT_UUID_INIT_128(BT_UUID_128_ENCODE(0x87654321, 0x4567, 0x2389, 0x1254, 0xf67f9fedcba8)); +static const struct bt_uuid_128 step_data_svc_uuid = + BT_UUID_INIT_128(BT_UUID_128_ENCODE(0x87654321, 0x4567, 0x2389, 0x1254, 0xf67f9fedcba9)); diff --git a/samples/bluetooth/channel_sounding/include/cs_test_params.h b/samples/bluetooth/channel_sounding/include/cs_test_params.h new file mode 100644 index 00000000000..dc141167b98 --- /dev/null +++ b/samples/bluetooth/channel_sounding/include/cs_test_params.h @@ -0,0 +1,58 @@ +/* + * Copyright (c) 2024 Nordic Semiconductor ASA + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include + +#define INITIATOR_ACCESS_ADDRESS 0x4D7B8A2F +#define REFLECTOR_ACCESS_ADDRESS 0x96F93DB1 +#define NUM_MODE_0_STEPS 3 + +static struct bt_le_cs_test_param test_params_get(enum bt_conn_le_cs_role role) +{ + struct bt_le_cs_test_param params; + + params.role = role; + params.main_mode = BT_CONN_LE_CS_MAIN_MODE_2; + params.sub_mode = BT_CONN_LE_CS_SUB_MODE_1; + params.main_mode_repetition = 1; + params.mode_0_steps = NUM_MODE_0_STEPS; + params.rtt_type = BT_CONN_LE_CS_RTT_TYPE_AA_ONLY; + params.cs_sync_phy = BT_CONN_LE_CS_SYNC_1M_PHY; + params.cs_sync_antenna_selection = BT_LE_CS_TEST_CS_SYNC_ANTENNA_SELECTION_ONE; + params.subevent_len = 5000; + params.subevent_interval = 0; + params.max_num_subevents = 1; + params.transmit_power_level = BT_HCI_OP_LE_CS_TEST_MAXIMIZE_TX_POWER; + params.t_ip1_time = 145; + params.t_ip2_time = 145; + params.t_fcs_time = 150; + params.t_pm_time = 40; + params.t_sw_time = 0; + params.tone_antenna_config_selection = BT_LE_CS_TONE_ANTENNA_CONFIGURATION_INDEX_ONE; + + params.initiator_snr_control = BT_LE_CS_INITIATOR_SNR_CONTROL_NOT_USED; + params.reflector_snr_control = BT_LE_CS_REFLECTOR_SNR_CONTROL_NOT_USED; + + params.drbg_nonce = 0x1234; + + params.override_config = BIT(2) | BIT(5); + params.override_config_0.channel_map_repetition = 1; + + memset(params.override_config_0.not_set.channel_map, 0, 10); + + for (uint8_t i = 40; i < 75; i++) { + BT_LE_CS_CHANNEL_BIT_SET_VAL(params.override_config_0.not_set.channel_map, i, 1); + } + + params.override_config_0.not_set.channel_selection_type = BT_CONN_LE_CS_CHSEL_TYPE_3B; + params.override_config_0.not_set.ch3c_shape = BT_CONN_LE_CS_CH3C_SHAPE_HAT; + params.override_config_0.not_set.ch3c_jump = 2; + params.override_config_2.main_mode_steps = 8; + params.override_config_5.cs_sync_aa_initiator = INITIATOR_ACCESS_ADDRESS; + params.override_config_5.cs_sync_aa_reflector = REFLECTOR_ACCESS_ADDRESS; + + return params; +} diff --git a/samples/bluetooth/channel_sounding/include/distance_estimation.h b/samples/bluetooth/channel_sounding/include/distance_estimation.h new file mode 100644 index 00000000000..56f67f85736 --- /dev/null +++ b/samples/bluetooth/channel_sounding/include/distance_estimation.h @@ -0,0 +1,11 @@ +/* + * Copyright (c) 2024 Nordic Semiconductor ASA + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include + +void 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); diff --git a/samples/bluetooth/channel_sounding/src/connected_cs_initiator.c b/samples/bluetooth/channel_sounding/src/connected_cs_initiator.c new file mode 100644 index 00000000000..a6fb5c2ce88 --- /dev/null +++ b/samples/bluetooth/channel_sounding/src/connected_cs_initiator.c @@ -0,0 +1,356 @@ +/* + * Copyright (c) 2024 Nordic Semiconductor ASA + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include +#include +#include +#include +#include "distance_estimation.h" +#include "common.h" + +#define CS_CONFIG_ID 0 +#define NUM_MODE_0_STEPS 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_data_received, 0, 1); + +static ssize_t on_attr_write_cb(struct bt_conn *conn, const struct bt_gatt_attr *attr, + const void *buf, uint16_t len, uint16_t offset, uint8_t flags); +static struct bt_conn *connection; +static uint8_t n_ap; +static uint8_t latest_num_steps_reported; +static uint16_t latest_step_data_len; +static uint8_t latest_local_steps[STEP_DATA_BUF_LEN]; +static uint8_t latest_peer_steps[STEP_DATA_BUF_LEN]; + +static struct bt_gatt_attr gatt_attributes[] = { + BT_GATT_PRIMARY_SERVICE(&step_data_svc_uuid), + BT_GATT_CHARACTERISTIC(&step_data_char_uuid.uuid, BT_GATT_CHRC_WRITE, + BT_GATT_PERM_WRITE | BT_GATT_PERM_PREPARE_WRITE, NULL, + on_attr_write_cb, NULL), +}; +static struct bt_gatt_service step_data_gatt_service = BT_GATT_SERVICE(gatt_attributes); +static const char sample_str[] = "CS Sample"; + +static ssize_t on_attr_write_cb(struct bt_conn *conn, const struct bt_gatt_attr *attr, + const void *buf, uint16_t len, uint16_t offset, uint8_t flags) +{ + if (flags & BT_GATT_WRITE_FLAG_PREPARE) { + return 0; + } + + if (offset) { + return BT_GATT_ERR(BT_ATT_ERR_INVALID_OFFSET); + } + + if (len != sizeof(latest_local_steps)) { + return BT_GATT_ERR(BT_ATT_ERR_INVALID_ATTRIBUTE_LEN); + } + + if (flags & BT_GATT_WRITE_FLAG_EXECUTE) { + uint8_t *data = (uint8_t *)buf; + + memcpy(latest_peer_steps, &data[offset], len); + k_sem_give(&sem_data_received); + } + + return len; +} + +static void subevent_result_cb(struct bt_conn *conn, struct bt_conn_le_cs_subevent_result *result) +{ + 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 <= STEP_DATA_BUF_LEN) { + memcpy(latest_local_steps, result->step_data_buf->data, + result->step_data_buf->len); + latest_step_data_len = result->step_data_buf->len; + } else { + printk("Not enough memory to store step data. (%d > %d)\n", + result->step_data_buf->len, STEP_DATA_BUF_LEN); + latest_num_steps_reported = 0; + } + } + + if (result->header.procedure_done_status == BT_CONN_LE_CS_PROCEDURE_COMPLETE) { + k_sem_give(&sem_procedure_done); + } +} + +static void mtu_exchange_cb(struct bt_conn *conn, uint8_t err, + struct bt_gatt_exchange_params *params) +{ + printk("MTU exchange %s (%u)\n", err == 0U ? "success" : "failed", bt_gatt_get_mtu(conn)); +} + +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); + + __ASSERT(connection == conn, "Unexpected connected callback"); + + if (err) { + bt_conn_unref(conn); + connection = NULL; + } + + 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("%s: MTU exchange failed (err %d)\n", __func__, err); + } + + k_sem_give(&sem_connected); +} + +static void disconnected_cb(struct bt_conn *conn, uint8_t reason) +{ + printk("Disconnected (reason 0x%02X)\n", reason); + + bt_conn_unref(conn); + connection = NULL; +} + +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 bool data_cb(struct bt_data *data, void *user_data) +{ + char *name = user_data; + uint8_t len; + + switch (data->type) { + case BT_DATA_NAME_SHORTENED: + case BT_DATA_NAME_COMPLETE: + len = MIN(data->data_len, NAME_LEN - 1); + memcpy(name, data->data, len); + name[len] = '\0'; + return false; + default: + return true; + } +} + +static void device_found(const bt_addr_le_t *addr, int8_t rssi, uint8_t type, + struct net_buf_simple *ad) +{ + char addr_str[BT_ADDR_LE_STR_LEN]; + char name[NAME_LEN] = {}; + int err; + + if (connection) { + return; + } + + /* We're only interested in connectable events */ + if (type != BT_GAP_ADV_TYPE_ADV_IND && type != BT_GAP_ADV_TYPE_ADV_DIRECT_IND) { + return; + } + + bt_data_parse(ad, data_cb, name); + + if (strcmp(name, sample_str)) { + return; + } + + if (bt_le_scan_stop()) { + return; + } + + printk("Found device with name %s, connecting...\n", name); + + err = bt_conn_le_create(addr, BT_CONN_LE_CREATE_CONN, BT_LE_CONN_PARAM_DEFAULT, + &connection); + if (err) { + printk("Create conn to %s failed (%u)\n", addr_str, err); + } +} + +BT_CONN_CB_DEFINE(conn_cb) = { + .connected = connected_cb, + .disconnected = disconnected_cb, + .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 Demo\n"); + + /* Initialize the Bluetooth Subsystem */ + err = bt_enable(NULL); + if (err) { + printk("Bluetooth init failed (err %d)\n", err); + return 0; + } + + err = bt_gatt_service_register(&step_data_gatt_service); + if (err) { + printk("bt_gatt_service_register() returned err %d\n", err); + return 0; + } + + err = bt_le_scan_start(BT_LE_SCAN_ACTIVE_CONTINUOUS, device_found); + if (err) { + printk("Scanning failed to start (err %d)\n", err); + return 0; + } + + k_sem_take(&sem_connected, K_FOREVER); + + 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); + } + + err = bt_conn_set_security(connection, BT_SECURITY_L2); + if (err) { + printk("Failed to encrypt connection (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_FOREVER); + + 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 = 10, + .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 = 1, + .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_FOREVER); + + 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_FOREVER); + + const struct bt_le_cs_set_procedure_parameters_param procedure_params = { + .config_id = CS_CONFIG_ID, + .max_procedure_len = 12, + .min_procedure_interval = 100, + .max_procedure_interval = 100, + .max_procedure_count = 0, + .min_subevent_len = 6750, + .max_subevent_len = 6750, + .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 (true) { + k_sem_take(&sem_procedure_done, K_FOREVER); + k_sem_take(&sem_data_received, K_FOREVER); + + estimate_distance( + latest_local_steps, latest_step_data_len, latest_peer_steps, + latest_step_data_len - + NUM_MODE_0_STEPS * + (sizeof(struct bt_hci_le_cs_step_data_mode_0_initiator) - + sizeof(struct bt_hci_le_cs_step_data_mode_0_reflector)), + n_ap, BT_CONN_LE_CS_ROLE_INITIATOR); + } + + return 0; +} diff --git a/samples/bluetooth/channel_sounding/src/connected_cs_reflector.c b/samples/bluetooth/channel_sounding/src/connected_cs_reflector.c new file mode 100644 index 00000000000..2563d604387 --- /dev/null +++ b/samples/bluetooth/channel_sounding/src/connected_cs_reflector.c @@ -0,0 +1,248 @@ +/* + * Copyright (c) 2024 Nordic Semiconductor ASA + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include +#include +#include +#include +#include "common.h" + +#define CS_CONFIG_ID 0 +#define NUM_MODE_0_STEPS 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_discovered, 0, 1); +static K_SEM_DEFINE(sem_written, 0, 1); + +static uint16_t step_data_attr_handle; +static struct bt_conn *connection; +static uint8_t latest_local_steps[STEP_DATA_BUF_LEN]; + +static const char sample_str[] = "CS Sample"; +static const struct bt_data ad[] = { + BT_DATA(BT_DATA_NAME_COMPLETE, "CS Sample", sizeof(sample_str) - 1), +}; + +static void subevent_result_cb(struct bt_conn *conn, struct bt_conn_le_cs_subevent_result *result) +{ + if (result->step_data_buf) { + if (result->step_data_buf->len <= STEP_DATA_BUF_LEN) { + memcpy(latest_local_steps, result->step_data_buf->data, + result->step_data_buf->len); + } else { + printk("Not enough memory to store step data. (%d > %d)\n", + result->step_data_buf->len, STEP_DATA_BUF_LEN); + } + } + + if (result->header.procedure_done_status == BT_CONN_LE_CS_PROCEDURE_COMPLETE) { + k_sem_give(&sem_procedure_done); + } +} + +static void mtu_exchange_cb(struct bt_conn *conn, uint8_t err, + struct bt_gatt_exchange_params *params) +{ + printk("MTU exchange %s (%u)\n", err == 0U ? "success" : "failed", bt_gatt_get_mtu(conn)); +} + +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); + + __ASSERT(connection == conn, "Unexpected connected callback"); + + if (err) { + bt_conn_unref(conn); + connection = NULL; + } + + connection = bt_conn_ref(conn); + + 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("%s: MTU exchange failed (err %d)\n", __func__, err); + } + + k_sem_give(&sem_connected); +} + +static void disconnected_cb(struct bt_conn *conn, uint8_t reason) +{ + printk("Disconnected (reason 0x%02X)\n", reason); + + bt_conn_unref(conn); + connection = NULL; +} + +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 uint8_t discover_func(struct bt_conn *conn, const struct bt_gatt_attr *attr, + struct bt_gatt_discover_params *params) +{ + struct bt_gatt_chrc *chrc; + char str[BT_UUID_STR_LEN]; + + printk("Discovery: attr %p\n", attr); + + if (!attr) { + return BT_GATT_ITER_STOP; + } + + chrc = (struct bt_gatt_chrc *)attr->user_data; + + bt_uuid_to_str(chrc->uuid, str, sizeof(str)); + printk("UUID %s\n", str); + + if (!bt_uuid_cmp(chrc->uuid, &step_data_char_uuid.uuid)) { + step_data_attr_handle = chrc->value_handle; + + printk("Found expected UUID\n"); + + k_sem_give(&sem_discovered); + } + + return BT_GATT_ITER_STOP; +} + +static void write_func(struct bt_conn *conn, uint8_t err, struct bt_gatt_write_params *params) +{ + if (err) { + printk("Write failed (err %d)\n", err); + + return; + } + + k_sem_give(&sem_written); +} + +BT_CONN_CB_DEFINE(conn_cb) = { + .connected = connected_cb, + .disconnected = disconnected_cb, + .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; + struct bt_gatt_discover_params discover_params; + struct bt_gatt_write_params write_params; + + printk("Starting Channel Sounding Demo\n"); + + /* Initialize the Bluetooth Subsystem */ + err = bt_enable(NULL); + if (err) { + printk("Bluetooth init failed (err %d)\n", err); + return 0; + } + + err = bt_le_adv_start(BT_LE_ADV_PARAM(BT_LE_ADV_OPT_CONN, BT_GAP_ADV_FAST_INT_MIN_1, + BT_GAP_ADV_FAST_INT_MAX_1, NULL), + ad, ARRAY_SIZE(ad), NULL, 0); + if (err) { + printk("Advertising failed to start (err %d)\n", err); + return 0; + } + + k_sem_take(&sem_connected, K_FOREVER); + + const struct bt_le_cs_set_default_settings_param default_settings = { + .enable_initiator_role = false, + .enable_reflector_role = true, + .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); + } + + discover_params.uuid = &step_data_char_uuid.uuid; + discover_params.func = discover_func; + discover_params.start_handle = BT_ATT_FIRST_ATTRIBUTE_HANDLE; + discover_params.end_handle = BT_ATT_LAST_ATTRIBUTE_HANDLE; + discover_params.type = BT_GATT_DISCOVER_CHARACTERISTIC; + + err = bt_gatt_discover(connection, &discover_params); + if (err) { + printk("Discovery failed (err %d)\n", err); + return 0; + } + + err = k_sem_take(&sem_discovered, K_SECONDS(10)); + if (err) { + printk("Timed out during GATT discovery\n"); + return 0; + } + + while (true) { + k_sem_take(&sem_procedure_done, K_FOREVER); + + write_params.func = write_func; + write_params.handle = step_data_attr_handle; + write_params.length = STEP_DATA_BUF_LEN; + write_params.data = &latest_local_steps[0]; + write_params.offset = 0; + + err = bt_gatt_write(connection, &write_params); + if (err) { + printk("Write failed (err %d)\n", err); + return 0; + } + + err = k_sem_take(&sem_written, K_SECONDS(10)); + if (err) { + printk("Timed out during GATT write\n"); + return 0; + } + } + + return 0; +} diff --git a/samples/bluetooth/channel_sounding/src/cs_test_initiator.c b/samples/bluetooth/channel_sounding/src/cs_test_initiator.c new file mode 100644 index 00000000000..c733df18657 --- /dev/null +++ b/samples/bluetooth/channel_sounding/src/cs_test_initiator.c @@ -0,0 +1,276 @@ +/* + * Copyright (c) 2024 Nordic Semiconductor ASA + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include +#include +#include +#include +#include +#include "distance_estimation.h" +#include "common.h" +#include "cs_test_params.h" + +static K_SEM_DEFINE(sem_results_available, 0, 1); +static K_SEM_DEFINE(sem_test_complete, 0, 1); +static K_SEM_DEFINE(sem_connected, 0, 1); +static K_SEM_DEFINE(sem_disconnected, 0, 1); +static K_SEM_DEFINE(sem_data_received, 0, 1); + +static ssize_t on_attr_write_cb(struct bt_conn *conn, const struct bt_gatt_attr *attr, + const void *buf, uint16_t len, uint16_t offset, uint8_t flags); +static struct bt_conn *connection; +static uint8_t n_ap; +static uint8_t latest_num_steps_reported; +static uint16_t latest_step_data_len; +static uint8_t latest_local_steps[STEP_DATA_BUF_LEN]; +static uint8_t latest_peer_steps[STEP_DATA_BUF_LEN]; + +static struct bt_gatt_attr gatt_attributes[] = { + BT_GATT_PRIMARY_SERVICE(&step_data_svc_uuid), + BT_GATT_CHARACTERISTIC(&step_data_char_uuid.uuid, BT_GATT_CHRC_WRITE, + BT_GATT_PERM_WRITE | BT_GATT_PERM_READ, NULL, on_attr_write_cb, + NULL), +}; +static struct bt_gatt_service step_data_gatt_service = BT_GATT_SERVICE(gatt_attributes); +static const char sample_str[] = "CS Test Sample"; + +static ssize_t on_attr_write_cb(struct bt_conn *conn, const struct bt_gatt_attr *attr, + const void *buf, uint16_t len, uint16_t offset, uint8_t flags) +{ + if (flags & BT_GATT_WRITE_FLAG_PREPARE) { + return 0; + } + + if (offset) { + return BT_GATT_ERR(BT_ATT_ERR_INVALID_OFFSET); + } + + if (len != sizeof(latest_local_steps)) { + return BT_GATT_ERR(BT_ATT_ERR_INVALID_ATTRIBUTE_LEN); + } + + if (flags & BT_GATT_WRITE_FLAG_EXECUTE) { + uint8_t *data = (uint8_t *)buf; + + memcpy(latest_peer_steps, &data[offset], len); + k_sem_give(&sem_data_received); + } + + return len; +} + +static void subevent_result_cb(struct bt_conn_le_cs_subevent_result *result) +{ + 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 <= STEP_DATA_BUF_LEN) { + memcpy(latest_local_steps, result->step_data_buf->data, + result->step_data_buf->len); + latest_step_data_len = result->step_data_buf->len; + } else { + printk("Not enough memory to store step data. (%d > %d)\n", + result->step_data_buf->len, STEP_DATA_BUF_LEN); + latest_num_steps_reported = 0; + } + } + + if (result->header.procedure_done_status == BT_CONN_LE_CS_PROCEDURE_COMPLETE || + result->header.procedure_done_status == BT_CONN_LE_CS_PROCEDURE_ABORTED) { + k_sem_give(&sem_results_available); + } +} + +static void end_cb(void) +{ + k_sem_give(&sem_test_complete); +} + +static void mtu_exchange_cb(struct bt_conn *conn, uint8_t err, + struct bt_gatt_exchange_params *params) +{ + printk("MTU exchange %s (%u)\n", err == 0U ? "success" : "failed", bt_gatt_get_mtu(conn)); +} + +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); + + __ASSERT(connection == conn, "Unexpected connected callback"); + + if (err) { + bt_conn_unref(conn); + connection = NULL; + } + + 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("%s: MTU exchange failed (err %d)\n", __func__, err); + } + + k_sem_give(&sem_connected); +} + +static void disconnected_cb(struct bt_conn *conn, uint8_t reason) +{ + printk("Disconnected (reason 0x%02X)\n", reason); + + bt_conn_unref(conn); + connection = NULL; + + k_sem_give(&sem_disconnected); +} + +static bool data_cb(struct bt_data *data, void *user_data) +{ + char *name = user_data; + uint8_t len; + + switch (data->type) { + case BT_DATA_NAME_SHORTENED: + case BT_DATA_NAME_COMPLETE: + len = MIN(data->data_len, NAME_LEN - 1); + memcpy(name, data->data, len); + name[len] = '\0'; + return false; + default: + return true; + } +} + +static void device_found(const bt_addr_le_t *addr, int8_t rssi, uint8_t type, + struct net_buf_simple *ad) +{ + char addr_str[BT_ADDR_LE_STR_LEN]; + char name[NAME_LEN] = {}; + int err; + + if (connection) { + return; + } + + /* We're only interested in connectable events */ + if (type != BT_GAP_ADV_TYPE_ADV_IND && type != BT_GAP_ADV_TYPE_ADV_DIRECT_IND) { + return; + } + + bt_data_parse(ad, data_cb, name); + + if (strcmp(name, sample_str)) { + return; + } + + if (bt_le_scan_stop()) { + return; + } + + printk("Found device with name %s, connecting...\n", name); + + err = bt_conn_le_create(addr, BT_CONN_LE_CREATE_CONN, BT_LE_CONN_PARAM_DEFAULT, + &connection); + if (err) { + printk("Create conn to %s failed (%u)\n", addr_str, err); + } +} + +BT_CONN_CB_DEFINE(conn_cb) = { + .connected = connected_cb, + .disconnected = disconnected_cb, +}; + +int main(void) +{ + int err; + struct bt_le_cs_test_param test_params; + + printk("Starting Channel Sounding Demo\n"); + + /* Initialize the Bluetooth Subsystem */ + err = bt_enable(NULL); + if (err) { + printk("Bluetooth init failed (err %d)\n", err); + return 0; + } + + struct bt_le_cs_test_cb cs_test_cb = { + .le_cs_test_subevent_data_available = subevent_result_cb, + .le_cs_test_end_complete = end_cb, + }; + + err = bt_le_cs_test_cb_register(cs_test_cb); + if (err) { + printk("Failed to register callbacks (err %d)\n", err); + return 0; + } + + err = bt_gatt_service_register(&step_data_gatt_service); + if (err) { + printk("bt_gatt_service_register() returned err %d\n", err); + return 0; + } + + while (true) { + while (true) { + k_sleep(K_SECONDS(2)); + + test_params = test_params_get(BT_CONN_LE_CS_ROLE_INITIATOR); + + err = bt_le_cs_start_test(&test_params); + if (err) { + printk("Failed to start CS test (err %d)\n", err); + return 0; + } + + k_sem_take(&sem_results_available, K_SECONDS(5)); + + err = bt_le_cs_stop_test(); + if (err) { + printk("Failed to stop CS test (err %d)\n", err); + return 0; + } + + k_sem_take(&sem_test_complete, K_FOREVER); + + if (latest_num_steps_reported > NUM_MODE_0_STEPS) { + break; + } + } + + err = bt_le_scan_start(BT_LE_SCAN_ACTIVE_CONTINUOUS, device_found); + if (err) { + printk("Scanning failed to start (err %d)\n", err); + return 0; + } + + k_sem_take(&sem_connected, K_FOREVER); + + k_sem_take(&sem_data_received, K_FOREVER); + + estimate_distance( + latest_local_steps, latest_step_data_len, latest_peer_steps, + latest_step_data_len - + NUM_MODE_0_STEPS * + (sizeof(struct bt_hci_le_cs_step_data_mode_0_initiator) - + sizeof(struct bt_hci_le_cs_step_data_mode_0_reflector)), + n_ap, BT_CONN_LE_CS_ROLE_INITIATOR); + + bt_conn_disconnect(connection, BT_HCI_ERR_REMOTE_USER_TERM_CONN); + + k_sem_take(&sem_disconnected, K_FOREVER); + + printk("Re-running CS test...\n"); + } + + return 0; +} diff --git a/samples/bluetooth/channel_sounding/src/cs_test_reflector.c b/samples/bluetooth/channel_sounding/src/cs_test_reflector.c new file mode 100644 index 00000000000..6f8352bc903 --- /dev/null +++ b/samples/bluetooth/channel_sounding/src/cs_test_reflector.c @@ -0,0 +1,247 @@ +/* + * Copyright (c) 2024 Nordic Semiconductor ASA + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include +#include +#include +#include +#include +#include "distance_estimation.h" +#include "common.h" +#include "cs_test_params.h" + +static K_SEM_DEFINE(sem_results_available, 0, 1); +static K_SEM_DEFINE(sem_test_complete, 0, 1); +static K_SEM_DEFINE(sem_connected, 0, 1); +static K_SEM_DEFINE(sem_disconnected, 0, 1); +static K_SEM_DEFINE(sem_discovered, 0, 1); +static K_SEM_DEFINE(sem_written, 0, 1); + +static uint16_t step_data_attr_handle; +static struct bt_conn *connection; +static uint8_t latest_num_steps_reported; +static uint8_t latest_local_steps[STEP_DATA_BUF_LEN]; + +static const char sample_str[] = "CS Test Sample"; +static const struct bt_data ad[] = { + BT_DATA(BT_DATA_NAME_COMPLETE, "CS Test Sample", sizeof(sample_str) - 1), +}; + +static void subevent_result_cb(struct bt_conn_le_cs_subevent_result *result) +{ + latest_num_steps_reported = result->header.num_steps_reported; + + if (result->step_data_buf) { + if (result->step_data_buf->len <= STEP_DATA_BUF_LEN) { + memcpy(latest_local_steps, result->step_data_buf->data, + result->step_data_buf->len); + } else { + printk("Not enough memory to store step data. (%d > %d)\n", + result->step_data_buf->len, STEP_DATA_BUF_LEN); + latest_num_steps_reported = 0; + } + } + + if (result->header.procedure_done_status == BT_CONN_LE_CS_PROCEDURE_COMPLETE || + result->header.procedure_done_status == BT_CONN_LE_CS_PROCEDURE_ABORTED) { + k_sem_give(&sem_results_available); + } +} + +static void end_cb(void) +{ + k_sem_give(&sem_test_complete); +} + +static void mtu_exchange_cb(struct bt_conn *conn, uint8_t err, + struct bt_gatt_exchange_params *params) +{ + printk("MTU exchange %s (%u)\n", err == 0U ? "success" : "failed", bt_gatt_get_mtu(conn)); +} + +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); + + __ASSERT(connection == conn, "Unexpected connected callback"); + + if (err) { + bt_conn_unref(conn); + connection = NULL; + } + + connection = bt_conn_ref(conn); + + 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("%s: MTU exchange failed (err %d)\n", __func__, err); + } + + k_sem_give(&sem_connected); +} + +static void disconnected_cb(struct bt_conn *conn, uint8_t reason) +{ + printk("Disconnected (reason 0x%02X)\n", reason); + + bt_conn_unref(conn); + connection = NULL; + + k_sem_give(&sem_disconnected); +} + +static uint8_t discover_func(struct bt_conn *conn, const struct bt_gatt_attr *attr, + struct bt_gatt_discover_params *params) +{ + struct bt_gatt_chrc *chrc; + char str[BT_UUID_STR_LEN]; + + printk("Discovery: attr %p\n", attr); + + if (!attr) { + return BT_GATT_ITER_STOP; + } + + chrc = (struct bt_gatt_chrc *)attr->user_data; + + bt_uuid_to_str(chrc->uuid, str, sizeof(str)); + printk("UUID %s\n", str); + + if (!bt_uuid_cmp(chrc->uuid, &step_data_char_uuid.uuid)) { + step_data_attr_handle = chrc->value_handle; + + printk("Found expected UUID\n"); + + k_sem_give(&sem_discovered); + } + + return BT_GATT_ITER_STOP; +} + +static void write_func(struct bt_conn *conn, uint8_t err, struct bt_gatt_write_params *params) +{ + if (err) { + printk("Write failed (err %d)\n", err); + + return; + } + + k_sem_give(&sem_written); +} + +BT_CONN_CB_DEFINE(conn_cb) = { + .connected = connected_cb, + .disconnected = disconnected_cb, +}; + +int main(void) +{ + int err; + struct bt_le_cs_test_param test_params; + struct bt_gatt_discover_params discover_params; + struct bt_gatt_write_params write_params; + + printk("Starting Channel Sounding Demo\n"); + + /* Initialize the Bluetooth Subsystem */ + err = bt_enable(NULL); + if (err) { + printk("Bluetooth init failed (err %d)\n", err); + return 0; + } + + struct bt_le_cs_test_cb cs_test_cb = { + .le_cs_test_subevent_data_available = subevent_result_cb, + .le_cs_test_end_complete = end_cb, + }; + + err = bt_le_cs_test_cb_register(cs_test_cb); + if (err) { + printk("Failed to register callbacks (err %d)\n", err); + return 0; + } + + while (true) { + while (true) { + k_sleep(K_SECONDS(2)); + + test_params = test_params_get(BT_CONN_LE_CS_ROLE_REFLECTOR); + + err = bt_le_cs_start_test(&test_params); + if (err) { + printk("Failed to start CS test (err %d)\n", err); + return 0; + } + + k_sem_take(&sem_results_available, K_SECONDS(5)); + + err = bt_le_cs_stop_test(); + if (err) { + printk("Failed to stop CS test (err %d)\n", err); + return 0; + } + + k_sem_take(&sem_test_complete, K_FOREVER); + + if (latest_num_steps_reported > NUM_MODE_0_STEPS) { + break; + } + } + + err = bt_le_adv_start(BT_LE_ADV_PARAM(BT_LE_ADV_OPT_CONN, BT_GAP_ADV_FAST_INT_MIN_1, + BT_GAP_ADV_FAST_INT_MAX_1, NULL), + ad, ARRAY_SIZE(ad), NULL, 0); + if (err) { + printk("Advertising failed to start (err %d)\n", err); + return 0; + } + + k_sem_take(&sem_connected, K_FOREVER); + + discover_params.uuid = &step_data_char_uuid.uuid; + discover_params.func = discover_func; + discover_params.start_handle = BT_ATT_FIRST_ATTRIBUTE_HANDLE; + discover_params.end_handle = BT_ATT_LAST_ATTRIBUTE_HANDLE; + discover_params.type = BT_GATT_DISCOVER_CHARACTERISTIC; + + err = bt_gatt_discover(connection, &discover_params); + if (err) { + printk("Discovery failed (err %d)\n", err); + return 0; + } + + err = k_sem_take(&sem_discovered, K_SECONDS(10)); + if (err) { + printk("Timed out during GATT discovery\n"); + return 0; + } + + write_params.func = write_func; + write_params.handle = step_data_attr_handle; + write_params.length = STEP_DATA_BUF_LEN; + write_params.data = latest_local_steps; + write_params.offset = 0; + + err = bt_gatt_write(connection, &write_params); + if (err) { + printk("Write failed (err %d)\n", err); + return 0; + } + + k_sem_take(&sem_disconnected, K_FOREVER); + + printk("Re-running CS test...\n"); + } + + return 0; +} diff --git a/samples/bluetooth/channel_sounding/src/distance_estimation.c b/samples/bluetooth/channel_sounding/src/distance_estimation.c new file mode 100644 index 00000000000..8d1fb0b4a9c --- /dev/null +++ b/samples/bluetooth/channel_sounding/src/distance_estimation.c @@ -0,0 +1,304 @@ +/* + * Copyright (c) 2024 Nordic Semiconductor ASA + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include "distance_estimation.h" + +#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 { + bool local_steps; + 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_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; + + if (context->local_steps) { + 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 { + 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; + + 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->local_steps) { + 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; + } + } else { + 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++; + } + + return true; +} + +void 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) +{ + struct net_buf_simple buf; + + struct processing_context context = { + .local_steps = true, + .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)); + + net_buf_simple_init_with_data(&buf, local_steps, local_steps_len); + + bt_le_cs_step_data_parse(&buf, process_step_data, &context); + + context.mode_1_data_index = 0; + context.mode_2_data_index = 0; + context.local_steps = false; + + net_buf_simple_init_with_data(&buf, peer_steps, peer_steps_len); + + bt_le_cs_step_data_parse(&buf, process_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); + } +}