My code for Sequential Multi-Reflector Channel Sounding is Slow and Occasionally Drops Data

Hi,

i acheived writting code that reconnects sequentially with multiple reflectors for my 1 initiator. When i am indoor i had good results, but yesterday i tried it with 6 reflectors and it didn't work that quick and fluent. Sometimes the furthest reflectors didn't give data IN THE BEGINNING (after it runs for a while, it does produces IFFT distance estimates even for the further reflectors).

this is 1 output file i got and my code for this matter:

#include <math.h>
#include <zephyr/kernel.h>
#include <zephyr/types.h>
#include <zephyr/sys/reboot.h>
#include <zephyr/bluetooth/cs.h>
#include <zephyr/bluetooth/gatt.h>
#include <zephyr/bluetooth/conn.h>
#include <bluetooth/scan.h>
#include <bluetooth/services/ras.h>
#include <bluetooth/gatt_dm.h>
#include <bluetooth/cs_de.h>

#include <dk_buttons_and_leds.h>

#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(app_main, LOG_LEVEL_INF);

/* Maak WARNING & ERROR no-ops */
#undef LOG_WRN
#define LOG_WRN(...)    do { } while (0)
#undef LOG_ERR
#define LOG_ERR(...)    do { } while (0)


#define CON_STATUS_LED DK_LED1

#define CS_CONFIG_ID	       0
#define NUM_MODE_0_STEPS       3
#define PROCEDURE_COUNTER_NONE (-1)
#define PROCEDURE_COUNTER_DROPPED_NONE (-2)
#define DE_SLIDING_WINDOW_SIZE (10)
#define MAX_AP		       (CONFIG_BT_RAS_MAX_ANTENNA_PATHS)

// Totale buffer voor maximaal aantal stappen per ranging-procedure voor de LOCAL & PEER (initiator & reflector)
#define LOCAL_PROCEDURE_MEM                                                                        \
	((BT_RAS_MAX_STEPS_PER_PROCEDURE * sizeof(struct bt_le_cs_subevent_step)) +                \
	 (BT_RAS_MAX_STEPS_PER_PROCEDURE * BT_RAS_MAX_STEP_DATA_LEN))

// Buffermaat voor een volledige ontvangen ranging-procedure (inclusief headers, subevent headers, mode info en alle step data)
#define BT_RAS_PROCEDURE_MEM                                                                       \
	(BT_RAS_RANGING_HEADER_LEN +                                                               \
	 (BT_RAS_MAX_SUBEVENTS_PER_PROCEDURE * BT_RAS_SUBEVENT_HEADER_LEN) +                       \
	 (BT_RAS_MAX_STEPS_PER_PROCEDURE * BT_RAS_STEP_MODE_LEN) +                                 \
	 (BT_RAS_MAX_STEPS_PER_PROCEDURE * BT_RAS_MAX_STEP_DATA_LEN))


static K_SEM_DEFINE(sem_remote_capabilities_obtained, 0, 1);
static K_SEM_DEFINE(sem_config_created, 0, 1);
static K_SEM_DEFINE(sem_cs_security_enabled, 0, 1);
static K_SEM_DEFINE(sem_connected, 0, 1);
static K_SEM_DEFINE(sem_discovery_done, 0, 1);
static K_SEM_DEFINE(sem_mtu_exchange_done, 0, 1);
static K_SEM_DEFINE(sem_security, 0, 1);
static K_SEM_DEFINE(sem_local_steps, 1, 1);
static K_SEM_DEFINE(sem_disconnected, 0, 1);
static K_SEM_DEFINE(sem_ranging_done, 0, 1);
static K_SEM_DEFINE(sem_nrf_noise, 0, 1);
static K_SEM_DEFINE(sem_debug_error, 0, 1);	//To STOP output when a problem occurs

static K_MUTEX_DEFINE(distance_estimate_buffer_mutex);


/* For creating a buffer and a counter to correctly handle the step data */
NET_BUF_SIMPLE_DEFINE_STATIC(latest_local_steps, LOCAL_PROCEDURE_MEM);
NET_BUF_SIMPLE_DEFINE_STATIC(latest_peer_steps, BT_RAS_PROCEDURE_MEM);
static int32_t most_recent_local_ranging_counter = PROCEDURE_COUNTER_NONE;
static int32_t dropped_ranging_counter = PROCEDURE_COUNTER_NONE;
static int32_t ranging_counter_offset = 0;	// Nodig om een doorlopende counter te hebben om niet in DROPPED continu te komen!!


/* For scanning & connecting with multiple devices */
#define MAX_DEVICES 6
static bt_addr_le_t addresses[MAX_DEVICES];
static int num_found = 0;
static bool scanning_for_initial_devices = true;
static bool connection_in_progress = false;
static struct bt_conn *connection = NULL;
static struct bt_conn *connections[MAX_DEVICES];
int device = 0;
int device_counter = 0;

/* For parsing the name of devices */
#define NAME_LEN 30
char reflector_names[MAX_DEVICES][NAME_LEN];
char reflector_name[NAME_LEN] = "(onbekend)";

/* For the ranging process */
#define RANGING_ROUNDS 5
static bool ranging_done = false;

/* For buffering Distance estimations */
struct device_estimate_buffers {
    uint8_t buffer_index;
    uint8_t buffer_num_valid;
    cs_de_dist_estimates_t distances[MAX_AP][DE_SLIDING_WINDOW_SIZE];
};

/* one per device slot */
static struct device_estimate_buffers device_estimates[MAX_DEVICES];


//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


