Improve Biquad filter

examples/Effects/Filter/Filter.ino

@@ -3,54 +3,39 @@
 #include <SPI.h>
 #include <SD.h>
 
-const int myInput = AUDIO_INPUT_LINEIN;
-//const int myInput = AUDIO_INPUT_MIC;
-
-// each filter requires a set up parameters
-//  http://forum.pjrc.com/threads/24793-Audio-Library?p=40179&viewfull=1#post40179
-//
-int myFilterParameters[] = {  // lowpass, Fc=800 Hz, Q=0.707
-  3224322, 6448644, 3224322, 1974735214, -913890679, 0, 0, 0};
-
-// Create the Audio components.  These should be created in the
-// order data flows, inputs/sources -> processing -> outputs
-//
-AudioInputI2S       audioInput;         // audio shield: mic or line-in
-AudioFilterBiquad   myFilter(myFilterParameters);
-AudioOutputI2S      audioOutput;        // audio shield: headphones & line-out
-
-// Create Audio connections between the components
-//
-AudioConnection c1(audioInput, 0, audioOutput, 0);
-AudioConnection c2(audioInput, 0, myFilter, 0);
-AudioConnection c3(myFilter, 0, audioOutput, 1);
+// GUItool: begin automatically generated code
+AudioInputI2S            i2s1;           //xy=99,60
+AudioFilterBiquad        biquad1;        //xy=257,60
+AudioOutputI2S           i2s2;           //xy=416,60
+AudioConnection          patchCord1(i2s1, 0, biquad1, 0);
+AudioConnection          patchCord2(biquad1, 0, i2s2, 0);
+AudioConnection          patchCord3(biquad1, 0, i2s2, 1);
+AudioControlSGTL5000     sgtl5000_1;     //xy=305,132
+// GUItool: end automatically generated code
 
-// Create an object to control the audio shield.
-// 
-AudioControlSGTL5000 audioShield;
 
+const int myInput = AUDIO_INPUT_LINEIN;
+//const int myInput = AUDIO_INPUT_MIC;
 
 void setup() {
-  // Audio connections require memory to work.  For more
-  // detailed information, see the MemoryAndCpuUsage example
   AudioMemory(12);
 
-  // Enable the audio shield and set the output volume.
-  audioShield.enable();
-  audioShield.inputSelect(myInput);
-  audioShield.volume(0.6);
+  sgtl5000_1.enable();  // Enable the audio shield
+  sgtl5000_1.inputSelect(myInput);
+  sgtl5000_1.volume(0.5);
+
+  // Butterworth filter, 12 db/octave
+  biquad1.setLowpass(0, 800, 0.707);
+
+  // Linkwitz-Riley filter, 48 dB/octave
+  //biquad1.setLowpass(0, 800, 0.54);
+  //biquad1.setLowpass(1, 800, 1.3);
+  //biquad1.setLowpass(2, 800, 0.54);
+  //biquad1.setLowpass(3, 800, 1.3);
 }
 
-elapsedMillis volmsec=0;
 
 void loop() {
-  // every 50 ms, adjust the volume
-  if (volmsec > 50) {
-    float vol = analogRead(15);
-    vol = vol / 1024;
-    audioShield.volume(vol);
-    volmsec = 0;
-  }
 }
 
 

filter_biquad.cpp

