From c2549adcabed2fb0b97f405c494e6305fd5f54c8 Mon Sep 17 00:00:00 2001
From: Holger Wirtz <wirtz@parasitstudio.de>
Date: Thu, 15 Aug 2019 13:49:45 +0200
Subject: [PATCH] Added arm_biquad LP filter for modulation.

---
 config.h                   |  1 -
 effect_modulated_delay.cpp | 44 +++++++++++++++-----------------------
 effect_modulated_delay.h   | 13 +++++------
 3 files changed, 22 insertions(+), 36 deletions(-)

diff --git a/config.h b/config.h
index 227502a..975d19b 100644
--- a/config.h
+++ b/config.h
@@ -60,7 +60,6 @@
 //#define CHORUS_MODULATOR_FILTER_FRQ 10 // see https://www.earlevel.com/main/2013/10/13/biquad-calculator-v2/
 //#define CHORUS_MODULATOR_FILTER_Q 0.7
 #define CHORUS_MODULATOR_BIQUAD 1
-//#define CHORUS_OUTPUT_BIQUAD 1
 //*************************************************************************************************
 //* DEBUG OUTPUT SETTINGS
 //*************************************************************************************************
diff --git a/effect_modulated_delay.cpp b/effect_modulated_delay.cpp
index 5e9861f..c4025c5 100644
--- a/effect_modulated_delay.cpp
+++ b/effect_modulated_delay.cpp
@@ -59,20 +59,15 @@ boolean AudioEffectModulatedDelay::begin(short *delayline, int d_length)
   _delay_length = d_length;
 
 #ifdef CHORUS_MODULATOR_BIQUAD
-  mod_lp_coeffs[0] = -1.98982427; // https://arachnoid.com/BiQuadDesigner/ Lopass Fc=50Hz, Q=0.7
-  mod_lp_coeffs[1] = 0.98987476;
-  mod_lp_coeffs[2] = 1.26228228e-5;
-  mod_lp_coeffs[3] = 2.52456456e-5;
-  mod_lp_coeffs[4] = 1.26228228e-5;
-  biquad_mod = {1, mod_lp_state, mod_lp_coeffs};
-#endif
-#ifdef CHORUS_OUTUT_BIQUAD // https://web.fhnw.ch/technik/projekte/eit/Fruehling2016/MuelZum/html/parametric__equalizer__example_8c_source.html
-  out_lp_coeffs[0] = -0.10987036; // https://arachnoid.com/BiQuadDesigner/ Lopass Fc=10000Hz, Q=0.3
-  out_lp_coeffs[1] = -0.24497694;
-  out_lp_coeffs[2] = 0.16128817;
-  out_lp_coeffs[3] = 0.32257635;
-  out_lp_coeffs[4] = 0.16128817;
-  biquad_out = {1, out_lp_state, out_lp_coeffs, 0};
+  filter_lp_mod.numStages = 1;
+  filter_lp_mod.pState =  filter_lp_state;
+  filter_lp_mod.pCoeffs = filter_lp_coeffs;
+
+  filter_lp_coeffs[0] = 0.072959657268266670;
+  filter_lp_coeffs[1] = 0.072959657268266670;
+  filter_lp_coeffs[2] = 0.0;
+  filter_lp_coeffs[3] = 0.854080685463466605;
+  filter_lp_coeffs[4] = 0.0;
 #endif
   return (true);
 }
@@ -95,13 +90,12 @@ void AudioEffectModulatedDelay::update(void)
     float mod_idx;
     float mod_number;
     float mod_fraction;
-    float mod_float[AUDIO_BLOCK_SAMPLES];
-
+    
     bp = block->data;
-    arm_q15_to_float(modulation->data, mod_float, AUDIO_BLOCK_SAMPLES);
-    mp = mod_float;
+    arm_q15_to_float(modulation->data, modulation_f32, AUDIO_BLOCK_SAMPLES);
+    mp = modulation_f32;
 #ifdef CHORUS_MODULATOR_BIQUAD
-    arm_biquad_cascade_df1_f32(&biquad_mod, mod_float, mod_float, AUDIO_BLOCK_SAMPLES);
+    arm_biquad_cascade_df1_f32(&filter_lp_mod, modulation_f32, modulation_f32, AUDIO_BLOCK_SAMPLES);
 #endif
 
     for (uint16_t i = 0; i < AUDIO_BLOCK_SAMPLES; i++)
@@ -133,14 +127,13 @@ void AudioEffectModulatedDelay::update(void)
       }
       else
       {
-        idx[0] = c_mod_idx + 1;
-        idx[1] = c_mod_idx;
+        idx[0] = c_mod_idx;
+        idx[1] = c_mod_idx + 1;
       }
       if (mod_idx < 0.0)
-        *bp = round(float(_delayline[idx[0]]) * (mod_fraction) + float(_delayline[idx[1]]) * (1.0 - mod_fraction));
-      else
         *bp = round(float(_delayline[idx[0]]) * (1.0 - mod_fraction) + float(_delayline[idx[1]]) * mod_fraction);
-
+      else
+        *bp = round(float(_delayline[idx[0]]) * mod_fraction + float(_delayline[idx[1]]) * (1.0 - mod_fraction));
 
       // push the pointers forward
       bp++; // next audio data
@@ -154,9 +147,6 @@ void AudioEffectModulatedDelay::update(void)
 
   if (block)
   {
-#ifdef CHORUS_OUTPUT_BIQUAD
-    arm_biquad_cascade_df1_fast_q15(&biquad_out, (q15_t*)block, (q15_t*)block, AUDIO_BLOCK_SAMPLES);
-#endif
     transmit(block, 0);
     release(block);
   }
diff --git a/effect_modulated_delay.h b/effect_modulated_delay.h
index 4543990..03c243f 100644
--- a/effect_modulated_delay.h
+++ b/effect_modulated_delay.h
@@ -57,15 +57,12 @@ class AudioEffectModulatedDelay :
     int16_t c_mod_idx;
     uint16_t idx[2];
 #ifdef CHORUS_MODULATOR_BIQUAD
-    arm_biquad_casd_df1_inst_f32 biquad_mod;
-    float mod_lp_state[2];
-    float mod_lp_coeffs[5];
-#endif
-#ifdef CHORUS_OUTPUT_BIQUAD
-    arm_biquad_casd_df1_inst_q15 biquad_out;
-    float out_lp_state[2];
-    float out_lp_coeffs[5];
+    arm_biquad_casd_df1_inst_f32 filter_lp_mod;
+    float filter_lp_state[4];
+    float filter_lp_coeffs[5];
+    float modulation_f32[AUDIO_BLOCK_SAMPLES];
 #endif
+
 };
 
 #endif