1 files changed, 25 insertions, 7 deletions
diff --git a/firmware/src/adc.hpp b/firmware/src/adc.hpp
index d7842aa..bff03b0 100644
--- a/firmware/src/adc.hpp
+++ b/firmware/src/adc.hpp
@@ -1,11 +1,25 @@
 #pragma once
-#include <array>
+
 #include "hardware/adc.h"
+#include <array>
 #include <cstdint>
+#include <algorithm>
 
 namespace
 {
-std::array<std::uint32_t, 2> previous_samples { 0xffff'ffffu, 0xffff'ffffu };
+
+std::array<std::uint8_t, 2> previous_samples { 0xffu, 0xffu };
+
+std::uint8_t
+raw_to_midi(std::uint16_t new_raw, std::uint8_t previous_midi)
+{
+    std::int32_t constexpr interval = 4096 / 128;
+    std::int32_t constexpr hysteresis = 3;
+    std::int32_t const lower = std::max(0l, static_cast<std::int32_t>(previous_midi) * interval - hysteresis);
+    std::int32_t const upper = std::min(4095l, lower + interval - 1l + hysteresis);
+    std::uint8_t const new_midi = static_cast<uint8_t>(new_raw * 128u / 4096u);
+    return new_raw > upper || new_raw < lower ? new_midi : previous_midi;
+}
 
 }
 
@@ -14,7 +28,8 @@ namespace adc
 
 struct Sample
 {
-    std::uint8_t value {0};
+    std::uint16_t raw {0};
+    std::uint8_t midi {0};
     bool changed {false};
 };
 
@@ -34,13 +49,16 @@ get_samples()
     {
         adc_select_input(i);
 
-        std::uint16_t const measurement = adc_read();
-        samples[i].changed = measurement != previous_samples[i];
-        previous_samples[i] = measurement;
-        samples[i].value = static_cast<uint8_t>(measurement * 127u / 4095u); // TODO add hysteresis
+        auto & sample = samples[i];
+        sample.raw = adc_read();
+        sample.midi = raw_to_midi(sample.raw, sample.midi);
+        sample.changed = samples[i].midi != previous_samples[i];
+
+        previous_samples[i] = sample.midi;
     }
 
     return samples;
 }
 
 }
+