W: conn failed to establish. RF noise?

[NickDeLeenheer]

Hi,

when i try making a new connection with a new device using my for loop (reconnecting using the adresses that i stored in an array), i sometimes can get a warning which will end my MULTICONNECTION CS process...

Can someone help me with HOW to solve this issue and where i should solve this in my code? I think i should try to solve it in main(), since no other Zephyr Callback functions are available for this failed connection setup. I also think it is weird that i don't get the error from the bt_conn_le_create in my loggs...

kind regards,

Nick De Leenheer

[Susheel Nuguru]

Hi Nick, 

Can you please tell us more on your test setup and also show us the console output where you see the warning you mentioned? It sounds like a configuration issue with the software but I can try to assist you more after you provide more information that I requested.

[NickDeLeenheer]

I am working on a Bluetooth Low Energy (BLE) application using Zephyr, where you attempt to reconnect to previously detected devices in a for loop using bt_conn_le_create() and using an array with STATIC addresses stored in an array. However, I'm encountering an intermittent issue where the connection to a device fails to establish. When this happens, I see a warning in the console as shown in the screenshot above:

W: conn ... failed to establish. RF noise?

This warning suggests that the connection attempt was unsuccessful, possibly due to interference (RF noise) or the remote device not being available. The key issue is that this failure occurs asynchronously—after the call to bt_conn_le_create() returns. As a result, the error isn't caught by the return value of bt_conn_le_create(), and my code proceeds to wait indefinitely on a semaphore as shown in the screenshot:

k_sem_take(&sem_connected, K_FOREVER);

To fix this, should I replace the infinite semaphore wait with a timed wait?

such as:

if (k_sem_take(&sem_connected, K_SECONDS(3)) != 0) {
    // Connection timeout: clean up and try next device
    bt_conn_disconnect(connections[i], BT_HCI_ERR_REMOTE_USER_TERM_CONN);
    bt_conn_unref(connections[i]);
    connections[i] = NULL;
    continue;
}

I can share you my entire code asswell if you want to...

Kind regards,

Nick De Leenheer

[Susheel Nuguru]

Isn't this the same issue as mentioned here ?

Yes, the infinite loop seems wrong like you already suspected and it is better to have a timeout and move on. I suggest the below for the cleanup code

if (k_sem_take(&sem_connected, K_SECONDS(4)) != 0) {
    printk("W: Connection to device %d timed out. Skipping...\n", i);
    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
}

[Susheel Nuguru]

Also please do not open multiple threads on the same issue and please close on of these two threads.

[NickDeLeenheer]

Hi,

Yes i am sorry, I will not do this in the future anymore.

I thought this would solve my problem aswell, but it does not:

And this is a random amount of time that this happens after this:


Where you can see the initial scanning and i used static addresses for the reflectors. I also had a problem before with not being able to filter my scan on the device name (full/short). Some other engineer from Nordic asked me about my versions of Zephyr and stuff but he never responded afterwards because I accidentally closed that ticket.

So this code did not help.

This is a problem that i want to resolve when doing indore real life measurements when it needs to work without stopping randomly...

Should i share you my code?