@@ -30,8 +30,8 @@
 void AudioFilterBiquad::update(void)
 {
 	audio_block_t *block;
-	int32_t a0, a1, a2, b1, b2, sum;
-	uint32_t in2, out2, aprev, bprev, flag;
+	int32_t b0, b1, b2, a1, a2, sum;
+	uint32_t in2, out2, bprev, aprev, flag;
 	uint32_t *data, *end;
 	int32_t *state;
 	block = receiveWritable();
@@ -39,65 +39,58 @@ void AudioFilterBiquad::update(void)
 	end = (uint32_t *)(block->data) + AUDIO_BLOCK_SAMPLES/2;
 	state = (int32_t *)definition;
 	do {
-		a0 = *state++;
-		a1 = *state++;
-		a2 = *state++;
+		b0 = *state++;
 		b1 = *state++;
 		b2 = *state++;
-		aprev = *state++;
+		a1 = *state++;
+		a2 = *state++;
 		bprev = *state++;
+		aprev = *state++;
 		sum = *state & 0x3FFF;
 		data = end - AUDIO_BLOCK_SAMPLES/2;
 		do {
 			in2 = *data;
-			sum = signed_multiply_accumulate_32x16b(sum, a0, in2);
-			sum = signed_multiply_accumulate_32x16t(sum, a1, aprev);
-			sum = signed_multiply_accumulate_32x16b(sum, a2, aprev);
+			sum = signed_multiply_accumulate_32x16b(sum, b0, in2);
 			sum = signed_multiply_accumulate_32x16t(sum, b1, bprev);
 			sum = signed_multiply_accumulate_32x16b(sum, b2, bprev);
-			out2 = (uint32_t)sum >> 14;
+			sum = signed_multiply_accumulate_32x16t(sum, a1, aprev);
+			sum = signed_multiply_accumulate_32x16b(sum, a2, aprev);
+			out2 = signed_saturate_rshift(sum, 16, 14);
 			sum &= 0x3FFF;
-			sum = signed_multiply_accumulate_32x16t(sum, a0, in2);
-			sum = signed_multiply_accumulate_32x16b(sum, a1, in2);
-			sum = signed_multiply_accumulate_32x16t(sum, a2, aprev);
-			sum = signed_multiply_accumulate_32x16b(sum, b1, out2);
+			sum = signed_multiply_accumulate_32x16t(sum, b0, in2);
+			sum = signed_multiply_accumulate_32x16b(sum, b1, in2);
 			sum = signed_multiply_accumulate_32x16t(sum, b2, bprev);
-			aprev = in2;
-			bprev = pack_16x16(sum >> 14, out2);
+			sum = signed_multiply_accumulate_32x16b(sum, a1, out2);
+			sum = signed_multiply_accumulate_32x16t(sum, a2, aprev);
+			bprev = in2;
+			aprev = pack_16x16(
+				signed_saturate_rshift(sum, 16, 14), out2);
 			sum &= 0x3FFF;
-			aprev = in2;
-			*data++ = bprev;
+			bprev = in2;
+			*data++ = aprev;
 		} while (data < end);
 		flag = *state & 0x80000000;
 		*state++ = sum | flag;
-		*(state-2) = bprev;
-		*(state-3) = aprev;
+		*(state-2) = aprev;
+		*(state-3) = bprev;
 	} while (flag);
 	transmit(block);
 	release(block);
 }
 
-void AudioFilterBiquad::updateCoefs(uint8_t set,int *source, bool doReset)
+void AudioFilterBiquad::setCoefficients(uint32_t stage, const int *coefficients)
 {
-	int32_t *dest=(int32_t *)definition;
-	while(set)
-	{
-		dest+=7;
-		if(!(*dest)&0x80000000) return;
-		dest++;
-		set--;
-	}
+	if (stage >= 4) return;
+	int32_t *dest = definition + (stage << 3);
 	__disable_irq();
-	for(uint8_t index=0;index<5;index++)
-	{
-		*dest++=*source++;
-	}
-	if(doReset)
-	{
-		*dest++=0;
-		*dest++=0;
-		*dest++=0;
-	}
+	if (stage > 0) *(dest - 1) |= 0x80000000;
+	*dest++ = *coefficients++;
+	*dest++ = *coefficients++;
+	*dest++ = *coefficients++;
+	*dest++ = *coefficients++ * -1;
+	*dest++ = *coefficients++ * -1;
+	*dest++ = 0;
+	*dest++ = 0;
+	*dest   &= 0x80000000;
 	__enable_irq();
 }
-

filter_biquad.h

