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]

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

[Susheel Nuguru]

There is a bug in your code, in the connected_cb() you are not giving the semaphore when the connection fails. 

So when there is a connection failure your main loop is stuck. You also still have k_sem_take waiting for K_FOREVER.

I suggest you one of the two changes

1) In your connected_cb()

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);
        k_sem_give(&sem_connected);    // ADD THIS NEW LINE ADDED SUSHEEL
        return;
    }

    /* On success the same give(), which you are doing already. */
    k_sem_give(&sem_connected);
    …
}

2) Make sure your k_sem_take are not waiting for ever and PLEASE DO NOT IGNORE RETURN VALUES of function calls like k_sem_take

[NickDeLeenheer]

Hi,

Thank you very much! I will test your first suggestion (1), and indeed, this makes sense! This way we get to the error instead of just stopping... thank you very much!

now i will get to go the the next device (continue;) in this code i think:

[NickDeLeenheer]

Hi,

This actually did not fix the problem.


Do you know what else it could be?