Hi,
i acheived writting code that reconnects sequentially with multiple reflectors for my 1 initiator. When i am indoor i had good results, but yesterday i tried it with 6 reflectors and it didn't work that quick and fluent. Sometimes the furthest reflectors didn't give data IN THE BEGINNING (after it runs for a while, it does produces IFFT distance estimates even for the further reflectors).
this is 1 output file i got and my code for this matter:
#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);
/* Maak WARNING & ERROR no-ops */
#undef LOG_WRN
#define LOG_WRN(...) do { } while (0)
#undef LOG_ERR
#define LOG_ERR(...) do { } while (0)
#define CON_STATUS_LED DK_LED1
#define CS_CONFIG_ID 0
#define NUM_MODE_0_STEPS 3
#define PROCEDURE_COUNTER_NONE (-1)
#define PROCEDURE_COUNTER_DROPPED_NONE (-2)
#define DE_SLIDING_WINDOW_SIZE (10)
#define MAX_AP (CONFIG_BT_RAS_MAX_ANTENNA_PATHS)
// Totale buffer voor maximaal aantal stappen per ranging-procedure voor de LOCAL & PEER (initiator & reflector)
#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))
// Buffermaat voor een volledige ontvangen ranging-procedure (inclusief headers, subevent headers, mode info en alle step data)
#define BT_RAS_PROCEDURE_MEM \
(BT_RAS_RANGING_HEADER_LEN + \
(BT_RAS_MAX_SUBEVENTS_PER_PROCEDURE * BT_RAS_SUBEVENT_HEADER_LEN) + \
(BT_RAS_MAX_STEPS_PER_PROCEDURE * BT_RAS_STEP_MODE_LEN) + \
(BT_RAS_MAX_STEPS_PER_PROCEDURE * BT_RAS_MAX_STEP_DATA_LEN))
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_SEM_DEFINE(sem_debug_error, 0, 1); //To STOP output when a problem occurs
static K_MUTEX_DEFINE(distance_estimate_buffer_mutex);
/* For creating a buffer and a counter to correctly handle the step data */
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;
static int32_t ranging_counter_offset = 0; // Nodig om een doorlopende counter te hebben om niet in DROPPED continu te komen!!
/* For scanning & connecting with multiple devices */
#define MAX_DEVICES 6
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];
int device = 0;
int device_counter = 0;
/* 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 5
static bool ranging_done = false;
/* For buffering Distance estimations */
struct device_estimate_buffers {
uint8_t buffer_index;
uint8_t buffer_num_valid;
cs_de_dist_estimates_t distances[MAX_AP][DE_SLIDING_WINDOW_SIZE];
};
/* one per device slot */
static struct device_estimate_buffers device_estimates[MAX_DEVICES];
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Storen van de distance_estimates van het REPORT => per‐device BUFFER
static void store_distance_estimates(cs_de_report_t *p_report, int nr)
{
struct device_estimate_buffers *buf = &device_estimates[nr];
/* Lock the shared mutex so no two contexts write at once */
int lock_state = k_mutex_lock(&distance_estimate_buffer_mutex, K_FOREVER);
__ASSERT_NO_MSG(lock_state == 0);
/* For each antenna‐path, copy the new estimate into this device’s circular buffer */
uint8_t idx = buf->buffer_index;
for (uint8_t ap = 0; ap < p_report->n_ap; ap++) {
memcpy(&buf->distances[ap][idx],
&p_report->distance_estimates[ap],
sizeof(cs_de_dist_estimates_t));
}
/* Advance that device’s buffer index and valid‐count */
buf->buffer_index = (idx + 1) % DE_SLIDING_WINDOW_SIZE;
if (buf->buffer_num_valid < DE_SLIDING_WINDOW_SIZE) {
buf->buffer_num_valid++;
}
k_mutex_unlock(&distance_estimate_buffer_mutex);
}
// Calculate AVERAGE DISTANCE over this device’s buffer window
static cs_de_dist_estimates_t get_distance(uint8_t ap, int nr)
{
struct device_estimate_buffers *buf = &device_estimates[nr];
cs_de_dist_estimates_t avg = {};
uint8_t cnt_ifft = 0;
uint8_t cnt_phase_slope = 0;
uint8_t cnt_rtt = 0;
/* Lock so store_distance_estimates() can’t write concurrently */
k_mutex_lock(&distance_estimate_buffer_mutex, K_FOREVER);
for (uint8_t i = 0; i < buf->buffer_num_valid; i++) {
float ifft = buf->distances[ap][i].ifft;
float phase_slope = buf->distances[ap][i].phase_slope;
float rtt = buf->distances[ap][i].rtt;
if (isfinite(ifft)) {
avg.ifft += ifft;
cnt_ifft++;
}
if (isfinite(phase_slope)) {
avg.phase_slope += phase_slope;
cnt_phase_slope++;
}
if (isfinite(rtt)) {
avg.rtt += rtt;
cnt_rtt++;
}
}
k_mutex_unlock(&distance_estimate_buffer_mutex);
if (cnt_ifft) { avg.ifft /= cnt_ifft; }
if (cnt_phase_slope) { avg.phase_slope /= cnt_phase_slope; }
if (cnt_rtt) { avg.rtt /= cnt_rtt; }
// LOG_INF("Estimates from device %d done", nr+1);
return avg;
}
// De LOCAL en PEER step data worden gecombineerd en verwerkt tot een afstandsrapport en kijkt men naar de kwaliteit
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 from device %d with ranging counter %d (err %d)", device +1, ranging_counter, err);
/* Reset both step buffers */
net_buf_simple_reset(&latest_local_steps);
net_buf_simple_reset(&latest_peer_steps);
/* Give the semaphore back so the next subevent can proceed */
k_sem_give(&sem_local_steps);
return;
}
// LOG_INF("ranging_data_get_complete %d", ranging_counter);
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;
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);
// HIER WORDEN DE BEREKENINGEN GEDAAN IN "cs_de.c" EN IN ONS cs_de_report GEZET
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, device); // Afstandsrapport wordt hier bewaard
}
}
}
}
// MANAGE binnenkomende step data van de LOCAL (initiator) in result
static void subevent_result_cb(struct bt_conn *conn, struct bt_conn_le_cs_subevent_result *result)
{
uint16_t raw_counter = bt_ras_rreq_get_ranging_counter(result->header.procedure_counter);
uint16_t logical_counter = ranging_counter_offset + raw_counter;
// LOG_INF("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
// LOG_INF("dropped ranging counter = %d", dropped_ranging_counter);
// LOG_INF("Procedure counter = %d", result->header.procedure_counter);
// LOG_INF("raw counter = %d | logical counter = %d", raw_counter, logical_counter);
// als we deze precedure (result) ooit al hebben "gedropped" door een fout of overlap dan doen we niets met deze data
if (dropped_ranging_counter == logical_counter) {
// LOG_INF("We are dropping the result");
return;
}
// Niets doen, want de laatste ranging round is vaak niet completed.
if (raw_counter == RANGING_ROUNDS-1){
return;
}
// Indien de local ranging counter nog niet in SYNC is met de huidige ranging counter van de data, updaten we hem
if (most_recent_local_ranging_counter != logical_counter) {
int sem_state = k_sem_take(&sem_local_steps, K_NO_WAIT);
if (sem_state < 0) {
dropped_ranging_counter = logical_counter;
// LOG_INF("Dropped subevent results due to unfinished ranging data request.");
// int debug = k_sem_take(&sem_debug_error, K_FOREVER);
return;
}
// Syncen tussen PEER & LOCAL (initiator & reflector STEP DATA) is nodig om niet geen data door elkaar te hebben
most_recent_local_ranging_counter = logical_counter;
// LOG_INF("most_recent_local_ranging_counter = %d", most_recent_local_ranging_counter);
}
// Subevent data (bevat mode steps) heeft status "ABORTED" (niet succesvol afgerond) en dus genegeerd
if (result->header.subevent_done_status == BT_CONN_LE_CS_SUBEVENT_ABORTED) {
/* The steps from this subevent will not be used. */
}
// Subevent STEP DATA succesvol afgerond => data in Latest Local Steps (Memory van onze initiator) geplaatst
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);
// We drop this procedure
dropped_ranging_counter = result->header.procedure_counter;
return;
}
}
dropped_ranging_counter = PROCEDURE_COUNTER_NONE;
// Procedure COMPLETED :)
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) + ranging_counter_offset;
// LOG_INF("procedure complete");
}
// Procedure ABORTED :(
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);
}
if (raw_counter == (RANGING_ROUNDS - 1)) {
ranging_counter_offset += RANGING_ROUNDS;
}
}
// Haalt de step data van de PEER (reflector) op zodra de LOCAL procedure compleet is
static void ranging_data_ready_cb(struct bt_conn *conn, uint16_t ranging_counter)
{
LOG_DBG("Ranging data ready %i", ranging_counter);
ranging_counter = ranging_counter + ranging_counter_offset;
// LOG_INF("ranging_counter = %d", ranging_counter);
// Enkel alle data ophalen wanneer de lokale huidige ranging counter overeenkomt met de verwachte meetronde
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);
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;
device = i;
struct device_estimate_buffers *buf = &device_estimates[i];
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,
};
// Dus alle channels MOETEN aanstaan voor IFFT!
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++) {
// onderaan in ranging_data_get_complete_cb:
k_sleep(K_MSEC(500));
if (buf->buffer_num_valid != 0) {
for (uint8_t ap = 0; ap < MAX_AP; ap++) {
cs_de_dist_estimates_t distance_on_ap = get_distance(ap, device);
// 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);
if(distance_on_ap.ifft != 0.0){
LOG_INF("IFFT: %f", (double)distance_on_ap.ifft);
}
}
}
}
/* 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;
}
}
}
I: Device 1: Reflector355 I: Device 2: Reflector651 I: IFFT: 6.733620 I: IFFT: 7.172769 I: Device 3: Reflector604 I: IFFT: 7.171619 I: IFFT: 7.171619 I: Device 4: Reflector921 I: IFFT: 5.332509 I: IFFT: 5.332509 I: Device 5: Reflector494 I: IFFT: 3.805959 I: IFFT: 3.659576 I: Device 6: Reflector771 I: Device 1: Reflector355 I: IFFT: 3.758502 I: IFFT: 3.684786 I: Device 2: Reflector651 I: IFFT: 7.172769 I: IFFT: 7.319151 I: IFFT: 7.319151 I: Device 3: Reflector604 I: IFFT: 7.171619 I: IFFT: 7.196850 I: IFFT: 7.221393 I: Device 4: Reflector921 I: IFFT: 5.332509 I: IFFT: 5.332509 I: IFFT: 5.415856 I: Device 5: Reflector494 I: IFFT: 3.659576 I: IFFT: 3.610781 I: IFFT: 3.610781 I: Device 6: Reflector771 W: Local step data appears malformed. I: Device 1: Reflector355 I: IFFT: 3.684786 I: IFFT: 3.684786 I: IFFT: 3.684786 I: Device 2: Reflector651 I: IFFT: 7.319151 I: IFFT: 7.319151 I: IFFT: 7.319151 I: Device 3: Reflector604 I: IFFT: 7.221393 I: IFFT: 7.221393 I: IFFT: 7.234760 I: Device 4: Reflector921 I: IFFT: 5.415856 I: IFFT: 5.074401 I: IFFT: 5.074401 I: Device 5: Reflector494 I: IFFT: 3.610781 I: IFFT: 3.610781 I: IFFT: 3.586384 I: Device 6: Reflector771 I: Device 1: Reflector355 I: IFFT: 3.684786 I: IFFT: 3.684786 I: IFFT: 3.684786 I: Device 2: Reflector651 I: IFFT: 7.319151 I: IFFT: 7.458704 I: IFFT: 7.489347 I: Device 3: Reflector604 I: IFFT: 7.234760 I: IFFT: 7.258756 I: IFFT: 6.390523 I: Device 4: Reflector921 I: IFFT: 5.074401 I: IFFT: 5.074401 I: IFFT: 5.074401 I: Device 5: Reflector494 I: IFFT: 3.586384 I: IFFT: 3.630299 I: IFFT: 3.659576 I: Device 6: Reflector771 I: IFFT: 10.832344 I: IFFT: 11.564260 I: Device 1: Reflector355 I: IFFT: 3.684786 I: IFFT: 3.616436 I: IFFT: 2.921262 I: Device 2: Reflector651 I: IFFT: 7.489347 I: IFFT: 7.558569 I: IFFT: 7.566190 W: conn 0x20002660 failed to establish. RF noise? I: Device 4: Reflector921 I: IFFT: 5.074401 I: IFFT: 5.871063 I: IFFT: 6.337609 I: Device 5: Reflector494 I: IFFT: 3.659576 I: IFFT: 3.659576 I: IFFT: 3.659576 W: conn 0x200028b8 failed to establish. RF noise? I: Device 1: Reflector355 I: IFFT: 2.921262 I: IFFT: 3.065313 I: IFFT: 3.065313 I: Device 2: Reflector651 I: IFFT: 7.566190 I: IFFT: 7.618394 I: IFFT: 7.618394 I: Device 3: Reflector604 I: IFFT: 6.390523 I: IFFT: 6.390523 I: IFFT: 6.564244 W: conn 0x200027f0 failed to establish. RF noise? I: Device 5: Reflector494 I: IFFT: 3.659576 I: IFFT: 3.659576 I: IFFT: 3.555016 I: Device 6: Reflector771 I: IFFT: 11.564260 I: IFFT: 11.564260 I: IFFT: 11.222699 I: Device 1: Reflector355 I: IFFT: 3.065313 I: IFFT: 3.065313 I: IFFT: 3.065313 I: Device 2: Reflector651 I: IFFT: 7.618394 I: IFFT: 7.618394 I: IFFT: 7.744791 I: Device 3: Reflector604 I: IFFT: 6.564244 I: IFFT: 6.622012 I: IFFT: 6.622012 I: Device 4: Reflector921 I: IFFT: 6.337609 I: IFFT: 6.709648 I: IFFT: 6.709648 I: Device 5: Reflector494 I: IFFT: 3.555016 I: IFFT: 3.659576 I: IFFT: 3.675841 I: Device 6: Reflector771 I: IFFT: 11.222699 I: IFFT: 11.222699 I: IFFT: 11.222699 I: Device 1: Reflector355 I: IFFT: 3.065313 I: IFFT: 3.065313 I: IFFT: 2.899145 I: Device 2: Reflector651 I: IFFT: 7.744791 I: IFFT: 7.744791 I: IFFT: 7.766937 I: Device 3: Reflector604 I: IFFT: 6.622012 I: IFFT: 6.622012 I: IFFT: 6.634413 I: Device 4: Reflector921 I: IFFT: 6.709648 I: IFFT: 6.709648 I: IFFT: 6.709648 I: Device 5: Reflector494 I: IFFT: 3.675841 I: IFFT: 3.659575 I: IFFT: 3.586384 I: Device 6: Reflector771 I: IFFT: 11.222699 I: IFFT: 11.564260 I: IFFT: 11.564260 I: Device 1: Reflector355 I: IFFT: 2.899145 I: IFFT: 2.899145 I: IFFT: 2.899145 I: Device 2: Reflector651 I: IFFT: 7.766937 I: IFFT: 7.766937 I: IFFT: 7.766937 I: Device 3: Reflector604 I: IFFT: 6.634413 I: IFFT: 6.337369 I: IFFT: 6.226780 I: Device 4: Reflector921 I: IFFT: 6.709648 I: IFFT: 6.709648 I: IFFT: 6.575851 W: conn 0x20002728 failed to establish. RF noise? I: Device 6: Reflector771 I: IFFT: 11.564260 I: IFFT: 11.593536 I: IFFT: 11.593536 I: Device 1: Reflector355 I: IFFT: 2.899145 I: IFFT: 2.899145 I: IFFT: 2.721164 I: Device 2: Reflector651 I: IFFT: 7.766937 I: IFFT: 7.766937 I: IFFT: 7.766937 W: conn 0x20002660 failed to establish. RF noise? W: conn 0x20002728 failed to establish. RF noise? I: Device 5: Reflector494 I: IFFT: 3.586384 I: IFFT: 3.659575 I: IFFT: 3.659575 I: Device 6: Reflector771 I: IFFT: 11.593536 I: IFFT: 11.613053 I: IFFT: 10.874168 I: Device 1: Reflector355 I: IFFT: 2.721164 I: IFFT: 2.275441 I: IFFT: 2.349810 I: Device 2: Reflector651 I: IFFT: 7.766937 I: IFFT: 7.766937 I: IFFT: 7.766937 I: Device 3: Reflector604 I: IFFT: 6.226780 I: IFFT: 6.196269 I: IFFT: 6.196269 I: Device 4: Reflector921 I: IFFT: 6.575851 I: IFFT: 6.813890 I: IFFT: 6.813890 I: Device 5: Reflector494 I: IFFT: 3.659575 I: IFFT: 3.680487 I: IFFT: 3.596840 I: Device 6: Reflector771 I: IFFT: 10.874168 I: IFFT: 10.874168 I: IFFT: 10.874168 I: Device 1: Reflector355 I: IFFT: 2.349810 I: IFFT: 2.879648 I: IFFT: 2.689180 I: Device 2: Reflector651 I: IFFT: 7.766937 I: IFFT: 7.786615 I: IFFT: 7.786615 I: Device 3: Reflector604 I: IFFT: 6.196269 I: IFFT: 6.165209 I: IFFT: 6.157895 I: Device 4: Reflector921 I: IFFT: 6.813890 I: IFFT: 6.813890 I: IFFT: 6.635229 I: Device 5: Reflector494 I: IFFT: 3.596840 I: IFFT: 3.555016 I: IFFT: 3.659576 W: conn 0x20002660 failed to establish. RF noise? I: Device 1: Reflector355 I: IFFT: 2.689180 I: IFFT: 2.795430 I: IFFT: 2.434484 I: Device 2: Reflector651 I: IFFT: 7.786615 I: IFFT: 7.748694 I: IFFT: 7.748694 I: Device 3: Reflector604 I: IFFT: 6.157895 I: IFFT: 6.194490 I: IFFT: 5.896568 I: Device 4: Reflector921 I: IFFT: 6.635229 I: IFFT: 5.897981 I: IFFT: 5.897981 I: Device 5: Reflector494 I: IFFT: 3.659576 I: IFFT: 3.561987 I: IFFT: 3.513193 I: Device 6: Reflector771 I: IFFT: 10.874168 I: IFFT: 10.874168 I: IFFT: 11.041463 I: Device 1: Reflector355 I: IFFT: 2.434484 I: IFFT: 2.434484 I: IFFT: 2.434484 I: Device 2: Reflector651 I: IFFT: 7.748694 I: IFFT: 7.665047 I: IFFT: 7.665047 I: Device 3: Reflector604 I: IFFT: 5.896568 I: IFFT: 5.875535 I: IFFT: 5.420121 I: Device 4: Reflector921 I: IFFT: 5.897981 I: IFFT: 5.947828 I: IFFT: 6.021019 I: Device 5: Reflector494 I: IFFT: 3.513193 I: IFFT: 3.513193 I: IFFT: 3.513193 I: Device 6: Reflector771 I: IFFT: 11.041463 I: IFFT: 11.041463 I: IFFT: 10.957815 I: Device 1: Reflector355 I: IFFT: 2.434484 I: IFFT: 2.434484 I: IFFT: 2.434484 I: Device 2: Reflector651 I: IFFT: 7.665047 I: IFFT: 7.665047 I: IFFT: 7.611918 I: Device 3: Reflector604 I: IFFT: 5.420121 I: IFFT: 5.452650 I: IFFT: 5.571953 I: Device 4: Reflector921 I: IFFT: 6.021019 I: IFFT: 6.021019 I: IFFT: 5.866686 I: Device 5: Reflector494 I: IFFT: 3.513193 I: IFFT: 3.513193 I: IFFT: 3.513193 I: Device 6: Reflector771 I: IFFT: 10.957815 I: IFFT: 10.623225 I: IFFT: 10.623225 I: Device 1: Reflector355 I: IFFT: 2.434484 I: IFFT: 2.335849 I: IFFT: 2.335849 I: Device 2: Reflector651 I: IFFT: 7.611918 I: IFFT: 7.611917 I: IFFT: 7.611917 I: Device 3: Reflector604 I: IFFT: 5.571953 I: IFFT: 5.438381 I: IFFT: 5.438381 W: conn 0x200027f0 failed to establish. RF noise?
I would like to know how to make my code better and i would like to really get some good help for better (faster and more consistently) working ranging. If this would mean i would try out multi-connections PARALLEL instead of sequentially, i would like to try it. But i really would like to get some good coding help with this if it is possible...
Kind regards,
Nick De Leenheer
