diff --git a/firmware/include/biquad.h b/firmware/include/biquad.h index b698234..65590ec 100644 --- a/firmware/include/biquad.h +++ b/firmware/include/biquad.h @@ -1,7 +1,8 @@ #pragma once -template -struct Biquad { +template +struct Biquad +{ T b0; T b1; T b2; @@ -13,16 +14,18 @@ struct Biquad { T yn2; Biquad() = default; - Biquad(T a1, T a2, T b0, T b1, T b2) : b0(b0), b1(b1), b2(b2), a1(a1), a2(a2) {}; - Biquad(T a0, T a1, T a2, T b0, T b1, T b2) { - this->a1 = a1/a0; - this->a2 = a2/a0; - this->b0 = b0/a0; - this->b1 = b1/a0; - this->b2 = b2/a0; + Biquad(T a1, T a2, T b0, T b1, T b2) : b0(b0), b1(b1), b2(b2), a1(a1), a2(a2){}; + Biquad(T a0, T a1, T a2, T b0, T b1, T b2) + { + this->a1 = a1 / a0; + this->a2 = a2 / a0; + this->b0 = b0 / a0; + this->b1 = b1 / a0; + this->b2 = b2 / a0; } - T update(T x) { + T update(T x) + { T y = this->b0 * x + this->b1 * this->xn1 + this->b2 * this->xn2 - this->yn1 * this->a1 - this->yn2 * this->a2; this->xn2 = this->xn1; diff --git a/firmware/include/dc_cancelation.h b/firmware/include/dc_cancelation.h index 36efdef..c86caf4 100644 --- a/firmware/include/dc_cancelation.h +++ b/firmware/include/dc_cancelation.h @@ -1,15 +1,17 @@ #pragma once -template -struct DcCancelation { +template +struct DcCancelation +{ T x_n1; T y_n1; T R; - DcCancelation(T R) : R(R) {}; + DcCancelation(T R) : R(R){}; - T update(T x) { - T y = x-this->x_n1 + this->R * this->y_n1; + T update(T x) + { + T y = x - this->x_n1 + this->R * this->y_n1; this->x_n1 = x; this->y_n1 = y; diff --git a/firmware/include/pt1.h b/firmware/include/pt1.h index 4d08d54..db99f02 100644 --- a/firmware/include/pt1.h +++ b/firmware/include/pt1.h @@ -1,13 +1,14 @@ #pragma once -template -struct Pt1 { +template +struct Pt1 +{ T y_n1; T K; T T1; /* PT1: y = y_(n-1) + (Ku - y_(n-1)) * dt/T1 */ - Pt1(T K, T T1) : T1(T1), K(K) {}; + Pt1(T K, T T1) : T1(T1), K(K){}; T update(T u, T dt) { diff --git a/firmware/src/beat_detect.cpp b/firmware/src/beat_detect.cpp index 453094f..76de9bd 100644 --- a/firmware/src/beat_detect.cpp +++ b/firmware/src/beat_detect.cpp @@ -1,16 +1,19 @@ -#include "zauberstab.h" #include + #include "biquad.h" #include "pt1.h" +#include "zauberstab.h" #undef NUM_LEDS #define NUM_LEDS 45 -#define SAMPLING_FREQUENCY_BP 40 // number of energy chunks per second -#define SAMPLING_FREQUENCY_CONTROL 1 // check number of times per second if the current band pass is the best one -#define Q 20. // quality factor of band pass filters +#define SAMPLING_FREQUENCY_BP 40 // number of energy chunks per second +#define SAMPLING_FREQUENCY_CONTROL \ + 1 // check number of times per second if the current band pass is the best + // one +#define Q 20. // quality factor of band pass filters #define PI 3.1415926535897932384626433832795 -#define n_BP 30 //number of band pass filters +#define n_BP 30 // number of band pass filters static unsigned long sampling_period_bp = 1000000L / SAMPLING_FREQUENCY_BP; static unsigned long sampling_period_control = 1000000L / SAMPLING_FREQUENCY_CONTROL; @@ -34,7 +37,7 @@ static float y_fil[n_BP]; static float angle; static float angle2; -//static double energy_fil = 800.; +// static double energy_fil = 800.; static float pos_target = NUM_LEDS / 2; static float pos_target_filtered = NUM_LEDS / 2; @@ -45,7 +48,8 @@ static int candidate = 15; static int rounds = 0; static int n_samples = 0; -static int get_value(int pos, float pos0) +static int +get_value(int pos, float pos0) { if (abs(pos0 - pos) > 5) { @@ -57,7 +61,8 @@ static int get_value(int pos, float pos0) } } -static void set_filter() +static void +set_filter() { for (int i = 0; i < n_BP; i++) { @@ -81,8 +86,8 @@ void setup() Serial.begin(250000); set_filter(); initial_time = micros(); - } + void loop() { float sample = get_sample(); @@ -93,7 +98,7 @@ void loop() { n_samples = 0; last_us_bp += sampling_period_bp; - //energy_fil += (energy - energy_fil) * 0.01; + // energy_fil += (energy - energy_fil) * 0.01; for (int i = 0; i < n_BP; i++) { @@ -104,12 +109,14 @@ void loop() yy3[i] = yy2[i]; yy2[i] = yy1[i]; yy1[i] = y[i]; - y_fil[i] = y_filter.update(std::abs(y[i]), 0.005f); //linie der scheitelpunkte - //y_fil[i] += (abs(y[i]) - y_fil[i]) * 0.005; //linie der scheitelpunkte - + y_fil[i] = y_filter.update(std::abs(y[i]), + 0.005f); // linie der scheitelpunkte + // y_fil[i] += (abs(y[i]) - y_fil[i]) * 0.005; //linie der + // scheitelpunkte } - float delays = constrain(SAMPLING_FREQUENCY_BP * 0.25 / (1.75 + active * (2.4 - 1.75) / n_BP), 4., 6.); + float delays = constrain(SAMPLING_FREQUENCY_BP * 0.25 / (1.75 + active * (2.4 - 1.75) / n_BP), + 4., 6.); float delayed = 0; if (delays > 5) @@ -136,7 +143,8 @@ void loop() if (pos_target > pos_target_filtered) { - pos_target_filtered = pos_filter.update(pos_target, 0.35f); } + pos_target_filtered = pos_filter.update(pos_target, 0.35f); + } else { pos_filter.y_n1 = pos_target; @@ -148,12 +156,11 @@ void loop() for (int i = 0; i < NUM_LEDS; i++) { leds[i].g = get_value(i, pos_target_filtered); - leds[i].r = get_value(i, pos_target_filtered+2); - leds[i].b = get_value(i, pos_target_filtered-2); + leds[i].r = get_value(i, pos_target_filtered + 2); + leds[i].b = get_value(i, pos_target_filtered - 2); - - //leds[i].setRGB(brightness_red, brightness_green, brightness_blue); - //leds[i].setHSV(160, (rounds == 6) ? 0xFF : 0, brightness); + // leds[i].setRGB(brightness_red, brightness_green, brightness_blue); + // leds[i].setHSV(160, (rounds == 6) ? 0xFF : 0, brightness); } FastLED.show(); } diff --git a/firmware/src/fft.cpp b/firmware/src/fft.cpp index 35dd1de..15e0d83 100644 --- a/firmware/src/fft.cpp +++ b/firmware/src/fft.cpp @@ -17,46 +17,54 @@ void fft(std::complex *samples, std::complex *output, uint32_t N) { uint8_t log2n = (uint8_t)std::log2(N) + 0.5f; std::complex I(0.0, 1.0); - if (N == 1) { + if (N == 1) + { output[0] = samples[0]; return; } - //shuffle array - for (int i = 0; i < N; i++) { + // shuffle array + for (int i = 0; i < N; i++) + { output[i] = samples[bitReverse(i, log2n)]; } - for(int s = 1; s <= log2n; s++) { - uint32_t m = 1 << s; // 2^s - std::complex wm = std::exp(-2.0f*(float)M_PI*I/(std::complex)m); - for (int k = 0; k < N; k += m) { + for (int s = 1; s <= log2n; s++) + { + uint32_t m = 1 << s; // 2^s + std::complex wm = std::exp(-2.0f * (float)M_PI * I / (std::complex)m); + for (int k = 0; k < N; k += m) + { std::complex w = 1.f; - for (int j = 0; j < m/2; j++) { - std::complex t = w * output[k+j+m/2]; - std::complex u = output[k+j]; - output[k+j] = u+t; - output[k+j+m/2] = u-t; - w = w*wm; + for (int j = 0; j < m / 2; j++) + { + std::complex t = w * output[k + j + m / 2]; + std::complex u = output[k + j]; + output[k + j] = u + t; + output[k + j + m / 2] = u - t; + w = w * wm; } } } } -void rfft(std::complex *input, std::complex *output, uint32_t N){ +void rfft(std::complex *input, std::complex *output, uint32_t N) +{ std::complex I(0.0, 1.0); - for(int i = 0; i< N/2; i++){ - input[i] = input[i] + I * input[i + N/2]; + for (int i = 0; i < N / 2; i++) + { + input[i] = input[i] + I * input[i + N / 2]; } - fft(input, output, N/2); - - for(int i = 0; i < N/2; i++){ - output[i] = (output[i] + std::conj(output[(N/2) - i]))/2.f; + fft(input, output, N / 2); + + for (int i = 0; i < N / 2; i++) + { + output[i] = (output[i] + std::conj(output[(N / 2) - i])) / 2.f; } - for(int i = N/2; i < N; i++) { - output[i] = -I * (output[i] - std::conj(output[(N/2) - i]))/2.f; + for (int i = N / 2; i < N; i++) + { + output[i] = -I * (output[i] - std::conj(output[(N / 2) - i])) / 2.f; } - } \ No newline at end of file diff --git a/firmware/src/zauberstab.cpp b/firmware/src/zauberstab.cpp index c4b2936..51a3489 100644 --- a/firmware/src/zauberstab.cpp +++ b/firmware/src/zauberstab.cpp @@ -5,17 +5,20 @@ DcCancelation dc_blocker{0.95}; CRGB leds[NUM_LEDS]; static int16_t mic_offset = 0; -static uint16_t read_mic() { +static uint16_t read_mic() +{ return analogRead(MIC_PIN); } -int zauberstab_init() { +int zauberstab_init() +{ FastLED.addLeds(leds, NUM_LEDS); - //FastLED.setMaxPowerInVoltsAndMilliamps(5, 300); + // FastLED.setMaxPowerInVoltsAndMilliamps(5, 300); return 0; } -float get_sample() { +float get_sample() +{ float sample = read_mic(); sample = dc_blocker.update(sample); return sample; diff --git a/firmware/temp/main.cpp b/firmware/temp/main.cpp index 520efd6..4ea6ade 100644 --- a/firmware/temp/main.cpp +++ b/firmware/temp/main.cpp @@ -1,9 +1,10 @@ #include "zauberstab.h" -void setup() { +void setup() +{ zauberstab_init(); } -void loop() { - +void loop() +{ } \ No newline at end of file diff --git a/firmware/temp/vumeter.cpp b/firmware/temp/vumeter.cpp index 09bc8f9..6c95c0d 100644 --- a/firmware/temp/vumeter.cpp +++ b/firmware/temp/vumeter.cpp @@ -1,9 +1,9 @@ -#include "zauberstab.h" #include "pt1.h" +#include "zauberstab.h" unsigned long last_sample_time; static int sample_counter = 0; -unsigned int top_led_pos = 0; +unsigned int top_led_pos = 0; float rms_avg = 0; float vu_filt = 0.0f; float vu_filt_slow = 0.0f; @@ -19,37 +19,42 @@ void setup() FastLED.setBrightness(100); } -void loop() { - if (micros()-last_sample_time >= 500){ +void loop() +{ + if (micros() - last_sample_time >= 500) + { last_sample_time = micros(); int32_t sample = get_sample(); - float in = sample*sample; + float in = sample * sample; sample_counter++; - rms_avg += (in - rms_avg)/(sample_counter + 1); + rms_avg += (in - rms_avg) / (sample_counter + 1); } - - EVERY_N_MILLIS(10){ + + EVERY_N_MILLIS(10) + { float vu = 20 * log10f(rms_avg); vu_filt = vu_pt1_fast.update(vu, 0.01f); vu_filt_slow = vu_pt1_slow.update(vu_filt, 0.01f); - //Serial.println(vu); + // Serial.println(vu); int max_led = vu_filt; int top_led = vu_filt_slow; max_led = max_led > 0xFF ? 0xFF : max_led; - if (top_led < max_led){ + if (top_led < max_led) + { vu_pt1_slow.y_n1 = vu_filt; top_led = max_led; } fill_solid(leds, NUM_LEDS, CRGB::Black); - for(int i = 0; i < max_led; i++) { + for (int i = 0; i < max_led; i++) + { int idx = map(i, 0, NUM_LEDS, 0, 0xFF); leds[i] = ColorFromPalette(RainbowColors_p, idx); } - + leds[top_led] = CRGB::White; FastLED.show();