Distance measurement. NRF Mesh dm_cli and dm_srv communication issue:

[Matthew_Max]

I was trying to establish a mesh network to perform distance measurement and localization. The source code is available at https://github.com/nrfconnect/sdk-nrf/blob/main/subsys/bluetooth/mesh/vnd/dm_srv.c#L221 and https://github.com/nrfconnect/sdk-nrf/blob/main/subsys/bluetooth/mesh/vnd/dm_cli.c. 

The system consists of one central node, three beacons, and one target node.

Both the client and the servers were added to the mesh network through the NRF Mesh App.

The workflow of the system is introduced here: https://developer.nordicsemi.com/nRF_Connect_SDK/doc/latest/nrf/libraries/bluetooth_services/mesh/vnd/dm_srv.html

The issue occurred when the client asked the servers to start distance measurement and then waited for a response. During this process, the client would sometimes face a timeout error or get stuck (meaning there were no errors returned, but the system was suspended).

More specifically, the requests could be successfully sent to the beacons, signaling them to begin the measurement process. The beacons were also able to accurately obtain the distance data. However, the beacons failed to transmit this data back to the central node, which is an issue we have been debugging for a long time.

Currently, we are testing with one target, one central node, and two beacons to see whether the central node can receive the correct measured distances transmitted by the two beacons.

Here is the output of one of the beacons:

Here is the output from the central node. We can clearly observe that in the first iteration, the central node attempts to retrieve results from the two beacons individually. It successfully prints out the two addresses (0x6 and 0x8). However, a timeout error prevents the information from being updated. In the second iteration, the system gets stuck at 0x6 and does not continue to inquire about the other beacon. At the same time, the beacons could continue measuring the distance and displaying the distances even though the output was stuck at the client (which was confusing for me).

Here is the code we used:

main.c for client:

/*
 * Copyright (c) 2012-2014 Wind River Systems, Inc.
 *
 * SPDX-License-Identifier: Apache-2.
 */
#include <zephyr/kernel.h>
#include <zephyr/settings/settings.h>
#include <zephyr/sys/byteorder.h>
#include <zephyr/drivers/hwinfo.h>

#include <bluetooth/mesh/dk_prov.h>
#include <zephyr/bluetooth/bluetooth.h>
#include <zephyr/bluetooth/mesh.h>
#include <zephyr/logging/log.h>

#include "../services/hp_cli.h"
#include "../services/trilateration.h"

/* Hardcoded mesh device addresses. Addresses are found on the nRF Mesh app. */
#define TARGET_ADDR  0x0005// target asset to locate 
#define BEACON1_ADDR 0x0006		
#define BEACON2_ADDR 0x0008
#define BEACON3_ADDR 0x0005
#define MAX_REF_ENTRIES 10
#define LOG_MODULE_NAME dm_mesh_client

LOG_MODULE_REGISTER(LOG_MODULE_NAME, LOG_LEVEL_DBG);

/* Mesh provisioning callbacks */
static int output_number(bt_mesh_output_action_t action, uint32_t number)
{
	printk("OOB Number: %u\n", number);
	return 0;
}

static void prov_complete(uint16_t net_idx, uint16_t addr)
{

}

static void prov_reset(void)
{
	bt_mesh_prov_enable(BT_MESH_PROV_ADV | BT_MESH_PROV_GATT);
}

static uint8_t dev_uuid[16];

static const struct bt_mesh_prov prov = {
	.uuid = dev_uuid,
	.output_size = 4,
	.output_actions = BT_MESH_DISPLAY_NUMBER,
	.output_number = output_number,
	.complete = prov_complete,
	.reset = prov_reset,
};

const struct bt_mesh_prov *prov_init(void)
{
	int err;
	if (IS_ENABLED(CONFIG_HWINFO)) {
		err = hwinfo_get_device_id(dev_uuid, sizeof(dev_uuid));
	} else {
		dev_uuid[0] = 0xdd;
		dev_uuid[1] = 0xdd;
	}

	return &prov;
}

/* Health server definitions */
static void attention_on(struct bt_mesh_model *mod)
{
	LOG_INF("Attention on.\n");
}

static void attention_off(struct bt_mesh_model *mod)
{
	LOG_INF("Attention off.\n");
}

static const struct bt_mesh_health_srv_cb health_cb = {
	.attn_on = attention_on,
	.attn_off = attention_off,
};

static struct bt_mesh_health_srv health_srv = {
	.cb = &health_cb,
};

BT_MESH_HEALTH_PUB_DEFINE(health_pub, 0);

/* Location calculation setup */
float dist1; // distance from beacon 1 to target
float dist2; // distance from beacon 2 to target
float dist3; // distance from beacon 3 to target

/**
 * Beacon coordinates:
 * localPool[0] -> beacon 1 x-coordinate
 * localPool[1] -> beacon 1 y-coordinate
 * localPool[2] -> beacon 1 z-coordinate
 * localPool[3] -> beacon 2 x-coordinate
 * localPool[4] -> beacon 2 y-coordinate
 * localPool[5] -> beacon 1 z-coordinate
 * localPool[6] -> Beacon 3 x-coordinate
 * localPool[7] -> Beacon 3 y-coordinate
 * localPool[8] -> Beacon 3 z-coordinate
 */
float * localPool; 

/* DM result handlers */
/**
 * Prints out a single high-precision MCPD reading in metres with the 
 * coordinates of the beacon received from.
 *
 * @param[in] results The raw MCPD result in cm derived from the API.
 */
static void print_result(const struct bt_mesh_dm_cli_results_hp *results)
{
	printk("High-precision: %.2f\n", results->res->res.mcpd.best / 100.0);
	printk("Received from beacon %u at (%.2f, %.2f, %.2f).\n", 
		results->id, results->ref_location.x, results->ref_location.y, 
		results->ref_location.z);
}

/**
 * Updates the set of measurements used for trilateration after receiving a 
 * reading from a beacon. Updates MCPD distance from beacon to target and x, y, 
 * z coordinates of the beacon.
 * 
 * @param[in] results The raw MCPD result derived from the API.
 */
