Hi,
I am trying to do trilateration using one initiator and 3 reflectors. For now I am just trying to get 2 reflectors to work.
Sadly I cannot get the initiator to work like I want it to.
The reflector is running zephyr/samples/bluetooth/channel_sounding/cs_test/reflector, but the advertised names are "channel reflector 1" and "channel reflector 2"
The initiator was modified from zephyr/samples/bluetooth/channel_sounding/cs_test/initiator to use those 2 reflectors.
How can I get this to work?
/*
* 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"
#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 uint16_t latest_peer_steps_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 reflector1_str[] = "channel reflector 1";
static const char reflector2_str[] = "channel reflector 2";
/* Round-robin state */
static int current_reflector_index = 0;
static const char* reflector_names[] = {reflector1_str, reflector2_str};
static const int num_reflectors = 2;
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);
latest_peer_steps_len = 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);
/* Check if this is the reflector we're looking for in round-robin */
const char *target_name = reflector_names[current_reflector_index];
if (strcmp(name, target_name))
{
return;
}
if (bt_le_scan_stop())
{
return;
}
printk("Found target reflector %d: %s, connecting...\n", current_reflector_index + 1, 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 Multi-Reflector 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)
{
printk("\n=== Starting new CS round ===\n");
/* Original working logic: Run CS test first */
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;
}
}
/* Original working logic: Scan and connect with timeout */
printk("Scanning for reflector %d: %s\n", current_reflector_index + 1, reflector_names[current_reflector_index]);
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;
}
/* Wait for connection with timeout */
err = k_sem_take(&sem_connected, K_SECONDS(10));
if (err)
{
printk("Timeout waiting for reflector %d, moving to next\n", current_reflector_index + 1);
bt_le_scan_stop();
current_reflector_index = (current_reflector_index + 1) % num_reflectors;
continue;
}
k_sem_take(&sem_data_received, K_FOREVER);
/* Calculate distance using actual peer steps length */
estimate_distance(
latest_local_steps, latest_step_data_len, latest_peer_steps,
latest_peer_steps_len,
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);
/* Move to next reflector in round-robin */
current_reflector_index = (current_reflector_index + 1) % num_reflectors;
printk("Next target: reflector %d (%s)\n", current_reflector_index + 1, reflector_names[current_reflector_index]);
printk("Re-running CS test...\n");
}
return 0;
}
CONFIG_BT=y
CONFIG_BT_SMP=y
CONFIG_BT_CENTRAL=y
CONFIG_BT_GAP_AUTO_UPDATE_CONN_PARAMS=n
CONFIG_BT_AUTO_PHY_UPDATE=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_BT_MAX_PAIRED=4
CONFIG_MAIN_STACK_SIZE=4096
CONFIG_CBPRINTF_FP_SUPPORT=y
CONFIG_BT_CHANNEL_SOUNDING_TEST=y
CONFIG_CONSOLE=y
I am on nRF Connect SDK v3.0.2
Any help would be much appreciated.
Regards
