applications: nrf5340_audio: Reimplement function for calculating err_us

- Fix bug where remainder operations were done on negative numbers - Create helper function to fix both uses in one place Signed-off-by: Alexander Svensen <[email protected]>
nrfconnect · Nov 27, 2023 · 855339c · 855339c
1 parent 8f28ba5
commit 855339c
Showing 1 changed file with 38 additions and 10 deletions.
diff --git a/applications/nrf5340_audio/src/audio/audio_datapath.c b/applications/nrf5340_audio/src/audio/audio_datapath.c
@@ -160,6 +160,42 @@ static bool tone_active;
 static uint16_t test_tone_buf[CONFIG_AUDIO_SAMPLE_RATE_HZ / 100];
 static size_t test_tone_size;
 
+/**
+ * @brief	Calculate error between sdu_ref and frame_start_ts.
+ *
+ * @note	Used to adjust audio clock to account for drift.
+ *
+ * @param	sdu_ref_us	Timestamp for sdu.
+ * @param	frame_start_ts	Timestamp for I2S.
+ *
+ * @return	err_us	Error in microseconds.
+ */
+static int32_t err_us_calculate(uint32_t sdu_ref_us, uint32_t frame_start_ts)
+{
+	bool err_neg = false;
+	int64_t total_err = (sdu_ref_us - frame_start_ts);
+
+	/* Store sign for later use, since remainder operation is undefined for negatives */
+	if (total_err < 0) {
+		err_neg = true;
+		total_err *= -1;
+	}
+
+	/* Check diff below 1000 us, diff above 1000 us is fixed later on */
+	int32_t err_us = total_err % BLK_PERIOD_US;
+
+	if (err_us > (BLK_PERIOD_US / 2)) {
+		err_us = err_us - BLK_PERIOD_US;
+	}
+
+	/* Restore the sign */
+	if (err_neg) {
+		err_us *= -1;
+	}
+
+	return err_us;
+}
+
 static void hfclkaudio_set(uint16_t freq_value)
 {
 	uint16_t freq_val = freq_value;
@@ -233,11 +269,7 @@ static void audio_datapath_drift_compensation(uint32_t frame_start_ts)
 			return;
 		}
 
-		int32_t err_us = (ctrl_blk.previous_sdu_ref_us - frame_start_ts) % BLK_PERIOD_US;
-
-		if (err_us > (BLK_PERIOD_US / 2)) {
-			err_us = err_us - BLK_PERIOD_US;
-		}
+		int32_t err_us = err_us_calculate(ctrl_blk.previous_sdu_ref_us, frame_start_ts);
 
 		int32_t freq_adj = APLL_FREQ_ADJ(err_us);
 
@@ -255,11 +287,7 @@ static void audio_datapath_drift_compensation(uint32_t frame_start_ts)
 			return;
 		}
 
-		int32_t err_us = (ctrl_blk.previous_sdu_ref_us - frame_start_ts) % BLK_PERIOD_US;
-
-		if (err_us > (BLK_PERIOD_US / 2)) {
-			err_us = err_us - BLK_PERIOD_US;
-		}
+		int32_t err_us = err_us_calculate(ctrl_blk.previous_sdu_ref_us, frame_start_ts);
 
 		/* Use asymptotic correction with small errors */
 		err_us /= 2;