Skip to content

Commit 146a44c

Browse files
GadgetoidGadgetoid
committed
Add FFT
1 parent 5793b05 commit 146a44c

File tree

4 files changed

+213
-0
lines changed

4 files changed

+213
-0
lines changed

‎CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@ add_executable(${NAME}
1818
${CMAKE_CURRENT_LIST_DIR}/picow_bt_example_background.cpp # int main()
1919
${CMAKE_CURRENT_LIST_DIR}/a2dp_sink_demo.cpp
2020
${CMAKE_CURRENT_LIST_DIR}/btstack_audio_pico.cpp
21+
${CMAKE_CURRENT_LIST_DIR}/fixed_fft.cpp
2122
)
2223

2324
target_link_libraries(${NAME}

‎btstack_audio_pico.cpp‎

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -57,11 +57,13 @@
5757
#include "pico/stdlib.h"
5858

5959
#include "galactic_unicorn.hpp"
60+
#include "fixed_fft.hpp"
6061

6162
#define DRIVER_POLL_INTERVAL_MS 5
6263
#define SAMPLES_PER_BUFFER 512
6364

6465
pimoroni::GalacticUnicorn galactic;
66+
FIX_FFT fft;
6567

6668
// client
6769
static void (*playback_callback)(int16_t * buffer, uint16_t num_samples);
@@ -138,6 +140,26 @@ static void btstack_audio_pico_sink_fill_buffers(void){
138140
}
139141
}
140142

143+
for (auto i = 0u; i < SAMPLES_PER_BUFFER; i++) {
144+
fft.sample_array[i] = buffer16[i];
145+
}
146+
fft.update();
147+
for (auto i = 0u; i < galactic.WIDTH; i++) {
148+
uint16_t sample = std::min((int16_t)2800, (int16_t)fft.get_scaled(i + 2, 1));
149+
sample = std::max((uint16_t)0, sample);
150+
for (auto y = 0; y < 11; y++) {
151+
uint8_t r = std::min((uint16_t)255, sample);
152+
uint8_t b = r;
153+
galactic.set_pixel(i, galactic.HEIGHT - 1 - y, r, 0, b >> 4);
154+
155+
if(sample >= 255) {
156+
sample -= 255;
157+
} else {
158+
sample = 0;
159+
}
160+
}
161+
}
162+
141163
audio_buffer->sample_count = audio_buffer->max_sample_count;
142164
give_audio_buffer(btstack_audio_pico_audio_buffer_pool, audio_buffer);
143165
}

‎fixed_fft.cpp‎