// Storen van de distance_estimates van het REPORT => per‐device BUFFER
static void store_distance_estimates(cs_de_report_t *p_report, int nr)
{
    struct device_estimate_buffers *buf = &device_estimates[nr];

    /* Lock the shared mutex so no two contexts write at once */
    int lock_state = k_mutex_lock(&distance_estimate_buffer_mutex, K_FOREVER);
    __ASSERT_NO_MSG(lock_state == 0);

    /* For each antenna‐path, copy the new estimate into this device’s circular buffer */
    uint8_t idx = buf->buffer_index;
    for (uint8_t ap = 0; ap < p_report->n_ap; ap++) {
        memcpy(&buf->distances[ap][idx],
               &p_report->distance_estimates[ap],
               sizeof(cs_de_dist_estimates_t));
    }

    /* Advance that device’s buffer index and valid‐count */
    buf->buffer_index = (idx + 1) % DE_SLIDING_WINDOW_SIZE;
    if (buf->buffer_num_valid < DE_SLIDING_WINDOW_SIZE) {
        buf->buffer_num_valid++;
    }

    k_mutex_unlock(&distance_estimate_buffer_mutex);
}


// Calculate AVERAGE DISTANCE over this device’s buffer window
static cs_de_dist_estimates_t get_distance(uint8_t ap, int nr)
{
    struct device_estimate_buffers *buf = &device_estimates[nr];
    cs_de_dist_estimates_t avg = {};
    uint8_t cnt_ifft        = 0;
    uint8_t cnt_phase_slope = 0;
    uint8_t cnt_rtt         = 0;

    /* Lock so store_distance_estimates() can’t write concurrently */
    k_mutex_lock(&distance_estimate_buffer_mutex, K_FOREVER);

    for (uint8_t i = 0; i < buf->buffer_num_valid; i++) {
        float ifft        = buf->distances[ap][i].ifft;
        float phase_slope = buf->distances[ap][i].phase_slope;
        float rtt         = buf->distances[ap][i].rtt;

        if (isfinite(ifft)) {
            avg.ifft += ifft;
            cnt_ifft++;
        }
        if (isfinite(phase_slope)) {
            avg.phase_slope += phase_slope;
            cnt_phase_slope++;
        }
        if (isfinite(rtt)) {
            avg.rtt += rtt;
            cnt_rtt++;
        }
    }

    k_mutex_unlock(&distance_estimate_buffer_mutex);

    if (cnt_ifft)        { avg.ifft        /= cnt_ifft;       }
    if (cnt_phase_slope) { avg.phase_slope /= cnt_phase_slope; }
    if (cnt_rtt)         { avg.rtt         /= cnt_rtt;       }

	// LOG_INF("Estimates from device %d done", nr+1);

    return avg;
}


// De LOCAL en PEER step data worden gecombineerd en verwerkt tot een afstandsrapport en kijkt men naar de kwaliteit
static void ranging_data_get_complete_cb(struct bt_conn *conn, uint16_t ranging_counter, int err)
{
	ARG_UNUSED(conn);

	if (err) {
		LOG_ERR("Error when getting ranging data from device %d with ranging counter %d (err %d)", device +1, ranging_counter, err);
		
		/* Reset both step buffers */
        net_buf_simple_reset(&latest_local_steps);
        net_buf_simple_reset(&latest_peer_steps);

        /* Give the semaphore back so the next subevent can proceed */
        k_sem_give(&sem_local_steps);

		return;
	}

	// LOG_INF("ranging_data_get_complete  %d", ranging_counter);
	LOG_DBG("Ranging data get completed for ranging counter %d", ranging_counter);

	/* This struct is static to avoid putting it on the stack (it's very large) */
	static cs_de_report_t cs_de_report;

	cs_de_populate_report(&latest_local_steps, &latest_peer_steps, BT_CONN_LE_CS_ROLE_INITIATOR, &cs_de_report);

	net_buf_simple_reset(&latest_local_steps);
	net_buf_simple_reset(&latest_peer_steps);
	k_sem_give(&sem_local_steps);
	
	// HIER WORDEN DE BEREKENINGEN GEDAAN IN "cs_de.c" EN IN ONS cs_de_report GEZET
	cs_de_quality_t quality = cs_de_calc(&cs_de_report);

	if (quality == CS_DE_QUALITY_OK) {
		for (uint8_t ap = 0; ap < cs_de_report.n_ap; ap++) {
			if (cs_de_report.tone_quality[ap] == CS_DE_TONE_QUALITY_OK) {
				store_distance_estimates(&cs_de_report, device);	// Afstandsrapport wordt hier bewaard
			}
		}
	}
}