@@ -32,17 +32,85 @@
 class AudioFilterBiquad : public AudioStream
 {
 public:
-	AudioFilterBiquad(int *parameters)
-	   : AudioStream(1, inputQueueArray), definition(parameters) { }
+	AudioFilterBiquad(void) : AudioStream(1, inputQueueArray) {
+		// by default, the filter will not pass anything
+		for (int i=0; i<32; i++) definition[i] = 0;
+	}
 	virtual void update(void);
-	
-	void updateCoefs(uint8_t set,int *source, bool doReset);
 
-	void updateCoefs(uint8_t set,int *source) { updateCoefs(set,source,false); }
-	void updateCoefs(int *source, bool doReset) { updateCoefs(0,source,doReset); }
-	void updateCoefs(int *source) { updateCoefs(0,source,false); }
+	// Set the biquad coefficients directly
+	void setCoefficients(uint32_t stage, const int *coefficients);
+	void setCoefficients(uint32_t stage, const double *coefficients) {
+		int coef[5];
+		coef[0] = coefficients[0] * 1073741824.0;
+		coef[1] = coefficients[1] * 1073741824.0;
+		coef[2] = coefficients[2] * 1073741824.0;
+		coef[3] = coefficients[3] * 1073741824.0;
+		coef[4] = coefficients[4] * 1073741824.0;
+		setCoefficients(stage, coef);
+	}
+
+	// Compute common filter functions
+	// http://www.musicdsp.org/files/Audio-EQ-Cookbook.txt
+	void setLowpass(uint32_t stage, float frequency, float q = 0.7071) {
+		int coef[5];
+		double w0 = frequency * (2 * 3.141592654 / AUDIO_SAMPLE_RATE_EXACT);
+		double sinW0 = sin(w0);
+		double alpha = sinW0 / ((double)q * 2.0);
+		double cosW0 = cos(w0);
+		double scale = 1073741824.0 / (1.0 + alpha);
+		/* b0 */ coef[0] = ((1.0 - cosW0) / 2.0) * scale;
+		/* b1 */ coef[1] = (1.0 - cosW0) * scale;
+		/* b2 */ coef[2] = coef[0];
+		/* a1 */ coef[3] = (-2.0 * cosW0) * scale;
+		/* a2 */ coef[4] = (1.0 - alpha) * scale;
+		setCoefficients(stage, coef);
+	}
+	void setHighpass(uint32_t stage, float frequency, float q = 0.7071) {
+		int coef[5];
+		double w0 = frequency * (2 * 3.141592654 / AUDIO_SAMPLE_RATE_EXACT);
+		double sinW0 = sin(w0);
+		double alpha = sinW0 / ((double)q * 2.0);
+		double cosW0 = cos(w0);
+		double scale = 1073741824.0 / (1.0 + alpha);
+		/* b0 */ coef[0] = ((1.0 + cosW0) / 2.0) * scale;
+		/* b1 */ coef[1] = -(1.0 + cosW0) * scale;
+		/* b2 */ coef[2] = coef[0];
+		/* a1 */ coef[3] = (-2.0 * cosW0) * scale;
+		/* a2 */ coef[4] = (1.0 - alpha) * scale;
+		setCoefficients(stage, coef);
+	}
+	void setBandpass(uint32_t stage, float frequency, float q = 1.0) {
+		int coef[5];
+		double w0 = frequency * (2 * 3.141592654 / AUDIO_SAMPLE_RATE_EXACT);
+		double sinW0 = sin(w0);
+		double alpha = sinW0 / ((double)q * 2.0);
+		double cosW0 = cos(w0);
+		double scale = 1073741824.0 / (1.0 + alpha);
+		/* b0 */ coef[0] = alpha * scale;
+		/* b1 */ coef[1] = 0;
+		/* b2 */ coef[2] = (-alpha) * scale;
+		/* a1 */ coef[3] = (-2.0 * cosW0) * scale;
+		/* a2 */ coef[4] = (1.0 - alpha) * scale;
+		setCoefficients(stage, coef);
+	}
+	void setNotch(uint32_t stage, float frequency, float q = 1.0) {
+		int coef[5];
+		double w0 = frequency * (2 * 3.141592654 / AUDIO_SAMPLE_RATE_EXACT);
+		double sinW0 = sin(w0);
+		double alpha = sinW0 / ((double)q * 2.0);
+		double cosW0 = cos(w0);
+		double scale = 1073741824.0 / (1.0 + alpha);
+		/* b0 */ coef[0] = scale;
+		/* b1 */ coef[1] = (-2.0 * cosW0) * scale;
+		/* b2 */ coef[2] = coef[0];
+		/* a1 */ coef[3] = (-2.0 * cosW0) * scale;
+		/* a2 */ coef[4] = (1.0 - alpha) * scale;
+		setCoefficients(stage, coef);
+	}
+
 private:
-	int *definition;
+	int32_t definition[32];  // up to 4 cascaded biquads
 	audio_block_t *inputQueueArray[1];
 };
 