Lines changed: 132 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,132 @@
1+
/**
2+
* Hunter Adams ([email protected])
3+
* Reproduced and modified with explicit permission
4+
*
5+
* Original code in action:
6+
* https://www.youtube.com/watch?v=8aibPy4yzCk
7+
*
8+
*/
9+
#include "fixed_fft.hpp"
10+
#include <algorithm>
11+
12+
// Adapted from https://github.com/raspberrypi/pico-sdk/blob/master/src/host/pico_bit_ops/bit_ops.c
13+
uint16_t __always_inline __revs(uint16_t v) {
14+
v = ((v & 0x5555u) << 1u) | ((v >> 1u) & 0x5555u);
15+
v = ((v & 0x3333u) << 2u) | ((v >> 2u) & 0x3333u);
16+
v = ((v & 0x0f0fu) << 4u) | ((v >> 4u) & 0x0f0fu);
17+
return ((v >> 8u) & 0x00ffu) | ((v & 0x00ffu) << 8u);
18+
}
19+
20+
FIX_FFT::~FIX_FFT() {
21+
}
22+
23+
int FIX_FFT::get_scaled(unsigned int i, unsigned int scale) {
24+
return fix15_to_int(multiply_fix15(fr[i], int_to_fix15(scale)));
25+
}
26+
27+
void FIX_FFT::init() {
28+
29+
// Populate Filter and Sine tables
30+
for (auto ii = 0u; ii < SAMPLE_COUNT; ii++) {
31+
// Full sine wave with period NUM_SAMPLES
32+
// Wolfram Alpha: Plot[(sin(2 * pi * (x / 1.0))), {x, 0, 1}]
33+
sine_table[ii] = float_to_fix15(0.5f * sin((M_PI * 2.0f) * ((float) ii) / (float)SAMPLE_COUNT));
34+
35+
// This is a crude approximation of a Lanczos window.
36+
// Wolfram Alpha Comparison: Plot[0.5 * (1.0 - cos(2 * pi * (x / 1.0))), {x, 0, 1}], Plot[LanczosWindow[x - 0.5], {x, 0, 1}]
37+
filter_window[ii] = float_to_fix15(0.5f * (1.0f - cos((M_PI * 2.0f) * ((float) ii) / ((float)SAMPLE_COUNT))));
38+
}
39+
}
40+
41+
void FIX_FFT::update() {
42+
float max_freq = 0;
43+
44+
// Copy/window elements into a fixed-point array
45+
for (auto i = 0u; i < SAMPLE_COUNT; i++) {
46+
fr[i] = multiply_fix15(int_to_fix15((int)sample_array[i]), filter_window[i]);
47+
fi[i] = (fix15)0;
48+
}
49+
50+
// Compute the FFT
51+
FFT();
52+
53+
// Find the magnitudes
54+
for (auto i = 0u; i < (SAMPLE_COUNT / 2u); i++) {
55+
// get the approx magnitude
56+
fr[i] = abs(fr[i]); //>>9
57+
fi[i] = abs(fi[i]);
58+
// reuse fr to hold magnitude
59+
fr[i] = std::max(fr[i], fi[i]) +
60+
multiply_fix15(std::min(fr[i], fi[i]), float_to_fix15(0.4f));
61+
62+
// Keep track of maximum
63+
if (fr[i] > max_freq && i >= 5u) {
64+
max_freq = FIX_FFT::fr[i];
65+
max_freq_dex = i;
66+
}
67+
}
68+
}
69+
70+
float FIX_FFT::max_frequency() {
71+
return max_freq_dex * (sample_rate / SAMPLE_COUNT);
72+
}
73+
74+
void FIX_FFT::FFT() {
75+
// Bit Reversal Permutation
76+
// Bit reversal code below originally based on that found here:
77+
// https://graphics.stanford.edu/~seander/bithacks.html#BitReverseObvious
78+
// https://en.wikipedia.org/wiki/Bit-reversal_permutation
79+
// Detail here: https://vanhunteradams.com/FFT/FFT.html#Single-point-transforms-(reordering)
80+
//
81+
// PH: Converted to stdlib functions and __revs so it doesn't hurt my eyes
82+
for (auto m = 1u; m < SAMPLE_COUNT - 1u; m++) {
83+
unsigned int mr = __revs(m) >> shift_amount;
84+
// don't swap that which has already been swapped
85+
if (mr <= m) continue;
86+
// swap the bit-reveresed indices
87+
std::swap(fr[m], fr[mr]);
88+
std::swap(fi[m], fi[mr]);
89+
}
90+
91+
// Danielson-Lanczos
92+
// Adapted from code by:
93+
// Tom Roberts 11/8/89 and Malcolm Slaney 12/15/94 [email protected]
94+
// Detail here: https://vanhunteradams.com/FFT/FFT.html#Two-point-transforms
95+
// Length of the FFT's being combined (starts at 1)
96+
//
97+
// PH: Moved variable declarations to first-use so types are visually explicit.
98+
// PH: Removed div 2 on sine table values, have computed the sine table pre-divided.
99+
unsigned int L = 1;
100+
int k = log2_samples - 1;
101+
102+
// While the length of the FFT's being combined is less than the number of gathered samples
103+
while (L < SAMPLE_COUNT) {
104+
// Determine the length of the FFT which will result from combining two FFT's
105+
int istep = L << 1;
106+
// For each element in the FFT's that are being combined
107+
for (auto m = 0u; m < L; ++m) {
108+
// Lookup the trig values for that element
109+
int j = m << k; // index into sine_table
110+
fix15 wr = sine_table[j + SAMPLE_COUNT / 4];
111+
fix15 wi = -sine_table[j];
112+
// i gets the index of one of the FFT elements being combined
113+
for (auto i = m; i < SAMPLE_COUNT; i += istep) {
114+
// j gets the index of the FFT element being combined with i
115+
int j = i + L;
116+
// compute the trig terms (bottom half of the above matrix)
117+
fix15 tr = multiply_fix15(wr, fr[j]) - multiply_fix15(wi, fi[j]);
118+
fix15 ti = multiply_fix15(wr, fi[j]) + multiply_fix15(wi, fr[j]);
119+
// divide ith index elements by two (top half of above matrix)
120+
fix15 qr = fr[i] >> 1;
121+
fix15 qi = fi[i] >> 1;
122+
// compute the new values at each index
123+
fr[j] = qr - tr;
124+
fi[j] = qi - ti;
125+
fr[i] = qr + tr;
126+
fi[i] = qi + ti;
127+
}
128+
}
129+
--k;
130+
L = istep;
131+
}
132+
}

