Add AudioFilterBiquad object

Audio.cpp

@@ -1856,7 +1856,7 @@ static inline int32_t signed_multiply_32x16t(int32_t a, uint32_t b)
 }
 
 
-// computes ((a[15:0) << 16) | b[15:0])
+// computes ((a[15:0] << 16) | b[15:0])
 static inline uint32_t pack_16x16(int32_t a, int32_t b) __attribute__((always_inline));
 static inline uint32_t pack_16x16(int32_t a, int32_t b)
 {
@@ -1874,6 +1874,31 @@ static inline uint32_t signed_add_16_and_16(uint32_t a, uint32_t b)
 	return out;
 }
 
+// computes (sum + ((a[31:0] * b[15:0]) >> 16))
+static inline int32_t signed_multiply_accumulate_32x16b(int32_t sum, int32_t a, uint32_t b)
+{
+	int32_t out;
+	asm volatile("smlawb %0, %2, %3, %1" : "=r" (out) : "r" (sum), "r" (a), "r" (b));
+	return out;
+}
+
+// computes (sum + ((a[31:0] * b[31:16]) >> 16))
+static inline int32_t signed_multiply_accumulate_32x16t(int32_t sum, int32_t a, uint32_t b)
+{
+	int32_t out;
+	asm volatile("smlawt %0, %2, %3, %1" : "=r" (out) : "r" (sum), "r" (a), "r" (b));
+	return out;
+}
+
+// computes logical and, forces compiler to allocate register and use single cycle instruction
+static inline uint32_t logical_and(uint32_t a, uint32_t b)
+{
+	asm volatile("and %0, %1" : "+r" (a) : "r" (b));
+	return a;
+}
+
+
+
 
 void applyGain(int16_t *data, int32_t mult)
 {
@@ -1947,6 +1972,65 @@ void AudioMixer4::update(void)
 }
 
 
+/******************************************************************/
+
+
+
+
+
+void AudioFilterBiquad::update(void)
+{
+	audio_block_t *block;
+	int32_t a0, a1, a2, b1, b2, sum;
+	uint32_t in2, out2, aprev, bprev, flag;
+	uint32_t *data, *end;
+	int32_t *state;
+
+	block = receiveWritable();
+	if (!block) return;
+	data = (uint32_t *)(block->data);
+	end = data + AUDIO_BLOCK_SAMPLES/2;
+	state = (int32_t *)definition;
+	do {
+		a0 = *state++;
+		a1 = *state++;
+		a2 = *state++;
+		b1 = *state++;
+		b2 = *state++;
+		aprev = *state++;
+		bprev = *state++;
+		sum = *state & 0x3FFF;
+		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_32x16t(sum, b1, bprev);
+			sum = signed_multiply_accumulate_32x16b(sum, b2, bprev);
+			out2 = (uint32_t)sum >> 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, b2, bprev);
+			aprev = in2;
+			bprev = pack_16x16(sum >> 14, out2);
+			sum &= 0x3FFF;
+			aprev = in2;
+			*data++ = bprev;
+		} while (data < end);
+		flag = *state & 0x80000000;
+		*state++ = sum | flag;
+		*(state-2) = bprev;
+		*(state-3) = aprev;
+	} while (flag);
+	transmit(block);
+	release(block);
+}
+
+
+
 
 
 /******************************************************************/

Audio.h

@@ -346,6 +346,24 @@ private:
 
 
 
+
+
+class AudioFilterBiquad : public AudioStream
+{
+public:
+	AudioFilterBiquad(int *parameters)
+	   : AudioStream(1, inputQueueArray), definition(parameters) { }
+	virtual void update(void);
+private:
+	int *definition;
+	audio_block_t *inputQueueArray[1];
+};
+
+
+
+
+
+
 // TODO: more audio processing objects....
 //  N-channel mixer, adjustable gain on each channel
 //  sine wave with frequency modulation (phase)

examples/Filter/Filter.ino

@@ -0,0 +1,53 @@
+#include <Audio.h>
+#include <Wire.h>
+#include <SD.h>
+
+const int myInput = AUDIO_INPUT_LINEIN;
+//const int myInput = AUDIO_INPUT_MIC;
+
+// each filter requires a set up parameters
+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);
+
+// Create an object to control the audio shield.
+// 
+AudioControlSGTL5000 audioShield;
+
+
+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(60);
+}
+
+elapsedMillis volmsec=0;
+
+void loop() {
+  // every 50 ms, adjust the volume
+  if (volmsec > 50) {
+    float vol = analogRead(15);
+    vol = vol / 10.24;
+    audioShield.volume(vol);
+    volmsec = 0;
+  }
+}
+
+