From 749ffc04118570036cd27f8bee2eabddfe47e039 Mon Sep 17 00:00:00 2001
From: colin <colin@uninsane.org>
Date: Tue, 28 Jun 2022 03:25:01 -0700
Subject: [PATCH] median-filter the samples before sending them over itm

we're limited by the itm busrate. but we can at least get a cleaner
signal by sampling at a higher rate and then downsampling before sending
to the PC.
---
 Cargo.toml            |  1 +
 scripts/itm-to-pcm.py | 52 ++++++++++++++++++++++++++++++++++++-------
 src/main.rs           | 18 ++++++++++++++-
 3 files changed, 62 insertions(+), 9 deletions(-)

diff --git a/Cargo.toml b/Cargo.toml
index 080fd73..3536497 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -8,6 +8,7 @@ version = "0.1.0"
 cortex-m = "0.5.6"
 cortex-m-rt = "0.6.3"
 panic-itm = "0.4.0"
+pdqsort = "1.0"
 
 [dependencies.f3]
 features = ["rt"]
diff --git a/scripts/itm-to-pcm.py b/scripts/itm-to-pcm.py
index ca1d0c4..9d00e04 100755
--- a/scripts/itm-to-pcm.py
+++ b/scripts/itm-to-pcm.py
@@ -8,6 +8,7 @@ itm-to-pcm.py dump.txt dump.wav
 
 import wave
 import sys
+from math import pi
 
 class Fifo(object):
     def __init__(self, size):
@@ -46,16 +47,36 @@ def med_filt_stream(stream, kernel=5):
         accum.append(f)
         yield median(accum.list())
 
-def dc_filt_stream(stream, kernel=2):
+def dc_filt_stream(stream, kernel=5000):
     accum = Fifo(kernel)
     for f in stream:
         accum.append(f)
         dc = sum(accum.list()) / len(accum.list())
         yield f - dc
 
+def scale_stream(stream, gamma=1.2, makeup=1000):
+    # map x -> scale*x^gamma, but maintain:
+    #   0xffff = scale*0xffff^gamma
+    scale = makeup * 0xffff / 0xffff**gamma
+    for f in stream:
+        if f > 0:
+            yield scale * f**gamma
+        else:
+            yield -scale * (-f)**gamma
+
+def hpf(stream, f_c=0.2):
+    # y[i] := α * (y[i-1] + x[i] - x[i-1])
+    alpha = 1 / (2*pi*f_c + 1)
+    y_prev, x_prev = 0, 0
+    for x in stream:
+        y = alpha * (y_prev + x - x_prev)
+        yield y
+        y_prev = y
+
 def frames_from_samples(itm):
     b = bytearray()
     for sample in itm:
+        sample = round(sample)
         sample = (sample + 0x10000) % 0x10000 # counteract Python's signed mod badness
         lsb0 = sample % 0x100
         lsb1 = (sample // 0x100) % 0x100
@@ -71,30 +92,45 @@ def median(samples):
 def stream_to_wav(file_in, file_out):
     # broken
     itm_stream = stream_itm(file_in)
-    clean_stream = dc_filt_stream(med_filt_stream(itm_stream))
+    #clean_stream = dc_filt_stream(med_filt_stream(itm_stream))
+    #clean_stream = dc_filt_stream(itm_stream)
+    clean_stream = dc_filt_stream(itm_stream)
+    #clean_stream = scale_stream(itm_stream)
     wave_out = wave.open(file_out, 'wb')
     wave_out.setnchannels(1)
     wave_out.setsampwidth(2)
-    wave_out.setframerate(1000)
+    wave_out.setframerate(2300)
     wave_out.writeframes(frames_from_samples(list(clean_stream)))
 
 def stream_to_term(file_in):
-    stream = dc_filt_stream(med_filt_stream(stream_itm(file_in)))
+    #stream = dc_filt_stream(med_filt_stream(stream_itm(file_in)))
+    #stream = scale_stream(dc_filt_stream(stream_itm(file_in)))
+    stream = scale_stream(hpf(dc_filt_stream(stream_itm(file_in))))
+    #stream = stream_itm(file_in)
     for f in stream:
         print(ansi_rgb(color_for_sample(f), '█'), end='')
 
 def ansi_rgb(color, text):
     r, g, b = color
+    #print(color)
     return '\033[38;2;{r};{g};{b}m{text}\033[0m'.format(**locals())
 
 def color_for_sample(sample):
     r, g, b = 0, 0, 0
     if sample < 0:
-        b = int(-sample)
+        b = -sample / 0x100
     else:
-        r = int(sample)
-    #return (128, 0, 0)
-    return r, g, b
+        r = sample / 0x100
+    if b > 0xff:
+        g = min(0xff, b / 16)
+        b = 0xff
+    if r > 0xff:
+        g = min(0xff, r / 16)
+        r = 0xff
+    assert r < 0x100, sample
+    assert g < 0x100, sample
+    assert b < 0x100, sample
+    return int(r), int(g), int(b)
 
 def main():
     if len(sys.argv) == 3:
diff --git a/src/main.rs b/src/main.rs
index 6594282..f5ae827 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -115,6 +115,7 @@ mod bsp;
 use cortex_m::{iprintln, itm};
 use cortex_m::asm::{bkpt, delay};
 use cortex_m_rt::entry;
+use pdqsort;
 
 #[entry]
 fn main() -> ! {
@@ -296,7 +297,22 @@ fn main() -> ! {
     //     itm::write_all(&mut per.itm.stim[0], &[sample]);
     // }
     loop {
-        let sample = (per.adc1.dr.read().bits() << 4) as u16;
+        // median filter decimation
+        // NB: 3.6 KB/sec for 63 filter width
+        //     4.4 KB/sec for 53 filter width
+        //     4.6 KB/sec for 51 filter width
+        //     5.0 KB/sec for 49 filter width  vv--- these rates and below show alias patterns
+        //     5.0 KB/sec for 47 filter width
+        //     5.0 KB/sec for 31 filter width
+        //     5.0 KB/sec for 15 filter width
+        // measured: ll /tmp/itm.txt ; sleep 10 ; ll /tmp/itm.txt
+        let mut buf = [0; 51];
+        for sub_sample in buf.iter_mut() {
+            *sub_sample = (per.adc1.dr.read().bits() << 4) as u16;
+        }
+        pdqsort::sort(&mut buf);
+        let sample = buf[25];
+
         let high = (sample >> 8) as u8;
         let low = (sample & 0xff) as u8;
         itm::write_all(&mut per.itm.stim[0], &[low, high]);