Hi,
I succeeded in implementing Channel sounding for multiple devices (based on previous samples). But now there is a little difference in how the data gets collected (buffer) and how the range estimation gets computed (somewhere outside the files).
As you can see, the connections and scanning works already fine when i re-implement my code for reconnecting sequentially with the devices. But the data doesn't get updated correctly:
I think i should change some things in data collected / range data collect (the first 6 functions, which are new from the new sample)
Kind regards,
Nick De Leenheer
/*
* Copyright (c) 2024 Nordic Semiconductor ASA
*
* SPDX-License-Identifier: LicenseRef-Nordic-5-Clause
*/
/** @file
* @brief Channel Sounding Initiator with Ranging Requestor sample
*/
#include <math.h>
#include <zephyr/kernel.h>
#include <zephyr/types.h>
#include <zephyr/sys/reboot.h>
#include <zephyr/bluetooth/cs.h>
#include <zephyr/bluetooth/gatt.h>
#include <zephyr/bluetooth/conn.h>
#include <bluetooth/scan.h>
#include <bluetooth/services/ras.h>
#include <bluetooth/gatt_dm.h>
#include <bluetooth/cs_de.h>
#include <dk_buttons_and_leds.h>
#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(app_main, LOG_LEVEL_INF);
#define CON_STATUS_LED DK_LED1
#define CS_CONFIG_ID 0
#define NUM_MODE_0_STEPS 3
#define PROCEDURE_COUNTER_NONE (-1)
#define DE_SLIDING_WINDOW_SIZE (10)
#define MAX_AP (CONFIG_BT_RAS_MAX_ANTENNA_PATHS)
// #define LOCAL_PROCEDURE_MEM \
// ((BT_RAS_MAX_STEPS_PER_PROCEDURE * sizeof(struct bt_le_cs_subevent_step)) + \
// (BT_RAS_MAX_STEPS_PER_PROCEDURE * BT_RAS_MAX_STEP_DATA_LEN))
#define LOCAL_PROCEDURE_MEM 6000
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_connected, 0, 1);
static K_SEM_DEFINE(sem_discovery_done, 0, 1);
static K_SEM_DEFINE(sem_mtu_exchange_done, 0, 1);
static K_SEM_DEFINE(sem_security, 0, 1);
static K_SEM_DEFINE(sem_local_steps, 1, 1);
static K_SEM_DEFINE(sem_disconnected, 0, 1);
static K_SEM_DEFINE(sem_ranging_done, 0, 1);
static K_SEM_DEFINE(sem_nrf_noise, 0, 1);
static K_MUTEX_DEFINE(distance_estimate_buffer_mutex);
static struct bt_conn *connection;
NET_BUF_SIMPLE_DEFINE_STATIC(latest_local_steps, LOCAL_PROCEDURE_MEM);
NET_BUF_SIMPLE_DEFINE_STATIC(latest_peer_steps, BT_RAS_PROCEDURE_MEM);
static int32_t most_recent_local_ranging_counter = PROCEDURE_COUNTER_NONE;
static int32_t dropped_ranging_counter = PROCEDURE_COUNTER_NONE;
/* For scanning & connecting with multiple devices */
#define MAX_DEVICES 3
static bt_addr_le_t addresses[MAX_DEVICES];
static int num_found = 0;
static bool scanning_for_initial_devices = true;
static bool connection_in_progress = false;
static struct bt_conn *connection = NULL;
static struct bt_conn *connections[MAX_DEVICES];
/* For parsing the name of devices */
#define NAME_LEN 30
char reflector_names[MAX_DEVICES][NAME_LEN];
char reflector_name[NAME_LEN] = "(onbekend)";
/* For the ranging process */
#define RANGING_ROUNDS 1
static bool ranging_done = false;
/* For buffering Distance estimations */
static uint8_t buffer_index;
static uint8_t buffer_num_valid;
static cs_de_dist_estimates_t distance_estimate_buffer[MAX_AP][DE_SLIDING_WINDOW_SIZE];
static void store_distance_estimates(cs_de_report_t *p_report)
{
int lock_state = k_mutex_lock(&distance_estimate_buffer_mutex, K_FOREVER);
__ASSERT_NO_MSG(lock_state == 0);
for (uint8_t ap = 0; ap < p_report->n_ap; ap++) {
memcpy(&distance_estimate_buffer[ap][buffer_index],
&p_report->distance_estimates[ap], sizeof(cs_de_dist_estimates_t));
}
buffer_index = (buffer_index + 1) % DE_SLIDING_WINDOW_SIZE;
if (buffer_num_valid < DE_SLIDING_WINDOW_SIZE) {
buffer_num_valid++;
}
k_mutex_unlock(&distance_estimate_buffer_mutex);
}
static cs_de_dist_estimates_t get_distance(uint8_t ap)
{
cs_de_dist_estimates_t averaged_result = {};
uint8_t num_ifft = 0;
uint8_t num_phase_slope = 0;
uint8_t num_rtt = 0;
int lock_state = k_mutex_lock(&distance_estimate_buffer_mutex, K_FOREVER);
__ASSERT_NO_MSG(lock_state == 0);
for (uint8_t i = 0; i < buffer_num_valid; i++) {
if (isfinite(distance_estimate_buffer[ap][i].ifft)) {
num_ifft++;
averaged_result.ifft += distance_estimate_buffer[ap][i].ifft;
}
if (isfinite(distance_estimate_buffer[ap][i].phase_slope)) {
num_phase_slope++;
averaged_result.phase_slope += distance_estimate_buffer[ap][i].phase_slope;
}
if (isfinite(distance_estimate_buffer[ap][i].rtt)) {
num_rtt++;
averaged_result.rtt += distance_estimate_buffer[ap][i].rtt;
}
}
k_mutex_unlock(&distance_estimate_buffer_mutex);
if (num_ifft) {
averaged_result.ifft /= num_ifft;
}
if (num_phase_slope) {
averaged_result.phase_slope /= num_phase_slope;
}
if (num_rtt) {
averaged_result.rtt /= num_rtt;
}
return averaged_result;
}
static void ranging_data_get_complete_cb(struct bt_conn *conn, uint16_t ranging_counter, int err)
{
ARG_UNUSED(conn);
if (err) {
LOG_ERR("Error when getting ranging data with ranging counter %d (err %d)",
ranging_counter, err);
return;
}
// LOG_DBG("Ranging data get completed for ranging counter %d", ranging_counter);
/* This struct is static to avoid putting it on the stack (it's very large) */
static cs_de_report_t cs_de_report;
memset(&cs_de_report, 0, sizeof(cs_de_report));
cs_de_populate_report(&latest_local_steps, &latest_peer_steps, BT_CONN_LE_CS_ROLE_INITIATOR,
&cs_de_report);
net_buf_simple_reset(&latest_local_steps);
net_buf_simple_reset(&latest_peer_steps);
k_sem_give(&sem_local_steps);
cs_de_quality_t quality = cs_de_calc(&cs_de_report);
if (quality == CS_DE_QUALITY_OK) {
for (uint8_t ap = 0; ap < cs_de_report.n_ap; ap++) {
if (cs_de_report.tone_quality[ap] == CS_DE_TONE_QUALITY_OK) {
store_distance_estimates(&cs_de_report);
}
}
}
}
static void subevent_result_cb(struct bt_conn *conn, struct bt_conn_le_cs_subevent_result *result)
{
if (dropped_ranging_counter == result->header.procedure_counter) {
return;
}
if (most_recent_local_ranging_counter
!= bt_ras_rreq_get_ranging_counter(result->header.procedure_counter)) {
int sem_state = k_sem_take(&sem_local_steps, K_NO_WAIT);
if (sem_state < 0) {
dropped_ranging_counter = result->header.procedure_counter;
// LOG_DBG("Dropped subevent results due to unfinished ranging data request.");
return;
}
most_recent_local_ranging_counter =
bt_ras_rreq_get_ranging_counter(result->header.procedure_counter);
}
if (result->header.subevent_done_status == BT_CONN_LE_CS_SUBEVENT_ABORTED) {
/* The steps from this subevent will not be used. */
} else if (result->step_data_buf) {
if (result->step_data_buf->len <= net_buf_simple_tailroom(&latest_local_steps)) {
uint16_t len = result->step_data_buf->len;
uint8_t *step_data = net_buf_simple_pull_mem(result->step_data_buf, len);
net_buf_simple_add_mem(&latest_local_steps, step_data, len);
} else {
LOG_ERR("Not enough memory to store step data. (%d > %d)",
latest_local_steps.len + result->step_data_buf->len,
latest_local_steps.size);
net_buf_simple_reset(&latest_local_steps);
dropped_ranging_counter = result->header.procedure_counter;
return;
}
}
dropped_ranging_counter = PROCEDURE_COUNTER_NONE;
if (result->header.procedure_done_status == BT_CONN_LE_CS_PROCEDURE_COMPLETE) {
most_recent_local_ranging_counter =
bt_ras_rreq_get_ranging_counter(result->header.procedure_counter);
} else if (result->header.procedure_done_status == BT_CONN_LE_CS_PROCEDURE_ABORTED) {
LOG_WRN("Procedure %u aborted", result->header.procedure_counter);
net_buf_simple_reset(&latest_local_steps);
k_sem_give(&sem_local_steps);
}
}
static void ranging_data_ready_cb(struct bt_conn *conn, uint16_t ranging_counter)
{
LOG_DBG("Ranging data ready %i", ranging_counter);
if (ranging_counter == most_recent_local_ranging_counter) {
int err = bt_ras_rreq_cp_get_ranging_data(conn, &latest_peer_steps,
ranging_counter,
ranging_data_get_complete_cb);
if (err) {
LOG_ERR("Get ranging data failed (err %d)", err);
net_buf_simple_reset(&latest_local_steps);
net_buf_simple_reset(&latest_peer_steps);
k_sem_give(&sem_local_steps);
}
}
}
static void ranging_data_overwritten_cb(struct bt_conn *conn, uint16_t ranging_counter)
{
// LOG_INF("Ranging data overwritten %i", ranging_counter);
}
static void mtu_exchange_cb(struct bt_conn *conn, uint8_t err,
struct bt_gatt_exchange_params *params)
{
if (err) {
LOG_ERR("MTU exchange failed (err %d)", err);
return;
}
// LOG_INF("MTU exchange success (%u)", bt_gatt_get_mtu(conn));
k_sem_give(&sem_mtu_exchange_done);
}
static void discovery_completed_cb(struct bt_gatt_dm *dm, void *context)
{
int err;
// LOG_INF("The discovery procedure succeeded");
struct bt_conn *conn = bt_gatt_dm_conn_get(dm);
bt_gatt_dm_data_print(dm);
err = bt_ras_rreq_alloc_and_assign_handles(dm, conn);
if (err) {
LOG_ERR("RAS RREQ alloc init failed (err %d)", err);
}
err = bt_gatt_dm_data_release(dm);
if (err) {
LOG_ERR("Could not release the discovery data (err %d)", err);
}
k_sem_give(&sem_discovery_done);
}
static void discovery_service_not_found_cb(struct bt_conn *conn, void *context)
{
// LOG_INF("The service could not be found during the discovery, disconnecting");
bt_conn_disconnect(connection, BT_HCI_ERR_REMOTE_USER_TERM_CONN);
}
static void discovery_error_found_cb(struct bt_conn *conn, int err, void *context)
{
// LOG_INF("The discovery procedure failed (err %d)", err);
bt_conn_disconnect(connection, BT_HCI_ERR_REMOTE_USER_TERM_CONN);
}
static struct bt_gatt_dm_cb discovery_cb = {
.completed = discovery_completed_cb,
.service_not_found = discovery_service_not_found_cb,
.error_found = discovery_error_found_cb,
};
static void security_changed(struct bt_conn *conn, bt_security_t level, enum bt_security_err err)
{
char addr[BT_ADDR_LE_STR_LEN];
bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
if (err) {
LOG_ERR("Security failed: %s level %u err %d %s", addr, level, err,
bt_security_err_to_str(err));
return;
}
// LOG_INF("Security changed: %s level %u", addr, level);
k_sem_give(&sem_security);
}
static bool le_param_req(struct bt_conn *conn, struct bt_le_conn_param *param)
{
/* Ignore peer parameter preferences. */
return false;
}
static void connected_cb(struct bt_conn *conn, uint8_t err)
{
char addr[BT_ADDR_LE_STR_LEN];
(void)bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
// LOG_INF("Connected to %s", addr);
if (err) {
LOG_ERR("Connection failed (err 0x%02x)", err);
bt_conn_unref(conn);
k_sem_give(&sem_connected); // NEEDED to get to the error instead of "W: conn failed to establish. RF noise?"
return;
}
// When not in the scanning process we give a semaphore for being connected
if (!scanning_for_initial_devices) {
for (int i = 0; i < MAX_DEVICES; i++) {
if (connections[i] == NULL) {
connections[i] = bt_conn_ref(conn);
break;
}
}
dk_set_led_on(CON_STATUS_LED);
k_sem_give(&sem_connected);
}
// When still in the scanning process we disconnect
if (scanning_for_initial_devices) {
k_sleep(K_MSEC(200));
bt_conn_disconnect(conn, BT_HCI_ERR_REMOTE_USER_TERM_CONN);
connection_in_progress = false;
}
}
static void disconnected_cb(struct bt_conn *conn, uint8_t reason)
{
char addr[BT_ADDR_LE_STR_LEN];
(void)bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
// LOG_INF("Disconnected from %s, reason: reason 0x%02X", addr, reason);
if (reason == 0x3E){
// LOG_INF("nRF Noise detected");
k_sem_give(&sem_nrf_noise);
}
bt_conn_unref(conn);
connection = NULL;
dk_set_led_off(CON_STATUS_LED);
// Handle disconnection during initial device scan
if (scanning_for_initial_devices) {
// LOG_INF("disconnected during scanning phase");
k_sem_give(&sem_disconnected);
connection_in_progress = false;
// When more devices need to get detected we continue scanning
if (num_found < MAX_DEVICES) {
bt_scan_start(BT_SCAN_TYPE_SCAN_ACTIVE);
// When all devices has been found we stop scanning
} else {
scanning_for_initial_devices = false;
}
}
// Handle proper disconnection after ranging is done
if(ranging_done){
// LOG_INF("ranging is done");
k_sem_give(&sem_ranging_done);
}
}
static void remote_capabilities_cb(struct bt_conn *conn,
uint8_t status,
struct bt_conn_le_cs_capabilities *params)
{
ARG_UNUSED(conn);
ARG_UNUSED(params);
if (status == BT_HCI_ERR_SUCCESS) {
// LOG_INF("CS capability exchange completed.");
k_sem_give(&sem_remote_capabilities_obtained);
} else {
LOG_WRN("CS capability exchange failed. (HCI status 0x%02x)", status);
}
}
static void config_create_cb(struct bt_conn *conn,
uint8_t status,
struct bt_conn_le_cs_config *config)
{
ARG_UNUSED(conn);
if (status == BT_HCI_ERR_SUCCESS) {
// LOG_INF("CS config creation complete. ID: %d", config->id);
k_sem_give(&sem_config_created);
} else {
LOG_WRN("CS config creation failed. (HCI status 0x%02x)", status);
}
}
static void security_enable_cb(struct bt_conn *conn, uint8_t status)
{
ARG_UNUSED(conn);
if (status == BT_HCI_ERR_SUCCESS) {
// LOG_INF("CS security enabled.");
k_sem_give(&sem_cs_security_enabled);
} else {
LOG_WRN("CS security enable failed. (HCI status 0x%02x)", status);
}
}
static void procedure_enable_cb(struct bt_conn *conn,
uint8_t status,
struct bt_conn_le_cs_procedure_enable_complete *params)
{
ARG_UNUSED(conn);
if (status == BT_HCI_ERR_SUCCESS) {
if (params->state == 1) {
// LOG_INF("CS procedures enabled:\n"
// " - config ID: %u\n"
// " - antenna configuration index: %u\n"
// " - TX power: %d dbm\n"
// " - subevent length: %u us\n"
// " - subevents per event: %u\n"
// " - subevent interval: %u\n"
// " - event interval: %u\n"
// " - procedure interval: %u\n"
// " - procedure count: %u\n"
// " - maximum procedure length: %u",
// params->config_id, params->tone_antenna_config_selection,
// params->selected_tx_power, params->subevent_len,
// params->subevents_per_event, params->subevent_interval,
// params->event_interval, params->procedure_interval,
// params->procedure_count, params->max_procedure_len);
} else {
// LOG_INF("CS procedures disabled.");
}
} else {
LOG_WRN("CS procedures enable failed. (HCI status 0x%02x)", status);
}
}
static bool ad_parse_cb(struct bt_data *data, void *user_data)
{
char *name = user_data;
if (data->type == BT_DATA_NAME_COMPLETE || data->type == BT_DATA_NAME_SHORTENED) {
size_t len = MIN(data->data_len, NAME_LEN - 1);
memcpy(name, data->data, len);
name[len] = '\0';
return false;
}
return true;
}
static void scan_filter_match(struct bt_scan_device_info *device_info,
struct bt_scan_filter_match *filter_match, bool connectable)
{
// Not scanning for devices OR already connected to a device
if (!scanning_for_initial_devices || connection_in_progress) {
return;
}
// Device has already been detected
for (int i = 0; i < num_found; i++) {
if (!bt_addr_le_cmp(&addresses[i], device_info->recv_info->addr)) {
return;
}
}
// End scanning when ALL devices detected
if (num_found >= MAX_DEVICES) {
return;
}
// Retreiving data from the scanned device
struct net_buf_simple ad_copy = *device_info->adv_data;
snprintf(reflector_names[num_found], NAME_LEN, "(onbekend)");
bt_data_parse(&ad_copy, ad_parse_cb, reflector_names[num_found]);
bt_addr_le_copy(&addresses[num_found], device_info->recv_info->addr);
bt_scan_stop();
k_msleep(50);
// Setup a quick connection with the device
struct bt_conn *conn = NULL;
const struct bt_conn_le_create_param *create_param = BT_CONN_LE_CREATE_PARAM(BT_CONN_LE_OPT_NONE, 0x0060, 0x0030);
const struct bt_le_conn_param *conn_param = BT_LE_CONN_PARAM_DEFAULT;
connection_in_progress = true;
int err = bt_conn_le_create(&addresses[num_found], create_param, conn_param, &conn);
if (err) {
connection_in_progress = false;
return;
}
// LOG the information about the device
char addr_str[BT_ADDR_LE_STR_LEN];
bt_addr_le_to_str(&addresses[num_found], addr_str, sizeof(addr_str));
LOG_INF("Device %d/%d verbonden: \"%s\" (%s)", num_found + 1, MAX_DEVICES, reflector_names[num_found], addr_str);
num_found++;
}
static void scan_connecting_error(struct bt_scan_device_info *device_info)
{
int err;
// LOG_INF("Connecting failed, restarting scanning");
err = bt_scan_start(BT_SCAN_TYPE_SCAN_PASSIVE);
if (err) {
LOG_ERR("Failed to restart scanning (err %i)", err);
return;
}
}
static void scan_connecting(struct bt_scan_device_info *device_info, struct bt_conn *conn)
{
// LOG_INF("Connecting");
}
BT_SCAN_CB_INIT(scan_cb, scan_filter_match, NULL, scan_connecting_error, scan_connecting);
static int scan_init(void)
{
int err;
struct bt_scan_init_param param = {
.scan_param = NULL,
.conn_param = BT_LE_CONN_PARAM_DEFAULT,
.connect_if_match = 0};
bt_scan_init(¶m);
bt_scan_cb_register(&scan_cb);
// /** Filter for names. */
// BT_SCAN_FILTER_TYPE_NAME,
// /** Filter for short names. */
// BT_SCAN_FILTER_TYPE_SHORT_NAME,
// /** Filter for addresses. */
// BT_SCAN_FILTER_TYPE_ADDR,
// /** Filter for UUIDs. */
// BT_SCAN_FILTER_TYPE_UUID,
// /** Filter for appearances. */
// BT_SCAN_FILTER_TYPE_APPEARANCE,
// /** Filter for manufacturer data. */
// BT_SCAN_FILTER_TYPE_MANUFACTURER_DATA,
// err = bt_scan_filter_add(BT_SCAN_FILTER_TYPE_NAME, TARGET_NAME);
// if (err) {
// return err;
// }
err = bt_scan_filter_add(BT_SCAN_FILTER_TYPE_UUID, BT_UUID_RANGING_SERVICE);
if (err) {
return err;
}
err = bt_scan_filter_enable(BT_SCAN_UUID_FILTER, false); //false: 1 match required | true: ALL matches required
if (err) {
return err;
}
return 0;
}
BT_CONN_CB_DEFINE(conn_cb) = {
.connected = connected_cb,
.disconnected = disconnected_cb,
.le_param_req = le_param_req,
.security_changed = security_changed,
.le_cs_read_remote_capabilities_complete = remote_capabilities_cb,
.le_cs_config_complete = config_create_cb,
.le_cs_security_enable_complete = security_enable_cb,
.le_cs_procedure_enable_complete = procedure_enable_cb,
.le_cs_subevent_data_available = subevent_result_cb,
};
int main(void)
{
int err;
dk_leds_init();
err = bt_enable(NULL);
if (err) {
LOG_ERR("Bluetooth init failed (err %d)", err);
return 0;
}
err = scan_init();
if (err) {
LOG_ERR("Scan init failed (err %d)", err);
return 0;
}
err = bt_scan_start(BT_SCAN_TYPE_SCAN_PASSIVE);
if (err) {
LOG_ERR("Scanning failed to start (err %i)", err);
return 0;
}
for (int i = 0; i < MAX_DEVICES; i++) {
k_sem_take(&sem_disconnected, K_FOREVER);
}
LOG_INF("All devices detected => stop scanning");
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/* Reconnect one by one with previously detected devices */
while (true){
for (int i = 0; i < MAX_DEVICES; i++) {
const bt_addr_le_t *peer_addr = &addresses[i];
char addr_str[BT_ADDR_LE_STR_LEN];
bt_addr_le_to_str(peer_addr, addr_str, sizeof(addr_str));
const char *reflector_name = reflector_names[i];
int device_nr = i+1;
const struct bt_conn_le_create_param *create_param = BT_CONN_LE_CREATE_PARAM(BT_CONN_LE_OPT_NONE, 0x0060, 0x0030);
const struct bt_le_conn_param *conn_param = BT_LE_CONN_PARAM_DEFAULT;
if (connections[i]) {
bt_conn_disconnect(connections[i], BT_HCI_ERR_REMOTE_USER_TERM_CONN);
bt_conn_unref(connections[i]);
connections[i] = NULL;
}
// LOG_INF("Connecting with device %s (%s)", reflector_name, addr_str);
err = bt_conn_le_create(peer_addr, create_param, conn_param, &connections[i]);
if (err) {
LOG_ERR("Kon niet verbinden met %s (%s), foutcode: %d", reflector_name, addr_str, err);
continue;
}
/* Check for connection Time out: connection failed */
if (k_sem_take(&sem_connected, K_SECONDS(4)) != 0) {
if (connections[i]) {
bt_conn_disconnect(connections[i], BT_HCI_ERR_REMOTE_USER_TERM_CONN);
bt_conn_unref(connections[i]);
connections[i] = NULL;
}
continue; // Move on to the next device
}
/* Check for nrf Noise: connection succeeds, but disconnects right after */
if (k_sem_take(&sem_nrf_noise, K_MSEC(500)) == 0){
// LOG_INF("Skipping device %d, going to the next device", device_nr);
if (connections[i]) {
bt_conn_disconnect(connections[i], BT_HCI_ERR_REMOTE_USER_TERM_CONN);
connections[i] = NULL;
k_sleep(K_SECONDS(1));
}
continue; // Move on to the next device
}
/* To establish a stable connection */
k_sleep(K_MSEC(200));
// Set Security
err = bt_conn_set_security(connections[i], BT_SECURITY_L2);
if (err) {
LOG_ERR("Security failed for %s", reflector_name);
continue;
}
k_sem_take(&sem_security, K_FOREVER);
// MTU Exchange
static struct bt_gatt_exchange_params mtu_exchange_params = {.func = mtu_exchange_cb};
bt_gatt_exchange_mtu(connections[i], &mtu_exchange_params);
k_sem_take(&sem_mtu_exchange_done, K_FOREVER);
// GATT Discovery
err = bt_gatt_dm_start(connections[i], BT_UUID_RANGING_SERVICE, &discovery_cb, NULL);
if (err) {
LOG_ERR("Discovery failed for %s", reflector_name);
return 0;
}
k_sem_take(&sem_discovery_done, K_FOREVER);
// // Default settings
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(connections[i], &default_settings);
if (err) {
LOG_ERR("Failed to configure default CS settings for %s", reflector_name);
return 0;
}
// Subscriptions
err = bt_ras_rreq_rd_overwritten_subscribe(connections[i], ranging_data_overwritten_cb);
err |= bt_ras_rreq_rd_ready_subscribe(connections[i], ranging_data_ready_cb);
err |= bt_ras_rreq_on_demand_rd_subscribe(connections[i]);
err |= bt_ras_rreq_cp_subscribe(connections[i]);
if (err) {
LOG_ERR("Subscriptions failed for %s", reflector_name);
continue;
}
// Capabilities
err = bt_le_cs_read_remote_supported_capabilities(connections[i]);
if (err) {
LOG_ERR("Failed to exchange CS capabilities for %s", reflector_name);
return 0;
}
k_sem_take(&sem_remote_capabilities_obtained, K_FOREVER);
// CS Config
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 = 5,
.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 = 3,
.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(connections[i], &config_params,
BT_LE_CS_CREATE_CONFIG_CONTEXT_LOCAL_AND_REMOTE);
if (err) {
LOG_ERR("Failed to create CS config (err %d)", err);
return 0;
}
k_sem_take(&sem_config_created, K_FOREVER);
// Enable CS security
err = bt_le_cs_security_enable(connections[i]);
if (err) {
LOG_ERR("Failed to start CS Security for %s", reflector_name);
return 0;
}
k_sem_take(&sem_cs_security_enabled, K_FOREVER);
// Procedure params
const struct bt_le_cs_set_procedure_parameters_param procedure_params = {
.config_id = CS_CONFIG_ID,
.max_procedure_len = 1000,
.min_procedure_interval = 10,
.max_procedure_interval = 10,
.max_procedure_count = 0,
.min_subevent_len = 60000,
.max_subevent_len = 60000,
.tone_antenna_config_selection = BT_LE_CS_TONE_ANTENNA_CONFIGURATION_A1_B1,
.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_SNR_CONTROL_NOT_USED,
.snr_control_reflector = BT_LE_CS_SNR_CONTROL_NOT_USED,
};
err = bt_le_cs_set_procedure_parameters(connections[i], &procedure_params);
if (err) {
LOG_ERR("Failed to set procedure parameters for %s", reflector_name);
return 0;
}
// Enable procedure
struct bt_le_cs_procedure_enable_param params = {
.config_id = CS_CONFIG_ID,
.enable = 1,
};
err = bt_le_cs_procedure_enable(connections[i], ¶ms);
if (err) {
LOG_ERR("Failed to enable CS procedures for %s", reflector_name);
return 0;
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/* Ranging estimates */
LOG_INF("Device %d: %s", device_nr, reflector_name);
for (int round = 0; round < RANGING_ROUNDS; round++) {
k_sleep(K_MSEC(5000));
if (buffer_num_valid != 0) {
for (uint8_t ap = 0; ap < MAX_AP; ap++) {
cs_de_dist_estimates_t distance_on_ap = get_distance(ap);
LOG_INF("ifft: %f, phase_slope: %f, rtt: %f",
(double)distance_on_ap.ifft,
(double)distance_on_ap.phase_slope,
(double)distance_on_ap.rtt);
}
}
}
/* for stable disconnection */
ranging_done = true;
bt_conn_disconnect(connections[i], BT_HCI_ERR_REMOTE_USER_TERM_CONN);
connections[i] = NULL;
k_sem_take(&sem_ranging_done, K_FOREVER);
ranging_done = false;
}
}
}