static void update_distance(const struct bt_mesh_dm_cli_results_hp *results)
{	
	if (results->res->res.mcpd.best > 0) {
		if (results->id == 1) {
			dist1 = (float) results->res->res.mcpd.best / 100.0;
			localPool[0] = (float) results->ref_location.x;
			localPool[1] = (float) results->ref_location.y;
			localPool[2] = (float) results->ref_location.z;
		}
		else if (results->id == 2) {
			dist2 = (float) results->res->res.mcpd.best / 100.0;
			localPool[3] = (float) results->ref_location.x;
			localPool[4] = (float) results->ref_location.y;
			localPool[5] = (float) results->ref_location.z;
		}
		else if (results->id == 3) {
			dist3 = (float) results->res->res.mcpd.best / 100.0;
			localPool[6] = (float) results->ref_location.x;
			localPool[7] = (float) results->ref_location.y;
			localPool[8] = (float) results->ref_location.z;
		}
	}
}

/* DM client response setup */
static struct bt_mesh_dm_res_entry meas;

static void handle_configured(struct bt_mesh_dm_cli_hp *cli, 
							struct bt_mesh_msg_ctx *ctx, 
							const struct bt_mesh_dm_cli_cfg_status_hp *status)
{
}

/**
 * Callback upon receiving MCPD reading. This function is called from the API.
 * Wrapper to update global list of beacon measurements and coordinates via 
 * update_distance.
 * 
 * @param[in] cli DM Mesh client object handled by the API
 * @param[in] ctx DM context, set by the API
 * @param[in] results The raw MCPD result derived from the API.
 */
static void handle_received(struct bt_mesh_dm_cli_hp *cli, 
			struct bt_mesh_msg_ctx *ctx, 
			const struct bt_mesh_dm_cli_results_hp *results)
{
	if (results->entry_cnt > 0) {
		if (results->res->err_occurred) 
			printk("Error while measuring distance.\n");
		else {
			print_result(results); // uncomment for verbose output

			update_distance(results);
		}
	}	
}

static struct bt_mesh_dm_cli_handlers_hp handlers = {
	.cfg_status_handler = handle_configured,
	.result_handler = handle_received
};

struct bt_mesh_dm_cli_hp dm_cli = 
	BT_MESH_MODEL_DM_CLI_INIT_HP(&meas, MAX_REF_ENTRIES, &handlers);

/* Mesh composition setup */
static struct bt_mesh_model models[] = {
	BT_MESH_MODEL_CFG_SRV,
	BT_MESH_MODEL_HEALTH_SRV(&health_srv, &health_pub),
};

static struct bt_mesh_model vnd_models[] = {
    BT_MESH_MODEL_DM_CLI_HP(&dm_cli),
};

static struct bt_mesh_elem elements[] = {
	BT_MESH_ELEM(0, models, vnd_models),
};

static const struct bt_mesh_comp comp = {
	.cid = BT_COMP_ID_LF,
	.elem = elements,
	.elem_count = ARRAY_SIZE(elements),
};

/* Beacon round robin setup */
struct bt_mesh_msg_ctx ctx1 = {
	.addr = BEACON1_ADDR,
};

struct bt_mesh_msg_ctx ctx2 = {
	.addr = BEACON2_ADDR,
};

struct bt_mesh_msg_ctx ctx3 = {
	.addr = BEACON3_ADDR,
};

/**
 * Callback after enabling Bluetooth. If successfully initialised,
 * initialises mesh.
 * 
 * @param[in] err Error flag, set to 0 if successful, else -1
 */
static void bt_ready(int err)
{
	if (err) {
		LOG_ERR("Failed to initialise bluetooth.\n");
		return;
	} else {
		LOG_INF("Bluetooth initialised.\n");
	}

	err = bt_mesh_init(prov_init(), &comp);
	if (err) {
		LOG_ERR("Failed to initialise mesh.\n");
	}

	if (IS_ENABLED(CONFIG_SETTINGS)) {
		settings_load();
	}

	/* This will be a no-op if settings_load() loaded provisioning info */
	bt_mesh_prov_enable(BT_MESH_PROV_ADV | BT_MESH_PROV_GATT);

	LOG_INF("Mesh initialised.\n");
}

/* Perform distance measurement using DM Mesh API */
static struct bt_mesh_dm_cfg dm_cfg = {
	.ttl = 10,
	.timeout = 50,
	.delay = 0
};

static struct bt_mesh_dm_cli_start_hp start_param = {
	.reuse_transaction = false,
	.mode = DM_RANGING_MODE_MCPD,
	.addr = TARGET_ADDR,
	.cfg = &dm_cfg
};

/**
 * Queries a single beacon for its coordinates and distance from the target.
 * 
 * @param[in] cli DM client global structure
 * @param[in] ctx DM context, containing the address of the beacon to query
 * @param[in] res Object to store results from DM
 * 
 * @return 1 for success, 0 for failure
 */
int get_dm(struct bt_mesh_dm_cli_hp *cli, 
			struct bt_mesh_msg_ctx *ctx, struct bt_mesh_dm_cli_results_hp *res)
{
	int err = bt_mesh_dm_cli_config_hp(cli, ctx, &dm_cfg, NULL);
	if (err) {
		LOG_DBG("Failed to configure client: %d\n", err);
		return 0;
	} else {
		LOG_WRN("/n you are here %p",ctx->addr);
		err = bt_mesh_dm_cli_measurement_start_hp(cli, ctx, &start_param, res);
		// LOG_WRN("/n best is %f",res->res->res.mcpd.best);
		// LOG_WRN("/n rtt is %f",res->res->res.mcpd.ifft);
		// LOG_WRN("/n phase is %f",res->res->res.mcpd.phase_slope);
		// LOG_WRN("/n rssi is %f",res->res->res.mcpd.rssi_openspace);
		// LOG_WRN("/n rtt is %f",res->res->res.rtt);
		if (err) {
			LOG_WRN("Failed to start measurement: %d\n", err);
		}

		k_sleep(K_MSEC(500));
	}

	return 1;
}

/**
 * Wrapper for trilateration function. Calls trilaterate() using coordinates
 * of 3 beacons and distance from each beacon to target. Prints out calculated
 * location coordinates.
 */