// MANAGE binnenkomende step data van de LOCAL (initiator) in result
static void subevent_result_cb(struct bt_conn *conn, struct bt_conn_le_cs_subevent_result *result)
{
	uint16_t raw_counter = bt_ras_rreq_get_ranging_counter(result->header.procedure_counter);
	uint16_t logical_counter = ranging_counter_offset + raw_counter;

	// LOG_INF("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
	// LOG_INF("dropped ranging counter = %d", dropped_ranging_counter);
	// LOG_INF("Procedure counter = %d", result->header.procedure_counter);
	// LOG_INF("raw counter = %d | logical counter = %d", raw_counter, logical_counter);


	// als we deze precedure (result) ooit al hebben "gedropped" door een fout of overlap dan doen we niets met deze data
	if (dropped_ranging_counter == logical_counter) {
		// LOG_INF("We are dropping the result");
		return;
	}

	// Niets doen, want de laatste ranging round is vaak niet completed.
	if (raw_counter == RANGING_ROUNDS-1){
		return;
	}

	// Indien de local ranging counter nog niet in SYNC is met de huidige ranging counter van de data, updaten we hem
	if (most_recent_local_ranging_counter != logical_counter) {

		int sem_state = k_sem_take(&sem_local_steps, K_NO_WAIT);

		if (sem_state < 0) {
			dropped_ranging_counter = logical_counter;
			// LOG_INF("Dropped subevent results due to unfinished ranging data request.");
			// int debug = k_sem_take(&sem_debug_error, K_FOREVER);
			return;
		}
		// Syncen tussen PEER & LOCAL (initiator & reflector STEP DATA) is nodig om niet geen data door elkaar te hebben
		most_recent_local_ranging_counter = logical_counter;
		// LOG_INF("most_recent_local_ranging_counter = %d", most_recent_local_ranging_counter);
	}

	// Subevent data (bevat mode steps) heeft status "ABORTED" (niet succesvol afgerond) en dus genegeerd
	if (result->header.subevent_done_status == BT_CONN_LE_CS_SUBEVENT_ABORTED) {
		/* The steps from this subevent will not be used. */
	} 
	// Subevent STEP DATA succesvol afgerond => data in Latest Local Steps (Memory van onze initiator) geplaatst
	else if (result->step_data_buf) {
		if (result->step_data_buf->len <= net_buf_simple_tailroom(&latest_local_steps)) {
			uint16_t len = result->step_data_buf->len;
			uint8_t *step_data = net_buf_simple_pull_mem(result->step_data_buf, len);

			net_buf_simple_add_mem(&latest_local_steps, step_data, len);
		
		} else {
			LOG_ERR("Not enough memory to store step data. (%d > %d)",
				latest_local_steps.len + result->step_data_buf->len,
				latest_local_steps.size);
			net_buf_simple_reset(&latest_local_steps);
			// We drop this procedure
			dropped_ranging_counter = result->header.procedure_counter;
			return;
		}
	}

	dropped_ranging_counter = PROCEDURE_COUNTER_NONE;

	// Procedure COMPLETED :)
	if (result->header.procedure_done_status == BT_CONN_LE_CS_PROCEDURE_COMPLETE) {
		most_recent_local_ranging_counter = bt_ras_rreq_get_ranging_counter(result->header.procedure_counter) + ranging_counter_offset;
		// LOG_INF("procedure complete");
	} 
	// Procedure ABORTED   :(
	else if (result->header.procedure_done_status == BT_CONN_LE_CS_PROCEDURE_ABORTED) {
		LOG_WRN("Procedure %u aborted", result->header.procedure_counter);
		net_buf_simple_reset(&latest_local_steps);
		k_sem_give(&sem_local_steps);
	}

	if (raw_counter == (RANGING_ROUNDS - 1)) {
    	ranging_counter_offset += RANGING_ROUNDS;
	}

}

// Haalt de step data van de PEER (reflector) op zodra de LOCAL procedure compleet is
static void ranging_data_ready_cb(struct bt_conn *conn, uint16_t ranging_counter)
{
	LOG_DBG("Ranging data ready %i", ranging_counter);
	ranging_counter = ranging_counter + ranging_counter_offset;

	// LOG_INF("ranging_counter = %d", ranging_counter);
	// Enkel alle data ophalen wanneer de lokale huidige ranging counter overeenkomt met de verwachte meetronde
	if (ranging_counter == most_recent_local_ranging_counter) {
		int err = bt_ras_rreq_cp_get_ranging_data(conn, &latest_peer_steps, ranging_counter, ranging_data_get_complete_cb);
		if (err) {
			LOG_ERR("Get ranging data failed (err %d)", err);
			net_buf_simple_reset(&latest_local_steps);
			net_buf_simple_reset(&latest_peer_steps);
			k_sem_give(&sem_local_steps);
		}
	}
}

static void ranging_data_overwritten_cb(struct bt_conn *conn, uint16_t ranging_counter)
{
	// LOG_INF("Ranging data overwritten %i", ranging_counter);
}

static void mtu_exchange_cb(struct bt_conn *conn, uint8_t err, struct bt_gatt_exchange_params *params)
{
	if (err) {
		LOG_ERR("MTU exchange failed (err %d)", err);
		return;
	}

	// LOG_INF("MTU exchange success (%u)", bt_gatt_get_mtu(conn));
	k_sem_give(&sem_mtu_exchange_done);
}

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

	// LOG_INF("The discovery procedure succeeded");

	struct bt_conn *conn = bt_gatt_dm_conn_get(dm);

	bt_gatt_dm_data_print(dm);

	err = bt_ras_rreq_alloc_and_assign_handles(dm, conn);
	if (err) {
		LOG_ERR("RAS RREQ alloc init failed (err %d)", err);
	}

	err = bt_gatt_dm_data_release(dm);
	if (err) {
		LOG_ERR("Could not release the discovery data (err %d)", err);
	}

	k_sem_give(&sem_discovery_done);
}

static void discovery_service_not_found_cb(struct bt_conn *conn, void *context)
{
	// LOG_INF("The service could not be found during the discovery, disconnecting");
	bt_conn_disconnect(connection, BT_HCI_ERR_REMOTE_USER_TERM_CONN);
}

static void discovery_error_found_cb(struct bt_conn *conn, int err, void *context)
{
	// LOG_INF("The discovery procedure failed (err %d)", err);
	bt_conn_disconnect(connection, BT_HCI_ERR_REMOTE_USER_TERM_CONN);
}

static struct bt_gatt_dm_cb discovery_cb = {
	.completed = discovery_completed_cb,
	.service_not_found = discovery_service_not_found_cb,
	.error_found = discovery_error_found_cb,
};

static void security_changed(struct bt_conn *conn, bt_security_t level, enum bt_security_err err)
{
	char addr[BT_ADDR_LE_STR_LEN];

	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

	if (err) {
		LOG_ERR("Security failed: %s level %u err %d %s", addr, level, err,
			bt_security_err_to_str(err));
		return;
	}

	// LOG_INF("Security changed: %s level %u", addr, level);
	k_sem_give(&sem_security);
}

static bool le_param_req(struct bt_conn *conn, struct bt_le_conn_param *param)
{
	/* Ignore peer parameter preferences. */
	return false;
}

static void connected_cb(struct bt_conn *conn, uint8_t err)
{
	char addr[BT_ADDR_LE_STR_LEN];
	(void)bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
	// LOG_INF("Connected to %s", addr);

    if (err) {
        LOG_ERR("Connection failed (err 0x%02x)", err);
        bt_conn_unref(conn);
		k_sem_give(&sem_connected);	// NEEDED to get to the error instead of "W: conn failed to establish. RF noise?"
        return;
    }

    // When not in the scanning process we give a semaphore for being connected
    if (!scanning_for_initial_devices) {
        for (int i = 0; i < MAX_DEVICES; i++) {
            if (connections[i] == NULL) {
                connections[i] = bt_conn_ref(conn);
                break;
            }
        }
        dk_set_led_on(CON_STATUS_LED);
        k_sem_give(&sem_connected);
    }

    // When still in the scanning process we disconnect
    if (scanning_for_initial_devices) {
        k_sleep(K_MSEC(200));
        bt_conn_disconnect(conn, BT_HCI_ERR_REMOTE_USER_TERM_CONN);
		connection_in_progress = false;
    }
}

