From 855339c58c853eed9677f2f0002a5963a2964ea4 Mon Sep 17 00:00:00 2001 From: Alexander Svensen Date: Mon, 27 Nov 2023 13:06:59 +0100 Subject: [PATCH] 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 --- .../nrf5340_audio/src/audio/audio_datapath.c | 48 +++++++++++++++---- 1 file changed, 38 insertions(+), 10 deletions(-) diff --git a/applications/nrf5340_audio/src/audio/audio_datapath.c b/applications/nrf5340_audio/src/audio/audio_datapath.c index 0eb0086ca84..6ab35cae28f 100644 --- 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;