void get_trilateration(void)
{
	struct location current;
	int err = trilaterate(localPool[0], localPool[3], localPool[6], 
						localPool[1], localPool[4], localPool[7], 
						localPool[2], localPool[5], localPool[8], 
						dist1, dist2, dist3, &current);
	if (err) {
		printk("Target outside range of beacons.\n");
	}
	printk("Current location: (%.2f, %.2f, %.2f).\n", 
		current.x, current.y, current.z);
}

void main(void)
{
	int err = -1;

	err = bt_enable(bt_ready);
	if (err) {
		return;
	}

	struct bt_mesh_dm_cli_results_hp res;

	// allocate memory for beacon coordinates global
	localPool = (float *) k_malloc (MAX_REF_ENTRIES * sizeof(float));
	if (localPool == NULL) {
		LOG_DBG("DEBUG: Failed to malloc. Exiting.\n");
		exit(0);
	} 
	memset((void *)localPool, 0x00, MAX_REF_ENTRIES * sizeof(localPool[0]));

	int passed_config;
	for (;;) {
		passed_config = 0;

		passed_config = get_dm(&dm_cli, &ctx1, &res);
		print_result(&res);
		passed_config = get_dm(&dm_cli, &ctx2, &res);
		// passed_config = get_dm(&dm_cli, &ctx3, &res);

		// get_trilateration();

		/**
		 * if all 3 devices failed to configure, assume user is still setting up
		 * and wait for 5 seconds
		 */ 
		if (passed_config == 0) k_sleep(K_MSEC(5000)); 
	}
}

dm_cli.c:

/*
 * Copyright (c) 2022 Nordic Semiconductor ASA
 *
 * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause
 */

#include "hp_cli.h"
#include <../subsys/bluetooth/mesh/model_utils.h>
#include <dm.h>

static int handle_cfg_status(struct bt_mesh_model *model, struct bt_mesh_msg_ctx *ctx,
			     struct net_buf_simple *buf)
{
	struct bt_mesh_dm_cli_hp *cli = model->user_data;
	struct bt_mesh_dm_cli_cfg_status_hp status;
	struct bt_mesh_dm_cli_cfg_status_hp *rsp;
	uint8_t temp;

	status.status = net_buf_simple_pull_u8(buf);
	temp = net_buf_simple_pull_u8(buf);
	status.is_in_progress = temp >> 7;
	status.result_entry_cnt = temp & 0x7F;	status.def.delay = net_buf_simple_pull_u8(buf);
	status.def.ttl = net_buf_simple_pull_u8(buf);
	status.def.timeout = net_buf_simple_pull_u8(buf);

	if (bt_mesh_msg_ack_ctx_match(&cli->ack_ctx, BT_MESH_DM_CONFIG_STATUS_OP,
				      ctx->addr, (void **)&rsp)) {
		*rsp = status;
		bt_mesh_msg_ack_ctx_rx(&cli->ack_ctx);
	}

	if (cli->handlers && cli->handlers->cfg_status_handler) {
		cli->handlers->cfg_status_handler(cli, ctx, &status);
	}

	return 0;
}

static bool result_populate(struct bt_mesh_dm_res_entry *entry,
			    struct net_buf_simple *buf)
{
	uint8_t temp = net_buf_simple_pull_u8(buf);

	entry->mode = (temp >> 7) & 0x01;
	entry->quality = (temp >> 4) & 0x07;
	entry->err_occurred = (temp >> 3) & 0x01;
	entry->addr = net_buf_simple_pull_le16(buf);

	if (entry->err_occurred) {
		return true;
	}

	if (entry->mode == DM_RANGING_MODE_RTT && buf->len >= 2) {
		entry->res.rtt = net_buf_simple_pull_le16(buf);
	} else if (entry->mode == DM_RANGING_MODE_MCPD && buf->len >= 8) {
		entry->res.mcpd.best = net_buf_simple_pull_le16(buf);
		entry->res.mcpd.ifft = net_buf_simple_pull_le16(buf);
		entry->res.mcpd.phase_slope = net_buf_simple_pull_le16(buf);
		entry->res.mcpd.rssi_openspace = net_buf_simple_pull_le16(buf);
	} else {

		return false;
	}

	return true;
}

/**
 * @author Paramita Tejasvi (lines 83-87)
 */ 
static int handle_result_status(struct bt_mesh_model *model, struct bt_mesh_msg_ctx *ctx,
				struct net_buf_simple *buf)
{
	struct bt_mesh_dm_cli_hp *cli = model->user_data;
	struct bt_mesh_dm_cli_results_hp status = {
		.res = cli->res_arr,
		.entry_cnt = 0
	};
	struct bt_mesh_dm_cli_results_hp *rsp;

	status.status = net_buf_simple_pull_u8(buf);

	// Beacon ID and coordinates
	status.id = net_buf_simple_pull_u8(buf);
	status.ref_location.x = *((float *) net_buf_simple_pull_mem(buf, sizeof(float)));
	status.ref_location.y = *((float *) net_buf_simple_pull_mem(buf, sizeof(float)));
	status.ref_location.z = *((float *) net_buf_simple_pull_mem(buf, sizeof(float)));

	if (!status.status) {
		memset(cli->res_arr, 0, sizeof(struct bt_mesh_dm_res_entry) * cli->entry_cnt);

		for (int i = 0; i < cli->entry_cnt; i++) {
			if (buf->len < 3) {
				break;
			}

			if (!result_populate(&cli->res_arr[i], buf)) {
				break;
			}
			status.entry_cnt++;
		}
	}

	if (bt_mesh_msg_ack_ctx_match(&cli->ack_ctx, BT_MESH_DM_RESULT_STATUS_OP,
				      ctx->addr, (void **)&rsp)) {
		*rsp = status;
		bt_mesh_msg_ack_ctx_rx(&cli->ack_ctx);
	}

	if (cli->handlers && cli->handlers->result_handler) {
		cli->handlers->result_handler(cli, ctx, &status);
	}

	return 0;
}

