improve mcp23017 task

flight/src/hardware/channelization.rs

@@ -0,0 +1,2 @@
+pub const MCP23017_A_LED: u8 = 7;
+pub const MCP23017_B_LED: u8 = 7;

flight/src/hardware/mcp23017/driver.rs

@@ -0,0 +1,159 @@
+use crate::hardware::error::I2cError;
+use crate::hardware::mcp23017::pin::{Mcp23017OutputPinDriver, Mcp23017Pins};
+use crate::hardware::mcp23017::{Mcp23017, Mcp23017OutputPin};
+use anyhow::bail;
+use embedded_hal::i2c::I2c;
+use log::{error, trace};
+use std::fmt::{Debug, Formatter};
+use std::sync::atomic::{AtomicU16, Ordering};
+use std::sync::Mutex;
+
+pub struct Mcp23017Driver<I2C>
+where
+    I2C: I2c + Send + Sync,
+    I2C::Error: Send,
+    I2C::Error: Sync,
+    I2C::Error: 'static,
+{
+    i2c: Mutex<I2C>,
+    address: u8,
+    bank: AtomicU16,
+    last_flushed: AtomicU16,
+    in_use: AtomicU16,
+}
+
+impl<I2C> Debug for Mcp23017Driver<I2C>
+where
+    I2C: I2c + Send + Sync,
+    I2C::Error: Send,
+    I2C::Error: Sync,
+    I2C::Error: 'static,
+{
+    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
+        write!(f, "Mcp23017Driver {{ address: {} }}", self.address)
+    }
+}
+
+impl<I2C> Mcp23017Driver<I2C>
+where
+    I2C: I2c + Send + Sync,
+    I2C::Error: Send,
+    I2C::Error: Sync,
+    I2C::Error: 'static,
+{
+    pub fn new(i2c: I2C, address: u8) -> Self {
+        trace!("Mcp23017Driver::new(i2c, address: {address:07b})");
+        Self {
+            i2c: i2c.into(),
+            address,
+            bank: AtomicU16::new(0),
+            last_flushed: AtomicU16::new(0),
+            in_use: AtomicU16::new(0),
+        }
+    }
+}
+
+impl<I2C> Drop for Mcp23017Driver<I2C>
+where
+    I2C: I2c + Send + Sync,
+    I2C::Error: Send,
+    I2C::Error: Sync,
+    I2C::Error: 'static,
+{
+    fn drop(&mut self) {
+        trace!("Mcp23017Driver::drop(self: {self:?})");
+        if let Err(e) = self.flush() {
+            error!("Mcp23017Driver: Failed to flush on drop. {self:?} Error: {e}")
+        }
+    }
+}
+
+impl<I2C> Mcp23017Pins for Mcp23017Driver<I2C>
+where
+    I2C: I2c + Send + Sync,
+    I2C::Error: Send,
+    I2C::Error: Sync,
+    I2C::Error: 'static,
+{
+    fn set_pin(&self, pin: u8, value: bool) {
+        trace!("Mcp23017Driver::set_pin(self: {self:?}, pin: {pin}, value: {value})");
+        let pin_mask = 1u16 << pin;
+        if value {
+            // Relaxed because we don't care about the value
+            self.bank.fetch_or(pin_mask, Ordering::Relaxed);
+        } else {
+            // Relaxed because we don't care about the value
+            self.bank.fetch_and(!pin_mask, Ordering::Relaxed);
+        }
+    }
+
+    fn release_pin(&self, pin: u8) {
+        trace!("Mcp23017Driver::release_pin(self: {self:?}, pin: {pin})");
+        let pin_mask = 1u16 << pin;
+        // Sequentially Consistent because we want all accesses here
+        // to have a single modification order
+        self.in_use.fetch_and(!pin_mask, Ordering::SeqCst);
+    }
+}
+
+impl<I2C> Mcp23017 for Mcp23017Driver<I2C>
+where
+    I2C: I2c + Send + Sync,
+    I2C::Error: Send,
+    I2C::Error: Sync,
+    I2C::Error: 'static,
+{
+    fn init(&mut self) -> anyhow::Result<()> {
+        trace!("Mcp23017Driver::init(self: {self:?})");
+        // Set each pin as an output (Addresses 0x00 & 0x01)
+        let data: [u8; _] = [0x00, 0x00, 0x00];
+
+        match self.i2c.get_mut() {
+            Ok(lock) => lock.write(self.address, &data).map_err(I2cError)?,
+            Err(_) => bail!("Lock was poisoned"),
+        }
+
+        Ok(())
+    }
+
+    fn new_output_pin(&self, pin: u8) -> anyhow::Result<impl Mcp23017OutputPin> {
+        trace!("Mcp23017Driver::new_output_pin(self: {self:?}, pin: {pin})");
+        if !(0u8..16u8).contains(&pin) {
+            bail!("Invalid Pin ID")
+        }
+
+        let pin_mask = 1u16 << pin;
+        // Sequentially Consistent because we want all accesses here
+        // to have a single modification order
+        let previous_value = self.in_use.fetch_or(pin_mask, Ordering::SeqCst);
+        if (previous_value & pin_mask) != 0 {
+            // If the pin was previously enabled
+            bail!("Output Pin Already Exists");
+        }
+
+        Ok(Mcp23017OutputPinDriver {
+            mcp23017: self,
+            pin,
+            on_drop: None,
+        })
+    }
+
+    fn flush(&self) -> anyhow::Result<()> {
+        trace!("Mcp23017Driver::flush(self: {self:?})");
+        // Relaxed because we don't care about the value
+        let bank = self.bank.load(Ordering::Relaxed);
+        // Acquire-Release because we want all previous writes to be visible
+        let last_flushed = self.last_flushed.swap(bank, Ordering::AcqRel);
+        let dirty = bank != last_flushed;
+        if dirty {
+            let bytes = bank.to_le_bytes();
+            let data: [u8; _] = [0x12, bytes[0], bytes[1]];
+            // This blocks while writing
+            if let Ok(mut lock) = self.i2c.lock() {
+                lock.write(self.address, &data).map_err(I2cError)?;
+                self.i2c.clear_poison();
+            }
+        }
+        Ok(())
+    }
+}

