Tinker with Goertzelmaster

1 files changed, 31 insertions, 14 deletions

diff --git a/mictoled/mictoled.v b/mictoled/mictoled.v
index 0f14d71..eb41ca7 100644
--- a/mictoled/mictoled.v
+++ b/mictoled/mictoled.v
@@ -42,7 +42,7 @@ module mictoled
     assign mic_clk = clkdiv >= 12;
 
     /* CIC decimator by 64 => 2MHz -> 31.25 kHz */
-    reg [INTEGRATOR_WIDTH-1:0] integrator = 0;
+    reg signed [INTEGRATOR_WIDTH-1:0] integrator = 0;
     reg [5:0] decim_strobe_counter = 8'b0;
     always @ (posedge clk) begin
         if (mic_strobe) begin
@@ -57,8 +57,8 @@ module mictoled
 
     /* Comb stage after decimation */
     wire decim_strobe = decim_strobe_counter == 0 && mic_strobe;
-    reg [INTEGRATOR_WIDTH-1:0] previous_integrator = 0;
-    reg [COMB_WIDTH-1:0] comb_out = 0; // can be reduced_width
+    reg signed [INTEGRATOR_WIDTH-1:0] previous_integrator = 0;
+    reg signed [COMB_WIDTH-1:0] comb_out = 0; // can be reduced_width
     always @ (posedge clk) begin
         if (decim_strobe) begin
             comb_out = integrator - previous_integrator;
@@ -70,27 +70,44 @@ module mictoled
     // coef = 2 * cos(2 * PI * freq / sample_rate);
     // 440 Hz  -> 1.992178649
     // 1200 Hz -> 1.942068556
-    // Represent in fixed point in Q4.10, i.e. a scaling factor of 1024
-    //integer coef_440 = 2040; // 439.75 Hz
-    real coef_440 = 1.992178649;
+    // Represent in fixed point in Q.8, i.e. a scaling factor of 256
+    reg signed [31:0] coef_440 = 510; // 439.75 Hz
+    reg signed [31:0] q1_440 = 0;
+    reg signed [31:0] q2_440 = 0;
 
-    real q1_440 = 0;
-    real q2_440 = 0;
-    reg [9:0] goertzel_strobe_counter = 0;
+    real coef_440_f = 1.992178649;
+    real q1_440_f = 0;
+    real q2_440_f = 0;
 
-    real m_squared_440 = 0;
+    reg [7:0] goertzel_strobe_counter = 0;
+
+    real m_squared_440_f = 0;
+    reg signed [63:0] m_squared_440 = 0;
+
+    wire signed [31:0] comb_wide = comb_out << 8;
 
     always @ (posedge clk) begin
         if (decim_strobe) begin
-            goertzel_strobe_counter <= goertzel_strobe_counter + 1; // overflow at 1024
+            goertzel_strobe_counter <= goertzel_strobe_counter + 1; // overflow at 256
 
-            if (goertzel_strobe_counter % 256 == 0) begin
-                m_squared_440 <= q1_440 * q1_440 + q2_440 * q2_440 - coef_440 * q1_440 * q2_440;
+            if (goertzel_strobe_counter == 0) begin
+                m_squared_440_f <= q1_440_f * q1_440_f +
+                    q2_440_f * q2_440_f -
+                    coef_440_f * q1_440_f * q2_440_f;
+                q1_440_f <= 0;
+                q2_440_f <= 0;
+
+                m_squared_440 <= ((q1_440 * q1_440) >>> 8) +
+                    ((q2_440 * q2_440) >>> 8) -
+                    ((coef_440 * ((q1_440 * q2_440) >>> 8)) >>> 8);
                 q1_440 <= 0;
                 q2_440 <= 0;
             end
             else begin
-                q1_440 <= coef_440 * q1_440 - q2_440 + comb_out;
+                q1_440_f <= coef_440_f * q1_440_f - q2_440_f + $itor(comb_out);
+                q2_440_f <= q1_440_f;
+
+                q1_440 <= ((coef_440 * q1_440) >>> 8) - q2_440 + comb_wide;
                 q2_440 <= q1_440;
             end
         end