static void disconnected_cb(struct bt_conn *conn, uint8_t reason)
{
	char addr[BT_ADDR_LE_STR_LEN];
	(void)bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
	// LOG_INF("Disconnected from %s, reason: reason 0x%02X", addr, reason);

	if (reason == 0x3E){
		// LOG_INF("nRF Noise detected");
		k_sem_give(&sem_nrf_noise);
	}
	
    bt_conn_unref(conn);
    connection = NULL;
    dk_set_led_off(CON_STATUS_LED);

    // Handle disconnection during initial device scan
    if (scanning_for_initial_devices) {
		// LOG_INF("disconnected during scanning phase");
        k_sem_give(&sem_disconnected);
        connection_in_progress = false;

		// When more devices need to get detected we continue scanning
        if (num_found < MAX_DEVICES) {
            bt_scan_start(BT_SCAN_TYPE_SCAN_ACTIVE);

		// When all devices has been found we stop scanning
        } else {
            scanning_for_initial_devices = false;
        }
    } 
	
	// Handle proper disconnection after ranging is done
	if(ranging_done){
		// LOG_INF("ranging is done");
		k_sem_give(&sem_ranging_done);
	}
}

static void remote_capabilities_cb(struct bt_conn *conn, uint8_t status, struct bt_conn_le_cs_capabilities *params)
{
	ARG_UNUSED(conn);
	ARG_UNUSED(params);

	if (status == BT_HCI_ERR_SUCCESS) {
		// LOG_INF("CS capability exchange completed.");
		k_sem_give(&sem_remote_capabilities_obtained);
	} else {
		LOG_WRN("CS capability exchange failed. (HCI status 0x%02x)", status);
	}
}

static void config_create_cb(struct bt_conn *conn, uint8_t status, struct bt_conn_le_cs_config *config)
{
	ARG_UNUSED(conn);

	if (status == BT_HCI_ERR_SUCCESS) {
		// LOG_INF("CS config creation complete. ID: %d", config->id);
		k_sem_give(&sem_config_created);
	} else {
		LOG_WRN("CS config creation failed. (HCI status 0x%02x)", status);
	}
}

static void security_enable_cb(struct bt_conn *conn, uint8_t status)
{
	ARG_UNUSED(conn);

	if (status == BT_HCI_ERR_SUCCESS) {
		// LOG_INF("CS security enabled.");
		k_sem_give(&sem_cs_security_enabled);
	} else {
		LOG_WRN("CS security enable failed. (HCI status 0x%02x)", status);
	}
}

static void procedure_enable_cb(struct bt_conn *conn, uint8_t status, struct bt_conn_le_cs_procedure_enable_complete *params)
{
	ARG_UNUSED(conn);

	if (status == BT_HCI_ERR_SUCCESS) {
		if (params->state == 1) {
			// LOG_INF("CS procedures enabled:\n"
			// 	" - config ID: %u\n"
			// 	" - antenna configuration index: %u\n"
			// 	" - TX power: %d dbm\n"
			// 	" - subevent length: %u us\n"
			// 	" - subevents per event: %u\n"
			// 	" - subevent interval: %u\n"
			// 	" - event interval: %u\n"
			// 	" - procedure interval: %u\n"
			// 	" - procedure count: %u\n"
			// 	" - maximum procedure length: %u",
			// 	params->config_id, params->tone_antenna_config_selection,
			// 	params->selected_tx_power, params->subevent_len,
			// 	params->subevents_per_event, params->subevent_interval,
			// 	params->event_interval, params->procedure_interval,
			// 	params->procedure_count, params->max_procedure_len);
		} else {
			// LOG_INF("CS procedures disabled.");
		}
	} else {
		LOG_WRN("CS procedures enable failed. (HCI status 0x%02x)", status);
	}
}



static bool ad_parse_cb(struct bt_data *data, void *user_data)
{
    char *name = user_data;

    if (data->type == BT_DATA_NAME_COMPLETE || data->type == BT_DATA_NAME_SHORTENED) {
        size_t len = MIN(data->data_len, NAME_LEN - 1);
        memcpy(name, data->data, len);
        name[len] = '\0';
        return false;
    }

    return true;
}

static void scan_filter_match(struct bt_scan_device_info *device_info, struct bt_scan_filter_match *filter_match, bool connectable)
{
	// Not scanning for devices OR already connected to a device
    if (!scanning_for_initial_devices || connection_in_progress) {
        return;
    }

    // Device has already been detected
    for (int i = 0; i < num_found; i++) {
        if (!bt_addr_le_cmp(&addresses[i], device_info->recv_info->addr)) {
            return;
        }
    }

	// End scanning when ALL devices detected
    if (num_found >= MAX_DEVICES) {
        return;
    }

    // Retreiving data from the scanned device
    struct net_buf_simple ad_copy = *device_info->adv_data;
    snprintf(reflector_names[num_found], NAME_LEN, "(onbekend)");
    bt_data_parse(&ad_copy, ad_parse_cb, reflector_names[num_found]);

    bt_addr_le_copy(&addresses[num_found], device_info->recv_info->addr);


    bt_scan_stop();
    k_msleep(50);

	// Setup a quick connection with the device
    struct bt_conn *conn = NULL;
    const struct bt_conn_le_create_param *create_param = BT_CONN_LE_CREATE_PARAM(BT_CONN_LE_OPT_NONE, 0x0060, 0x0030);
    const struct bt_le_conn_param *conn_param = BT_LE_CONN_PARAM_DEFAULT;
	connection_in_progress = true;

    int err = bt_conn_le_create(&addresses[num_found], create_param, conn_param, &conn);
    if (err) {
        connection_in_progress = false;
        return;
    }

	// LOG the information about the device
    char addr_str[BT_ADDR_LE_STR_LEN];
    bt_addr_le_to_str(&addresses[num_found], addr_str, sizeof(addr_str));

    LOG_INF("Device %d/%d verbonden: \"%s\" (%s)", num_found + 1, MAX_DEVICES, reflector_names[num_found], addr_str);

    num_found++;
}