static int handle_msg(struct bt_mesh_model *model, struct bt_mesh_msg_ctx *ctx,
		      struct net_buf_simple *buf)
{
	uint8_t opcode = net_buf_simple_pull_u8(buf);

	switch (opcode) {
	case BT_MESH_DM_CONFIG_STATUS_OP:
		return handle_cfg_status(model, ctx, buf);
	case BT_MESH_DM_RESULT_STATUS_OP:
		return handle_result_status(model, ctx, buf);
	}

	return -EOPNOTSUPP;
}

const struct bt_mesh_model_op _bt_mesh_dm_cli_op_hp[] = {
	{
		BT_MESH_DM_CLI_OP,
		BT_MESH_LEN_MIN(BT_MESH_DM_RESULT_STATUS_MSG_MIN_LEN),
		handle_msg,
	},
	BT_MESH_MODEL_OP_END,
};

static int bt_mesh_dm_cli_init(struct bt_mesh_model *model)
{
	struct bt_mesh_dm_cli_hp *cli = model->user_data;

	cli->model = model;
	cli->pub.msg = &cli->pub_buf;
	net_buf_simple_init_with_data(&cli->pub_buf, cli->pub_data, sizeof(cli->pub_data));
	bt_mesh_msg_ack_ctx_init(&cli->ack_ctx);

	return 0;
}

static void bt_mesh_dm_cli_reset(struct bt_mesh_model *model)
{
	struct bt_mesh_dm_cli_hp *cli = model->user_data;

	net_buf_simple_reset(cli->pub.msg);
	bt_mesh_msg_ack_ctx_reset(&cli->ack_ctx);
}

const struct bt_mesh_model_cb _bt_mesh_dm_cli_cb_hp = {
	.init = bt_mesh_dm_cli_init,
	.reset = bt_mesh_dm_cli_reset,
};

int bt_mesh_dm_cli_config_hp(struct bt_mesh_dm_cli_hp *cli,
			  struct bt_mesh_msg_ctx *ctx,
			  const struct bt_mesh_dm_cfg *set,
			  struct bt_mesh_dm_cli_cfg_status_hp *rsp)
{
	BT_MESH_MODEL_BUF_DEFINE(msg, BT_MESH_DM_SRV_OP,
				 BT_MESH_DM_CONFIG_MSG_LEN_MAX);
	bt_mesh_model_msg_init(&msg, BT_MESH_DM_SRV_OP);

	net_buf_simple_add_u8(&msg, BT_MESH_DM_CONFIG_OP);

	if (set) {
		if ((set->ttl == 1 || set->ttl > 127) || set->timeout < 10) {
			return -EBADMSG;
		}

		net_buf_simple_add_u8(&msg, set->ttl);
		net_buf_simple_add_u8(&msg, set->timeout);
		net_buf_simple_add_u8(&msg, set->delay);
	}

	struct bt_mesh_msg_rsp_ctx rsp_ctx = {
		.ack = &cli->ack_ctx,
		.op = BT_MESH_DM_CONFIG_STATUS_OP,
		.user_data = rsp,
		.timeout = model_ackd_timeout_get(cli->model, ctx),
	};

	return bt_mesh_msg_ackd_send(cli->model, ctx, &msg, rsp ? &rsp_ctx : NULL);
}

int bt_mesh_dm_cli_measurement_start_hp(struct bt_mesh_dm_cli_hp *cli,
				     struct bt_mesh_msg_ctx *ctx,
				     const struct bt_mesh_dm_cli_start_hp *start,
				     struct bt_mesh_dm_cli_results_hp *rsp)
{
	if (!BT_MESH_ADDR_IS_UNICAST(start->addr)) {
		printk("hhhhh"); 
		return -EBADMSG;
	}

	BT_MESH_MODEL_BUF_DEFINE(msg, BT_MESH_DM_SRV_OP, BT_MESH_DM_START_MSG_LEN_MAX);
	bt_mesh_model_msg_init(&msg, BT_MESH_DM_SRV_OP);

	net_buf_simple_add_u8(&msg, BT_MESH_DM_START_OP);
	net_buf_simple_add_le16(&msg, (start->mode << 15) | (start->addr & 0x7FFF));
	net_buf_simple_add_u8(&msg, start->reuse_transaction ? cli->tid : cli->tid++);
	if (start->cfg) {
		if ((start->cfg->ttl == 1 || start->cfg->ttl > 127) || start->cfg->timeout < 10) {
			printk("kkkkk");
			return -EBADMSG;
		}

		net_buf_simple_add_u8(&msg, start->cfg->ttl);
		net_buf_simple_add_u8(&msg, start->cfg->timeout);
		net_buf_simple_add_u8(&msg, start->cfg->delay);
	}

	struct bt_mesh_msg_rsp_ctx rsp_ctx = {
		.ack = &cli->ack_ctx,
		.op = BT_MESH_DM_RESULT_STATUS_OP,
		.user_data = rsp,
		.timeout = model_ackd_timeout_get(cli->model, ctx),
	};

	return bt_mesh_msg_ackd_send(cli->model, ctx, &msg, rsp ? &rsp_ctx : NULL);
}

int bt_mesh_dm_cli_results_get_hp(struct bt_mesh_dm_cli_hp *cli,
			       struct bt_mesh_msg_ctx *ctx,
			       uint8_t entry_cnt,
			       struct bt_mesh_dm_cli_results_hp *rsp)
{
	if (entry_cnt > cli->entry_cnt) {
		return -EBADMSG;
	}

	BT_MESH_MODEL_BUF_DEFINE(msg, BT_MESH_DM_SRV_OP, BT_MESH_DM_RESULT_GET_MSG_LEN);
	bt_mesh_model_msg_init(&msg, BT_MESH_DM_SRV_OP);

	net_buf_simple_add_u8(&msg, BT_MESH_DM_RESULT_GET_OP);
	net_buf_simple_add_u8(&msg, entry_cnt);

	struct bt_mesh_msg_rsp_ctx rsp_ctx = {
		.ack = &cli->ack_ctx,
		.op = BT_MESH_DM_RESULT_STATUS_OP,
		.user_data = rsp,
		.timeout = model_ackd_timeout_get(cli->model, ctx),
	};