int main(void){
	int err;

	dk_leds_init();

	err = bt_enable(NULL);
	if (err) {
		return 0;
	}

	/* Start Scanning for Multiple Devices */
	err = scan_init();
	if (err) {
		return 0;
	}

	err = bt_scan_start(BT_SCAN_TYPE_SCAN_PASSIVE);
	if (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]) {
				// LOG_INF("unref conne/ction");
				bt_conn_disconnect(connections[i], BT_HCI_ERR_REMOTE_USER_TERM_CONN);
				bt_conn_unref(connections[i]);
				connections[i] = NULL;
			}

			// LOG_INF("start connection");
			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);
				return 0;
			}

			if (k_sem_take(&sem_connected, K_SECONDS(4)) != 0) {
				LOG_INF("W: Connection to device %d timed out. Skipping...", i);
				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
			}

			/* Add a small delay to allow the connection to stabilize */
			k_sleep(K_MSEC(200));

			
			// Security
			err = bt_conn_set_security(connections[i], BT_SECURITY_L2);
			if (err) {
				LOG_ERR("Security failed for %s", reflector_name);
				return 0;
			}
			k_sem_take(&sem_security, K_FOREVER);

			// MTU Exchange
			static struct bt_gatt_exchange_params mtu_exchange_params = { .func = mtu_exchange_cb };
			err = bt_gatt_exchange_mtu(connections[i], &mtu_exchange_params);
			if (err) {
				LOG_ERR("MTU exchange failed for %s", reflector_name);
				return 0;
			}
			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("GATT 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("Default settings failed 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);
				return 0;
			}

			// Capabilities
			err = bt_le_cs_read_remote_supported_capabilities(connections[i]);
			if (err) {
				LOG_ERR("Capabilities read failed 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_UNUSED,
				.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 = 1,
				.channel_selection_type = BT_CONN_LE_CS_CHSEL_TYPE_3B,
				.ch3c_shape = BT_CONN_LE_CS_CH3C_SHAPE_HAT,
				.ch3c_jump = 2,
			};

			// 15 Uniform verdeelde kanalen
			uint8_t small_channel_map[10] = {0}; // Alles eerst uitzetten

			BT_LE_CS_CHANNEL_BIT_SET_VAL(small_channel_map, 2, 1);
			BT_LE_CS_CHANNEL_BIT_SET_VAL(small_channel_map, 5, 1);
			BT_LE_CS_CHANNEL_BIT_SET_VAL(small_channel_map, 9, 1);
			BT_LE_CS_CHANNEL_BIT_SET_VAL(small_channel_map, 11, 1);
			BT_LE_CS_CHANNEL_BIT_SET_VAL(small_channel_map, 13, 1);
			BT_LE_CS_CHANNEL_BIT_SET_VAL(small_channel_map, 17, 1);
			BT_LE_CS_CHANNEL_BIT_SET_VAL(small_channel_map, 21, 1);
			BT_LE_CS_CHANNEL_BIT_SET_VAL(small_channel_map, 30, 1);
			BT_LE_CS_CHANNEL_BIT_SET_VAL(small_channel_map, 35, 1);
			BT_LE_CS_CHANNEL_BIT_SET_VAL(small_channel_map, 40, 1);
			BT_LE_CS_CHANNEL_BIT_SET_VAL(small_channel_map, 45, 1);
			BT_LE_CS_CHANNEL_BIT_SET_VAL(small_channel_map, 51, 1);
			BT_LE_CS_CHANNEL_BIT_SET_VAL(small_channel_map, 57, 1);
			BT_LE_CS_CHANNEL_BIT_SET_VAL(small_channel_map, 63, 1);
			BT_LE_CS_CHANNEL_BIT_SET_VAL(small_channel_map, 69, 1);
			BT_LE_CS_CHANNEL_BIT_SET_VAL(small_channel_map, 75, 1);

			memcpy(config_params.channel_map, small_channel_map, sizeof(small_channel_map));

			// 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("Config creation failed for %s", reflector_name);
				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("CS security enable failed 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_INDEX_ONE,
				.phy = BT_LE_CS_PROCEDURE_PHY_1M,
				.tx_power_delta = 0x80,
				.preferred_peer_antenna = BT_LE_CS_PROCEDURE_PREFERRED_PEER_ANTENNA_1,
				.snr_control_initiator = BT_LE_CS_INITIATOR_SNR_CONTROL_NOT_USED,
				.snr_control_reflector = BT_LE_CS_REFLECTOR_SNR_CONTROL_NOT_USED,
			};
			err = bt_le_cs_set_procedure_parameters(connections[i], &procedure_params);
			if (err) {
				LOG_ERR("Procedure params failed for %s", reflector_name);
				return 0;
			}

			// Enable procedure
			struct bt_le_cs_procedure_enable_param enable_params = {
				.config_id = CS_CONFIG_ID,
				.enable = 1,
			};
			err = bt_le_cs_procedure_enable(connections[i], &enable_params);
			if (err) {
				LOG_ERR("Procedure enable failed for %s", reflector_name);
				return 0;
			}


			// Ranging process
			LOG_INF("Device %d: %s", device_nr, reflector_name);
			minimal_distance = 1000.0;

			for (int round = 0; round < RANGING_ROUNDS; round++) {

				k_sem_take(&sem_procedure_done, K_FOREVER);
				distance_estimation_in_progress = true;

				err = k_sem_take(&sem_rd_ready, K_SECONDS(1));
				if (err) {
					LOG_WRN("Timeout waiting for rd_ready from %s", reflector_name);
					continue;
				}

				if (most_recent_peer_ranging_counter != most_recent_local_ranging_counter) {
					LOG_WRN("Mismatched counters on %s", reflector_name);
					continue;
				}

				err = bt_ras_rreq_cp_get_ranging_data(connections[i], &latest_peer_steps,
					most_recent_peer_ranging_counter, ranging_data_get_complete_cb);
				if (err) {
					LOG_ERR("Failed to get ranging data from %s (err %d)", reflector_name, err);
					continue;
				}

				err = k_sem_take(&sem_rd_complete, K_SECONDS(5));
				if (err) {
					LOG_ERR("Timeout waiting for rd_complete from %s", reflector_name);
					continue;
				}

				distance = estimate_distance(&latest_local_steps, &latest_peer_steps, n_ap,
											BT_CONN_LE_CS_ROLE_INITIATOR, reflector_name);

				// Find the SMALLEST PBR value to mitigate reflections and filter unstable measurements
				if (distance < minimal_distance) {
					minimal_distance = distance;
				}
				
				net_buf_simple_reset(&latest_local_steps);
				net_buf_simple_reset(&latest_peer_steps);
				distance_estimation_in_progress = false;
			}		

			LOG_INF("PBR (LoS) = %f", minimal_distance);

			bt_conn_disconnect(connections[i], BT_HCI_ERR_REMOTE_USER_TERM_CONN);
			connections[i] = NULL; 
		}	
	}
}

