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

#![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 {
    /// Constructs a new `Ultrasonic` and start threads.
    ///
    /// # Threads
    /// - `UltrasonicTrigger`
    /// - `UltrasonicEcho`
    ///
    /// # Parameter
    /// - `trigger_port` GPIO Port number of trigger pin.
    /// - `echo_port` GPIO Port number of echo pin.
    /// - `temperature` Room temperature in °C.
    pub fn new(trigger_port: u8, echo_port: u8, temperature: u8) -> std::io::Result<Ultrasonic> {
        let distance = Arc::new(AtomicU32::new(u32::max_value()));

        /// Create `UltrasonicEcho` thread with sync channel.
        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,
        };

        /// Create `UltrasonicTrigger` thread with sync channel.
        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 {
    /// Drop sync channels and join threads.
    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 {
    /// `UltrasonicTrigger` thread function.
    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 {
    /// `UltrasonicEcho` thread function.
    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();
        }
    }

    /// Start time measure or calculates distance based on port value
    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();
            }
        }
    }

    /// Calculates distance based on time measurement and `temperature` and stores it in `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;
    }
}