	return bt_mesh_msg_ackd_send(cli->model, ctx, &msg, rsp ? &rsp_ctx : NULL);
}

main.c for server:

/**
 * Distance measurement (DM) Bluetooth Mesh model
 * implementation for nrf52840.
 * */
#include <zephyr/kernel.h>
#include <zephyr/sys/printk.h>

#include <zephyr/settings/settings.h>
#include <zephyr/sys/byteorder.h>
#include <zephyr/drivers/hwinfo.h>

#include <bluetooth/mesh/dk_prov.h>
#include <zephyr/bluetooth/bluetooth.h>
#include <zephyr/bluetooth/mesh.h>
#include "../services/hp_srv.h"

#define ID 1 // beacon ID number, change before running

/* Mesh provisioning callbacks */
static int output_number(bt_mesh_output_action_t action, uint32_t number)
{
	printk("OOB Number: %u\n", number);
	return 0;
}

static void prov_complete(uint16_t net_idx, uint16_t addr)
{

}

static void prov_reset(void)
{
	bt_mesh_prov_enable(BT_MESH_PROV_ADV | BT_MESH_PROV_GATT);
}

static uint8_t dev_uuid[16];

static const struct bt_mesh_prov prov = {
	.uuid = dev_uuid,
	.output_size = 4,
	.output_actions = BT_MESH_DISPLAY_NUMBER,
	.output_number = output_number,
	.complete = prov_complete,
	.reset = prov_reset,
};

const struct bt_mesh_prov *prov_init(void)
{
	int err;
	if (IS_ENABLED(CONFIG_HWINFO)) {
		err = hwinfo_get_device_id(dev_uuid, sizeof(dev_uuid));
	} else {
		dev_uuid[0] = 0xdd;
		dev_uuid[1] = 0xdd;
	}

	return &prov;
}

/* Health server definitions */
static void attention_on(struct bt_mesh_model *mod)
{
	printk("Attention on.\n");
}

static void attention_off(struct bt_mesh_model *mod)
{
	printk("Attention off.\n");
}

static const struct bt_mesh_health_srv_cb health_cb = {
	.attn_on = attention_on,
	.attn_off = attention_off,
};

static struct bt_mesh_health_srv health_srv = {
	.cb = &health_cb,
};

BT_MESH_HEALTH_PUB_DEFINE(health_pub, 0);

/* DM server model setup */
static struct bt_mesh_dm_res_entry meas;

static struct bt_mesh_dm_res_entry *measurements[] = {
	&meas,
};

/* Hardcoded beacon locations */
struct location beacon1 = {
	.x = 2.8,
	.y = 0.0,
	.z = 1.0
};

struct location beacon2 = {
	.x = 6.5,
	.y = 1.0,
	.z = 1.3
};

struct location beacon3 = {
	.x = 0.5,
	.y = 5.1,
	.z = 1.0
};

// change location struct referenced based on beacon ID
static struct bt_mesh_dm_srv_hp dm_srv_hp = 
	BT_MESH_MODEL_DM_SRV_INIT_HP(&meas, ARRAY_SIZE(measurements), &beacon1, ID);

/* Mesh composition setup */
static struct bt_mesh_model models[] = {
	BT_MESH_MODEL_CFG_SRV,
	BT_MESH_MODEL_HEALTH_SRV(&health_srv, &health_pub),
};

static struct bt_mesh_model vnd_models[] = {
    BT_MESH_MODEL_DM_SRV_HP(&dm_srv_hp),
};

static struct bt_mesh_elem elements[] = {
	BT_MESH_ELEM(0, models, vnd_models),
};

static const struct bt_mesh_comp comp = {
	.cid = BT_COMP_ID_LF,
	.elem = elements,
	.elem_count = ARRAY_SIZE(elements),
};

/**
 * Callback after enabling bluetooth. If successfully enabled,
 * initialises mesh.
 * 
 * @param[in] err Error flag, set to 0 if successful, else -1
 */
static void bt_ready(int err)
{
	if (err) {
		printk("Failed to initialise bluetooth.\n");
		return;
	} else {
		printk("Bluetooth initialised.\n");
	}

	err = bt_mesh_init(prov_init(), &comp);
	if (err) {
		printk("Failed to initialise mesh.\n");
	}

	if (IS_ENABLED(CONFIG_SETTINGS)) {
		settings_load();
	}

	/* This will be a no-op if settings_load() loaded provisioning info */
	bt_mesh_prov_enable(BT_MESH_PROV_ADV | BT_MESH_PROV_GATT);

	printk("Mesh initialised.\n");
}

void main(void)
{
	int err = -1;

	err = bt_enable(bt_ready);
}

dm_srv.c

/*
 * Copyright (c) 2022 Nordic Semiconductor ASA
 *
 * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause
 */

#include <../subsys/bluetooth/mesh/model_utils.h>
#include "hp_srv.h"
#include "mesh/net.h"
#include "mesh/access.h"

#define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_MESH_DEBUG_MODEL)
#define LOG_MODULE_NAME bt_mesh_hp_srv
#define LOG_LEVEL CONFIG_BT_MESH_MODEL_LOG_LEVEL
#include "zephyr/logging/log.h"

BUILD_ASSERT(CONFIG_BT_MESH_DM_SRV_DEFAULT_TTL != 1, "Illegal TTL value for DM server.");
BUILD_ASSERT(CONFIG_MPSL_TIMESLOT_SESSION_COUNT >= 2, "Illegal MPSL timelsot cnt for DM server.");

struct bt_mesh_dm_srv_hp *dm_srv_hp;

struct settings_data {
	struct bt_mesh_dm_cfg cfg;
} __packed;

#if CONFIG_BT_SETTINGS
static void store_timeout(struct k_work *work)
{
	struct k_work_delayable *dwork = k_work_delayable_from_work(work);
	struct bt_mesh_dm_srv_hp *srv = CONTAINER_OF(
		dwork, struct bt_mesh_dm_srv_hp, store_timer);

	struct settings_data data = {
		.cfg = srv->cfg,
	};

	(void)bt_mesh_model_data_store(srv->model, true, NULL, &data,
				       sizeof(data));
}
#endif

