From 354d8e09bbc1658a60c76b7a00b0035d27408580 Mon Sep 17 00:00:00 2001
From: George Norton <george_norton_uk@hotmail.com>
Date: Sat, 2 Sep 2023 00:56:10 +0100
Subject: [PATCH] Fix distortion.

---
 firmware/code/fix16.inl      | 12 +++++-------
 firmware/tools/filter_test.c |  6 ++++--
 2 files changed, 9 insertions(+), 9 deletions(-)

diff --git a/firmware/code/fix16.inl b/firmware/code/fix16.inl
index 75e2e8d..aefa911 100644
--- a/firmware/code/fix16.inl
+++ b/firmware/code/fix16.inl
@@ -56,22 +56,20 @@ static inline int32_t norm_fix3_28_to_s16sample(fix3_28_t a) {
     */
 
     // Saturate the value if an overflow has occurred
-    uint32_t upper = (a >> 30);
+    uint32_t upper = (a >> 29);
     if (a < 0) {
-        if (~upper)
-        {
+        if (~upper) {
             return 0xff800000;
         }
     } else {
-        if (upper)
-        {
-            return 0x007fffff;
+        if (upper) {
+            return 0x00efffff;
         }
     }
     /* When we converted the USB audio sample to a fixed point number, we applied
        a normalization, or a gain of 1/65536. To convert it back, we can undo that
        by shifting it but we output 24bts, so the shift is reduced. */
-    return (a >> 4);
+    return (a >> 5);
 }
 
 static inline fix3_28_t fix3_28_from_flt(float a) {
diff --git a/firmware/tools/filter_test.c b/firmware/tools/filter_test.c
index 689b308..a2dd190 100644
--- a/firmware/tools/filter_test.c
+++ b/firmware/tools/filter_test.c
@@ -54,10 +54,12 @@ int main(int argc, char* argv[])
         out[i] = in[i];
     }
 
+    const fix3_28_t preamp = fix3_28_from_flt(0.92f);
+
     for (int i = 0; i < samples; i ++)
     {
         // Left channel filter
-        fix3_28_t x_f16 = norm_fix3_28_from_s16sample((int16_t) out[i]);
+        fix3_28_t x_f16 = fix16_mul(norm_fix3_28_from_s16sample((int16_t) out[i]), preamp);
 
         for (int j = 0; j < filter_stages; j++)
         {
@@ -69,7 +71,7 @@ int main(int argc, char* argv[])
 
         // Right channel filter
         i++;
-        x_f16 = norm_fix3_28_from_s16sample((int16_t) out[i]);
+        x_f16 = fix16_mul(norm_fix3_28_from_s16sample((int16_t) out[i]), preamp);
 
         for (int j = 0; j < filter_stages; j++)
         {