this is my main function... I figure that the scanning function are not really needed to share, since they work flawlessly.

Kind regards,

Nick De Leenheer

[Susheel Nuguru]

Yes, Please share you whole project zipped. I need to see what your code is doing in other callbacks like connected(). I also need to see how you are giving the semaphore sem_connected.

[NickDeLeenheer]

Fullscreen 422482.main.c Download

#include <zephyr/kernel.h>
#include <zephyr/types.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 "distance_estimation.h"

#include <dk_buttons_and_leds.h>

#include <zephyr/logging/log.h>
LOG_MODULE_DECLARE(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 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))

static K_SEM_DEFINE(sem_remote_capabilities_obtained, 0, 1);
static K_SEM_DEFINE(sem_config_created, 0, 1);
static K_SEM_DEFINE(sem_cs_security_enabled, 0, 1);
static K_SEM_DEFINE(sem_procedure_done, 0, 1);
static K_SEM_DEFINE(sem_connected, 0, 1);
static K_SEM_DEFINE(sem_rd_ready, 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_rd_complete, 0, 1);
static K_SEM_DEFINE(sem_security, 0, 1);
static K_SEM_DEFINE(sem_disconnected, 0, 1);



/* 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 uint8_t n_ap;
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_peer_ranging_counter = PROCEDURE_COUNTER_NONE;
static int32_t most_recent_local_ranging_counter = PROCEDURE_COUNTER_NONE;
static int32_t dropped_ranging_counter = PROCEDURE_COUNTER_NONE;
static bool distance_estimation_in_progress;

static void subevent_result_cb(struct bt_conn *conn, struct bt_conn_le_cs_subevent_result *result)
{
	if (distance_estimation_in_progress) {
		dropped_ranging_counter = result->header.procedure_counter;
		return;
	}

	if (result->header.subevent_done_status == BT_CONN_LE_CS_SUBEVENT_ABORTED) {
		dropped_ranging_counter = result->header.procedure_counter;
		net_buf_simple_reset(&latest_local_steps);
		return;
	}

	if (dropped_ranging_counter == result->header.procedure_counter) {
		return;
	}

	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 {
			net_buf_simple_reset(&latest_local_steps);
			dropped_ranging_counter = result->header.procedure_counter;
			return;
		}
	}

	dropped_ranging_counter = PROCEDURE_COUNTER_NONE;
	n_ap = result->header.num_antenna_paths;

	if (result->header.procedure_done_status == BT_CONN_LE_CS_PROCEDURE_COMPLETE) {
		most_recent_local_ranging_counter = result->header.procedure_counter;
		distance_estimation_in_progress = true;
		k_sem_give(&sem_procedure_done);
	} else if (result->header.procedure_done_status == BT_CONN_LE_CS_PROCEDURE_ABORTED) {
		net_buf_simple_reset(&latest_local_steps);
	}
}

static void ranging_data_get_complete_cb(struct bt_conn *conn, uint16_t ranging_counter, int err)
{
	ARG_UNUSED(conn);

	if (err) {
		return;
	}

	k_sem_give(&sem_rd_complete);
}

static void ranging_data_ready_cb(struct bt_conn *conn, uint16_t ranging_counter)
{
	most_recent_peer_ranging_counter = ranging_counter;
	k_sem_give(&sem_rd_ready);
}

static void ranging_data_overwritten_cb(struct bt_conn *conn, uint16_t ranging_counter)
{
}

static void mtu_exchange_cb(struct bt_conn *conn, uint8_t err,
				struct bt_gatt_exchange_params *params)
{
	if (err) {
		return;
	}

	k_sem_give(&sem_mtu_exchange_done);
}

static void discovery_completed_cb(struct bt_gatt_dm *dm, void *context)
{
	int err;

	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) {
	}

	err = bt_gatt_dm_data_release(dm);
	if (err) {
	}

	k_sem_give(&sem_discovery_done);
}

static void discovery_service_not_found_cb(struct bt_conn *conn, void *context)
{
	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)
{
	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)
{
	if (err) {
		return;
	}

	k_sem_give(&sem_security);
}

static bool le_param_req(struct bt_conn *conn, struct bt_le_conn_param *param)
{
	return false;
}

static void connected_cb(struct bt_conn *conn, uint8_t err)
{
    if (err) {
        LOG_ERR("Connection failed (err 0x%02x)", err);
        bt_conn_unref(conn);
        return;
    }

    // When not in the scanning process we give a semaphore for being detected
    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)
{
    bt_conn_unref(conn);
    connection = NULL;
    dk_set_led_off(CON_STATUS_LED);

    // Handle disconnection during initial device scan
    if (scanning_for_initial_devices) {
        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 disconnection during ranging phases
    } else {
        // LOG_INF("Disconnection occurred during configuration or ranging phase");
    }
}



static void remote_capabilities_cb(struct bt_conn *conn, struct bt_conn_le_cs_capabilities *params)
{
	ARG_UNUSED(params);
	k_sem_give(&sem_remote_capabilities_obtained);
}

static void config_created_cb(struct bt_conn *conn, struct bt_conn_le_cs_config *config)
{
	k_sem_give(&sem_config_created);
}

static void security_enabled_cb(struct bt_conn *conn)
{
	k_sem_give(&sem_cs_security_enabled);
}

static void procedure_enabled_cb(struct bt_conn *conn,
				struct bt_conn_le_cs_procedure_enable_complete *params)
{
}

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;

	err = bt_scan_start(BT_SCAN_TYPE_SCAN_PASSIVE);
	if (err) {
		return;
	}
}

static void scan_connecting(struct bt_scan_device_info *device_info, struct bt_conn *conn)
{
}

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;
}

/* This Zephyr macro registers callbacks which act when certain steps are made in the system */
BT_CONN_CB_DEFINE(conn_cb) = {
	.connected = connected_cb,			// When Zephyr makes a connection between devices it calls this cb-function
	.disconnected = disconnected_cb,	// When Zephyr makes a disconnection between devices it calls this function
	.le_param_req = le_param_req,
	.security_changed = security_changed,
	.le_cs_remote_capabilities_available = remote_capabilities_cb,
	.le_cs_config_created = config_created_cb,
	.le_cs_security_enabled = security_enabled_cb,
	.le_cs_procedure_enabled = procedure_enabled_cb,
	.le_cs_subevent_data_available = subevent_result_cb,
};



