From 6575400fbcd3693d2ccb3f5c313c288a197e1091 Mon Sep 17 00:00:00 2001
From: Steve Lascos <steve@blackaddr.com>
Date: Sun, 20 Sep 2020 12:28:34 -0400
Subject: [PATCH] Add function to recalibrate codec HPF

---
 examples/Tests/MeasureNoise/MeasureNoise.ino |  2 ++
 src/BAAudioControlWM8731.h                   |  6 ++++++
 src/peripherals/BAAudioControlWM8731.cpp     | 20 ++++++++++++++++++++
 3 files changed, 28 insertions(+)

diff --git a/examples/Tests/MeasureNoise/MeasureNoise.ino b/examples/Tests/MeasureNoise/MeasureNoise.ino
index 19da702..b222ba0 100644
--- a/examples/Tests/MeasureNoise/MeasureNoise.ino
+++ b/examples/Tests/MeasureNoise/MeasureNoise.ino
@@ -69,6 +69,8 @@ void setup() {
 
   rmsModule.enable();
   rmsModule.bypass(false);
+
+  codecControl.recalibrateDcOffset();
   
   
 }
diff --git a/src/BAAudioControlWM8731.h b/src/BAAudioControlWM8731.h
index 0eae269..da32df6 100644
--- a/src/BAAudioControlWM8731.h
+++ b/src/BAAudioControlWM8731.h
@@ -110,6 +110,12 @@ public:
 	/// @details This will cause the codec to reset its internal registers to default values.
 	void resetCodec(void);
 
+	/// Recalibrate the DC offset removal
+	/// @details This function will temporarily mute the inputs, enable the CODEC's
+	/// HPF in order to calculate the DC offset (which is subtracted out). Then the filter
+	/// is disabled.
+	void recalibrateDcOffset(void);
+
 	/// Write a custom command to the codec via I2C control interface.
 	/// @details See WM8731 datasheet for register map details.
 	/// @param addr The register address you wish to write to, range 0 to 15.
diff --git a/src/peripherals/BAAudioControlWM8731.cpp b/src/peripherals/BAAudioControlWM8731.cpp
index 5b421cd..b82b83b 100644
--- a/src/peripherals/BAAudioControlWM8731.cpp
+++ b/src/peripherals/BAAudioControlWM8731.cpp
@@ -361,6 +361,26 @@ void BAAudioControlWM8731::resetCodec(void)
 	resetInternalReg();
 }
 
+void BAAudioControlWM8731::recalibrateDcOffset(void)
+{
+    // mute the inputs
+    setLeftInMute(true);
+    setRightInMute(true);
+
+    // enable the HPF and DC offset store
+    setHPFDisable(false);
+    regArray[WM8731_HPF_STORE_OFFSET_ADDR] |= WM8731_HPF_STORE_OFFSET_MASK;
+    write(WM8731_REG_DIGITAL, regArray[WM8731_REG_DIGITAL]);
+
+    delay(1000); // wait for the DC offset to be calculated over 1 second
+    setHPFDisable(true); // disable the dynamic HPF calculation
+    delay(500);
+
+    // unmute the inputs
+    setLeftInMute(false);
+    setRightInMute(false);
+}
+
 // Direct write control to the codec
 bool BAAudioControlWM8731::writeI2C(unsigned int addr, unsigned int val)
 {