static void store(struct bt_mesh_dm_srv_hp *srv)
{
#if CONFIG_BT_SETTINGS
	k_work_schedule(
		&srv->store_timer,
		K_SECONDS(CONFIG_BT_MESH_MODEL_SRV_STORE_TIMEOUT));
#endif
}

static uint16_t result_convert_cm(float val)
{
	int32_t temp = val * 100;

	if (temp < 0) {
		return 0;
	}

	return temp > UINT16_MAX ? UINT16_MAX : (uint16_t)temp;
}

/** Gets the pointer reference to the latest valid entry relative to the offset */
static struct bt_mesh_dm_res_entry *entry_get(struct bt_mesh_dm_srv_hp *srv, uint8_t offset)
{
	return &srv->results.res[(srv->results.last_entry_idx + srv->results.entry_cnt - offset) %
				 srv->results.entry_cnt];
}

/* Update last entry index and return pointer to the new entry slot */
static struct bt_mesh_dm_res_entry *entry_create(struct bt_mesh_dm_srv_hp *srv)
{
	srv->results.last_entry_idx = (srv->results.last_entry_idx + 1) % srv->results.entry_cnt;

	return &srv->results.res[srv->results.last_entry_idx];
}

static void new_entry_store(struct dm_result *result)
{
	struct bt_mesh_dm_res_entry *entry = entry_create(dm_srv_hp);

	dm_srv_hp->results.available_entries =
		(dm_srv_hp->results.available_entries + 1) >= dm_srv_hp->results.entry_cnt ?
			dm_srv_hp->results.entry_cnt :
			(dm_srv_hp->results.available_entries + 1);

	memset(entry, 0, sizeof(struct bt_mesh_dm_res_entry));

	entry->mode = result->ranging_mode;
	entry->addr = dm_srv_hp->target_addr;
	entry->quality = result->quality;
	entry->err_occurred = !result->status;

	if (entry->err_occurred) {
		return;
	}

	if (entry->mode == DM_RANGING_MODE_RTT) {
		entry->res.rtt = result_convert_cm(result->dist_estimates.rtt.rtt);
	} else {
		entry->res.mcpd.best = result_convert_cm(result->dist_estimates.mcpd.high_precision);
		entry->res.mcpd.ifft = result_convert_cm(result->dist_estimates.mcpd.ifft);
		entry->res.mcpd.phase_slope =
			result_convert_cm(result->dist_estimates.mcpd.phase_slope);
		entry->res.mcpd.rssi_openspace =
			result_convert_cm(result->dist_estimates.mcpd.rssi_openspace);
	}
}

static void cfg_status_send(struct bt_mesh_model *model, struct bt_mesh_msg_ctx *rx_ctx,
			    uint8_t status)
{
	struct bt_mesh_dm_srv_hp *srv = model->user_data;

	BT_MESH_MODEL_BUF_DEFINE(msg, BT_MESH_DM_CLI_OP,
				 BT_MESH_DM_CONFIG_STATUS_MSG_LEN);
	bt_mesh_model_msg_init(&msg, BT_MESH_DM_CLI_OP);
	net_buf_simple_add_u8(&msg, BT_MESH_DM_CONFIG_STATUS_OP);
	net_buf_simple_add_u8(&msg, status);
	net_buf_simple_add_u8(&msg, (srv->is_busy << 7) |
					    (srv->results.available_entries & 0x7F));
	net_buf_simple_add_u8(&msg, srv->cfg.delay);
	net_buf_simple_add_u8(&msg, srv->cfg.ttl);
	net_buf_simple_add_u8(&msg, srv->cfg.timeout);

	(void)bt_mesh_model_send(model, rx_ctx, &msg, NULL, NULL);
}

static void result_pack(struct bt_mesh_dm_res_entry *entry, struct net_buf_simple *buf)
{
	uint8_t temp = (entry->mode << 7);

	temp |= ((entry->quality & 0x7) << 4);
	temp |= ((entry->err_occurred & 0x1) << 3);

	net_buf_simple_add_u8(buf, temp);
	net_buf_simple_add_le16(buf, entry->addr);

	if (entry->err_occurred) {
		return;
	}

	if (entry->mode == DM_RANGING_MODE_RTT) {
		net_buf_simple_add_le16(buf, entry->res.rtt);
	} else {
		net_buf_simple_add_le16(buf, entry->res.mcpd.best);
		net_buf_simple_add_le16(buf, entry->res.mcpd.ifft);
		net_buf_simple_add_le16(buf, entry->res.mcpd.phase_slope);
		net_buf_simple_add_le16(buf, entry->res.mcpd.rssi_openspace);
	}
}

static void result_status_populate(struct bt_mesh_model *model, struct net_buf_simple *buf,
				   uint8_t cnt)
{
	struct bt_mesh_dm_srv_hp *srv = model->user_data;

	for (int i = 0; i < MIN(cnt, srv->results.available_entries); i++) {
		struct bt_mesh_dm_res_entry *entry = entry_get(srv, i);

		result_pack(entry, buf);
	}
}

/**
 * @author Paramita Tejasvi (lines 175-178)
 */
static void result_status_send(struct bt_mesh_model *model, struct bt_mesh_msg_ctx *rx_ctx,
			       uint8_t cnt, int8_t status)
{
	BT_MESH_MODEL_BUF_DEFINE(msg, BT_MESH_DM_CLI_OP,
				 BT_MESH_DM_RESULT_STATUS_MSG_MAX_LEN);
	bt_mesh_model_msg_init(&msg, BT_MESH_DM_CLI_OP);
	net_buf_simple_add_u8(&msg, BT_MESH_DM_RESULT_STATUS_OP);
	net_buf_simple_add_u8(&msg, status);

	// Add beacon ID and coordinates
	struct bt_mesh_dm_srv_hp *srv = model->user_data;
	net_buf_simple_add_u8(&msg, srv->id);
	net_buf_simple_add_mem(&msg, srv->loc, sizeof(struct location));

	/* Omit last results field if status is not set to success*/
	if (status) {
		(void)bt_mesh_model_send(model, rx_ctx, &msg, NULL, NULL);
		return;
	}

	result_status_populate(model, &msg, cnt);

	(void)bt_mesh_model_send(model, rx_ctx, &msg, NULL, NULL);
}

