1 files changed, 42 insertions, 15 deletions

diff --git a/src/cirque.rs b/src/cirque.rs
index 0f40c51..b4a2255 100644
--- a/src/cirque.rs
+++ b/src/cirque.rs
@@ -140,7 +140,7 @@ impl<C> Cirque<C>
 where
     C: OutputPin,
 {
-    pub fn new<S>(cs_pin: C, spi: &mut S, sysclk_speed: u32) -> nb::Result<Self, S::Error>
+    pub fn new<S>(cs_pin: C, spi: &mut S, sysclk_speed: u32) -> Result<Self, S::Error>
     where
         S: spi::FullDuplex<u8>,
     {
@@ -152,24 +152,46 @@ where
         Ok(res)
     }
 
-    fn init<S>(&mut self, spi: &mut S) -> nb::Result<(), S::Error>
+    fn init<S>(&mut self, spi: &mut S) -> Result<(), S::Error>
     where
         S: spi::FullDuplex<u8>,
     {
         self.cs_pin.set_high().ok();
 
-        // spin until power on reset is flagged.
-        // while self.read_flags(spi)? & 0x8 != 0x8 {}
-        self.clear_flags(spi)?;
+        // let mut b = [0xfbu8];
+        // nb::block!(self.rd(spi, RAPAddress::SysConfig1, &mut b))?;
+        // logln!("b-reset: {:?}", b);
+
+        // self.reset(spi)?;
+
+        // let mut b = [0xfbu8];
+        // nb::block!(self.rd(spi, RAPAddress::SysConfig1, &mut b))?;
+        // logln!("a-reset: {:?}", b);
+
+        // nb::block!(self.wr(spi, RAPAddress::SysConfig1, 0x00))?;
+        nb::block!(self.clear_flags(spi))?;
 
-        self.wr(spi, RAPAddress::SysConfig1, 0x00)?;
-        self.wr(spi, RAPAddress::FeedConfig2, 0x1e)?;
-        self.wr(spi, RAPAddress::FeedConfig1, 0x03)?;
-        self.wr(spi, RAPAddress::ZIdle, 0x05)?;
+        // let mut b = [0xfbu8];
+        // nb::block!(self.rd(spi, RAPAddress::SysConfig1, &mut b))?;
+        // logln!("a-reset: {:?}", b);
+
+        nb::block!(self.wr(spi, RAPAddress::FeedConfig2, 0x1e))?;
+        nb::block!(self.wr(spi, RAPAddress::FeedConfig1, 0x03))?;
+        nb::block!(self.wr(spi, RAPAddress::ZIdle, 0x05))?;
 
         Ok(())
     }
 
+    // fn reset<S>(&mut self, spi: &mut S) -> Result<(), S::Error>
+    // where
+    //     S: spi::FullDuplex<u8>
+    // {
+    //     nb::block!(self.wr(spi, RAPAddress::SysConfig1, 0x01))?;
+    //     while nb::block!(self.read_flags(spi))? & 0x8 != 0x8 {}
+    //     nb::block!(self.clear_flags(spi))?;
+    //     Ok(())
+    // }
+
     // clears the hardware data ready flag
     fn clear_flags<S>(&mut self, spi: &mut S) -> nb::Result<(), S::Error>
     where
@@ -210,14 +232,19 @@ where
         self.rd(spi, RAPAddress::PacketByte0, &mut buf)?;
         self.clear_flags(spi)?;
 
-        let x = buf[2] as u16 | ((buf[4] as u16 & 0x0f) << 8);
-        let y = buf[3] as u16 | ((buf[4] as u16 & 0xf0) << 4);
-        let z = buf[5] & 0x3f;
+        let mut x = buf[2] as u16 | ((buf[4] as u16 & 0x0f) << 8);
+        let mut y = buf[3] as u16 | ((buf[4] as u16 & 0xf0) << 4);
+        let mut z = buf[5] & 0x3f;
         let buttons = 0;
-        let is_pressed = x != 0;
 
-        assert!(x < MAX_X);
-        assert!(y < MAX_Y);
+        // assert!(x < MAX_X);
+        // assert!(y < MAX_Y);
+        if x >= MAX_X || y >= MAX_Y {
+            x = 0;
+            y = 0;
+            z = 0;
+        }
+        let is_pressed = z != 0;
 
         Ok(TouchData {
             x,