flight/src/hardware/mcp23017/mod.rs

@@ -1,96 +1,23 @@
-use crate::hardware::error::I2cError;
+mod pin;
-use anyhow::{bail, Result};
+mod driver;
-use embedded_hal::i2c::I2c;
+mod task;
-use log::{info, trace};
+
-use std::fmt::{Debug, Formatter};
+use anyhow::Result;
-use std::time::Instant;
+use embedded_hal::digital::PinState;
 
 pub trait Mcp23017 {
     fn init(&mut self) -> Result<()>;
-    fn set_pin(&mut self, pin: u8, value: bool) -> Result<()>;
+
-    fn flush(&mut self) -> Result<()>;
+    fn new_output_pin(&self, pin: u8) -> Result<impl Mcp23017OutputPin>;
+
+    fn flush(&self) -> Result<()>;
 }
 
-pub struct Mcp23017Driver<I2C> {
+pub trait Mcp23017OutputPin {
-    i2c: I2C,
+    fn set_state(&mut self, pin_state: PinState);
-    address: u8,
+
-    bank: [u8; 2],
+    fn set_state_on_drop(&mut self, pin_state: PinState);
-    dirty: bool,
 }
 
-impl<I2C> Debug for Mcp23017Driver<I2C> {
+pub use driver::Mcp23017Driver;
-    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
+pub use task::Mcp23017Task;
-        write!(f, "Mcp23017Driver {{ address: {} }}", self.address)
-    }
-}
-
-impl<I2C> Mcp23017Driver<I2C>
-where
-    I2C: I2c,
-    I2C::Error: Send,
-    I2C::Error: Sync,
-    I2C::Error: 'static,
-{
-    pub fn new(i2c: I2C, address: u8) -> Self {
-        trace!("Mcp23017Driver::new(i2c, address: {address:07b})");
-        Self {
-            i2c,
-            address,
-            bank: [0u8; _],
-            dirty: false,
-        }
-    }
-}
-
-
-impl<I2C> Mcp23017 for Mcp23017Driver<I2C>
-where
-    I2C: I2c,
-    I2C::Error: Send,
-    I2C::Error: Sync,
-    I2C::Error: 'static,
-{
-    fn init(&mut self) -> Result<()> {
-        trace!("Mcp23017Driver::init(self: {self:?})");
-        // Set each pin as an output (Addresses 0x00 & 0x01)
-        let data: [u8; _] = [0x00, 0x00, 0x00];
-        self.i2c.write(self.address, &data).map_err(I2cError)?;
-
-        Ok(())
-    }
-
-    fn set_pin(&mut self, pin: u8, value: bool) -> Result<()> {
-        trace!("Mcp23017Driver::set_pin(self: {self:?}, pin: {pin}, value: {value})");
-        let (pin_bank, dirty_flag, pin_index) = match pin {
-            0..8 => {
-                (&mut self.bank[0], &mut self.dirty, pin as u32)
-            }
-            8..16 => {
-                (&mut self.bank[1], &mut self.dirty, (pin as u32) - 8)
-            }
-            _ => bail!("Invalid Pin ID"),
-        };
-        let pin_mask = 1u8.unbounded_shl(pin_index);
-        let initial = *pin_bank;
-        if value {
-            *pin_bank |= pin_mask;
-        } else {
-            *pin_bank &= !pin_mask;
-        }
-        let result = *pin_bank;
-        if initial != result {
-            *dirty_flag = true;
-        }
-        Ok(())
-    }
-
-    fn flush(&mut self) -> Result<()> {
-        trace!("Mcp23017Driver::flush(self: {self:?})");
-        if self.dirty {
-            let data: [u8; _] = [0x12, self.bank[0], self.bank[1]];
-            // This blocks while writing
-            self.i2c.write(self.address, &data).map_err(I2cError)?;
-        }
-        Ok(())
-    }
-}

