diff --git a/examples/xua_lite_example/src/fifo_impl.h b/examples/xua_lite_example/src/fifo_impl.h index 3341f32a..32c297dd 100644 --- a/examples/xua_lite_example/src/fifo_impl.h +++ b/examples/xua_lite_example/src/fifo_impl.h @@ -38,6 +38,14 @@ static inline unsigned fifo_get_fill_short(volatile mem_fifo_short_t * unsafe fi } } +static inline void fifo_init_short(volatile mem_fifo_short_t * unsafe fifo) { + unsafe{ + fifo->write_idx = 0; + fifo->read_idx = (fifo->size * 2) / 4; + memset(fifo->data_base_ptr , 0, fifo->size * sizeof(short)); + } +} + #pragma unsafe arrays static inline fifo_ret_t fifo_block_push(volatile mem_fifo_t * unsafe fifo, int data[], unsigned n) { unsafe{ diff --git a/examples/xua_lite_example/src/rate_controller.h b/examples/xua_lite_example/src/rate_controller.h new file mode 100644 index 00000000..23afa87f --- /dev/null +++ b/examples/xua_lite_example/src/rate_controller.h @@ -0,0 +1,17 @@ +#include +#include + +typedef int32_t xua_lite_fixed_point_t; + +typedef struct pid_state_t{ + xua_lite_fixed_point_t fifo_level_filtered_old; + xua_lite_fixed_point_t fifo_level_accum; +} pid_state_t; + + +//USB Adaptive mode helper +void do_rate_control(int fill_level, pid_state_t *pid_state, 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 + ,long long &feedback_value, unsigned &mod_from_last_time, unsigned fb_clocks[1]); \ No newline at end of file diff --git a/examples/xua_lite_example/src/rate_controller.xc b/examples/xua_lite_example/src/rate_controller.xc new file mode 100644 index 00000000..e381cacb --- /dev/null +++ b/examples/xua_lite_example/src/rate_controller.xc @@ -0,0 +1,190 @@ +#include +#include "xua_buffer_lite.h" +#include "rate_controller.h" +#define DEBUG_UNIT XUA_RATE_CONTROL +#define DEBUG_PRINT_ENABLE_XUA_RATE_CONTROL 1 +#include "debug_print.h" + +#define XUA_LIGHT_FIXED_POINT_Q_BITS 10 //Including sign bit. 10b gets us to +511.999999 to -512.000000 +#define XUA_LIGHT_FIXED_POINT_TOTAL_BITS (sizeof(xua_lite_fixed_point_t) * 8) +#define XUA_LIGHT_FIXED_POINT_FRAC_BITS (XUA_LIGHT_FIXED_POINT_TOTAL_BITS - XUA_LIGHT_FIXED_POINT_Q_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.8 //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 50 //How much noise to inject in percent of existing signal amplitude +#define RANDOMISATION_COEFF_A ((INT_MAX / 100) * RANDOMISATION_PERCENT) + +#define PID_CONTROL_P_TERM 6.0 +#define PID_CONTROL_I_TERM 0.0 +#define PID_CONTROL_D_TERM 0.0 + +#define PID_RATE_MULTIPLIER SOF_FREQ_HZ + +#define PID_CONTROL_P_TERM_COEFF ((xua_lite_fixed_point_t)(XUA_LIGHT_FIXED_POINT_ONE * (float)PID_CONTROL_P_TERM)) //scale to fixed point +#define PID_CONTROL_I_TERM_COEFF ((xua_lite_fixed_point_t)(XUA_LIGHT_FIXED_POINT_ONE * (float)PID_CONTROL_I_TERM / PID_RATE_MULTIPLIER)) //scale to fixed point +#define PID_CONTROL_D_TERM_COEFF ((xua_lite_fixed_point_t)(XUA_LIGHT_FIXED_POINT_ONE * (float)PID_CONTROL_D_TERM)) //scale to fixed point + +#define PID_CALC_OVERHEAD_BITS 6 //Allow large P,I or D constants, up to 2^(this number) + +static inline 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 inline 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 inline 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)); +} + +//Do PI control and modulation for adaptive USB audio +void do_rate_control(int fill_level, pid_state_t *pid_state, int *clock_nudge){ + + //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; + int fill_level_wrt_half = fill_level - half_full; //Will be +ve for more than half full and negative for less than half full + + //Low pass filter fill level and get error w.r.t. to set point which is depth = 0 (relative to half full) + xua_lite_fixed_point_t fifo_level_filtered = do_fifo_depth_lowpass_filter(pid_state->fifo_level_filtered_old , fill_level_wrt_half); + //printf("old fill_level: %f fill_level: %f\n", (float)pid_state->fifo_level_filtered_old/(1<fifo_level_accum + fifo_level_filtered; + + //clip the I term (which can wind up) to maximum fixed point representation. Check to see if overflow (which will change sign) + if (fifo_level_filtered >= 0){ //If it was positive before, ensure it still is else clip to positive + if (i_term_pre_clip >= pid_state->fifo_level_accum){ + //debug_printf("grow %d %d\n", (int32_t)i_term_pre_clip, (int32_t)pid_state->fifo_level_accum); + pid_state->fifo_level_accum = i_term_pre_clip; + } + else{ + pid_state->fifo_level_accum = INT_MAX; + //debug_printf("clip+ %d\n", pid_state->fifo_level_accum); + } + } + else{ //Value was negative so ensure it still is else clip negative + if (i_term_pre_clip <= pid_state->fifo_level_accum){ + pid_state->fifo_level_accum = i_term_pre_clip; + } + else{ + pid_state->fifo_level_accum = INT_MIN; + //debug_printf("clip- %d %d\n", pid_state->fifo_level_accum, fifo_level_filtered); + } + } + + //Calculate D term. No clipping required because it can never be larger than the D term, + //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; + + + //Do PID calculation. Note there is an implicit cast back to xua_lite_fixed_point_t before assignment + const unsigned pid_mul_r_shift_bits = XUA_LIGHT_FIXED_POINT_FRAC_BITS + PID_CALC_OVERHEAD_BITS; + xua_lite_fixed_point_t p_term = (((int64_t) fifo_level_filtered * (int64_t)PID_CONTROL_P_TERM_COEFF)) >> pid_mul_r_shift_bits; + xua_lite_fixed_point_t i_term = (((int64_t) pid_state->fifo_level_accum * (int64_t)PID_CONTROL_I_TERM_COEFF)) >> pid_mul_r_shift_bits; + xua_lite_fixed_point_t d_term = (((int64_t) fifo_level_delta * (int64_t)PID_CONTROL_D_TERM_COEFF)) >> pid_mul_r_shift_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); + //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",fill_level_wrt_half);} +} + + +//Calculate feedback for asynchronous USB audio +void do_feedback_calculation(unsigned &sof_count + ,const unsigned mclk_hz + ,unsigned mclk_port_counter + ,unsigned &mclk_port_counter_old + ,long long &feedback_value + ,unsigned &mod_from_last_time + ,unsigned fb_clocks[1]){ + // Assuming 48kHz from a 24.576 master clock (0.0407uS period) + // MCLK ticks per SOF = 125uS / 0.0407 = 3072 MCLK ticks per SOF. + // expected Feedback is 48000/8000 = 6 samples. so 0x60000 in 16:16 format. + // Average over 128 SOFs - 128 x 3072 = 0x60000. + + unsigned long long feedbackMul = 64ULL; + if(AUDIO_CLASS == 1) feedbackMul = 8ULL; // TODO Use 4 instead of 8 to avoid windows LSB issues? + + // Number of MCLK ticks in this SOF period (E.g = 125 * 24.576 = 3072) + int mclk_ticks_this_sof_period = (int) ((short)(mclk_port_counter - mclk_port_counter_old)); + unsigned long long full_result = mclk_ticks_this_sof_period * feedbackMul * DEFAULT_FREQ; + feedback_value += full_result; + + // Store MCLK for next time around... + mclk_port_counter_old = mclk_port_counter; + + // Reset counts based on SOF counting. Expect 16ms (128 HS SOFs/16 FS SOFS) per feedback poll + // We always count 128 SOFs, so 16ms @ HS, 128ms @ FS + if(sof_count == 128){ + //debug_printf("fb\n"); + sof_count = 0; + + feedback_value += mod_from_last_time; + unsigned clocks = feedback_value / mclk_hz; + mod_from_last_time = feedback_value % mclk_hz; + feedback_value = 0; + + //Scale for working out number of samps to take from device for input + if(AUDIO_CLASS == 2){ + clocks <<= 3; + } + else{ + clocks <<= 6; + } + asm volatile("stw %0, dp[g_speed]"::"r"(clocks)); // g_speed = clocks + + //Write to feedback EP buffer + if (AUDIO_CLASS == 2){ + fb_clocks[0] = clocks; + } + else{ + fb_clocks[0] = clocks >> 2; + } + } +} + diff --git a/examples/xua_lite_example/src/xua_buffer_lite.h b/examples/xua_lite_example/src/xua_buffer_lite.h index 0a5a3366..0436b29c 100644 --- a/examples/xua_lite_example/src/xua_buffer_lite.h +++ b/examples/xua_lite_example/src/xua_buffer_lite.h @@ -1,5 +1,24 @@ #include #include "xua_ep0_wrapper.h" +#include "xua.h" + +//Currently only single frequency supported +#define NOMINAL_SR_DEVICE DEFAULT_FREQ +#define NOMINAL_SR_HOST DEFAULT_FREQ + +#define DIV_ROUND_UP(n, d) (n / d + 1) //Always rounds up to the next integer. Needed for 48001Hz case etc. +#define BIGGEST(a, b) (a > b ? a : b) + +#define SOF_FREQ_HZ (8000 - ((2 - AUDIO_CLASS) * 7000) ) //1000 for FS or 8000 for HS + +//Defines for endpoint buffer sizes. Samples is total number of samples across all channels +#define MAX_OUT_SAMPLES_PER_SOF_PERIOD (DIV_ROUND_UP(MAX_FREQ, SOF_FREQ_HZ) * NUM_USB_CHAN_OUT) +#define MAX_IN_SAMPLES_PER_SOF_PERIOD (DIV_ROUND_UP(MAX_FREQ, SOF_FREQ_HZ) * NUM_USB_CHAN_IN) +#define MAX_OUTPUT_SLOT_SIZE 4 +#define MAX_INPUT_SLOT_SIZE 4 + +#define OUT_AUDIO_BUFFER_SIZE_BYTES (MAX_OUT_SAMPLES_PER_SOF_PERIOD * MAX_OUTPUT_SLOT_SIZE) +#define IN_AUDIO_BUFFER_SIZE_BYTES (MAX_IN_SAMPLES_PER_SOF_PERIOD * MAX_INPUT_SLOT_SIZE) unsafe void XUA_Buffer_lite(chanend c_ep0_out, chanend c_ep0_in, chanend c_aud_out, chanend ?c_feedback, chanend c_aud_in, chanend c_sof, in port p_for_mclk_count, streaming chanend c_audio_hub); [[combinable]] diff --git a/examples/xua_lite_example/src/xua_buffer_lite.xc b/examples/xua_lite_example/src/xua_buffer_lite.xc index fd2323ff..bed8e0ab 100644 --- a/examples/xua_lite_example/src/xua_buffer_lite.xc +++ b/examples/xua_lite_example/src/xua_buffer_lite.xc @@ -11,211 +11,8 @@ #include "xua.h" #include "fifo_impl.h" #include "xua_ep0_wrapper.h" - -//Currently only single frequency supported -#define NOMINAL_SR_DEVICE DEFAULT_FREQ -#define NOMINAL_SR_HOST DEFAULT_FREQ - -#define DIV_ROUND_UP(n, d) (n / d + 1) //Always rounds up to the next integer. Needed for 48001Hz case etc. -#define BIGGEST(a, b) (a > b ? a : b) - -#define SOF_FREQ_HZ (8000 - ((2 - AUDIO_CLASS) * 7000) ) - -//Defines for endpoint buffer sizes. Samples is total number of samples across all channels -#define MAX_OUT_SAMPLES_PER_SOF_PERIOD (DIV_ROUND_UP(MAX_FREQ, SOF_FREQ_HZ) * NUM_USB_CHAN_OUT) -#define MAX_IN_SAMPLES_PER_SOF_PERIOD (DIV_ROUND_UP(MAX_FREQ, SOF_FREQ_HZ) * NUM_USB_CHAN_IN) -#define MAX_OUTPUT_SLOT_SIZE 4 -#define MAX_INPUT_SLOT_SIZE 4 - -#define OUT_AUDIO_BUFFER_SIZE_BYTES (MAX_OUT_SAMPLES_PER_SOF_PERIOD * MAX_OUTPUT_SLOT_SIZE) -#define IN_AUDIO_BUFFER_SIZE_BYTES (MAX_IN_SAMPLES_PER_SOF_PERIOD * MAX_INPUT_SLOT_SIZE) - - -static void do_feedback_calculation(unsigned &sof_count - ,const unsigned mclk_hz - ,unsigned mclk_port_counter - ,unsigned &mclk_port_counter_old - ,long long &feedback_value - ,unsigned &mod_from_last_time - ,unsigned fb_clocks[1]){ - // Assuming 48kHz from a 24.576 master clock (0.0407uS period) - // MCLK ticks per SOF = 125uS / 0.0407 = 3072 MCLK ticks per SOF. - // expected Feedback is 48000/8000 = 6 samples. so 0x60000 in 16:16 format. - // Average over 128 SOFs - 128 x 3072 = 0x60000. - - unsigned long long feedbackMul = 64ULL; - if(AUDIO_CLASS == 1) feedbackMul = 8ULL; // TODO Use 4 instead of 8 to avoid windows LSB issues? - - // Number of MCLK ticks in this SOF period (E.g = 125 * 24.576 = 3072) - int mclk_ticks_this_sof_period = (int) ((short)(mclk_port_counter - mclk_port_counter_old)); - unsigned long long full_result = mclk_ticks_this_sof_period * feedbackMul * DEFAULT_FREQ; - feedback_value += full_result; - - // Store MCLK for next time around... - mclk_port_counter_old = mclk_port_counter; - - // Reset counts based on SOF counting. Expect 16ms (128 HS SOFs/16 FS SOFS) per feedback poll - // We always count 128 SOFs, so 16ms @ HS, 128ms @ FS - if(sof_count == 128){ - //debug_printf("fb\n"); - sof_count = 0; - - feedback_value += mod_from_last_time; - unsigned clocks = feedback_value / mclk_hz; - mod_from_last_time = feedback_value % mclk_hz; - feedback_value = 0; - - //Scale for working out number of samps to take from device for input - if(AUDIO_CLASS == 2){ - clocks <<= 3; - } - else{ - clocks <<= 6; - } - asm volatile("stw %0, dp[g_speed]"::"r"(clocks)); // g_speed = clocks - - //Write to feedback EP buffer - if (AUDIO_CLASS == 2){ - fb_clocks[0] = clocks; - } - else{ - fb_clocks[0] = clocks >> 2; - } - } -} - -#define CONTROL_LOOP 1 - -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 50 //How much noise to inject in percent of existing signal amplitude -#define RANDOMISATION_COEFF_A ((INT_MAX / 100) * RANDOMISATION_PERCENT) - -#define PI_CONTROL_P_TERM 15.0 -#define PI_CONTROL_I_TERM 1.1 -#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_filtered > 0){ //If it was positive before, ensure it still is else clip to positive - if (i_term_pre_clip > fifo_level_integrated){ - fifo_level_integrated = i_term_pre_clip; - } - else{ - fifo_level_integrated = INT_MAX; - debug_printf("clip+ %d\n", fifo_level_integrated); - } - } - else{ //Value was negative so ensure it still is else clip negative - if (i_term_pre_clip < fifo_level_integrated){ - fifo_level_integrated = i_term_pre_clip; - } - else{ - fifo_level_integrated = INT_MIN; - debug_printf("clip- %d %d\n", fifo_level_integrated, fifo_level_filtered); - } - } - - //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 f: %d\n", p_term >> XUA_LIGHT_FIXED_POINT_Q_BITS, i_term >> XUA_LIGHT_FIXED_POINT_Q_BITS, fill_level_wrt_half); - - //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); - - controller_out = add_noise(controller_out); - - - static xua_lite_fixed_point_t nudge_accumulator = 0; - nudge_accumulator += controller_out; - - //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; - //debug_printf("Nudge down\n"); - } - else if (fill_level <= trigger_low_upper){ - //debug_printf("Nudge up\n"); - clock_nudge = -1; - } - 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);} -} +#include "rate_controller.h" +#include "xua_buffer_lite.h" extern "C"{ @@ -260,6 +57,7 @@ unsafe void XUA_Buffer_lite(chanend c_ep0_out, chanend c_ep0_in, chanend c_aud_o //Adapative device clock control int clock_nudge = 0; + pid_state_t pid_state = {0, 0}; //Endpoints XUD_ep ep_aud_out = XUD_InitEp(c_aud_out); @@ -343,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); - fill_level_process(fill_level, clock_nudge); + if (isnull(c_feedback)) 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); @@ -432,6 +230,8 @@ unsafe void XUA_Buffer_lite2(server ep0_control_if i_ep0_ctl, chanend c_aud_out, //Adapative device clock control int clock_nudge = 0; + pid_state_t pid_state = {0, 0}; + //Endpoints XUD_ep ep_aud_out = XUD_InitEp(c_aud_out); @@ -475,6 +275,8 @@ unsafe void XUA_Buffer_lite2(server ep0_control_if i_ep0_ctl, chanend c_aud_out, select{ //Handle EP0 requests case i_ep0_ctl.set_output_interface(unsigned num): + //Reset output FIFO if moving from idle to streaming + if (num != 0 && output_interface_num == 0) fifo_init_short(host_to_device_fifo_ptr); output_interface_num = num; debug_printf("output_interface_num: %d\n", num); break; @@ -513,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); - fill_level_process(fill_level, clock_nudge); + if (isnull(c_feedback)) 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);