gui/list.html

@@ -1378,24 +1378,52 @@ The actual packets are taken
 </script>
 <script type="text/x-red" data-help-name="AudioFilterBiquad">
 	<h3>Summary</h3>
-	<p>description</p>
+	<div>
+	<p>Biquadratic cascaded filter, useful for all sorts of filtering.
+		Up to 4 stages may be cascaded.
+	</p>
+	<p align=center><img src="biquad.png"></p>
+	</div>
 	<h3>Audio Connections</h3>
 	<table class=doc align=center cellpadding=3>
 		<tr class=top><th>Port</th><th>Purpose</th></tr>
-		<tr class=odd><td align=center></td><td></td></tr>
-	</table>
-	<h3>Parameters</h3>
-	<table class=doc align=center cellpadding=3>
-		<tr class=top><th>Name</th><th>Type</th><th>Function</th></tr>
-		<tr class=odd><td align=center></td><td>Integer</td><td></td></tr>
+		<tr class=odd><td align=center>In 0</td><td>Signal to be filtered</td></tr>
+		<tr class=odd><td align=center>Out 0</td><td>Filtered Signal Output</td></tr>
 	</table>
-	<p>Extra description...  Section only present if object has params</p>
 	<h3>Functions</h3>
-	<p class=func><span class=keyword>function</span>(parm1, parm2);</p>
-	<p class=desc>blah blah blah blah
+	<p class=func><span class=keyword>setLowpass</span>(stage, frequency, Q);</p>
+	<p class=desc>Configure one stage of the filter (0 to 3) with low pass
+		response, with the specified corner frequency and Q shape.  If Q is
+		higher that 0.7071, be careful of filter gain (see below).
+	</p>
+	<p class=func><span class=keyword>setLowpass</span>(stage, frequency, Q);</p>
+	<p class=desc>Configure one stage of the filter (0 to 3) with high pass
+		response, with the specified corner frequency and Q shape.  If Q is
+		higher that 0.7071, be careful of filter gain (see below).
+	</p>
+	<p class=func><span class=keyword>setLowpass</span>(stage, frequency, Q);</p>
+	<p class=desc>Configure one stage of the filter (0 to 3) with band pass
+		response.  The filter has unity gain at the specified frequency.  Q
+		controls the width of frequencies allowed to pass.
+	</p>
+	<p class=func><span class=keyword>setLowpass</span>(stage, frequency, Q);</p>
+	<p class=desc>Configure one stage of the filter (0 to 3) with band reject (notch)
+		response.  Q controls the width of rejected frequencies.
+	</p>
+	<p class=func><span class=keyword>setCoefficients</span>(stage, array[5]);</p>
+	<p class=desc>Configure one stage of the filter (0 to 3) with an arbitrary
+		filter response.  The array of coefficients is in order: B0, B1, B2, A1, A2.
+		Each coefficient must be less than 2.0 and greater than -2.0.  The array
+		should be type double.  Alternately, it may be type int, where 1.0 is
+		represented with 1073741824 (2<sup>30</sup>).
 	</p>
 	<h3>Notes</h3>
-	<p></p>
+	<p>Filters can with gain must have their input signals attenuated, so the
+		signal does not exceed 1.0.
+	</p>
+	<p>This object implements up to 4 cascaded stages.  Unconfigured stages will
+		not pass any signal.
+	</p>
 </script>
 <script type="text/x-red" data-template-name="AudioFilterBiquad">
 	<div class="form-row">

keywords.txt

@@ -46,6 +46,11 @@ noteOff	KEYWORD2
 stop	KEYWORD2
 play	KEYWORD2
 updateCoefs	KEYWORD2
+setCoefficients	KEYWORD2
+setLowpass	KEYWORD2
+setHighpass	KEYWORD2
+setBandpass	KEYWORD2
+setNotch	KEYWORD2
 frequency	KEYWORD2
 phase	KEYWORD2
 amplitude	KEYWORD2