Abgabe

kawaii/kawaii-rs/dependencies/ultrasonic-irq/src/lib.rs

@@ -0,0 +1,194 @@
+#![feature(integer_atomics)]
+
+extern crate time;
+extern crate shuteye;
+extern crate kawaii;
+
+use std::time::Duration;
+use std::thread;
+use std::thread::JoinHandle;
+use std::sync::mpsc::{Sender, Receiver};
+use std::sync::mpsc;
+use std::sync::Arc;
+use std::sync::atomic::{AtomicU32, Ordering};
+
+use self::kawaii::gpio::{SyncPort, AsyncPort, Direction, Edge, Value};
+
+#[cfg(feature = "measure")]
+use self::kawaii::Measure;
+
+use self::time::precise_time_ns;
+use self::shuteye::sleep;
+
+#[derive(Debug)]
+struct ThreadData<T> {
+    thread: JoinHandle<T>,
+    tx: Sender<()>,
+}
+
+#[derive(Debug)]
+pub struct UltrasonicEcho {
+    echo: AsyncPort,
+    temperature: u8,
+    timestamp: u64,
+    distance: Arc<AtomicU32>,
+}
+
+#[derive(Debug)]
+pub struct UltrasonicTrigger {
+    trigger: SyncPort,
+}
+
+#[derive(Debug)]
+pub struct Ultrasonic {
+    trigger: Option<ThreadData<()>>,
+    echo: Option<ThreadData<()>>,
+    pub distance: Arc<AtomicU32>,
+}
+
+impl Ultrasonic {
+    pub fn new(trigger_port: u8, echo_port: u8, temperature: u8) -> std::io::Result<Ultrasonic> {
+        let distance = Arc::new(AtomicU32::new(u32::max_value()));
+        let echo = UltrasonicEcho {
+            echo: AsyncPort::new(echo_port, Edge::Both)?,
+            temperature: temperature,
+            timestamp: 0,
+            distance: distance.clone(),
+        };
+        let (tx, rx): (Sender<()>, Receiver<()>) = mpsc::channel();
+        let name = format!("Ultrasonic::echo(port = {})", echo_port);
+        let echo = ThreadData::<()> {
+            thread: thread::Builder::new()
+                .name(name)
+                .spawn(move || echo.thread(rx))?,
+            tx: tx,
+        };
+
+        let trigger = UltrasonicTrigger { trigger: SyncPort::new(trigger_port, Direction::Out)? };
+        let (tx, rx): (Sender<()>, Receiver<()>) = mpsc::channel();
+        let name = format!("Ultrasonic::trigger(port = {})", trigger_port);
+        let trigger = ThreadData::<()> {
+            thread: thread::Builder::new()
+                .name(name)
+                .spawn(move || trigger.thread(rx))?,
+            tx: tx,
+        };
+
+        Ok(Ultrasonic {
+               trigger: Some(trigger),
+               echo: Some(echo),
+               distance: distance,
+           })
+    }
+}
+
+impl Drop for Ultrasonic {
+    fn drop(&mut self) {
+        if let Some(echo) = self.echo.take() {
+            echo.tx.send(()).is_ok();
+            echo.thread.join().is_ok();
+        }
+
+        if let Some(trigger) = self.trigger.take() {
+            trigger.tx.send(()).is_ok();
+            trigger.thread.join().is_ok();
+        }
+    }
+}
+
+impl UltrasonicTrigger {
+    fn thread(mut self, stop_rx: Receiver<()>) {
+        #[cfg(feature = "measure")]
+        let mut measure = Measure::new(format!("Ultrasonic::trigger(port = {})", self.trigger.port.number));
+
+        while stop_rx.try_recv().is_err() {
+            #[cfg(feature = "measure")]
+            measure.start();
+            self.trigger.write(Value::Low).is_ok();
+            #[cfg(feature = "measure")]
+            measure.pause();
+
+            // sleep 10us (min length to trigger)
+            sleep(Duration::new(0, 10_000));
+
+            #[cfg(feature = "measure")]
+            measure.start();
+            self.trigger.write(Value::High).is_ok();
+            #[cfg(feature = "measure")]
+            measure.stop();
+
+            // sleep 20ms (max trigger frequency)
+            sleep(Duration::new(0, 20_000_000));
+        }
+    }
+}
+
+impl UltrasonicEcho {
+    fn thread(mut self, stop_rx: Receiver<()>) {
+        #[cfg(feature = "measure")]
+        let mut measure = Measure::new(format!("Ultrasonic::echo(port = {})", self.echo.port.number));
+
+        while stop_rx.try_recv().is_err() {
+            #[cfg(feature = "measure")]
+            measure.start();
+
+            #[cfg(not(feature = "measure"))]
+            let value = self.echo.poll(Some(Duration::new(1, 0)));
+
+            #[cfg(feature = "measure")]
+            let value = self.echo.poll_measure(Some(Duration::new(1, 0)), &mut measure);
+
+            match value {
+                Ok(value) => {
+                    if let Some(value) = value {
+                        self.on_value_changed(value);
+                    }
+                }
+                Err(_e) => {
+                    #[cfg(debug_assertions)]
+                    println!("POLL failed: e = {:?}", _e);
+                }
+            }
+
+            #[cfg(feature = "measure")]
+            measure.stop();
+        }
+    }
+
+    fn on_value_changed(&mut self, value: Value) {
+        match value {
+            Value::High => {
+                // Switched from Value::High to Value::High
+                // possibly because of trigger after timeout and slow reading
+                if self.timestamp > 0 {
+                    #[cfg(debug_assertions)]
+                    println!("{:?}", self);
+
+                    self.calc_distance();
+                }
+
+                self.timestamp = precise_time_ns()
+            }
+            Value::Low => {
+                // Switched from Value::Low to Value::Low
+                if self.timestamp == 0 {
+                    #[cfg(debug_assertions)]
+                    println!("{:?}", self);
+                }
+
+                self.calc_distance();
+            }
+        }
+    }
+
+    fn calc_distance(&mut self) {
+        let time_diff = precise_time_ns() - self.timestamp;
+        let distance = (3315 + self.temperature as u64 * 6) * time_diff / 20_000_000;
+        self.distance.store(distance as u32, Ordering::Relaxed);
+
+        #[cfg(debug_assertions)]
+        println!("time diff: {}\tdistance: {}mm", time_diff, distance);
+
+        self.timestamp = 0u64;
+    }
+}