OrgZephyr/zephyr
OrgZephyr
/
zephyr
mirror of https://github.com/zephyrproject-rtos/zephyr.git
1
0
Fork 0
Code Issues Releases Wiki Activity

bluetooth: samples: Add samples showing basic channel sounding features 

These samples demonstrate how to use the bluetooth 6.0 channel
sounding APIs. A basic distance estimation algorithm is included.

Signed-off-by: Olivier Lesage <olivier.lesage@nordicsemi.no>
This commit is contained in:
Olivier Lesage 2024-10-25 14:50:17 +02:00 committed by Anas Nashif
parent 42889628a9
commit fc8d9425c1
21 changed files with 1794 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

133
samples/bluetooth/channel_sounding/README.rst Normal file
View File

@ -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 <nrf54l15dk_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 <https://www.bluetooth.com/wp-content/uploads/Files/Specification/HTML/Core-60/out/en/architecture,-change-history,-and-conventions/architecture.html#UUID-a8d03618-5fcf-3043-2198-559653272b1b>`_
.. [#rtt_packets] `Bluetooth Core Specification v. 6.0: Vol. 1, Part A, 9.3 <https://www.bluetooth.com/wp-content/uploads/Files/Specification/HTML/Core-60/out/en/architecture,-change-history,-and-conventions/architecture.html#UUID-9d4969af-baa6-b7e4-03ca-70b340877adf>`_
.. [#cs_setup_phase] `Bluetooth Core Specification v. 6.0: Vol. 6, Part D, 6.34 <https://www.bluetooth.com/wp-content/uploads/Files/Specification/HTML/Core-60/out/en/low-energy-controller/message-sequence-charts.html#UUID-73ba2c73-f3c8-3b1b-2bdb-b18174b88059>`_
.. [#cs_start] `Bluetooth Core Specification v. 6.0: Vol. 6, Part D, 6.35 <https://www.bluetooth.com/wp-content/uploads/Files/Specification/HTML/Core-60/out/en/low-energy-controller/message-sequence-charts.html#UUID-c75cd2f9-0dd8-bd38-9afc-c7becfa7f073>`_

11
samples/bluetooth/channel_sounding/connected_cs/initiator/CMakeLists.txt Normal file
View File

@ -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
)

16
samples/bluetooth/channel_sounding/connected_cs/initiator/prj.conf Normal file
View File

@ -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

11
samples/bluetooth/channel_sounding/connected_cs/initiator/sample.yaml Normal file
View File

@ -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

11
samples/bluetooth/channel_sounding/connected_cs/reflector/CMakeLists.txt Normal file
View File

@ -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
)

14
samples/bluetooth/channel_sounding/connected_cs/reflector/prj.conf Normal file
View File

@ -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

11
samples/bluetooth/channel_sounding/connected_cs/reflector/sample.yaml Normal file
View File

@ -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

11
samples/bluetooth/channel_sounding/cs_test/initiator/CMakeLists.txt Normal file
View File

@ -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
)

15
samples/bluetooth/channel_sounding/cs_test/initiator/prj.conf Normal file
View File

@ -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

11
samples/bluetooth/channel_sounding/cs_test/initiator/sample.yaml Normal file
View File

@ -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

11
samples/bluetooth/channel_sounding/cs_test/reflector/CMakeLists.txt Normal file
View File

@ -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
)

13
samples/bluetooth/channel_sounding/cs_test/reflector/prj.conf Normal file
View File

@ -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

11
samples/bluetooth/channel_sounding/cs_test/reflector/sample.yaml Normal file
View File

@ -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

15
samples/bluetooth/channel_sounding/include/common.h Normal file
View File

@ -0,0 +1,15 @@
/*
* Copyright (c) 2024 Nordic Semiconductor ASA
*
* SPDX-License-Identifier: Apache-2.0
*/
#include <zephyr/bluetooth/gatt.h>
#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));

58
samples/bluetooth/channel_sounding/include/cs_test_params.h Normal file
View File

@ -0,0 +1,58 @@
/*
* Copyright (c) 2024 Nordic Semiconductor ASA
*
* SPDX-License-Identifier: Apache-2.0
*/
#include <zephyr/bluetooth/cs.h>
#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;
}

11
samples/bluetooth/channel_sounding/include/distance_estimation.h Normal file
View File

@ -0,0 +1,11 @@
/*
* Copyright (c) 2024 Nordic Semiconductor ASA
*
* SPDX-License-Identifier: Apache-2.0
*/
#include <stdint.h>
#include <zephyr/bluetooth/cs.h>
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);

356
samples/bluetooth/channel_sounding/src/connected_cs_initiator.c Normal file
View File

@ -0,0 +1,356 @@
/*
* Copyright (c) 2024 Nordic Semiconductor ASA
*
* SPDX-License-Identifier: Apache-2.0
*/
#include <math.h>
#include <zephyr/sys/byteorder.h>
#include <zephyr/bluetooth/bluetooth.h>
#include <zephyr/bluetooth/cs.h>
#include <zephyr/bluetooth/att.h>
#include <zephyr/bluetooth/gatt.h>
#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, &params);
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;
}

248
samples/bluetooth/channel_sounding/src/connected_cs_reflector.c Normal file
View File

@ -0,0 +1,248 @@
/*
* Copyright (c) 2024 Nordic Semiconductor ASA
*
* SPDX-License-Identifier: Apache-2.0
*/
#include <math.h>
#include <zephyr/sys/byteorder.h>
#include <zephyr/bluetooth/bluetooth.h>
#include <zephyr/bluetooth/cs.h>
#include <zephyr/bluetooth/att.h>
#include <zephyr/bluetooth/gatt.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_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;
}

276
samples/bluetooth/channel_sounding/src/cs_test_initiator.c Normal file
View File

@ -0,0 +1,276 @@
/*
* Copyright (c) 2024 Nordic Semiconductor ASA
*
* SPDX-License-Identifier: Apache-2.0
*/
#include <math.h>
#include <zephyr/console/console.h>
#include <zephyr/sys/byteorder.h>
#include <zephyr/bluetooth/bluetooth.h>
#include <zephyr/bluetooth/cs.h>
#include <zephyr/bluetooth/att.h>
#include <zephyr/bluetooth/gatt.h>
#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;
}

247
samples/bluetooth/channel_sounding/src/cs_test_reflector.c Normal file
View File

@ -0,0 +1,247 @@
/*
* Copyright (c) 2024 Nordic Semiconductor ASA
*
* SPDX-License-Identifier: Apache-2.0
*/
#include <math.h>
#include <zephyr/console/console.h>
#include <zephyr/sys/byteorder.h>
#include <zephyr/bluetooth/bluetooth.h>
#include <zephyr/bluetooth/cs.h>
#include <zephyr/bluetooth/att.h>
#include <zephyr/bluetooth/gatt.h>
#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;
}

304
samples/bluetooth/channel_sounding/src/distance_estimation.c Normal file
View File

@ -0,0 +1,304 @@
/*
* Copyright (c) 2024 Nordic Semiconductor ASA
*
* SPDX-License-Identifier: Apache-2.0
*/
#include <math.h>
#include <zephyr/bluetooth/cs.h>
#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);
}
}