From 34c2da44849f6678eb58b0680bdd4c51eb7176a5 Mon Sep 17 00:00:00 2001
From: Astro <astro@spaceboyz.net>
Date: Sun, 17 May 2020 02:11:53 +0200
Subject: [PATCH] ad7172: fix calibration

---
 src/ad7172/adc.rs | 42 +++++++++++++++++++++++++++++++++++++++---
 src/channels.rs   | 16 ++++++++++++++--
 2 files changed, 53 insertions(+), 5 deletions(-)

diff --git a/src/ad7172/adc.rs b/src/ad7172/adc.rs
index 9d9a341..ace673e 100644
--- a/src/ad7172/adc.rs
+++ b/src/ad7172/adc.rs
@@ -43,7 +43,7 @@ impl<SPI: Transfer<u8, Error = E>, NSS: OutputPin, E: fmt::Debug> Adc<SPI, NSS>
 
         let mut adc_mode = <regs::AdcMode as Register>::Data::empty();
         adc_mode.set_ref_en(true);
-        adc_mode.set_mode(Mode::ContinuousConversion);
+        adc_mode.set_mode(Mode::Standby);
         adc.write_reg(&regs::AdcMode, &mut adc_mode)?;
 
         Ok(adc)
@@ -96,16 +96,52 @@ impl<SPI: Transfer<u8, Error = E>, NSS: OutputPin, E: fmt::Debug> Adc<SPI, NSS>
         Ok(())
     }
 
+    pub fn disable_channel(
+        &mut self, index: u8
+    ) -> Result<(), SPI::Error> {
+        self.update_reg(&regs::Channel { index }, |data| {
+            data.set_enabled(false);
+        })?;
+        Ok(())
+    }
+
+    pub fn disable_all_channels(&mut self) -> Result<(), SPI::Error> {
+        for index in 0..4 {
+            self.update_reg(&regs::Channel { index }, |data| {
+                data.set_enabled(false);
+            })?;
+        }
+        Ok(())
+    }
+
     /// Calibrates offset registers
-    pub fn calibrate_offset(&mut self) -> Result<(), SPI::Error> {
+    pub fn calibrate(&mut self) -> Result<(), SPI::Error> {
+        // internal offset calibration
+        self.update_reg(&regs::AdcMode, |adc_mode| {
+            adc_mode.set_mode(Mode::InternalOffsetCalibration);
+        })?;
+        while ! self.read_reg(&regs::Status)?.ready() {}
+
+        // system offset calibration
         self.update_reg(&regs::AdcMode, |adc_mode| {
             adc_mode.set_mode(Mode::SystemOffsetCalibration);
         })?;
         while ! self.read_reg(&regs::Status)?.ready() {}
 
+        // system gain calibration
         self.update_reg(&regs::AdcMode, |adc_mode| {
-            adc_mode.set_mode(Mode::ContinuousConversion);
+            adc_mode.set_mode(Mode::SystemGainCalibration);
         })?;
+        while ! self.read_reg(&regs::Status)?.ready() {}
+
+        Ok(())
+    }
+
+    pub fn start_continuous_conversion(&mut self) -> Result<(), SPI::Error> {
+        let mut adc_mode = <regs::AdcMode as Register>::Data::empty();
+        adc_mode.set_ref_en(true);
+        adc_mode.set_mode(Mode::ContinuousConversion);
+        self.write_reg(&regs::AdcMode, &mut adc_mode)?;
 
         Ok(())
     }
diff --git a/src/channels.rs b/src/channels.rs
index d2f1f3c..c504b41 100644
--- a/src/channels.rs
+++ b/src/channels.rs
@@ -26,10 +26,22 @@ impl Channels {
         let mut adc = ad7172::Adc::new(pins.adc_spi, pins.adc_nss).unwrap();
         // Feature not used
         adc.set_sync_enable(false).unwrap();
-        // Setup channels
+
+        // Calibrate ADC channels individually
+        adc.disable_all_channels().unwrap();
+
+        adc.setup_channel(0, ad7172::Input::Ain0, ad7172::Input::Ain1).unwrap();
+        adc.calibrate().unwrap();
+        adc.disable_channel(0).unwrap();
+
+        adc.setup_channel(1, ad7172::Input::Ain2, ad7172::Input::Ain3).unwrap();
+        adc.calibrate().unwrap();
+        adc.disable_channel(1).unwrap();
+
+        // Setup channels and start ADC
         adc.setup_channel(0, ad7172::Input::Ain0, ad7172::Input::Ain1).unwrap();
         adc.setup_channel(1, ad7172::Input::Ain2, ad7172::Input::Ain3).unwrap();
-        adc.calibrate_offset().unwrap();
+        adc.start_continuous_conversion().unwrap();
 
         Channels { channel0, channel1, adc, pwm }
     }