Initial work on FFT256 for Teensy-LC (untested)

PaulStoffregen · Aug 29, 2015 · 3564193 · 3564193
1 parent d536e9e
commit 3564193
Show file tree

Hide file tree

Showing 2 changed files with 49 additions and 4 deletions.
diff --git a/analyze_fft256.cpp b/analyze_fft256.cpp
@@ -51,7 +51,6 @@ static void apply_window_to_fft_buffer(void *buffer, const void *window)
 		*buf = val >> 15;
 		buf += 2;
 	}
-
 }
 
 void AudioAnalyzeFFT256::update(void)
@@ -60,6 +59,7 @@ void AudioAnalyzeFFT256::update(void)
 
 	block = receiveReadOnly();
 	if (!block) return;
+#if AUDIO_BLOCK_SAMPLES == 128
 	if (!prevblock) {
 		prevblock = block;
 		return;
@@ -94,6 +94,36 @@ void AudioAnalyzeFFT256::update(void)
 	}
 	release(prevblock);
 	prevblock = block;
+#elif AUDIO_BLOCK_SAMPLES == 64
+	if (prevblocks[2] == NULL) {
+		prevblocks[2] = prevblocks[1];
+		prevblocks[1] = prevblocks[0];
+		prevblocks[0] = block;
+		return;
+	}
+	if (count == 0) {
+		count = 1;
+		copy_to_fft_buffer(buffer, prevblocks[2]->data);
+		copy_to_fft_buffer(buffer+128, prevblocks[1]->data);
+		copy_to_fft_buffer(buffer+256, prevblocks[1]->data);
+		copy_to_fft_buffer(buffer+384, block->data);
+		if (window) apply_window_to_fft_buffer(buffer, window);
+		arm_cfft_radix4_q15(&fft_inst, buffer);
+	} else {
+		count = 2;
+		const uint32_t *p = (uint32_t *)buffer;
+		for (int i=0; i < 128; i++) {
+			uint32_t tmp = *p++;
+			int16_t v1 = tmp & 0xFFFF;
+			int16_t v2 = tmp >> 16;
+			output[i] = sqrt_uint32_approx(v1 * v1 + v2 * v2);
+		}
+	}
+	release(prevblocks[2]);
+	prevblocks[2] = prevblocks[1];
+	prevblocks[1] = prevblocks[0];
+	prevblocks[0] = block;
+#endif
 }
 
 
diff --git a/analyze_fft256.h b/analyze_fft256.h
@@ -49,9 +49,16 @@ class AudioAnalyzeFFT256 : public AudioStream
 {
 public:
 	AudioAnalyzeFFT256() : AudioStream(1, inputQueueArray),
-	  window(AudioWindowHanning256), prevblock(NULL), count(0),
-	  naverage(8), outputflag(false) {
+	  window(AudioWindowHanning256), count(0), outputflag(false) {
 		arm_cfft_radix4_init_q15(&fft_inst, 256, 0, 1);
+#if AUDIO_BLOCK_SAMPLES == 128
+		prevblock = NULL;
+		naverage = 8;
+#elif AUDIO_BLOCK_SAMPLES == 64
+		prevblocks[0] = NULL;
+		prevblocks[1] = NULL;
+		prevblocks[2] = NULL;
+#endif
 	}
 	bool available() {
 		if (outputflag == true) {
@@ -79,8 +86,10 @@ class AudioAnalyzeFFT256 : public AudioStream
 		return (float)sum * (1.0 / 16384.0);
 	}
 	void averageTogether(uint8_t n) {
+#if AUDIO_BLOCK_SAMPLES == 128
 		if (n == 0) n = 1;
 		naverage = n;
+#endif
 	}
 	void windowFunction(const int16_t *w) {
 		window = w;
@@ -89,11 +98,17 @@ class AudioAnalyzeFFT256 : public AudioStream
 	uint16_t output[128] __attribute__ ((aligned (4)));
 private:
 	const int16_t *window;
+#if AUDIO_BLOCK_SAMPLES == 128
 	audio_block_t *prevblock;
+#elif AUDIO_BLOCK_SAMPLES == 64
+	audio_block_t *prevblocks[3];
+#endif
 	int16_t buffer[512] __attribute__ ((aligned (4)));
+#if AUDIO_BLOCK_SAMPLES == 128
 	uint32_t sum[128];
-	uint8_t count;
 	uint8_t naverage;
+#endif
+	uint8_t count;
 	bool outputflag;
 	audio_block_t *inputQueueArray[1];
 	arm_cfft_radix4_instance_q15 fft_inst;