flight/src/hardware/mcp23017/pin.rs

@@ -0,0 +1,44 @@
+use crate::hardware::mcp23017::Mcp23017OutputPin;
+use embedded_hal::digital::PinState;
+use log::trace;
+use std::fmt::{Debug, Formatter};
+
+pub(super) trait Mcp23017Pins {
+    fn set_pin(&self, pin: u8, value: bool);
+
+    fn release_pin(&self, pin: u8);
+}
+
+pub struct Mcp23017OutputPinDriver<'a, Device: Mcp23017Pins> {
+    pub(super) mcp23017: &'a Device,
+    pub(super) pin: u8,
+    pub(super) on_drop: Option<PinState>,
+}
+
+impl<'a, Device: Mcp23017Pins> Debug for Mcp23017OutputPinDriver<'a, Device> {
+    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
+        write!(f, "Mcp23017OutputPin {{ pin: {} }}", self.pin)
+    }
+}
+
+impl<'a, Device: Mcp23017Pins> Mcp23017OutputPin for Mcp23017OutputPinDriver<'a, Device> {
+    fn set_state(&mut self, pin_state: PinState) {
+        trace!("Mcp23017OutputPin::set_state(self: {self:?}, pin_state: {pin_state:?})");
+        self.mcp23017.set_pin(self.pin, pin_state.into())
+    }
+
+    fn set_state_on_drop(&mut self, pin_state: PinState) {
+        trace!("Mcp23017OutputPin::set_state_on_drop(self: {self:?}, pin_state: {pin_state:?})");
+        self.on_drop = Some(pin_state);
+    }
+}
+
+impl<'a, Device: Mcp23017Pins> Drop for Mcp23017OutputPinDriver<'a, Device> {
+    fn drop(&mut self) {
+        trace!("Mcp23017OutputPin::drop(self: {self:?})");
+        if let Some(pin_state) = self.on_drop {
+            self.mcp23017.set_pin(self.pin, pin_state.into());
+        }
+        self.mcp23017.release_pin(self.pin);
+    }
+}

flight/src/hardware/mcp23017/task.rs

