bluetooth: cs_de: Set tone quality ok for unused channels

This fixes an issue where the quality on the unused
channels counted towards the number of channels with bad quality.

Signed-off-by: Erik Sandgren <[email protected]>

Author: eriksandgren

include/bluetooth/cs_de.h

@@ -95,12 +95,12 @@ typedef struct {
  * @brief Partially populate the report.
  * This populates the report but does not set the distance estimates and the quality.
  * @param[in] local_steps Buffer to the local step data to parse.
- * @param[in] peer_steps   Buffer to the peer ranging data to parse.
- * @param[in] role Role of the local controller.
+ * @param[in] peer_steps Buffer to the peer ranging data to parse.
+ * @param[in] config CS config of the local controller.
  * @param[out] p_report Report populated with the raw data from the last ranging.
  */
 void cs_de_populate_report(struct net_buf_simple *local_steps, struct net_buf_simple *peer_steps,
-			   enum bt_conn_le_cs_role role, cs_de_report_t *p_report);
+			   struct bt_conn_le_cs_config *config, cs_de_report_t *p_report);
 
 /* Takes partially populated report and calculates distance estimates and quality. */
 cs_de_quality_t cs_de_calc(cs_de_report_t *p_report);

samples/bluetooth/channel_sounding_ras_initiator/src/main.c

@@ -60,6 +60,7 @@ static uint32_t ras_feature_bits;
 static uint8_t buffer_index;
 static uint8_t buffer_num_valid;
 static cs_de_dist_estimates_t distance_estimate_buffer[MAX_AP][DE_SLIDING_WINDOW_SIZE];
+static struct bt_conn_le_cs_config cs_config;
 
 static void store_distance_estimates(cs_de_report_t *p_report)
 {
@@ -180,8 +181,7 @@ static void ranging_data_cb(struct bt_conn *conn, uint16_t ranging_counter, int
 	/* 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);
+	cs_de_populate_report(&latest_local_steps, &latest_peer_steps, &cs_config, &cs_de_report);
 
 	net_buf_simple_reset(&latest_local_steps);
 
@@ -401,6 +401,7 @@ static void config_create_cb(struct bt_conn *conn,
 	ARG_UNUSED(conn);
 
 	if (status == BT_HCI_ERR_SUCCESS) {
+		cs_config = *config;
 		LOG_INF("CS config creation complete. ID: %d", config->id);
 		k_sem_give(&sem_config_created);
 	} else {

subsys/bluetooth/cs_de/cs_de.c

@@ -279,12 +279,12 @@ static void calculate_dist_rtt(cs_de_report_t *p_report)
 	}
 }
 
-static bool m_is_tone_quality_bad(cs_de_tone_quality_t *p_tone_qi)
+static bool m_is_tone_quality_bad(cs_de_tone_quality_t *p_tone_qi, uint8_t channel_map[10])
 {
 	uint8_t bad_tones_count = 0;
-
 	for (uint8_t i = 0; i < NUM_CHANNELS; ++i) {
-		if (p_tone_qi[i] == CS_DE_TONE_QUALITY_BAD) {
+		if (BT_LE_CS_CHANNEL_BIT_GET(channel_map, i + CHANNEL_INDEX_OFFSET) &&
+		    p_tone_qi[i] == CS_DE_TONE_QUALITY_BAD) {
 			bad_tones_count += 1;
 		}
 	}
@@ -422,25 +422,26 @@ static bool process_step_data(struct bt_le_cs_subevent_step *local_step,
 }
 
 void cs_de_populate_report(struct net_buf_simple *local_steps, struct net_buf_simple *peer_steps,
-			   enum bt_conn_le_cs_role role, cs_de_report_t *p_report)
+			   struct bt_conn_le_cs_config *config, cs_de_report_t *p_report)
 {
 	memset(p_report, 0x0, sizeof(*p_report));
 	memset(m_n_iqs, 0, sizeof(m_n_iqs));
 	memset(m_tone_quality_indicators, CS_DE_TONE_QUALITY_BAD,
 	       sizeof(m_tone_quality_indicators));
 
-	p_report->role = role;
+	p_report->role = config->role;
 
-	bt_ras_rreq_rd_subevent_data_parse(peer_steps, local_steps, role, process_ranging_header,
-					   NULL, process_step_data, p_report);
+	bt_ras_rreq_rd_subevent_data_parse(peer_steps, local_steps, config->role,
+					   process_ranging_header, NULL, process_step_data,
+					   p_report);
 
 	for (uint8_t ap = 0; ap < p_report->n_ap; ap++) {
 		p_report->distance_estimates[ap].ifft = NAN;
 		p_report->distance_estimates[ap].phase_slope = NAN;
 		p_report->distance_estimates[ap].rtt = NAN;
 		p_report->distance_estimates[ap].best = NAN;
 
-		if (m_is_tone_quality_bad(&m_tone_quality_indicators[ap][0])) {
+		if (m_is_tone_quality_bad(&m_tone_quality_indicators[ap][0], config->channel_map)) {
 			p_report->tone_quality[ap] = CS_DE_TONE_QUALITY_BAD;
 		} else {
 			p_report->tone_quality[ap] = CS_DE_TONE_QUALITY_OK;