Ranging CS data from multiple connections

[NickDeLeenheer]

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

Fullscreen 344104.main.c Download

/*
 * 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(&param);
	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], &params);
			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;
		}
	}
}

[Amanda Hsieh]

Hi,

What NCS version are you using?

Regards,
Amanda H.

[NickDeLeenheer]

NCS3.0.1

I am still trying to fix it myself, i have found the cause of my problem, but i need some help interpretting the "DROPPED counter"

I now have implemented a logical counter, which is a counter that keeps in track with the real counter, because now we keep resetting the counter, due to multiple device re-connections.

as you can see, there are always 2 subevents for 1 procedure. For the first device, it works perfectly.

But then i tried modifying the code and i tried to work with a logical counter, which keeps in track of the TRUE count.

But i am affraid that we now have other problems for the next devices in line:




i think the problem has actually to do with the fact that for my last ranging round, it doesn't finish with doing the ranging properly. Look at what i do in the main.c to get all the ranging estimates:



Therefore i come in this line of code "Dropped subevent results...":



So, if you can help me, or know someone who can really fix this as soon as possible, i would be very thankful.

I need to get this running tomorrow to do my measurements on Thursday (in 2 days)

Kind regards,

Nick De Leenheer

[Amanda Hsieh]

Based on your logs, it seems like you haven't enabled debug logs. 

CONFIG_BT_RAS_RREQ_LOG_LEVEL_DBG=y

CONFIG_BT_RAS_RRSP_LOG_LEVEL_DBG=y

This may help you debug your code

Briefly looking at your code. The sample there is intended for a single link. Extending it to make it work with multiple ACLs is non-trivial and would take a bit more work than what you have done there, every connection needs to be managed separately, probably with separate semaphores/queues for the local steps.

The controller connected to multiple devices will produce steps for each connection, and the ranging counters are not shared between these. The ranging data from different connections cannot be mixed either. 

Not really understand your comments in the code. I don't speak Dutch. 

[NickDeLeenheer]

Hi,

Is it possible to get some concrete code as examples on how to do it?

Also, i am wondering if the parallel connections would work better than sequential connections?

I am really struggling now with this code, since i don't know everything on how it should work...

Kind regards,

Nick De Leenheer

[Amanda Hsieh]

NickDeLeenheer said:

Is it possible to get some concrete code as examples on how to do it?

Unfortunately, there is no such code as you required. 

NickDeLeenheer said:

i am wondering if the parallel connections would work better than sequential connections?

We haven't tested that. 

NickDeLeenheer said:

I am really struggling now with this code, since i don't know everything on how it should work...

I see you created a similar case # 345569 and mentioned in this thread that you got a working code.

Since I will start vacation next week, I will not reply to this case during the period. If you need urgent support, please create a new support case.