@@ -0,0 +1,36 @@
+use crate::hardware::mcp23017::Mcp23017;
+use anyhow::Result;
+use std::sync::atomic::{AtomicBool, Ordering};
+use std::thread;
+use std::thread::{sleep, Scope, ScopedJoinHandle};
+use std::time::Duration;
+
+pub struct Mcp23017Task<'a, M: Mcp23017 + Sync> {
+    mcp23017: &'a M,
+    name: String,
+}
+
+impl<'a, M: Mcp23017 + Sync> Mcp23017Task<'a, M> {
+    pub fn new(
+        mcp23017: &'a M,
+        name: String,
+    ) -> Self {
+        Self {
+            mcp23017,
+            name,
+        }
+    }
+
+    pub fn start(self, scope: &'a Scope<'a, '_>, running: &'a AtomicBool, frequency: u64) -> Result<ScopedJoinHandle<'a, ()>> {
+        let period = Duration::from_nanos(1_000_000_000 / frequency);
+
+        Ok(thread::Builder::new()
+            .name(self.name)
+            .spawn_scoped(scope, move || {
+                while running.load(Ordering::Relaxed) {
+                    let _ = self.mcp23017.flush();
+                    sleep(period);
+                }
+            })?)
+    }
+}

flight/src/hardware/mod.rs

@@ -2,18 +2,12 @@ use crate::hardware::mcp23017::Mcp23017;
 use anyhow::Result;
 
 pub trait Hardware {
-    fn get_mcp23017_a(&self) -> impl Mcp23017;
+    fn new_mcp23017_a(&self) -> Result<impl Mcp23017 + Sync>;
-    fn get_mcp23017_b(&self) -> impl Mcp23017;
+    fn new_mcp23017_b(&self) -> Result<impl Mcp23017 + Sync>;
 
     fn get_battery_voltage(&self) -> Result<f64>;
 }
 
-// impl Hardware for () {
-//     fn get_i2c0_bus(&self) -> impl I2c {
-//         panic!()
-//     }
-// }
-
 #[cfg(feature = "raspi")]
 mod raspi;
 
@@ -38,3 +32,4 @@ mod error;
 
 pub mod mcp23017;
 mod mcp3208;
+pub mod channelization;

flight/src/hardware/raspi/mod.rs

