Added AudioFilterFIR which performs FIR filter

using user-supplied coefficients

Audio.cpp

@@ -3071,4 +3071,83 @@ bool AudioControlSGTL5000::volumeInteger(unsigned int n)
 
 
 
+/******************************************************************/
+/******************************************************************/
+
 
+// arm state arrays and FIR instances for left and right channels
+// the state arrays are defined to handle a maximum of MAX_COEFFS
+// coefficients in a filter
+q15_t AudioFilterFIR::l_StateQ15[AUDIO_BLOCK_SAMPLES + MAX_COEFFS];
+q15_t AudioFilterFIR::r_StateQ15[AUDIO_BLOCK_SAMPLES + MAX_COEFFS];
+arm_fir_instance_q15 AudioFilterFIR::l_fir_inst;
+arm_fir_instance_q15 AudioFilterFIR::r_fir_inst;
+
+// pointer to current coefficients or NULL or FIR_PASSTHRU
+short * AudioFilterFIR::coeff_p = NULL;
+
+void AudioFilterFIR::begin(short *cp,int n_coeffs)
+{
+  // pointer to coefficients
+  coeff_p = cp;
+  // Initialize FIR instances for the left and right channels
+  if(coeff_p && (coeff_p != FIR_PASSTHRU)) {
+    arm_fir_init_q15(&l_fir_inst, n_coeffs, coeff_p, &l_StateQ15[0], AUDIO_BLOCK_SAMPLES);
+    arm_fir_init_q15(&r_fir_inst, n_coeffs, coeff_p, &r_StateQ15[0], AUDIO_BLOCK_SAMPLES);
+  }
+}
+
+// This has the same effect as begin(NULL,0);
+void AudioFilterFIR::stop(void)
+{
+  coeff_p = NULL;
+}
+
+
+void AudioFilterFIR::update(void)
+{
+  audio_block_t *block,*b_new;
+  
+  // If there's no coefficient table, give up.  
+  if(coeff_p == NULL)return;
+
+  // do passthru
+  if(coeff_p == FIR_PASSTHRU) {
+    // Just passthrough
+    block = receiveWritable(0);
+    if(block) {
+      transmit(block,0);
+      release(block);
+    }
+    block = receiveWritable(1);
+    if(block) {
+      transmit(block,1);
+      release(block);
+    }
+    return;
+  }
+  // Left Channel
+  block = receiveWritable(0);
+  // get a block for the FIR output
+  b_new = allocate();
+  if(block && b_new) {
+    arm_fir_q15(&l_fir_inst, (q15_t *)block->data, (q15_t *)b_new->data, AUDIO_BLOCK_SAMPLES);
+    // send the FIR output to the left channel
+    transmit(b_new,0);
+  }
+  if(block)release(block);
+  if(b_new)release(b_new);
+
+  // Right Channel
+  block = receiveWritable(1);
+  b_new = allocate();
+  if(block && b_new) {
+    arm_fir_q15(&r_fir_inst, (q15_t *)block->data, (q15_t *)b_new->data, AUDIO_BLOCK_SAMPLES);
+    transmit(b_new,1);
+  }
+  if(block)release(block);
+  if(b_new)release(b_new);
+}
+
+/******************************************************************/
+/******************************************************************/

Audio.h

@@ -1,4 +1,5 @@
 #include "AudioStream.h"
+#include "arm_math.h"
 
 
 // When changing multiple audio object settings that must update at
@@ -540,14 +541,43 @@ protected:
 
 
 
+/******************************************************************/
+/******************************************************************/
 
+// Maximum number of coefficients in a FIR filter
+// The audio breaks up with 128 coefficients so a
+// maximum of 150 is more than sufficient
+#define MAX_COEFFS 150
 
+// Indicates that the code should just pass through the audio
+// without any filtering (as opposed to doing nothing at all)
+#define FIR_PASSTHRU ((short *) 1)
 
+class AudioFilterFIR : 
+public AudioStream
+{
+public:
+  AudioFilterFIR(void): 
+  AudioStream(2,inputQueueArray) { 
+  }
+
+  void begin(short *coeff_p,int f_pin);
+  virtual void update(void);
+  void stop(void);
+  
+private:
+  audio_block_t *inputQueueArray[2];
+  static q15_t l_StateQ15[];
+  static q15_t r_StateQ15[];
+  static arm_fir_instance_q15 l_fir_inst;
+  static arm_fir_instance_q15 r_fir_inst;
+  static short *coeff_p;
+};
 
 
 
-
-
+/******************************************************************/
+/******************************************************************/