static void sync_send(struct bt_mesh_model *model, struct bt_mesh_msg_ctx *ctx, uint8_t mode,
		      uint8_t timeout)
{
	BT_MESH_MODEL_BUF_DEFINE(msg, BT_MESH_DM_SRV_OP, BT_MESH_DM_SYNC_MSG_LEN);
	bt_mesh_model_msg_init(&msg, BT_MESH_DM_SRV_OP);
	net_buf_simple_add_u8(&msg, BT_MESH_DM_SYNC_OP);
	net_buf_simple_add_u8(&msg, mode);
	net_buf_simple_add_u8(&msg, timeout);
	(void)bt_mesh_model_send(model, ctx, &msg, NULL, NULL);
}

static int handle_cfg(struct bt_mesh_model *model, struct bt_mesh_msg_ctx *ctx,
		      struct net_buf_simple *buf)
{
	struct bt_mesh_dm_srv_hp *srv = model->user_data;

	if (!buf->len) {
		cfg_status_send(model, ctx, 0);
		return 0;
	}

	uint8_t ttl = net_buf_simple_pull_u8(buf);
	uint8_t timeout = net_buf_simple_pull_u8(buf);
	uint8_t delay = net_buf_simple_pull_u8(buf);

	if ((ttl == 1 || ttl > 127) || timeout < 10) {
		LOG_WARN("Invalid params for configuring DM");
		cfg_status_send(model, ctx, EBADMSG);
		return -EBADMSG;
	}

	srv->cfg.ttl = ttl;
	srv->cfg.timeout = timeout;
	srv->cfg.delay = 0;

	store(srv);

	cfg_status_send(model, ctx, 0);
	return 0;
}

static int handle_start(struct bt_mesh_model *model, 
			struct bt_mesh_msg_ctx *ctx,
			struct net_buf_simple *buf)
{
	int err = 0;
	struct bt_mesh_dm_srv_hp *srv = model->user_data;
	uint8_t timeout = srv->cfg.timeout;
	uint8_t delay = srv->cfg.delay;
	uint16_t temp = net_buf_simple_pull_le16(buf);
	uint8_t tid = net_buf_simple_pull_u8(buf);

	if (tid_check_and_update(&srv->prev_transaction, tid, ctx) != 0) {
		/* Ignore duplicate message */
		err = EAGAIN;
		goto rsp;
	}

	if (srv->is_busy) {
		LOG_ERR("Failed to start DM. Measurement already in progress");
		err = EBUSY;
		goto rsp;
	}

	srv->target_addr = temp & 0x7fff;

	enum dm_ranging_mode dm_mode = (enum dm_ranging_mode)(temp >> 15);
	struct bt_mesh_msg_ctx tx_ctx = {
		.net_idx = ctx->net_idx,
		.addr = srv->target_addr,
		.app_idx = ctx->app_idx,
		.send_ttl = srv->cfg.ttl,
	};

	/* If optional cfg params is available, overwrite default ones */
	if (buf->len == 3) {
		tx_ctx.send_ttl = net_buf_simple_pull_u8(buf);
		timeout = net_buf_simple_pull_u8(buf);
		delay = net_buf_simple_pull_u8(buf);
	}

	if ((tx_ctx.send_ttl == 1 || tx_ctx.send_ttl > 127) || (timeout < 10) || !tx_ctx.addr) {
		LOG_WARN("Invalid params for starting DM");
		err = EBADMSG;
		goto rsp;
	}

	struct dm_request req = {
		.role = DM_ROLE_REFLECTOR,
		.ranging_mode = dm_mode,
		.rng_seed = ((bt_mesh_primary_addr() + srv->model->elem_idx) << 16) +
			srv->target_addr,
		.start_delay_us = 0,
		.extra_window_time_us = CONFIG_BT_MESH_DM_SRV_REFLECTOR_RANGING_WINDOW_US
	};

	BT_INFO("Starting %s DM for target node 0x%04x",
		dm_mode == DM_RANGING_MODE_RTT ? "RTT" : "MCPD", dm_srv_hp->target_addr);

	srv->is_busy = true;
	k_sem_reset(&dm_srv_hp->dm_ready_sem);
	err = dm_request_add(&req);
	if (err) {
		LOG_ERR("DM add request failed: %d", err);
		srv->is_busy = false;
		goto rsp;
	}

	sync_send(model, &tx_ctx, dm_mode, timeout);
	srv->rsp_ctx = *ctx;
	srv->current_role = DM_ROLE_REFLECTOR;
	k_work_reschedule(&srv->timeout, K_MSEC(timeout * 100));
	return 0;

rsp:
	result_status_send(model, ctx, 1, err);
	return -err;
}

static int handle_result_get(struct bt_mesh_model *model, struct bt_mesh_msg_ctx *ctx,
			     struct net_buf_simple *buf)
{
	struct bt_mesh_dm_srv_hp *srv = model->user_data;
	uint8_t cnt = net_buf_simple_pull_u8(buf);

	if (!cnt) {
		result_status_send(model, ctx, cnt, EBADMSG);
		return 0;
	}

	if (!srv->results.available_entries) {
		result_status_send(model, ctx, cnt, ENOENT);
		return 0;
	}

	result_status_send(model, ctx, cnt, 0);

	return 0;
}

static int handle_sync(struct bt_mesh_model *model, struct bt_mesh_msg_ctx *ctx,
		       struct net_buf_simple *buf)
{
	int err;
	struct bt_mesh_dm_srv_hp *srv = model->user_data;

	if (srv->is_busy) {
		return -EBUSY;
	}

	srv->target_addr = ctx->addr;

	enum dm_ranging_mode mode = net_buf_simple_pull_u8(buf);
	uint8_t timeout = net_buf_simple_pull_u8(buf);
	struct dm_request req = {
		.role = DM_ROLE_INITIATOR,
		.ranging_mode = mode,
		.rng_seed = (ctx->addr << 16) + ctx->recv_dst,
		.start_delay_us = 0,
		.extra_window_time_us = CONFIG_BT_MESH_DM_SRV_INITIATOR_RANGING_WINDOW_US
	};

