Hi,
I was talking to Susheel Nuguru about the multiconnection problem that i had in my other ticket about multiconnections for Channel Sounding, which had the same title as this one.
I verified his answer and the ticket was closed it said, but my problem was actually not solved yet. I need this problem to be fixed pretty soon and i was worried that he maybe could not see my reply on his verified answer...
Here is my problem again:
somethimes i get this Warning when connecting with devices without getting any errors (which stops my code) and this happens at this moment in my main code:
I followed the suggestion to give a semaphore also when a connection fails (in connected_cb() ) to get in this SKIP part after 4 seconds, but this still doesn't make my code to skip this device and go on to the next one...
Kind regards,
Nick De Leenheer
#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); 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 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(¶m); 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]; 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; } 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; } 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); 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 %d: %s", device_nr, 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; } } }