int main(void){
	int err;

	dk_leds_init();

	err = bt_enable(NULL);
	if (err) {
		return 0;
	}

	/* Start Scanning for Multiple Devices */
	err = scan_init();
	if (err) {
		return 0;
	}

	err = bt_scan_start(BT_SCAN_TYPE_SCAN_PASSIVE);
	if (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];

			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;
			}

			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;
			}

			k_sem_take(&sem_connected, K_FOREVER);

			/* Add a small delay to allow the connection to stabilize */
			k_sleep(K_MSEC(200));


			// 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 };
			err = bt_gatt_exchange_mtu(connections[i], &mtu_exchange_params);
			if (err) {
				LOG_ERR("MTU exchange failed for %s", reflector_name);
				continue;
			}
			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("GATT discovery failed for %s", reflector_name);
				continue;
			}
			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("Default settings failed for %s", reflector_name);
				continue;
			}

			// 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("Capabilities read failed for %s", reflector_name);
				continue;
			}
			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_UNUSED,
				.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 = 1,
				.channel_selection_type = BT_CONN_LE_CS_CHSEL_TYPE_3B,
				.ch3c_shape = BT_CONN_LE_CS_CH3C_SHAPE_HAT,
				.ch3c_jump = 2,
			};
			bt_le_cs_set_valid_chmap_bits(config_params.channel_map);
			err = bt_le_cs_create_config(connections[i], &config_params, BT_LE_CS_CREATE_CONFIG_CONTEXT_LOCAL_AND_REMOTE);
			if (err) {
				LOG_ERR("Config creation failed for %s", reflector_name);
				continue;
			}
			k_sem_take(&sem_config_created, K_FOREVER);

			// Enable CS security
			err = bt_le_cs_security_enable(connections[i]);
			if (err) {
				LOG_ERR("CS security enable failed for %s", reflector_name);
				continue;
			}
			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_INDEX_ONE,
				.phy = BT_LE_CS_PROCEDURE_PHY_1M,
				.tx_power_delta = 0x80,
				.preferred_peer_antenna = BT_LE_CS_PROCEDURE_PREFERRED_PEER_ANTENNA_1,
				.snr_control_initiator = BT_LE_CS_INITIATOR_SNR_CONTROL_NOT_USED,
				.snr_control_reflector = BT_LE_CS_REFLECTOR_SNR_CONTROL_NOT_USED,
			};
			err = bt_le_cs_set_procedure_parameters(connections[i], &procedure_params);
			if (err) {
				LOG_ERR("Procedure params failed for %s", reflector_name);
				continue;
			}

			// Enable procedure
			struct bt_le_cs_procedure_enable_param enable_params = {
				.config_id = CS_CONFIG_ID,
				.enable = 1,
			};
			err = bt_le_cs_procedure_enable(connections[i], &enable_params);
			if (err) {
				LOG_ERR("Procedure enable failed for %s", reflector_name);
				continue;
			}

			LOG_INF("Device: %s", reflector_name);

			// Ranging process
			for (int round = 0; round < RANGING_ROUNDS; round++) {

				k_sem_take(&sem_procedure_done, K_FOREVER);
				distance_estimation_in_progress = true;

				err = k_sem_take(&sem_rd_ready, K_SECONDS(1));
				if (err) {
					LOG_WRN("Timeout waiting for rd_ready from %s", reflector_name);
					continue;
				}

				if (most_recent_peer_ranging_counter != most_recent_local_ranging_counter) {
					LOG_WRN("Mismatched counters on %s", reflector_name);
					continue;
				}

				err = bt_ras_rreq_cp_get_ranging_data(connections[i], &latest_peer_steps,
					most_recent_peer_ranging_counter, ranging_data_get_complete_cb);
				if (err) {
					LOG_ERR("Failed to get ranging data from %s (err %d)", reflector_name, err);
					continue;
				}

				err = k_sem_take(&sem_rd_complete, K_SECONDS(5));
				if (err) {
					LOG_ERR("Timeout waiting for rd_complete from %s", reflector_name);
					continue;
				}

				estimate_distance(&latest_local_steps, &latest_peer_steps, n_ap,
					BT_CONN_LE_CS_ROLE_INITIATOR, reflector_name);

				net_buf_simple_reset(&latest_local_steps);
				net_buf_simple_reset(&latest_peer_steps);
				distance_estimation_in_progress = false;
			}

			bt_conn_disconnect(connections[i], BT_HCI_ERR_REMOTE_USER_TERM_CONN);
			connections[i] = NULL; 
		}	
	}
}