Fix some dither clipping issues

3 files changed, 16 insertions, 8 deletions

diff --git a/src/codecs/include/sample.hpp b/src/codecs/include/sample.hpp
index 8d79a228..7e550680 100644
--- a/src/codecs/include/sample.hpp
+++ b/src/codecs/include/sample.hpp
@@ -28,12 +28,11 @@ constexpr auto Clip(int64_t v) -> Sample {
   return std::clamp<int64_t>(v, INT16_MIN, INT16_MAX);
 }
 
-auto applyDither(int64_t src, uint_fast8_t bits) -> int32_t;
+auto shiftWithDither(int64_t src, uint_fast8_t bits) -> Sample;
 
 constexpr auto FromSigned(int32_t src, uint_fast8_t bits) -> Sample {
   if (bits > 16) {
-    src = applyDither(src, bits - 16);
-    return src >> (bits - sizeof(Sample) * 8);
+    return shiftWithDither(src, bits - 16);
   } else if (bits < 16) {
     return src << (sizeof(Sample) * 8 - bits);
   }
diff --git a/src/codecs/sample.cpp b/src/codecs/sample.cpp
index c5c96fb9..d4860b94 100644
--- a/src/codecs/sample.cpp
+++ b/src/codecs/sample.cpp
@@ -5,6 +5,7 @@
  */
 
 #include "sample.hpp"
+#include <stdint.h>
 
 #include <cstdint>
 
@@ -15,11 +16,16 @@ namespace sample {
 static uint64_t sSeed1{0};
 static uint64_t sSeed2{0};
 
-auto applyDither(int64_t src, uint_fast8_t bits) -> int32_t {
-  uint64_t mask = 0xFFFFFFFF;                          // 32 ones
-  mask >>= 32 - bits;                                  // `bits` ones
-  uint64_t noise = komirand(&sSeed1, &sSeed2) & mask;  // `bits` random noise
-  return std::clamp<int64_t>(src + noise, INT32_MIN, INT32_MAX);
+auto shiftWithDither(int64_t src, uint_fast8_t bits) -> Sample {
+  // Generate `bits` random bits
+  uint64_t mask = 0xFFFFFFFF;
+  mask >>= 32 - bits;
+  int64_t noise = static_cast<int32_t>(komirand(&sSeed1, &sSeed2) & mask);
+  // Centre the noise around 0.
+  noise -= (mask >> 1);
+  // Apply to the sample, then clip and shift to 16 bit.
+  Sample clipped = Clip((src + noise) >> bits);
+  return clipped;
 }
 
 }  // namespace sample
diff --git a/src/drivers/i2s_dac.cpp b/src/drivers/i2s_dac.cpp
index ef1a64ab..021be901 100644
--- a/src/drivers/i2s_dac.cpp
+++ b/src/drivers/i2s_dac.cpp
@@ -97,6 +97,9 @@ I2SDac::I2SDac(IGpios& gpio, i2s_chan_handle_t i2s_handle)
   wm8523::WriteRegister(wm8523::Register::kReset, 1);
   vTaskDelay(pdMS_TO_TICKS(10));
   wm8523::WriteRegister(wm8523::Register::kPsCtrl, 0b0);
+
+  // Ramp volume changes
+  wm8523::WriteRegister(wm8523::Register::kDacCtrl, 0b11);
 }
 
 I2SDac::~I2SDac() {