@@ -41,12 +41,12 @@ impl RaspiHardware {
 }
 
 impl Hardware for RaspiHardware {
-    fn get_mcp23017_a(&self) -> impl Mcp23017 {
+    fn new_mcp23017_a(&self) -> Result<impl Mcp23017> {
-        Mcp23017Driver::new(MutexDevice::new(&self.i2c_bus), 0b0100000)
+        Ok(Mcp23017Driver::new(MutexDevice::new(&self.i2c_bus), 0b0100000))
     }
 
-    fn get_mcp23017_b(&self) -> impl Mcp23017 {
+    fn new_mcp23017_b(&self) -> Result<impl Mcp23017> {
-        Mcp23017Driver::new(MutexDevice::new(&self.i2c_bus), 0b0100001)
+        Ok(Mcp23017Driver::new(MutexDevice::new(&self.i2c_bus), 0b0100001))
     }
 
     fn get_battery_voltage(&self) -> Result<f64> {

flight/src/lib.rs

@@ -1,9 +1,15 @@
+use crate::hardware::channelization::{MCP23017_A_LED, MCP23017_B_LED};
 use crate::hardware::initialize;
-use crate::hardware::mcp23017::Mcp23017;
+use crate::hardware::mcp23017::Mcp23017OutputPin;
+use crate::hardware::mcp23017::{Mcp23017, Mcp23017Task};
 use crate::hardware::Hardware;
 use crate::logger::setup_logger;
+use crate::on_drop::on_drop;
 use anyhow::Result;
+use embedded_hal::digital::PinState;
 use log::info;
+use std::sync::atomic::{AtomicBool, Ordering};
+use std::thread;
 use std::thread::sleep;
 use std::time::Duration;
 
@@ -19,31 +25,43 @@ pub fn run() -> Result<()> {
 
     let hal = initialize()?;
 
-    let mut mcp23017_a = hal.get_mcp23017_a();
+    let mut mcp23017_a = hal.new_mcp23017_a()?;
-    let mut mcp23017_b = hal.get_mcp23017_b();
+    let mut mcp23017_b = hal.new_mcp23017_b()?;
 
     info!("Battery Voltage: {}", hal.get_battery_voltage()?);
 
     mcp23017_a.init()?;
     mcp23017_b.init()?;
 
-    mcp23017_a.set_pin(7u8, true)?;
+    let running = AtomicBool::new(true);
-    mcp23017_a.flush()?;
 
+    thread::scope(|scope| {
+        // This will automatically set running to false when it drops
+        // This means that if the main thread exits this scope, we will
+        // shut down any side branches
+        let _shutdown_threads = on_drop(|| running.store(false, Ordering::Relaxed));
+
+        Mcp23017Task::new(&mcp23017_a, "mcp23017-a".into())
+            .start(scope, &running, 10)?;
+
+        Mcp23017Task::new(&mcp23017_b, "mcp23017-b".into())
+            .start(scope, &running, 10)?;
+
+        let mut led_pin_a = mcp23017_a.new_output_pin(MCP23017_A_LED)?;
+        led_pin_a.set_state_on_drop(PinState::Low);
+        let mut led_pin_b = mcp23017_b.new_output_pin(MCP23017_B_LED)?;
+        led_pin_b.set_state_on_drop(PinState::Low);
+
+
+        led_pin_a.set_state(PinState::High);
+        sleep(Duration::from_secs(1));
+        led_pin_b.set_state(PinState::High);
         sleep(Duration::from_secs(1));
 
-    mcp23017_b.set_pin(7u8, true)?;
+        anyhow::Ok(())
-    mcp23017_b.flush()?;
+    })?;
 
-    sleep(Duration::from_secs(1));
+    info!("Shutting Down");
-
-    mcp23017_a.set_pin(7u8, false)?;
-    mcp23017_a.flush()?;
-
-    sleep(Duration::from_secs(1));
-
-    mcp23017_b.set_pin(7u8, false)?;
-    mcp23017_b.flush()?;
 
     // Explicitly drop these to allow the borrow checker to be sure that
     // dropping the hal is safe
@@ -58,3 +76,4 @@ pub fn run() -> Result<()> {
 #[cfg(test)]
 mod test_utils;
 mod data;
+mod on_drop;

flight/src/logger.rs

@@ -1,8 +1,8 @@
 use anyhow::Result;
 use log::debug;
-use std::env;
 use std::fs::create_dir_all;
 use std::str::FromStr;
+use std::{env, thread};
 
 pub fn setup_logger() -> Result<()> {
     let log_file = env::var("LOG_FILE").or_else(|_| {
@@ -21,12 +21,16 @@ pub fn setup_logger() -> Result<()> {
 
     fern::Dispatch::new()
         .format(|out, message, record| {
+            let binding = thread::current();
+            let thread_name = binding.name().unwrap_or("<unnamed>");
+
             out.finish(format_args!(
-                "[{}][{}][{}] {}",
+                "[{}][{}][{}][{}] {}",
-                chrono::Local::now().format("%Y-%m-%d %H:%M:%S"),
-                record.target(),
                 record.level(),
-                message
+                chrono::Local::now().format("%Y-%m-%d %H:%M:%S%.9f"),
+                thread_name,
+                record.target(),
+                message,
             ))
         })
         .chain(

flight/src/on_drop.rs

@@ -0,0 +1,18 @@
+use log::trace;
+
+pub struct OnDrop<F: FnMut()> {
+    func: F,
+}
+
+pub fn on_drop<F: FnMut()>(func: F) -> OnDrop<F> {
+    OnDrop {
+        func
+    }
+}
+
+impl<F: FnMut()> Drop for OnDrop<F> {
+    fn drop(&mut self) {
+        trace!("OnDrop::drop()");
+        (self.func)()
+    }
+}