Move setting the thresholds to tier2 features

1 files changed, 57 insertions, 0 deletions

diff --git a/src/devices/features/tier2.rs b/src/devices/features/tier2.rs
index 43dce5c..2aa649f 100644
--- a/src/devices/features/tier2.rs
+++ b/src/devices/features/tier2.rs
@@ -10,6 +10,18 @@ where
     DI: interface::WriteData<Error = E>,
     IC: ic::Resolution + ic::Tier2Features
 {
+    /// Set comparator lower threshold
+    pub fn set_low_threshold(&mut self, value: i16) -> Result<(), Error<E>> {
+        let register_value = convert_threshold::<IC, E>(value)?;
+        self.iface.write_register(Register::LOW_TH, register_value)
+    }
+
+    /// Set comparator upper threshold
+    pub fn set_high_threshold(&mut self, value: i16) -> Result<(), Error<E>> {
+        let register_value = convert_threshold::<IC, E>(value)?;
+        self.iface.write_register(Register::HIGH_TH, register_value)
+    }
+
     /// Set comparator mode
     pub fn set_comparator_mode(&mut self, mode: ComparatorMode) -> Result<(), Error<E>> {
         let config;
@@ -46,3 +58,48 @@ where
         Ok(())
     }
 }
+
+
+fn convert_threshold<IC, E>(value: i16) -> Result<u16, Error<E>>
+where
+    IC: ic::Resolution
+{
+    if IC::BITS == ic::ResolutionBits::_12 {
+        if value < -2048 || value > 2047 {
+            return Err(Error::InvalidInputData);
+        }
+        Ok((value << 4) as u16)
+    }
+    else {
+        Ok(value as u16)
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    fn assert_invalid_input_data<E>(result: Result<u16, Error<E>>) {
+        match result {
+            Err(Error::InvalidInputData) => (),
+            _ => panic!("InvalidInputData error was not returned.")
+        }
+    }
+
+    #[test]
+    fn convert_12_bits() {
+        assert_invalid_input_data(convert_threshold::<ic::Ads1013, ()>(2048));
+        assert_invalid_input_data(convert_threshold::<ic::Ads1013, ()>(-2049));
+
+        assert_eq!(     0, convert_threshold::<ic::Ads1013, ()>(0).unwrap());
+        assert_eq!(0x7FF0, convert_threshold::<ic::Ads1013, ()>(2047).unwrap());
+        assert_eq!(0x8000, convert_threshold::<ic::Ads1013, ()>(-2048).unwrap());
+        assert_eq!(0xFFF0, convert_threshold::<ic::Ads1013, ()>(-1).unwrap());
+    }
+
+    #[test]
+    fn convert_16_bits() {
+        assert_eq!(0x7FFF, convert_threshold::<ic::Ads1113, ()>(32767).unwrap());
+        assert_eq!(0x8000, convert_threshold::<ic::Ads1113,()>(-32768).unwrap());
+    }
+}