	srv->is_busy = true;
	k_sem_reset(&srv->dm_ready_sem);
	err = dm_request_add(&req);
	if (err) {
		LOG_ERR("DM add request failed: %d", err);
		srv->is_busy = false;
		return err;
	}

	srv->current_role = DM_ROLE_INITIATOR;
	k_work_reschedule(&srv->timeout, K_MSEC(timeout * 100));

	return 0;
}

static int handle_msg(struct bt_mesh_model *model, struct bt_mesh_msg_ctx *ctx,
		      struct net_buf_simple *buf)
{
	uint8_t opcode = net_buf_simple_pull_u8(buf);

	switch (opcode) {
	case BT_MESH_DM_CONFIG_OP:
		return handle_cfg(model, ctx, buf);
	case BT_MESH_DM_START_OP:
		return handle_start(model, ctx, buf);
	case BT_MESH_DM_RESULT_GET_OP:
		return handle_result_get(model, ctx, buf);
	case BT_MESH_DM_SYNC_OP:
		return handle_sync(model, ctx, buf);
	}

	return -EOPNOTSUPP;
}

const struct bt_mesh_model_op _bt_mesh_dm_srv_op_hp[] = {
	{
		BT_MESH_DM_SRV_OP,
		BT_MESH_LEN_MIN(BT_MESH_DM_CONFIG_MSG_LEN_MIN),
		handle_msg,
	},
	BT_MESH_MODEL_OP_END,
};

static void data_ready(struct dm_result *result)
{
	if (!dm_srv_hp->is_busy) {
		LOG_WARN("Unexpected DM result ready, discarding");
		return;
	}

	BT_INFO("New DM result ready for target node %04x", dm_srv_hp->target_addr);

	new_entry_store(result);
	if (k_work_delayable_is_pending(&dm_srv_hp->timeout)) {
		k_work_cancel_delayable(&dm_srv_hp->timeout);
		if (dm_srv_hp->current_role == DM_ROLE_REFLECTOR) {
			result_status_send(dm_srv_hp->model, &dm_srv_hp->rsp_ctx, 1, 0);
		}
	}

	dm_srv_hp->is_busy = false;
}

static struct dm_cb dm_cb = {
	.data_ready = data_ready,
};

static void timeout_work(struct k_work *work)
{
	LOG_ERR("DM attempt timed out");

	struct dm_result result = {
		.quality = DM_QUALITY_DO_NOT_USE,
		.status = false
	};

	new_entry_store(&result);
	if (dm_srv_hp->current_role == DM_ROLE_REFLECTOR) {
		result_status_send(dm_srv_hp->model, &dm_srv_hp->rsp_ctx, 1, ETIMEDOUT);
	}

	dm_srv_hp->is_busy = false;
}

static int bt_mesh_dm_srv_init(struct bt_mesh_model *model)
{
	int err;
	struct bt_mesh_dm_srv_hp *srv = model->user_data;

	srv->model = model;
	dm_srv_hp = srv;
	k_sem_init(&srv->dm_ready_sem, 0, 1);

	struct dm_init_param init_param = { .cb = &dm_cb};

	err = dm_init(&init_param);
	if (err) {
		LOG_ERR("Failed to initialize DM: %d", err);
		return err;
	}

	k_work_init_delayable(&srv->timeout, timeout_work);

#if CONFIG_BT_SETTINGS
	k_work_init_delayable(&srv->store_timer, store_timeout);
#endif
	return 0;
}

static void bt_mesh_dm_srv_reset(struct bt_mesh_model *model)
{
	struct bt_mesh_dm_srv_hp *srv = model->user_data;

	srv->cfg.ttl = CONFIG_BT_MESH_DM_SRV_DEFAULT_TTL;
	srv->cfg.timeout = CONFIG_BT_MESH_DM_SRV_DEFAULT_TIMEOUT,
	srv->cfg.delay = CONFIG_BT_MESH_DM_SRV_REFLECTOR_DELAY,
	net_buf_simple_reset(srv->pub.msg);

#if CONFIG_BT_SETTINGS
	(void)bt_mesh_model_data_store(srv->model, true, NULL, NULL, 0);
#endif
}

static int bt_mesh_dm_srv_settings_set(struct bt_mesh_model *model, const char *name,
						 size_t len_rd, settings_read_cb read_cb,
						 void *cb_arg)
{
	struct bt_mesh_dm_srv_hp *srv = model->user_data;
	struct settings_data data;
	ssize_t len;

	len = read_cb(cb_arg, &data, sizeof(data));
	if (len < sizeof(data)) {
		return -EINVAL;
	}

	srv->cfg = data.cfg;
	return 0;
}

const struct bt_mesh_model_cb _bt_mesh_dm_srv_cb_hp = {
	.init = bt_mesh_dm_srv_init,
	.reset = bt_mesh_dm_srv_reset,
	.settings_set = bt_mesh_dm_srv_settings_set
};

[tesc]

Hi,

Error return value -116 is ETIMEDOUT, and most likely it comes from bt_mesh_msg_ackd_send(), called from within bt_mesh_dm_cli_measurement_start_hp(). If that is the origin of the error code, it means the message was not acked (and the send has then timed out.)

I order to figure out where exactly the node hangs, please do a debug session.

Please note that we are entering the Easter holidays, with limited capacity this week and beginning of next week.

Regards,
Terje

[Matthew_Max]

Hi Terje, 

I also suspected that the timeout error might be originating from bt_mesh_msg_ackd_send(). As you suggested, I should do a debug session, but I'm not entirely sure how to proceed (apologies for the naive question, as I'm quite new to this). I also attempted to establish communication with one central node, one target, and one beacon, as I thought the issue could stem from multicasting a request from the client to two or more beacons, but the problem persisted.

And hope you have a wonderful Easter holiday! :)

Regards,

Matthew

[tesc]

Hi,

Please have a look at the Debugging an application section of the nRF Connect SDK documentation.

Regards,
Terje