static void scan_connecting_error(struct bt_scan_device_info *device_info)
{
	int err;

	// LOG_INF("Connecting failed, restarting scanning");

	err = bt_scan_start(BT_SCAN_TYPE_SCAN_PASSIVE);
	if (err) {
		LOG_ERR("Failed to restart scanning (err %i)", err);
		return;
	}
}

static void scan_connecting(struct bt_scan_device_info *device_info, struct bt_conn *conn)
{
	// LOG_INF("Connecting");
}

BT_SCAN_CB_INIT(scan_cb, scan_filter_match, NULL, scan_connecting_error, scan_connecting);

static int scan_init(void)
{
	int err;

	struct bt_scan_init_param param = {
		.scan_param = NULL, 
		.conn_param = BT_LE_CONN_PARAM_DEFAULT,
		.connect_if_match = 0};

	bt_scan_init(&param);
	bt_scan_cb_register(&scan_cb);

	err = bt_scan_filter_add(BT_SCAN_FILTER_TYPE_UUID, BT_UUID_RANGING_SERVICE);
	if (err) {
		return err;
	}

	err = bt_scan_filter_enable(BT_SCAN_UUID_FILTER, false);	//false: 1 match required | true: ALL matches required
	if (err) {													
		return err;
	}

	return 0;
}

BT_CONN_CB_DEFINE(conn_cb) = {
	.connected = connected_cb,
	.disconnected = disconnected_cb,
	.le_param_req = le_param_req,
	.security_changed = security_changed,
	.le_cs_read_remote_capabilities_complete = remote_capabilities_cb,
	.le_cs_config_complete = config_create_cb,
	.le_cs_security_enable_complete = security_enable_cb,
	.le_cs_procedure_enable_complete = procedure_enable_cb,
	.le_cs_subevent_data_available = subevent_result_cb,
};

