event_filter: add filters for converting abs mouse to trackball

6 files changed, 199 insertions, 56 deletions

diff --git a/Cargo.lock b/Cargo.lock
index 118d04a..bf2e3bf 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -304,6 +304,7 @@ dependencies = [
  "cortex-m",
  "cortex-m-rt",
  "embedded-hal",
+ "fugit",
  "nb 1.0.0",
  "stm32f1xx-hal",
  "usb-device",
diff --git a/Cargo.toml b/Cargo.toml
index db27d00..4780b6c 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -19,6 +19,7 @@ codegen-units = 1
 [dependencies]
 cortex-m = "0.7.6"
 cortex-m-rt = "0.7.1"
+fugit = "0.3.5"
 nb = "1.0.0"
 #panic-semihosting = "0.6.0"
 usb-device = "0.2.9"
diff --git a/src/bin/luchie.rs b/src/bin/luchie.rs
index 2799609..be89fa7 100755
--- a/src/bin/luchie.rs
+++ b/src/bin/luchie.rs
@@ -3,10 +3,9 @@
 
 //extern crate panic_semihosting;
 
-use core::cmp;
-
 use luchie::{
     cirque::Cirque,
+    event_filter::{AbsToRel, TrackBall},
     log, logger, logln,
 };
 
@@ -25,13 +24,14 @@ use stm32f1xx_hal::{
 };
 use usb_device::prelude::*;
 use usbd_human_interface_device::{
-    prelude::*,
     device::mouse::{WheelMouseInterface, WheelMouseReport},
+    prelude::*,
 };
 use usbd_serial::{SerialPort, USB_CLASS_CDC};
 
 #[entry]
 fn main() -> ! {
+    let cp = cortex_m::Peripherals::take().unwrap();
     let dp = pac::Peripherals::take().unwrap();
 
     let mut flash = dp.FLASH.constrain();
@@ -68,6 +68,14 @@ fn main() -> ! {
 
     logln!("🐁 luchie starting…");
 
+    logln!("⏲️ init timer");
+    let mut syst = cp.SYST;
+    syst.set_clock_source(cortex_m::peripheral::syst::SystClkSource::Core);
+    let syst_reload = 0xffffff;
+    syst.set_reload(syst_reload);
+    syst.clear_current();
+    syst.enable_counter();
+
     // cirque spi connections to spi1:
     //
     // pb0 - dr
@@ -106,8 +114,6 @@ fn main() -> ! {
     // will not reset your device when you upload new firmware.
     let mut usb_dp = gpioa.pa12.into_push_pull_output(&mut gpioa.crh);
     usb_dp.set_low();
-    // let mut delay = dp.TIM2.delay_us(&clocks);
-    // delay.delay_ms(10u8);
     cortex_m::asm::delay(clocks.sysclk().raw() / 100);
 
     let usb = usb::Peripheral {
@@ -131,9 +137,9 @@ fn main() -> ! {
         .device_class(USB_CLASS_CDC)
         .build();
 
-    while usb_dev.state() != UsbDeviceState::Configured {
-        usb_dev.poll(&mut [&mut serial, &mut mouse]);
-    }
+    // while usb_dev.state() != UsbDeviceState::Configured {
+    //     usb_dev.poll(&mut [&mut serial, &mut mouse]);
+    // }
 
     logln!("💡 init led");
     let mut gpiob = dp.GPIOB.split();
@@ -142,11 +148,20 @@ fn main() -> ! {
 
     logln!("🎉 luchie started!");
 
-    let mut is_pressed = false;
-    let mut last_x = 0u16;
-    let mut last_y = 0u16;
+    let mut abs_to_rel = AbsToRel::new();
+    let mut trackball = TrackBall::new(1);
+    let ticks_per_microsec = clocks.sysclk().to_MHz();
+    let mut last_time = 0;
+    let mut time_error = 0;
     loop {
-        // logln!(".");
+        let mut time = syst_reload - cortex_m::peripheral::SYST::get_current();
+        if syst.has_wrapped() {
+            time += syst_reload + 1;
+        }
+        let elapsed = time.wrapping_sub(last_time) + time_error;
+        time_error = elapsed % ticks_per_microsec;
+        let elapsed = elapsed / ticks_per_microsec;
+        last_time = time & syst_reload;
         usb_dev.poll(&mut [&mut serial, &mut mouse]);
 
         let mut buf = [0u8; 64];
@@ -176,38 +191,29 @@ fn main() -> ! {
             _ => {}
         }
 
-        if let Ok(td) = cirque.poll(&mut spi) {
-            logln!("td: {:?}", td);
-            if td.is_pressed {
-                if is_pressed {
-                    /*
-                     * The trackpad's actual valid return values are
-                     * only about 2^11, so overflow isn't possible
-                     * when converting to signed 16 bit integers.
-                     */
-                    let s_x: i16 = td.x as i16 - last_x as i16;
-                    let s_y: i16 = td.y as i16 - last_y as i16;
-
-                    // Clamp to i8 range.
-                    let raw_x = cmp::max(i8::MIN as i16, cmp::min(i8::MAX as i16, s_x));
-                    let raw_y = cmp::max(i8::MIN as i16, cmp::min(i8::MAX as i16, s_y));
-                    mouse_report.x = raw_x as i8;
-                    mouse_report.y = raw_y as i8;
-                }
-                is_pressed = true;
-                last_x = td.x;
-                last_y = td.y;
-            } else {
-                mouse_report.x = 0;
-                mouse_report.y = 0;
-                is_pressed = false;
-            }
+        // TODO: get current µs for passing in to event filters.
+        let rel_p = cirque
+            .poll(&mut spi)
+            .ok()
+            .map(|td| {
+                //logln!("td: {:?}", td);
+                abs_to_rel.update(&td)
+            })
+            .flatten();
+        if let Some(p) = rel_p {
+            logln!("rel_p: {:?}", p);
+        }
+        let p = trackball.update(rel_p, elapsed);
+        if p.0 != 0 || p.1 != 0 {
+            mouse_report.x = p.0;
+            mouse_report.y = p.1;
+            logln!("p: {:?}", p);
             match mouse.interface().write_report(&mouse_report) {
-                Err(UsbHidError::WouldBlock) => {},
+                Err(UsbHidError::WouldBlock) => {}
                 Err(e) => {
                     panic!("couldn't write mouse report: {:?}", e)
-                },
-                _ => {},
+                }
+                _ => {}
             }
         }
     }
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,
diff --git a/src/event_filter.rs b/src/event_filter.rs
new file mode 100644
index 0000000..9eb4620
--- /dev/null
+++ b/src/event_filter.rs
@@ -0,0 +1,107 @@
+use crate::cirque::TouchData;
+
+use core::{
+    cmp,
+    ops::Sub,
+};
+
+#[derive(Copy, Clone, Debug)]
+pub struct Point<T>(pub T, pub T);
+
+impl<T> Point<T>
+where
+    T: Sub<Output = T>,
+{
+    pub fn delta(self, p: Point<T>) -> Point<T> {
+        Point(p.0 - self.0, p.1 - self.1)
+    }
+}
+
+impl Point<isize> {
+    pub fn scale(self) -> Self {
+        Point(self.0 >> 2, self.1 >> 2)
+    }
+}
+
+impl From<&TouchData> for Point<isize> {
+    fn from(td: &TouchData) -> Self {
+        Self(td.x as _, td.y as _)
+    }
+}
+
+impl From<Point<isize>> for Point<i8> {
+    fn from(p: Point<isize>) -> Self {
+        Point(
+            cmp::min(i8::MAX as isize, cmp::max(p.0, i8::MIN as isize)) as i8,
+            cmp::min(i8::MAX as isize, cmp::max(p.1, i8::MIN as isize)) as i8,
+        )
+    }
+}
+
+pub struct AbsToRel {
+    last_touch: Point<isize>,
+    is_collecting: bool,
+}
+
+impl AbsToRel {
+    pub fn new() -> Self {
+        Self {
+            last_touch: Point(0, 0),
+            is_collecting: false,
+        }
+    }
+
+    pub fn update(&mut self, td: &TouchData) -> Option<Point<i8>> {
+        let p: Point<isize> = td.into();
+        let p = p.scale();
+
+        let res = self.last_touch.delta(p);
+        self.last_touch = p;
+        if self.is_collecting && td.is_pressed {
+            Some(res.into())
+        } else {
+            self.is_collecting = td.is_pressed;
+            None
+        }
+    }
+}
+
+pub struct TrackBall(Point<i8>, i8, u32);
+
+impl TrackBall
+where
+{
+    pub fn new(friction: i8) -> Self {
+        Self(Point(i8::default(), i8::default()), friction, 0)
+    }
+
+    pub fn update(&mut self, point: Option<Point<i8>>, elapsed: u32) -> Point<i8> {
+        if let Some(p) = point {
+            self.0 = p;
+            self.2 = 0;
+        } else {
+            let elapsed = elapsed + self.2;
+            // TODO: configure time divisor
+            let decel: i8 = self.1.saturating_mul((elapsed >> 15).try_into().unwrap_or(i8::MAX));
+            let rem = elapsed & (1 << 15) - 1;
+            self.2 = rem;
+
+            if decel > 0 {
+                let x = Self::abs_add(self.0.0, decel);
+                let y = Self::abs_add(self.0.1, decel);
+                self.0 = Point(x, y);
+            }
+        }
+        self.0
+    }
+
+    fn abs_add(speed: i8, decel: i8) -> i8 {
+        if speed > decel {
+            speed - decel
+        } else if speed < -decel {
+            speed + decel
+        } else {
+            0
+        }
+    }
+}
diff --git a/src/lib.rs b/src/lib.rs
index 821bdeb..fbfba93 100755
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -1,6 +1,7 @@
 #![no_std]
 
 pub mod cirque;
+pub mod event_filter;
 pub mod logger;
 
 #[cfg(test)]