Initial PID implementation

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){}

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;
     }

examples/xua_lite_example/src/xua_buffer_lite.xc

@@ -1,5 +1,5 @@
 #include <stdint.h>
-
+#include <limits.h>
 #include <xs1.h>
 
 #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){
+#define CONTROL_LOOP 1
-  //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;
 
-  const int trigger_high_upper = half_full_out + 2;
+typedef int32_t xua_lite_fixed_point_t;
-  const int trigger_low_upper = half_full_out - 2;
+#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

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)