diff --git a/examples/xua_lite_example/plot_fill_level.sh b/examples/xua_lite_example/plot_fill_level.sh index 8adfd632..bb2ad5a3 100644 --- a/examples/xua_lite_example/plot_fill_level.sh +++ b/examples/xua_lite_example/plot_fill_level.sh @@ -1 +1 @@ -grep f: dump.txt | grep -Eo "\-?\d+" > proc.txt&& gnuplot -p -e 'set term png; plot "proc.txt" with lines' > plot.png && open plot.png \ No newline at end of file +grep pid: dump.txt | grep -Eo "\-?\d+" > proc.txt && gnuplot -p -e 'set term png; plot "proc.txt" with lines' > plot.png && open plot.png \ No newline at end of file diff --git a/examples/xua_lite_example/src/rate_controller.h b/examples/xua_lite_example/src/rate_controller.h index 23afa87f..8dee3f75 100644 --- a/examples/xua_lite_example/src/rate_controller.h +++ b/examples/xua_lite_example/src/rate_controller.h @@ -10,7 +10,11 @@ typedef struct pid_state_t{ //USB Adaptive mode helper -void do_rate_control(int fill_level, pid_state_t *pid_state, int *clock_nudge); +xua_lite_fixed_point_t do_rate_control(int fill_level, pid_state_t *pid_state); + +//PDM modulator for clock control +void do_clock_nudge_pdm(xua_lite_fixed_point_t controller_out, int *clock_nudge); + //USB Asynch mode helper void do_feedback_calculation(unsigned &sof_count, const unsigned mclk_hz, unsigned mclk_port_counter,unsigned &mclk_port_counter_old diff --git a/examples/xua_lite_example/src/rate_controller.xc b/examples/xua_lite_example/src/rate_controller.xc index 6638f535..c6bc348b 100644 --- a/examples/xua_lite_example/src/rate_controller.xc +++ b/examples/xua_lite_example/src/rate_controller.xc @@ -11,7 +11,7 @@ #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.949 //Proportion of signal from y[-1]. +#define FIFO_LEVEL_EMA_COEFF 0.939 //Proportion of signal from y[-1]. //0.939 gives ~10Hz 3db cutoff low pass filter for filter rate of 1kHz //dsp.stackexchange.com/questions/40462/exponential-moving-average-cut-off-frequency/40465 #define FIFO_LEVEL_A_COEFF ((int32_t)(INT_MAX * FIFO_LEVEL_EMA_COEFF)) //Scale to signed 1.31 format @@ -20,7 +20,7 @@ #define RANDOMISATION_PERCENT 20 //How much radnom noise to inject in percent of existing signal amplitude #define RANDOMISATION_COEFF_A ((INT_MAX / 100) * RANDOMISATION_PERCENT) -#define PID_CALC_OVERHEAD_BITS 6 //Allow large P,I or D constants, up to 2^(this number) +#define PID_CALC_OVERHEAD_BITS 2 //Allow large P,I or D constants, up to 2^(this number) #define PID_CONTROL_P_TERM 10.0 @@ -58,8 +58,33 @@ static inline xua_lite_fixed_point_t add_noise(xua_lite_fixed_point_t input){ return (xua_lite_fixed_point_t)( output_64 >> (64 - XUA_LIGHT_FIXED_POINT_TOTAL_BITS - 1)); } +//Convert the control input into a pdm output (dither) with optional noise +void do_clock_nudge_pdm(xua_lite_fixed_point_t controller_out, int *clock_nudge){ + + //Randomise - add a proportion of rectangular probability distribution noise to spread the spectrum + controller_out = add_noise(controller_out); + + //Convert to pulse density modulation (sigma-delta) + static xua_lite_fixed_point_t nudge_accumulator = 0; + nudge_accumulator += controller_out; //Note no overflow check as if we reach XUA_LIGHT_FIXED_POINT_Q_BITS + //something is very wrong + //printf("co: %d ratio: %f \n", controller_out, (float)controller_out/XUA_LIGHT_FIXED_POINT_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; + } +} + + //Do PI control and modulation for adaptive USB audio -void do_rate_control(int fill_level, pid_state_t *pid_state, int *clock_nudge){ +xua_lite_fixed_point_t do_rate_control(int fill_level, pid_state_t *pid_state){ //We always check the FIFO level after USB has produced a block, and total FIFO size is 2x max, so half full is at 3/4 const int half_full = ((MAX_OUT_SAMPLES_PER_SOF_PERIOD * 2) * 3) / 4; @@ -97,42 +122,26 @@ void do_rate_control(int fill_level, pid_state_t *pid_state, int *clock_nudge){ //which is already scaled to fit within the fixed point representation xua_lite_fixed_point_t fifo_level_delta = fifo_level_filtered - pid_state->fifo_level_filtered_old; + //Save to struct for next iteration + pid_state->fifo_level_filtered_old = fifo_level_filtered; //Do PID calculation. Note there is an implicit cast back to xua_lite_fixed_point_t before assignment xua_lite_fixed_point_t p_term = (((int64_t) fifo_level_filtered * (int64_t)PID_CONTROL_P_TERM_COEFF)) >> XUA_LIGHT_FIXED_POINT_FRAC_BITS; xua_lite_fixed_point_t i_term = (((int64_t) pid_state->fifo_level_accum * (int64_t)PID_CONTROL_I_TERM_COEFF)) >> XUA_LIGHT_FIXED_POINT_FRAC_BITS; xua_lite_fixed_point_t d_term = (((int64_t) fifo_level_delta * (int64_t)PID_CONTROL_D_TERM_COEFF)) >> XUA_LIGHT_FIXED_POINT_FRAC_BITS; - //debug_printf("p: %d i: %d f: %d\n", p_term >> XUA_LIGHT_FIXED_POINT_Q_BITS, i_term >> XUA_LIGHT_FIXED_POINT_Q_BITS, fill_level_wrt_half); + //debug_printf("p: %d i: %d f: %d\n", p_term >> XUA_LIGHT_FIXED_POINT_Q_BITS, i_term >> XUA_LIGHT_FIXED_POINT_Q_BITS, fifo_level_filtered >> (XUA_LIGHT_FIXED_POINT_FRAC_BITS - 10)); //printf("p: %f i: %f d: %f filtered: %f integrated: %f\n", (float)p_term / (1<<(XUA_LIGHT_FIXED_POINT_FRAC_BITS-PID_CALC_OVERHEAD_BITS)), (float)i_term / (1<<(XUA_LIGHT_FIXED_POINT_FRAC_BITS-PID_CALC_OVERHEAD_BITS)), (float)d_term / (1<<(XUA_LIGHT_FIXED_POINT_FRAC_BITS-PID_CALC_OVERHEAD_BITS)), (float)fifo_level_filtered/(1<fifo_level_accum/(1<> (XUA_LIGHT_FIXED_POINT_Q_BITS - 1 - PID_CALC_OVERHEAD_BITS); - //Randomise - add a proportion of rectangular probability distribution noise to spread the spectrum - controller_out = add_noise(controller_out); - //Convert to pulse density modulation (sigma-delta) - static xua_lite_fixed_point_t nudge_accumulator = 0; - nudge_accumulator += controller_out; //Note no overflow check as if we reach XUA_LIGHT_FIXED_POINT_Q_BITS - //something is very wrong - //printf("co: %d ratio: %f \n", controller_out, (float)controller_out/XUA_LIGHT_FIXED_POINT_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; - } - - pid_state->fifo_level_filtered_old = fifo_level_filtered; //debug_printf("filtered: %d raw: %d\n", fifo_level_filtered >> 22, fill_level_wrt_half); - static unsigned counter; counter++; if (counter>100){counter = 0; debug_printf("f: %d\n",fifo_level_filtered >> (XUA_LIGHT_FIXED_POINT_FRAC_BITS - 10));} + //static unsigned counter; counter++; if (counter>100){counter = 0; debug_printf("pid: %d\n",i_term >> (XUA_LIGHT_FIXED_POINT_FRAC_BITS - 10));} + debug_printf("co: %d\n", controller_out >> XUA_LIGHT_FIXED_POINT_FRAC_BITS); + return controller_out; } diff --git a/examples/xua_lite_example/src/xua_buffer_lite.xc b/examples/xua_lite_example/src/xua_buffer_lite.xc index bed8e0ab..d86cdf38 100644 --- a/examples/xua_lite_example/src/xua_buffer_lite.xc +++ b/examples/xua_lite_example/src/xua_buffer_lite.xc @@ -141,7 +141,7 @@ unsafe void XUA_Buffer_lite(chanend c_ep0_out, chanend c_ep0_in, chanend c_aud_o num_samples_to_send_to_host = num_samples_received_from_host; int fill_level = fifo_get_fill_short(host_to_device_fifo_ptr); - if (isnull(c_feedback)) do_rate_control(fill_level, &pid_state, &clock_nudge); + if (isnull(c_feedback)) do_clock_nudge_pdm(do_rate_control(fill_level, &pid_state), &clock_nudge); //Mark EP as ready for next frame from host XUD_SetReady_OutPtr(ep_aud_out, (unsigned)buffer_aud_out.long_words); @@ -315,7 +315,7 @@ unsafe void XUA_Buffer_lite2(server ep0_control_if i_ep0_ctl, chanend c_aud_out, num_samples_to_send_to_host = num_samples_received_from_host; int fill_level = fifo_get_fill_short(host_to_device_fifo_ptr); - if (isnull(c_feedback)) do_rate_control(fill_level, &pid_state, &clock_nudge); + if (isnull(c_feedback)) do_clock_nudge_pdm(do_rate_control(fill_level, &pid_state), &clock_nudge); //Mark EP as ready for next frame from host XUD_SetReady_OutPtr(ep_aud_out, (unsigned)buffer_aud_out.long_words);