diff --git a/examples/xua_lite_example/src/audio_config.xc b/examples/xua_lite_example/src/audio_config.xc index 92506acf..9e1dcb82 100755 --- a/examples/xua_lite_example/src/audio_config.xc +++ b/examples/xua_lite_example/src/audio_config.xc @@ -60,6 +60,7 @@ enum clock_nudge{ PLL_FASTER = 1 }; +//These steps provide just under +-0.1% frequency jumps #define PLL_LOW 0xC003FE18 // This is 3.069MHz #define PLL_NOM 0xC003FF18 // This is 3.072MHz #define PLL_HIGH 0xC0040018 // This is 3.075MHz @@ -273,6 +274,6 @@ void AudioHwConfigure(unsigned samFreq, client i2c_master_if i_i2c) } -//These are here just to silence compiler warnings +//These are here just to silence compiler warnings about unimplemented xua callbacks (not needed in xua lite) void AudioHwInit(){} void AudioHwConfig(unsigned samFreq, unsigned mClk, unsigned dsdMode, unsigned sampRes_DAC, unsigned sampRes_ADC){} \ No newline at end of file diff --git a/examples/xua_lite_example/src/audio_hub.xc b/examples/xua_lite_example/src/audio_hub.xc index 863d8505..1fb3c4f0 100644 --- a/examples/xua_lite_example/src/audio_hub.xc +++ b/examples/xua_lite_example/src/audio_hub.xc @@ -58,19 +58,23 @@ void AudioHub(server i2s_frame_callback_if i2s, restart = I2S_NO_RESTART; // Keep on looping timer tmr; int t0, t1; tmr :> t0; - //Transfer samples + //Transfer samples. Takes about 25 ticks for (int i = 0; i < NUM_USB_CHAN_OUT; i++) c_audio :> samples_out[i]; if (XUA_ADAPTIVE) c_audio :> clock_nudge; for (int i = 0; i < NUM_USB_CHAN_IN; i++) c_audio <: raw_mics[i]; - //Grab mics + //Grab mics. Takes about 200 ticks currently current = mic_array_get_next_time_domain_frame(c_ds_output, decimatorCount, buffer, mic_audio_frame, dc); + //50 ticks unsafe { for (int i = 0; i < XUA_NUM_PDM_MICS; i++) raw_mics[i] = current->data[i][0]; } + //Taking about 160 ticks when adjusting, 100 when not + tmr :> t0; pll_nudge(clock_nudge); - //tmr :> t1; if (t1-t0 > 500) debug_printf("*%d\n", t1 - t0); + tmr :> t1; + if (t1-t0 > 500) debug_printf("*%d\n", t1 - t0); //delay_microseconds(10); //Test backpressure tolerance break; } diff --git a/examples/xua_lite_example/src/xua_buffer_lite.xc b/examples/xua_lite_example/src/xua_buffer_lite.xc index bf0ceecc..b31f35ef 100644 --- a/examples/xua_lite_example/src/xua_buffer_lite.xc +++ b/examples/xua_lite_example/src/xua_buffer_lite.xc @@ -1,5 +1,5 @@ #include - +#include #include #include "xua_commands.h" @@ -84,12 +84,119 @@ static void do_feedback_calculation(unsigned &sof_count } } -void fill_level_process(int fill_level, int &clock_nudge){ - //Because we always check level after USB has produced a block, and total FIFO size is 2x max, half full is at 3/4 - const int half_full_out = ((MAX_OUT_SAMPLES_PER_SOF_PERIOD * 2) * 3) / 4; +#define CONTROL_LOOP 1 - const int trigger_high_upper = half_full_out + 2; - const int trigger_low_upper = half_full_out - 2; +typedef int32_t xua_lite_fixed_point_t; +#define XUA_LIGHT_FIXED_POINT_Q_BITS 10 //Including sign bit +#define XUA_LIGHT_FIXED_POINT_FRAC_BITS (32 - XUA_LIGHT_FIXED_POINT_Q_BITS) +#define XUA_LIGHT_FIXED_POINT_TOTAL_BITS (XUA_LIGHT_FIXED_POINT_Q_BITS + XUA_LIGHT_FIXED_POINT_FRAC_BITS) +#define XUA_LIGHT_FIXED_POINT_ONE (1 << XUA_LIGHT_FIXED_POINT_FRAC_BITS) +#define XUA_LIGHT_FIXED_POINT_MINUS_ONE (-XUA_LIGHT_FIXED_POINT_ONE) + +#define FIFO_LEVEL_EMA_COEFF 0.98 //Proportion of signal from y[-1] +#define FIFO_LEVEL_A_COEFF ((int32_t)(INT_MAX * FIFO_LEVEL_EMA_COEFF)) //Scale to signed 1.31 format +#define FIFO_LEVEL_B_COEFF (INT_MAX - FIFO_LEVEL_A_COEFF) + +#define RANDOMISATION_PERCENT 0 //How much noise to inject in percent of existing signal amplitude +#define RANDOMISATION_COEFF_A ((INT_MAX / 100) * (100 - RANDOMISATION_PERCENT)) + +#define PI_CONTROL_P_TERM 3.0 +#define PI_CONTROL_I_TERM 1.5 +#define PI_CONTROL_P_TERM_COEFF ((xua_lite_fixed_point_t)(XUA_LIGHT_FIXED_POINT_ONE * PI_CONTROL_P_TERM)) //scale to fixed point +#define PI_CONTROL_I_TERM_COEFF ((xua_lite_fixed_point_t)(XUA_LIGHT_FIXED_POINT_ONE * PI_CONTROL_I_TERM)) //scale to fixed point + + +xua_lite_fixed_point_t do_fifo_depth_lowpass_filter(xua_lite_fixed_point_t old, int fifo_depth){ + //we grow from 32b to 64b for intermediate + int64_t intermediate = ((int64_t)(fifo_depth << XUA_LIGHT_FIXED_POINT_FRAC_BITS) * (int64_t)FIFO_LEVEL_B_COEFF) + ((int64_t)old * (int64_t)FIFO_LEVEL_A_COEFF); + xua_lite_fixed_point_t new_fifo_depth = (xua_lite_fixed_point_t)( intermediate >> (64 - XUA_LIGHT_FIXED_POINT_TOTAL_BITS - 1)); //-1 because signed int + return new_fifo_depth; +} + +static int32_t get_random_number(void) +{ + static const unsigned random_poly = 0xEDB88320; + static unsigned random = 0x12345678; + crc32(random, -1, random_poly); + return (int32_t) random; +} + +static xua_lite_fixed_point_t add_noise(xua_lite_fixed_point_t input){ + //Note the input number cannot be bigger than 2 ^ (FIXED_POINT_Q_BITS - 1) * (1 + PERCENT) else we could oveflow + //Eg. if Q bits = 10 then biggest input value is 255.9999 + int32_t random = get_random_number(); + int32_t input_fraction = ((int64_t)input * (int64_t)RANDOMISATION_COEFF_A) >> (XUA_LIGHT_FIXED_POINT_TOTAL_BITS - 1); + int64_t output_64 = ((int64_t)input << (XUA_LIGHT_FIXED_POINT_TOTAL_BITS - 1)) + ((int64_t)input_fraction * (int64_t)random); + return (xua_lite_fixed_point_t)( output_64 >> (64 - XUA_LIGHT_FIXED_POINT_TOTAL_BITS - 1)); +} + +static void fill_level_process(int fill_level, int &clock_nudge){ + //Because we always check level after USB has produced a block, and total FIFO size is 2x max, half full is at 3/4 + const int half_full = ((MAX_OUT_SAMPLES_PER_SOF_PERIOD * 2) * 3) / 4; + +#if CONTROL_LOOP + static xua_lite_fixed_point_t fifo_level_filtered = 0; + static xua_lite_fixed_point_t fifo_level_filtered_old = 0; + static xua_lite_fixed_point_t fifo_level_integrated = 0; + int fill_level_wrt_half = fill_level - half_full; //Will be +ve for more than half full and negative for less than half full + + //Do PI control + //Low pass filter fill level and get error w.r.t. to set point which is depth = 0 (relative to half full) + fifo_level_filtered = do_fifo_depth_lowpass_filter(fifo_level_filtered_old , fill_level_wrt_half); + //debug_printf("o: %d n: %d\n", fifo_level_filtered_old, fifo_level_filtered); + //Calculate integral term which is the accumulated fill level error + xua_lite_fixed_point_t i_term_pre_clip = fifo_level_integrated + fifo_level_filtered; + + //clip the I term. Check to see if overflow (which will change sign) + if (fifo_level_integrated > 0){ //If it was positive before, ensure it still is else clip to positive + if (i_term_pre_clip < 0){ + fifo_level_integrated = INT_MAX; + } + else{ + fifo_level_integrated = i_term_pre_clip; + } + } + else{ //Value was negative so ensure it still is else clip negative + if (i_term_pre_clip > 0){ + fifo_level_integrated = INT_MIN; + } + else{ + fifo_level_integrated = i_term_pre_clip; + } + } + + //Do PID calculation + xua_lite_fixed_point_t p_term = (((int64_t) fifo_level_filtered * (int64_t)PI_CONTROL_P_TERM_COEFF)) >> (64 - XUA_LIGHT_FIXED_POINT_TOTAL_BITS + 2); + xua_lite_fixed_point_t i_term = (((int64_t) fifo_level_integrated * (int64_t)PI_CONTROL_I_TERM_COEFF)) >> (64 - XUA_LIGHT_FIXED_POINT_TOTAL_BITS + 2); + debug_printf("p: %d i: %d\n", p_term >> XUA_LIGHT_FIXED_POINT_Q_BITS, i_term >> XUA_LIGHT_FIXED_POINT_Q_BITS); + + //Sum and scale to +- 1.0 (important it does not exceed these values for following step) + //xua_lite_fixed_point_t controller_out = (p_term + i_term) >> (XUA_LIGHT_FIXED_POINT_Q_BITS - 1); + xua_lite_fixed_point_t controller_out = p_term + i_term; + + static xua_lite_fixed_point_t nudge_accumulator = 0; + nudge_accumulator += controller_out; + //nudge_accumulator = add_noise(nudge_accumulator); + + //debug_printf("na: %d -1: %d\n", nudge_accumulator, XUA_LIGHT_FIXED_POINT_MINUS_ONE); + + if (nudge_accumulator >= XUA_LIGHT_FIXED_POINT_ONE){ + clock_nudge = 1; + nudge_accumulator -= XUA_LIGHT_FIXED_POINT_ONE; + } + else if (nudge_accumulator <= XUA_LIGHT_FIXED_POINT_MINUS_ONE){ + nudge_accumulator -= XUA_LIGHT_FIXED_POINT_MINUS_ONE; + clock_nudge = -1; + } + else{ + clock_nudge = 0; + } + + fifo_level_filtered_old = fifo_level_filtered; + //debug_printf("filtered: %d raw: %d\n", fifo_level_filtered >> 22, fill_level_wrt_half); +#else + const int trigger_high_upper = half_full + 2; + const int trigger_low_upper = half_full - 2; if (fill_level >= trigger_high_upper){ clock_nudge = 1; @@ -101,6 +208,7 @@ void fill_level_process(int fill_level, int &clock_nudge){ } else clock_nudge = 0; //debug_printf("%d\n", clock_nudge); +#endif static unsigned counter; counter++; if (counter>SOF_FREQ_HZ){counter = 0; debug_printf("f: %d\n",fill_level);} } @@ -385,7 +493,7 @@ unsafe void XUA_Buffer_lite2(server ep0_control_if i_ep0_ctl, chanend c_aud_out, //tmr :> t1; debug_printf("s%d\n", t1 - t0); uint16_t port_counter; p_sda <: 1 @ port_counter; - p_sda @ port_counter + 10 <: 0; + p_sda @ port_counter + 100 <: 0; break; //Receive samples from host diff --git a/examples/xua_lite_example/todo.txt b/examples/xua_lite_example/todo.txt index d97eaa02..6c483267 100644 --- a/examples/xua_lite_example/todo.txt +++ b/examples/xua_lite_example/todo.txt @@ -1,7 +1,7 @@ - Bring ep0 serivice into xua_buffer select (make control mem) (DONE) - Tidy feeback endpoint (DONE) - Input path + FIFO (DONE) -- Function prototypes into includes +- Function prototypes into includes (DONE) - Single input/ouput format (DONE) - Get UAC1 / FS working (DONE) - Optimised EP buffer (either triple block or block FIFO) @@ -9,6 +9,7 @@ - Add timer to xua_buffer to prepare for exchange with audio (remove backpressure to audio) (WONT DO - use port buffer and reduce case overhead) - Adaptive endpoint EP and descriptors (DONE) - Adpative clock control (IN PROGRESS) +- Proper control loop w/filtering - Switchable MICS using define (WONT DO - separate app) - DFU - Combinable EP0 (DONE)