Vergil_Wong/lib_xua
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

PID loop development + move rate control to own source files

Browse Source
This commit is contained in:
Ed Clarke
2018-11-30 10:14:34 +00:00
parent 0b2839c9fc
commit 45d84a2b84
5 changed files with 243 additions and 207 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
examples/xua_lite_example/src/fifo_impl.h
View File

@@ -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 #pragma unsafe arrays
static inline fifo_ret_t fifo_block_push(volatile mem_fifo_t * unsafe fifo, int data[], unsigned n) { static inline fifo_ret_t fifo_block_push(volatile mem_fifo_t * unsafe fifo, int data[], unsigned n) {
unsafe{ unsafe{

17
examples/xua_lite_example/src/rate_controller.h Normal file
View File

@@ -0,0 +1,17 @@
#include <stdint.h>
#include <limits.h>
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]);

190
examples/xua_lite_example/src/rate_controller.xc Normal file
View File

@@ -0,0 +1,190 @@
#include <xs1.h>
#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<<XUA_LIGHT_FIXED_POINT_FRAC_BITS) , (float)fifo_level_filtered/(1<<XUA_LIGHT_FIXED_POINT_FRAC_BITS) );
//Calculate the value for the integral term which is the accumulated fill level error
xua_lite_fixed_point_t i_term_pre_clip = pid_state->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<<XUA_LIGHT_FIXED_POINT_FRAC_BITS), (float)pid_state->fifo_level_accum/(1<<XUA_LIGHT_FIXED_POINT_FRAC_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 + d_term) >> (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;
}
}
}

19
examples/xua_lite_example/src/xua_buffer_lite.h
View File

@@ -1,5 +1,24 @@
#include <xs1.h> #include <xs1.h>
#include "xua_ep0_wrapper.h" #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); 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]] [[combinable]]

216
examples/xua_lite_example/src/xua_buffer_lite.xc
View File

@@ -11,211 +11,8 @@
#include "xua.h" #include "xua.h"
#include "fifo_impl.h" #include "fifo_impl.h"
#include "xua_ep0_wrapper.h" #include "xua_ep0_wrapper.h"
#include "rate_controller.h"
//Currently only single frequency supported #include "xua_buffer_lite.h"
#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);}
}
extern "C"{ 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 //Adapative device clock control
int clock_nudge = 0; int clock_nudge = 0;
pid_state_t pid_state = {0, 0};
//Endpoints //Endpoints
XUD_ep ep_aud_out = XUD_InitEp(c_aud_out); 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; num_samples_to_send_to_host = num_samples_received_from_host;
int fill_level = fifo_get_fill_short(host_to_device_fifo_ptr); 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 //Mark EP as ready for next frame from host
XUD_SetReady_OutPtr(ep_aud_out, (unsigned)buffer_aud_out.long_words); 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 //Adapative device clock control
int clock_nudge = 0; int clock_nudge = 0;
pid_state_t pid_state = {0, 0};
//Endpoints //Endpoints
XUD_ep ep_aud_out = XUD_InitEp(c_aud_out); 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{ select{
//Handle EP0 requests //Handle EP0 requests
case i_ep0_ctl.set_output_interface(unsigned num): 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; output_interface_num = num;
debug_printf("output_interface_num: %d\n", num); debug_printf("output_interface_num: %d\n", num);
break; 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; num_samples_to_send_to_host = num_samples_received_from_host;
int fill_level = fifo_get_fill_short(host_to_device_fifo_ptr); 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 //Mark EP as ready for next frame from host
XUD_SetReady_OutPtr(ep_aud_out, (unsigned)buffer_aud_out.long_words); XUD_SetReady_OutPtr(ep_aud_out, (unsigned)buffer_aud_out.long_words);