Fix audio distortion (#8)

* Fix an overflow in the fix16_t usage which can lead to audio distortion.

* Add a simple test application for running the filtering stages on a PC.

* Fix readme typos.

* Rename test to tools.

* Remove volume multiplier

---------

Co-authored-by: George Norton <george.norton@mediakind.com>

firmware/code/fix16.c

@@ -22,7 +22,7 @@
 #include <stdio.h>
 #include <inttypes.h>
 #include <math.h>
-
+#include <limits.h>
 #include "fix16.h"
 
 fix16_t fix16_from_int(int16_t a) {
@@ -30,9 +30,23 @@ fix16_t fix16_from_int(int16_t a) {
 }
 
 int16_t fix16_to_int(fix16_t a) {
-    if (a >= 0)
-        return (a + (fix16_one >> 1)) / fix16_one;
-    return (a - (fix16_one >> 1)) / fix16_one;
+    // Handle rounding up front, adding one can cause an overflow/underflow
+    a+=(fix16_one >> 1);
+
+    // Saturate the value if an overflow has occurred
+    uint32_t upper = (a >> 30);
+    if (a < 0) {
+        if (~upper)
+        {
+            return SHRT_MIN;
+        }
+    } else {
+        if (upper)
+        {
+            return SHRT_MAX;
+        }
+    }
+    return (a >> 15);
 }
 
 fix16_t fix16_from_dbl(double a) {
@@ -49,16 +63,6 @@ double fix16_to_dbl(fix16_t a) {
 fix16_t fix16_mul(fix16_t inArg0, fix16_t inArg1) {
     int64_t product = (int64_t)inArg0 * inArg1;
 
-    uint32_t upper = (product >> 47);
-    if (product < 0) {
-        if (~upper)
-            return fix16_overflow;
-        product--;
-    } else {
-        if (upper)
-            return fix16_overflow;
-    }
-
     fix16_t result = product >> 15;
     result += (product & 0x4000) >> 14;
 

firmware/code/run.c

@@ -91,7 +91,7 @@ static void _as_audio_packet(struct usb_endpoint *ep) {
     int samples = usb_buffer->data_len / 2;
 
     for (int i = 0; i < samples; i++)
-        out[i] = in[i] / 4; // fixes digital distortion bug
+        out[i] = in[i];
 
     multicore_fifo_push_blocking(CORE0_READY);
     multicore_fifo_push_blocking(samples);
@@ -113,7 +113,7 @@ static void _as_audio_packet(struct usb_endpoint *ep) {
 
     // Multiply the outgoing signal with the volume multiple
     for (int i = 0; i < samples; i++)
-        out[i] = out[i] * (int32_t) vol_mul * 4;
+        out[i] = out[i] * (int32_t) vol_mul;
 
     i2s_stream_write(&i2s_write_obj, userbuf, samples * 4);
 

firmware/tools/CMakeLists.txt

@@ -0,0 +1,25 @@
+cmake_minimum_required(VERSION 3.13)
+
+project(ploopy_headphones_project C CXX ASM)
+set(CMAKE_C_STANDARD 11)
+set(CMAKE_CXX_STANDARD 17)
+
+add_executable(filter_test
+    filter_test.c
+    ../code/fix16.c
+    ../code/bqf.c
+    ../code/user.c
+)
+
+target_include_directories(filter_test PRIVATE ${CMAKE_SOURCE_DIR}/../code)
+
+# TODO: user.c includes run.h to get the definition for SAMPLING_FREQ, but this
+# pulls in the whole pico sdk as a dependency. A little refactoring would fix it all.
+target_compile_definitions(filter_test PRIVATE
+    SAMPLING_FREQ=48000
+    RUN_H
+)
+
+target_link_libraries(filter_test
+    m
+)

firmware/tools/README.md

@@ -0,0 +1,23 @@
+## filter_test
+This is a basic utility for testing the Ploopy headphones filtering routines on a PC.
+
+### Usage
+Find a source file and use ffmpeg to convert it to 16bit stereo PCM samples:
+
+```
+ffmpeg -i <input file> -map 0:6 -vn -f s16le -acodec pcm_s16le input.pcm
+```
+
+Run `filter_test` to process the PCM samples. The `filter_test` program takes two arguments an input file and an output file:
+
+```
+./filter_test input.pcm output.pcm
+```
+
+You can listen to the PCM files using ffplay (which is usually included with ffmpeg):
+
+```
+./ffplay -f s16le -ar 48000 -ac 2 output.pcm
+```
+
+If there are no obvious problems, go ahead and flash your firmware.

firmware/tools/filter_test.c

@@ -0,0 +1,91 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include "bqf.h"
+#include "fix16.h"
+#include "user.h"
+
+bqf_coeff_t bqf_filters_left[FILTER_STAGES];
+bqf_coeff_t bqf_filters_right[FILTER_STAGES];
+bqf_mem_t bqf_filters_mem_left[FILTER_STAGES];
+bqf_mem_t bqf_filters_mem_right[FILTER_STAGES];
+
+const char* usage = "Usage: %s INFILE OUTFILE\n\n"
+    "Reads 16bit stereo PCM data from INFILE, runs it through the Ploopy headphones\n"
+    "filters then writes it out to OUTFILE.\n";
+
+int main(int argc, char* argv[])
+{
+    if (argc != 3)
+    {
+        fprintf(stdout, usage, argv[0]);
+        exit(1);
+    }
+
+    // Load the input data into a buffer
+    FILE* input = fopen(argv[1], "rb");
+    if (!input) {
+        fprintf(stderr, "Cannot open input file '%s'\n", argv[1]);
+        exit(1);
+    }
+
+    // Get the file size
+    fseek(input , 0L , SEEK_END);
+    size_t input_size = ftell(input);
+    rewind(input);
+
+    // Allocate our input and output buffers. This could be optimized
+    // we dont need to store the whole input and output files in memory.
+    int samples = input_size / 2;
+    int16_t *in = (int16_t *) calloc(samples, sizeof(int16_t));
+    int16_t *out = (int16_t *) calloc(samples, sizeof(int16_t));
+
+    fread(in, samples, sizeof(int16_t), input);
+    fclose(input);
+
+    // Open the output file.
+    FILE* output = fopen(argv[2], "wb");
+    if (!output)
+    {
+        fprintf(stderr, "Cannot open output file '%s'\n", argv[2]);
+        exit(1);
+    }
+
+    // The smaple proccesing code, essentially the same as the
+    // code in the firmware's run.c file.
+    define_filters();
+
+    for (int i = 0; i < samples; i++)
+    {
+        out[i] = in[i];
+    }
+
+    for (int j = 0; j < FILTER_STAGES; j++)
+    {
+        for (int i = 0; i < samples; i ++)
+        {
+            // Left channel filter
+            fix16_t x_f16 = fix16_from_int((int16_t) out[i]);
+
+            x_f16 = bqf_transform(x_f16, &bqf_filters_left[j],
+                &bqf_filters_mem_left[j]);
+
+            out[i] = (int32_t) fix16_to_int(x_f16);
+
+            // Right channel filter
+            i++;
+            x_f16 = fix16_from_int((int16_t) out[i]);
+
+            x_f16 = bqf_transform(x_f16, &bqf_filters_right[j],
+                &bqf_filters_mem_right[j]);
+
+            out[i] = (int16_t) fix16_to_int(x_f16);
+        }
+    }
+
+    // Write out the processed audio.
+    fwrite(out, samples, sizeof(int16_t), output);
+    fclose(output);
+
+    free(in);
+    free(out);
+}