Fix AudioAnalyzeFFT256 magnitude & averaging

analyze_fft256.cpp

@@ -25,6 +25,8 @@
  */
 
 #include "analyze_fft256.h"
+#include "sqrt_integer.h"
+#include "utility/dspinst.h"
 #include "arm_math.h"
 
 // TODO: this should be a class member, so more than one FFT can be used
@@ -34,28 +36,6 @@ void AudioAnalyzeFFT256::init(void)
 {
 	// TODO: replace this with static const version
 	arm_cfft_radix4_init_q15(&fft_inst, 256, 0, 1);
-
-	//for (int i=0; i<2048; i++) {
-		//buffer[i] = i * 3;
-	//}
-	//__disable_irq();
-	//ARM_DEMCR |= ARM_DEMCR_TRCENA;
-	//ARM_DWT_CTRL |= ARM_DWT_CTRL_CYCCNTENA;
-	//uint32_t n = ARM_DWT_CYCCNT;
-	//arm_cfft_radix2_q15(&fft_inst, buffer);
-	//n = ARM_DWT_CYCCNT - n;
-	//__enable_irq();
-	//cycles = n;
-	//arm_cmplx_mag_q15(buffer, buffer, 512);
-
-	// each audio block is 278525 cycles @ 96 MHz
-	// 256 point fft2 takes 65408 cycles
-	// 256 point fft4 takes 49108 cycles
-	// 128 point cmag takes 10999 cycles
-	// 1024 point fft2 takes 125948 cycles
-	// 1024 point fft4 takes 125840 cycles
-	// 512 point cmag takes 43764 cycles
-
 	//state = 0;
 	//outputflag = false;
 }
@@ -102,53 +82,30 @@ void AudioAnalyzeFFT256::update(void)
 	//window = NULL;
 	if (window) apply_window_to_fft_buffer(buffer, window);
 	arm_cfft_radix4_q15(&fft_inst, buffer);
-	// TODO: is this averaging correct?  G. Heinzel's paper says we're
-	// supposed to average the magnitude squared, then do the square
-	// root at the end after dividing by naverage.
-	arm_cmplx_mag_q15(buffer, buffer, 128);
+	// G. Heinzel's paper says we're supposed to average the magnitude
+	// squared, then do the square root at the end.
 	if (count == 0) {
 		for (int i=0; i < 128; i++) {
-			output[i] = buffer[i];
+			uint32_t tmp = *((uint32_t *)buffer + i);
+			uint32_t magsq = multiply_16tx16t_add_16bx16b(tmp, tmp);
+			sum[i] = magsq / naverage;
 		}
 	} else {
 		for (int i=0; i < 128; i++) {
-			output[i] += buffer[i];
+			uint32_t tmp = *((uint32_t *)buffer + i);
+			uint32_t magsq = multiply_16tx16t_add_16bx16b(tmp, tmp);
+			sum[i] += magsq / naverage;
 		}
 	}
 	if (++count == naverage) {
 		count = 0;
 		for (int i=0; i < 128; i++) {
-			output[i] /= naverage;
+			output[i] = sqrt_uint32_approx(sum[i]);
 		}
 		outputflag = true;
 	}
-
 	release(prevblock);
 	prevblock = block;
-
-#if 0
-	block = receiveReadOnly();
-	if (state == 0) {
-		//Serial.print("0");
-		if (block == NULL) return;
-		copy_to_fft_buffer(buffer, block->data);
-		state = 1;
-	} else if (state == 1) {
-		//Serial.print("1");
-		if (block == NULL) return;
-		copy_to_fft_buffer(buffer+256, block->data);
-		arm_cfft_radix4_q15(&fft_inst, buffer);
-		state = 2;
-	} else {
-		//Serial.print("2");
-		arm_cmplx_mag_q15(buffer, output, 128);
-		outputflag = true;
-		if (block == NULL) return;
-		copy_to_fft_buffer(buffer, block->data);
-		state = 1;
-	}
-	release(block);
-#endif
 }
 
 

analyze_fft256.h

@@ -60,12 +60,13 @@ public:
 	}
 	virtual void update(void);
 	//uint32_t cycles;
-	int32_t output[128] __attribute__ ((aligned (4)));
+	uint16_t output[128] __attribute__ ((aligned (4)));
 private:
 	void init(void);
 	const int16_t *window;
 	audio_block_t *prevblock;
 	int16_t buffer[512] __attribute__ ((aligned (4)));
+	uint32_t sum[128];
 	uint8_t count;
 	uint8_t naverage;
 	bool outputflag;