From 872946fa9a9e150a4987bc99ad677a7c761bf7ce Mon Sep 17 00:00:00 2001
From: Thomas Schmid <tom@lfence.de>
Date: Sun, 19 Jun 2022 15:15:24 +0200
Subject: [PATCH] app beat_detect: initilalize state on every init call

Signed-off-by: Thomas Schmid <tom@lfence.de>
---
 firmware/include/biquad.h                 |  7 +++++++
 firmware/include/pt1.h                    |  4 ++++
 firmware/src/applications/beat_detect.cpp | 22 +++++++++++++++++-----
 3 files changed, 28 insertions(+), 5 deletions(-)

diff --git a/firmware/include/biquad.h b/firmware/include/biquad.h
index 65590ec..7d4ea33 100644
--- a/firmware/include/biquad.h
+++ b/firmware/include/biquad.h
@@ -36,4 +36,11 @@ struct Biquad
 
         return y;
     }
+
+    void reset() {
+        this->xn1 = 0.0;
+        this->xn2 = 0.0;
+        this->yn1 = 0.0;
+        this->yn1 = 0.0;
+    }
 };
\ No newline at end of file
diff --git a/firmware/include/pt1.h b/firmware/include/pt1.h
index db99f02..ec627d9 100644
--- a/firmware/include/pt1.h
+++ b/firmware/include/pt1.h
@@ -16,4 +16,8 @@ struct Pt1
         this->y_n1 = y;
         return y;
     }
+
+    void reset() {
+        this->y_n1 = 0.f;
+    }
 };
\ No newline at end of file
diff --git a/firmware/src/applications/beat_detect.cpp b/firmware/src/applications/beat_detect.cpp
index 4587887..096ab09 100644
--- a/firmware/src/applications/beat_detect.cpp
+++ b/firmware/src/applications/beat_detect.cpp
@@ -16,9 +16,9 @@
 #define PI 3.1415926535897932384626433832795
 #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;
-static double energy = 0;
+static const unsigned long sampling_period_bp = 1000000L / SAMPLING_FREQUENCY_BP;
+static const unsigned long sampling_period_control = 1000000L / SAMPLING_FREQUENCY_CONTROL;
+static float energy = 0;
 static unsigned long last_us_bp = 0L;
 static unsigned long last_us_control = 0L;
 
@@ -85,6 +85,18 @@ void BeatDetectApp::init()
 {
     set_filter();
     initial_time = micros();
+    pos_target = NUM_LEDS / 2;
+    pos_target_filtered = NUM_LEDS / 2;
+    active = 15;
+    candidate = 15;
+    rounds = 0;
+    n_samples = 0;
+
+    pos_filter.reset();
+
+    for (int i = 0; i<n_BP; i++){
+        bp_filters[i].reset();
+    }
 }
 
 void BeatDetectApp::deinit()
@@ -101,7 +113,7 @@ void BeatDetectApp::loop()
     if (micros() - last_us_bp > sampling_period_bp)
     {
         n_samples = 0;
-        last_us_bp += sampling_period_bp;
+        last_us_bp = micros();
         // energy_fil += (energy - energy_fil) * 0.01;
 
         for (int i = 0; i < n_BP; i++)
@@ -171,7 +183,7 @@ void BeatDetectApp::loop()
 
     if (micros() - last_us_control > sampling_period_control)
     {
-        last_us_control += sampling_period_control;
+        last_us_control = micros();
         int argmax = -1;
         float valuemax = 0;
         for (int i = 0; i < n_BP; i++)