From b9489b076228cf262f12eba856f91a6da2eededf Mon Sep 17 00:00:00 2001
From: Alexis Murzeau <amubtdx@gmail.com>
Date: Sat, 13 Apr 2024 18:44:59 +0200
Subject: [PATCH] damc: fix equalizer at low frequencies

Using +0.5 to avoid denormal was reducing the accuracy of float and low frequency EQ  (< 500Hz) was not working at all.

Use flush-to-zero instead.

Also use direct form 1 for biquad for a slight performance boost.
---
 Core/Src/main.c                   |  2 ++
 damc/damc_common/BiquadFilter.cpp | 27 +++++++++++++--------------
 damc/damc_common/BiquadFilter.h   |  8 +++++---
 3 files changed, 20 insertions(+), 17 deletions(-)

diff --git a/Core/Src/main.c b/Core/Src/main.c
index 5c3ee44..de9e562 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -130,6 +130,8 @@ int main(void)
 
   // memtester_stm32((void*)0x60000000, 512*1024, 10);
 
+  // Enable Flush-To-Zero to avoid denormals
+  FPU->FPDSCR |= FPU_FPDSCR_FZ_Msk;
 
   DAMC_init();
 
diff --git a/damc/damc_common/BiquadFilter.cpp b/damc/damc_common/BiquadFilter.cpp
index aaff39f..58c44ea 100644
--- a/damc/damc_common/BiquadFilter.cpp
+++ b/damc/damc_common/BiquadFilter.cpp
@@ -19,9 +19,14 @@ float BiquadFilter::put(float input) {
 	const float* const a = a_coefs;
 	const float* const b = b_coefs;
 
-	float y = b[0] * input + s1 - 0.5;
-	s1 = s2 + b[1] * input - a[0] * y;
-	s2 = b[2] * input - a[1] * y + 0.5;
+	// Direct form 1 implementation, leads to better performance than Transposed direct form 2. (18% cpu in audio
+	// processing vs 20% for 10 EqFilters (percentages without other processing))
+	float y = b[0] * input + b[1] * input1 + b[2] * input2 - a[0] * output1 - a[1] * output2;
+
+	input2 = input1;
+	input1 = input;
+	output2 = output1;
+	output1 = y;
 
 	return y;
 }
@@ -37,14 +42,8 @@ std::complex<float> BiquadFilter::getResponse(float f0, float fs) {
 	return (b[0] + b[1] * z_1 + b[2] * z_2) / (std::complex<float>(1) + a[0] * z_1 + a[1] * z_2);
 }
 
-void BiquadFilter::computeFilter(bool enabled,
-                                 FilterType filterType,
-                                 float f0,
-                                 float fs,
-                                 float gain,
-                                 float Q,
-                                 float a_coefs[3],
-                                 float b_coefs[3]) {
+void BiquadFilter::computeFilter(
+    bool enabled, FilterType filterType, float f0, float fs, float gain, float Q, float a_coefs[3], float b_coefs[3]) {
 	if(!enabled || Q == 0 || fs == 0) {
 		b_coefs[0] = 1;
 		b_coefs[1] = 0;
@@ -53,9 +52,9 @@ void BiquadFilter::computeFilter(bool enabled,
 		a_coefs[1] = 0;
 		a_coefs[2] = 0;
 	} else {
-		float A = pow(10, gain / 40);
-		float w0 = 2 * M_PI * f0 / fs;
-		float alpha = sin(w0) / (2 * Q);
+		double A = pow(10.0, gain / 40.0);
+		double w0 = 2.0 * M_PI * f0 / fs;
+		double alpha = sin(w0) / (2.0 * Q);
 
 		switch(filterType) {
 			default:
diff --git a/damc/damc_common/BiquadFilter.h b/damc/damc_common/BiquadFilter.h
index 3a901d8..3890e5d 100644
--- a/damc/damc_common/BiquadFilter.h
+++ b/damc/damc_common/BiquadFilter.h
@@ -34,9 +34,11 @@ class BiquadFilter {
 	std::complex<float> getResponse(float f0, float fs);
 
 private:
-	// Use offset of 0.5 to avoid denormals
-	float s1 = 0.5;
-	float s2 = 0.5;
 	float b_coefs[3] = {};
 	float a_coefs[2] = {};
+
+	float input1 = 0;
+	float input2 = 0;
+	float output1 = 0;
+	float output2 = 0;
 };