int main(void)
{
	int err;

	dk_leds_init();

	err = bt_enable(NULL);
	if (err) {
		LOG_ERR("Bluetooth init failed (err %d)", err);
		return 0;
	}

	err = scan_init();
	if (err) {
		LOG_ERR("Scan init failed (err %d)", err);
		return 0;
	}

	err = bt_scan_start(BT_SCAN_TYPE_SCAN_PASSIVE);
	if (err) {
		LOG_ERR("Scanning failed to start (err %i)", err);
		return 0;
	}

	for (int i = 0; i < MAX_DEVICES; i++) {
		k_sem_take(&sem_disconnected, K_FOREVER);
	}	

	LOG_INF("All devices detected => stop scanning");


	////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

	/* Reconnect one by one with previously detected devices */
	while (true){
		for (int i = 0; i < MAX_DEVICES; i++) {

			const bt_addr_le_t *peer_addr = &addresses[i];
			char addr_str[BT_ADDR_LE_STR_LEN];
			bt_addr_le_to_str(peer_addr, addr_str, sizeof(addr_str));

			const char *reflector_name = reflector_names[i];
			int device_nr = i+1;
			device = i;
			struct device_estimate_buffers *buf = &device_estimates[i];


			const struct bt_conn_le_create_param *create_param = BT_CONN_LE_CREATE_PARAM(BT_CONN_LE_OPT_NONE, 0x0060, 0x0030);
			const struct bt_le_conn_param *conn_param = BT_LE_CONN_PARAM_DEFAULT;
			if (connections[i]) {
				bt_conn_disconnect(connections[i], BT_HCI_ERR_REMOTE_USER_TERM_CONN);
				bt_conn_unref(connections[i]);
				connections[i] = NULL;
			}

			// LOG_INF("Connecting with device %s (%s)", reflector_name, addr_str);
			err = bt_conn_le_create(peer_addr, create_param, conn_param, &connections[i]);
			if (err) {
				LOG_ERR("Kon niet verbinden met %s (%s), foutcode: %d", reflector_name, addr_str, err);
				continue;
			}

			/* Check for connection Time out: connection failed */
			if (k_sem_take(&sem_connected, K_SECONDS(4)) != 0) {
				if (connections[i]) {
					bt_conn_disconnect(connections[i], BT_HCI_ERR_REMOTE_USER_TERM_CONN);
					bt_conn_unref(connections[i]);
					connections[i] = NULL;
				}
				continue; // Move on to the next device
			}

			/* Check for nrf Noise: connection succeeds, but disconnects right after  */
			if (k_sem_take(&sem_nrf_noise, K_MSEC(500)) == 0){
				// LOG_INF("Skipping device %d, going to the next device", device_nr);
				if (connections[i]) {
					bt_conn_disconnect(connections[i], BT_HCI_ERR_REMOTE_USER_TERM_CONN);
					connections[i] = NULL;
					k_sleep(K_SECONDS(1));
				}
				continue; // Move on to the next device
			}

			/* To establish a stable connection */
			k_sleep(K_MSEC(200));

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

			// MTU Exchange
			static struct bt_gatt_exchange_params mtu_exchange_params = {.func = mtu_exchange_cb};
			bt_gatt_exchange_mtu(connections[i], &mtu_exchange_params);
			k_sem_take(&sem_mtu_exchange_done, K_FOREVER);

			// GATT Discovery
			err = bt_gatt_dm_start(connections[i], BT_UUID_RANGING_SERVICE, &discovery_cb, NULL);
			if (err) {
				LOG_ERR("Discovery failed for %s", reflector_name);
				return 0;
			}
			k_sem_take(&sem_discovery_done, K_FOREVER);

			// // Default settings
			const struct bt_le_cs_set_default_settings_param default_settings = {
				.enable_initiator_role = true,
				.enable_reflector_role = false,
				.cs_sync_antenna_selection = BT_LE_CS_ANTENNA_SELECTION_OPT_REPETITIVE,
				.max_tx_power = BT_HCI_OP_LE_CS_MAX_MAX_TX_POWER,
			};
			err = bt_le_cs_set_default_settings(connections[i], &default_settings);
			if (err) {
				LOG_ERR("Failed to configure default CS settings for %s", reflector_name);
				return 0;
			}

			// Subscriptions
			err  = bt_ras_rreq_rd_overwritten_subscribe(connections[i], ranging_data_overwritten_cb);
			err |= bt_ras_rreq_rd_ready_subscribe(connections[i], ranging_data_ready_cb);
			err |= bt_ras_rreq_on_demand_rd_subscribe(connections[i]);
			err |= bt_ras_rreq_cp_subscribe(connections[i]);
			if (err) {
				LOG_ERR("Subscriptions failed for %s", reflector_name);
				continue;
			}

			// Capabilities
			err = bt_le_cs_read_remote_supported_capabilities(connections[i]);
			if (err) {
				LOG_ERR("Failed to exchange CS capabilities for %s", reflector_name);
				return 0;
			}
			k_sem_take(&sem_remote_capabilities_obtained, K_FOREVER);

			// CS Config
			struct bt_le_cs_create_config_params config_params = {
				.id = CS_CONFIG_ID,
				.main_mode_type = BT_CONN_LE_CS_MAIN_MODE_2,
				.sub_mode_type = BT_CONN_LE_CS_SUB_MODE_1,
				.min_main_mode_steps = 2,
				.max_main_mode_steps = 5,
				.main_mode_repetition = 0,
				.mode_0_steps = NUM_MODE_0_STEPS,
				.role = BT_CONN_LE_CS_ROLE_INITIATOR,
				.rtt_type = BT_CONN_LE_CS_RTT_TYPE_AA_ONLY,
				.cs_sync_phy = BT_CONN_LE_CS_SYNC_1M_PHY,
				.channel_map_repetition = 3,
				.channel_selection_type = BT_CONN_LE_CS_CHSEL_TYPE_3B,
				.ch3c_shape = BT_CONN_LE_CS_CH3C_SHAPE_HAT,
				.ch3c_jump = 2,
			};
			// Dus alle channels MOETEN aanstaan voor IFFT!
			bt_le_cs_set_valid_chmap_bits(config_params.channel_map);
			err = bt_le_cs_create_config(connections[i], &config_params,
							BT_LE_CS_CREATE_CONFIG_CONTEXT_LOCAL_AND_REMOTE);
			if (err) {
				LOG_ERR("Failed to create CS config (err %d)", err);
				return 0;
			}
			k_sem_take(&sem_config_created, K_FOREVER);

			// Enable CS security
			err = bt_le_cs_security_enable(connections[i]);
			if (err) {
				LOG_ERR("Failed to start CS Security for %s", reflector_name);
				return 0;
			}
			k_sem_take(&sem_cs_security_enabled, K_FOREVER);

			// Procedure params
			const struct bt_le_cs_set_procedure_parameters_param procedure_params = {
				.config_id = CS_CONFIG_ID,
				.max_procedure_len = 1000,
				.min_procedure_interval = 10,
				.max_procedure_interval = 10,
				.max_procedure_count = 0,
				.min_subevent_len = 60000,
				.max_subevent_len = 60000,
				.tone_antenna_config_selection = BT_LE_CS_TONE_ANTENNA_CONFIGURATION_A1_B1,
				.phy = BT_LE_CS_PROCEDURE_PHY_1M,
				.tx_power_delta = 0x80,
				.preferred_peer_antenna = BT_LE_CS_PROCEDURE_PREFERRED_PEER_ANTENNA_1,
				.snr_control_initiator = BT_LE_CS_SNR_CONTROL_NOT_USED,
				.snr_control_reflector = BT_LE_CS_SNR_CONTROL_NOT_USED,
			};
			err = bt_le_cs_set_procedure_parameters(connections[i], &procedure_params);
			if (err) {
				LOG_ERR("Failed to set procedure parameters for %s", reflector_name);
				return 0;
			}

			// Enable procedure
			struct bt_le_cs_procedure_enable_param params = {
				.config_id = CS_CONFIG_ID,
				.enable = 1,
			};
			err = bt_le_cs_procedure_enable(connections[i], &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++) {
				// onderaan in ranging_data_get_complete_cb:
				k_sleep(K_MSEC(500));

				if (buf->buffer_num_valid != 0) {
					for (uint8_t ap = 0; ap < MAX_AP; ap++) {
						cs_de_dist_estimates_t distance_on_ap = get_distance(ap, device);

						// LOG_INF("ifft: %f, phase_slope: %f, rtt: %f", 
						// 	(double)distance_on_ap.ifft,
						// 	(double)distance_on_ap.phase_slope,
						// 	(double)distance_on_ap.rtt);

						if(distance_on_ap.ifft != 0.0){
							LOG_INF("IFFT: %f", (double)distance_on_ap.ifft);
						}
					}
				}
			}

			/* for stable disconnection */
			ranging_done = true;
			bt_conn_disconnect(connections[i], BT_HCI_ERR_REMOTE_USER_TERM_CONN);
			connections[i] = NULL; 

			k_sem_take(&sem_ranging_done, K_FOREVER);
			ranging_done = false;
		}
	}
}
I: Device 1: Reflector355
I: Device 2: Reflector651
I: IFFT: 6.733620
I: IFFT: 7.172769
I: Device 3: Reflector604
I: IFFT: 7.171619
I: IFFT: 7.171619
I: Device 4: Reflector921
I: IFFT: 5.332509
I: IFFT: 5.332509
I: Device 5: Reflector494
I: IFFT: 3.805959
I: IFFT: 3.659576
I: Device 6: Reflector771
I: Device 1: Reflector355
I: IFFT: 3.758502
I: IFFT: 3.684786
I: Device 2: Reflector651
I: IFFT: 7.172769
I: IFFT: 7.319151
I: IFFT: 7.319151
I: Device 3: Reflector604
I: IFFT: 7.171619
I: IFFT: 7.196850
I: IFFT: 7.221393
I: Device 4: Reflector921
I: IFFT: 5.332509
I: IFFT: 5.332509
I: IFFT: 5.415856
I: Device 5: Reflector494
I: IFFT: 3.659576
I: IFFT: 3.610781
I: IFFT: 3.610781
I: Device 6: Reflector771
W: Local step data appears malformed.
I: Device 1: Reflector355
I: IFFT: 3.684786
I: IFFT: 3.684786
I: IFFT: 3.684786
I: Device 2: Reflector651
I: IFFT: 7.319151
I: IFFT: 7.319151
I: IFFT: 7.319151
I: Device 3: Reflector604
I: IFFT: 7.221393
I: IFFT: 7.221393
I: IFFT: 7.234760
I: Device 4: Reflector921
I: IFFT: 5.415856
I: IFFT: 5.074401
I: IFFT: 5.074401
I: Device 5: Reflector494
I: IFFT: 3.610781
I: IFFT: 3.610781
I: IFFT: 3.586384
I: Device 6: Reflector771
I: Device 1: Reflector355
I: IFFT: 3.684786
I: IFFT: 3.684786
I: IFFT: 3.684786
I: Device 2: Reflector651
I: IFFT: 7.319151
I: IFFT: 7.458704
I: IFFT: 7.489347
I: Device 3: Reflector604
I: IFFT: 7.234760
I: IFFT: 7.258756
I: IFFT: 6.390523
I: Device 4: Reflector921
I: IFFT: 5.074401
I: IFFT: 5.074401
I: IFFT: 5.074401
I: Device 5: Reflector494
I: IFFT: 3.586384
I: IFFT: 3.630299
I: IFFT: 3.659576
I: Device 6: Reflector771
I: IFFT: 10.832344
I: IFFT: 11.564260
I: Device 1: Reflector355
I: IFFT: 3.684786
I: IFFT: 3.616436
I: IFFT: 2.921262
I: Device 2: Reflector651
I: IFFT: 7.489347
I: IFFT: 7.558569
I: IFFT: 7.566190
W: conn 0x20002660 failed to establish. RF noise?
I: Device 4: Reflector921
I: IFFT: 5.074401
I: IFFT: 5.871063
I: IFFT: 6.337609
I: Device 5: Reflector494
I: IFFT: 3.659576
I: IFFT: 3.659576
I: IFFT: 3.659576
W: conn 0x200028b8 failed to establish. RF noise?
I: Device 1: Reflector355
I: IFFT: 2.921262
I: IFFT: 3.065313
I: IFFT: 3.065313
I: Device 2: Reflector651
I: IFFT: 7.566190
I: IFFT: 7.618394
I: IFFT: 7.618394
I: Device 3: Reflector604
I: IFFT: 6.390523
I: IFFT: 6.390523
I: IFFT: 6.564244
W: conn 0x200027f0 failed to establish. RF noise?
I: Device 5: Reflector494
I: IFFT: 3.659576
I: IFFT: 3.659576
I: IFFT: 3.555016
I: Device 6: Reflector771
I: IFFT: 11.564260
I: IFFT: 11.564260
I: IFFT: 11.222699
I: Device 1: Reflector355
I: IFFT: 3.065313
I: IFFT: 3.065313
I: IFFT: 3.065313
I: Device 2: Reflector651
I: IFFT: 7.618394
I: IFFT: 7.618394
I: IFFT: 7.744791
I: Device 3: Reflector604
I: IFFT: 6.564244
I: IFFT: 6.622012
I: IFFT: 6.622012
I: Device 4: Reflector921
I: IFFT: 6.337609
I: IFFT: 6.709648
I: IFFT: 6.709648
I: Device 5: Reflector494
I: IFFT: 3.555016
I: IFFT: 3.659576
I: IFFT: 3.675841
I: Device 6: Reflector771
I: IFFT: 11.222699
I: IFFT: 11.222699
I: IFFT: 11.222699
I: Device 1: Reflector355
I: IFFT: 3.065313
I: IFFT: 3.065313
I: IFFT: 2.899145
I: Device 2: Reflector651
I: IFFT: 7.744791
I: IFFT: 7.744791
I: IFFT: 7.766937
I: Device 3: Reflector604
I: IFFT: 6.622012
I: IFFT: 6.622012
I: IFFT: 6.634413
I: Device 4: Reflector921
I: IFFT: 6.709648
I: IFFT: 6.709648
I: IFFT: 6.709648
I: Device 5: Reflector494
I: IFFT: 3.675841
I: IFFT: 3.659575
I: IFFT: 3.586384
I: Device 6: Reflector771
I: IFFT: 11.222699
I: IFFT: 11.564260
I: IFFT: 11.564260
I: Device 1: Reflector355
I: IFFT: 2.899145
I: IFFT: 2.899145
I: IFFT: 2.899145
I: Device 2: Reflector651
I: IFFT: 7.766937
I: IFFT: 7.766937
I: IFFT: 7.766937
I: Device 3: Reflector604
I: IFFT: 6.634413
I: IFFT: 6.337369
I: IFFT: 6.226780
I: Device 4: Reflector921
I: IFFT: 6.709648
I: IFFT: 6.709648
I: IFFT: 6.575851
W: conn 0x20002728 failed to establish. RF noise?
I: Device 6: Reflector771
I: IFFT: 11.564260
I: IFFT: 11.593536
I: IFFT: 11.593536
I: Device 1: Reflector355
I: IFFT: 2.899145
I: IFFT: 2.899145
I: IFFT: 2.721164
I: Device 2: Reflector651
I: IFFT: 7.766937
I: IFFT: 7.766937
I: IFFT: 7.766937
W: conn 0x20002660 failed to establish. RF noise?
W: conn 0x20002728 failed to establish. RF noise?
I: Device 5: Reflector494
I: IFFT: 3.586384
I: IFFT: 3.659575
I: IFFT: 3.659575
I: Device 6: Reflector771
I: IFFT: 11.593536
I: IFFT: 11.613053
I: IFFT: 10.874168
I: Device 1: Reflector355
I: IFFT: 2.721164
I: IFFT: 2.275441
I: IFFT: 2.349810
I: Device 2: Reflector651
I: IFFT: 7.766937
I: IFFT: 7.766937
I: IFFT: 7.766937
I: Device 3: Reflector604
I: IFFT: 6.226780
I: IFFT: 6.196269
I: IFFT: 6.196269
I: Device 4: Reflector921
I: IFFT: 6.575851
I: IFFT: 6.813890
I: IFFT: 6.813890
I: Device 5: Reflector494
I: IFFT: 3.659575
I: IFFT: 3.680487
I: IFFT: 3.596840
I: Device 6: Reflector771
I: IFFT: 10.874168
I: IFFT: 10.874168
I: IFFT: 10.874168
I: Device 1: Reflector355
I: IFFT: 2.349810
I: IFFT: 2.879648
I: IFFT: 2.689180
I: Device 2: Reflector651
I: IFFT: 7.766937
I: IFFT: 7.786615
I: IFFT: 7.786615
I: Device 3: Reflector604
I: IFFT: 6.196269
I: IFFT: 6.165209
I: IFFT: 6.157895
I: Device 4: Reflector921
I: IFFT: 6.813890
I: IFFT: 6.813890
I: IFFT: 6.635229
I: Device 5: Reflector494
I: IFFT: 3.596840
I: IFFT: 3.555016
I: IFFT: 3.659576
W: conn 0x20002660 failed to establish. RF noise?
I: Device 1: Reflector355
I: IFFT: 2.689180
I: IFFT: 2.795430
I: IFFT: 2.434484
I: Device 2: Reflector651
I: IFFT: 7.786615
I: IFFT: 7.748694
I: IFFT: 7.748694
I: Device 3: Reflector604
I: IFFT: 6.157895
I: IFFT: 6.194490
I: IFFT: 5.896568
I: Device 4: Reflector921
I: IFFT: 6.635229
I: IFFT: 5.897981
I: IFFT: 5.897981
I: Device 5: Reflector494
I: IFFT: 3.659576
I: IFFT: 3.561987
I: IFFT: 3.513193
I: Device 6: Reflector771
I: IFFT: 10.874168
I: IFFT: 10.874168
I: IFFT: 11.041463
I: Device 1: Reflector355
I: IFFT: 2.434484
I: IFFT: 2.434484
I: IFFT: 2.434484
I: Device 2: Reflector651
I: IFFT: 7.748694
I: IFFT: 7.665047
I: IFFT: 7.665047
I: Device 3: Reflector604
I: IFFT: 5.896568
I: IFFT: 5.875535
I: IFFT: 5.420121
I: Device 4: Reflector921
I: IFFT: 5.897981
I: IFFT: 5.947828
I: IFFT: 6.021019
I: Device 5: Reflector494
I: IFFT: 3.513193
I: IFFT: 3.513193
I: IFFT: 3.513193
I: Device 6: Reflector771
I: IFFT: 11.041463
I: IFFT: 11.041463
I: IFFT: 10.957815
I: Device 1: Reflector355
I: IFFT: 2.434484
I: IFFT: 2.434484
I: IFFT: 2.434484
I: Device 2: Reflector651
I: IFFT: 7.665047
I: IFFT: 7.665047
I: IFFT: 7.611918
I: Device 3: Reflector604
I: IFFT: 5.420121
I: IFFT: 5.452650
I: IFFT: 5.571953
I: Device 4: Reflector921
I: IFFT: 6.021019
I: IFFT: 6.021019
I: IFFT: 5.866686
I: Device 5: Reflector494
I: IFFT: 3.513193
I: IFFT: 3.513193
I: IFFT: 3.513193
I: Device 6: Reflector771
I: IFFT: 10.957815
I: IFFT: 10.623225
I: IFFT: 10.623225
I: Device 1: Reflector355
I: IFFT: 2.434484
I: IFFT: 2.335849
I: IFFT: 2.335849
I: Device 2: Reflector651
I: IFFT: 7.611918
I: IFFT: 7.611917
I: IFFT: 7.611917
I: Device 3: Reflector604
I: IFFT: 5.571953
I: IFFT: 5.438381
I: IFFT: 5.438381
W: conn 0x200027f0 failed to establish. RF noise?

I would like to know how to make my code better and i would like to really get some good help for better (faster and more consistently) working ranging. If this would mean i would try out multi-connections PARALLEL instead of sequentially, i would like to try it. But i really would like to get some good coding help with this if it is possible...

Kind regards,

Nick De Leenheer

Parents
  • Hi Nick

    Unfortunately we don't have much in terms of assistance to provide here, except some general guidelines. Since you're running into more issues with 6 than 3 reflectors I think you need to allocate more time for each ranging process to complete before moving to the next ranging. 

    You have already gotten reples regarding the parallel connection ranging in this ticket for example, and this is not something we have tested.

    Providing a finished code example for multi-connection ranging will be outside the scope of what support we can provide right now I'm afraid.

    Best regards,

    Simon

Reply
  • Hi Nick

    Unfortunately we don't have much in terms of assistance to provide here, except some general guidelines. Since you're running into more issues with 6 than 3 reflectors I think you need to allocate more time for each ranging process to complete before moving to the next ranging. 

    You have already gotten reples regarding the parallel connection ranging in this ticket for example, and this is not something we have tested.

    Providing a finished code example for multi-connection ranging will be outside the scope of what support we can provide right now I'm afraid.

    Best regards,

    Simon

Children
No Data
Related