‎fixed_fft.hpp‎

Lines changed: 58 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,58 @@
1+
#include <stdio.h>
2+
#include <stdlib.h>
3+
#include <math.h>
4+
#include <cstring>
5+
6+
#include "pico/stdlib.h"
7+
8+
typedef signed int fix15;
9+
10+
// Helpers for 16.15 fixed-point arithmetic
11+
constexpr __always_inline fix15 multiply_fix15(fix15 a, fix15 b) {return (fix15)(((signed long long)(a) * (signed long long)(b)) >> 15);}
12+
constexpr __always_inline fix15 float_to_fix15(float a) {return (fix15)(a * 32768.0f);}
13+
constexpr __always_inline float fix15_to_float(fix15 a) {return (float)(a) / 32768.0f;}
14+
constexpr __always_inline fix15 int_to_fix15(int a) {return (fix15)(a << 15);}
15+
constexpr __always_inline int fix15_to_int(fix15 a) {return (int)(a >> 15);}
16+
17+
constexpr unsigned int SAMPLE_COUNT = 512u;
18+
19+
class FIX_FFT {
20+
private:
21+
float sample_rate;
22+
23+
unsigned int log2_samples;
24+
unsigned int shift_amount;
25+
26+
// Lookup tables
27+
fix15 sine_table[SAMPLE_COUNT]; // a table of sines for the FFT
28+
fix15 filter_window[SAMPLE_COUNT]; // a table of window values for the FFT
29+
30+
// And here's where we'll copy those samples for FFT calculation
31+
fix15 fr[SAMPLE_COUNT];
32+
fix15 fi[SAMPLE_COUNT];
33+
34+
int max_freq_dex = 0;
35+
36+
void FFT();
37+
void init();
38+
public:
39+
uint16_t sample_array[SAMPLE_COUNT];
40+
41+
FIX_FFT() : FIX_FFT(44100.0f) {};
42+
FIX_FFT(float sample_rate) : sample_rate(sample_rate) {
43+
log2_samples = log2(SAMPLE_COUNT);
44+
shift_amount = 16u - log2_samples;
45+
46+
memset(sample_array, 0, SAMPLE_COUNT);
47+
48+
memset(fr, 0, SAMPLE_COUNT * sizeof(fix15));
49+
memset(fi, 0, SAMPLE_COUNT * sizeof(fix15));
50+
51+
init();
52+
};
53+
~FIX_FFT();
54+
55+
void update();
56+
float max_frequency();
57+
int get_scaled(unsigned int i, unsigned int scale);
58+
};

0 commit comments

Comments
 (0)