diff --git a/flight/src/hardware/channelization.rs b/flight/src/hardware/channelization.rs new file mode 100644 index 0000000..7cbf35a --- /dev/null +++ b/flight/src/hardware/channelization.rs @@ -0,0 +1,2 @@ +pub const MCP23017_A_LED: u8 = 7; +pub const MCP23017_B_LED: u8 = 7; diff --git a/flight/src/hardware/mcp23017/driver.rs b/flight/src/hardware/mcp23017/driver.rs new file mode 100644 index 0000000..2f563ce --- /dev/null +++ b/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 +where + I2C: I2c + Send + Sync, + I2C::Error: Send, + I2C::Error: Sync, + I2C::Error: 'static, +{ + i2c: Mutex, + address: u8, + bank: AtomicU16, + last_flushed: AtomicU16, + in_use: AtomicU16, +} + +impl Debug for Mcp23017Driver +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 Mcp23017Driver +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 Drop for Mcp23017Driver +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 Mcp23017Pins for Mcp23017Driver +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 Mcp23017 for Mcp23017Driver +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 { + 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(()) + } +} diff --git a/flight/src/hardware/mcp23017/mod.rs b/flight/src/hardware/mcp23017/mod.rs index 2bceb4f..8aa0113 100644 --- a/flight/src/hardware/mcp23017/mod.rs +++ b/flight/src/hardware/mcp23017/mod.rs @@ -1,96 +1,23 @@ -use crate::hardware::error::I2cError; -use anyhow::{bail, Result}; -use embedded_hal::i2c::I2c; -use log::{info, trace}; -use std::fmt::{Debug, Formatter}; -use std::time::Instant; +mod pin; +mod driver; +mod task; + +use anyhow::Result; +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; + + fn flush(&self) -> Result<()>; } -pub struct Mcp23017Driver { - i2c: I2C, - address: u8, - bank: [u8; 2], - dirty: bool, +pub trait Mcp23017OutputPin { + fn set_state(&mut self, pin_state: PinState); + + fn set_state_on_drop(&mut self, pin_state: PinState); } -impl Debug for Mcp23017Driver { - fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result { - write!(f, "Mcp23017Driver {{ address: {} }}", self.address) - } -} - -impl Mcp23017Driver -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 Mcp23017 for Mcp23017Driver -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(()) - } -} +pub use driver::Mcp23017Driver; +pub use task::Mcp23017Task; diff --git a/flight/src/hardware/mcp23017/pin.rs b/flight/src/hardware/mcp23017/pin.rs new file mode 100644 index 0000000..12141f6 --- /dev/null +++ b/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, +} + +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); + } +} diff --git a/flight/src/hardware/mcp23017/task.rs b/flight/src/hardware/mcp23017/task.rs new file mode 100644 index 0000000..c687b47 --- /dev/null +++ b/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> { + 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); + } + })?) + } +} \ No newline at end of file diff --git a/flight/src/hardware/mod.rs b/flight/src/hardware/mod.rs index 8ea9ebb..92f8516 100644 --- a/flight/src/hardware/mod.rs +++ b/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 get_mcp23017_b(&self) -> impl Mcp23017; + fn new_mcp23017_a(&self) -> Result; + fn new_mcp23017_b(&self) -> Result; fn get_battery_voltage(&self) -> Result; } -// 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; diff --git a/flight/src/hardware/raspi/mod.rs b/flight/src/hardware/raspi/mod.rs index e211885..bdbc37e 100644 --- a/flight/src/hardware/raspi/mod.rs +++ b/flight/src/hardware/raspi/mod.rs @@ -41,12 +41,12 @@ impl RaspiHardware { } impl Hardware for RaspiHardware { - fn get_mcp23017_a(&self) -> impl Mcp23017 { - Mcp23017Driver::new(MutexDevice::new(&self.i2c_bus), 0b0100000) + fn new_mcp23017_a(&self) -> Result { + Ok(Mcp23017Driver::new(MutexDevice::new(&self.i2c_bus), 0b0100000)) } - fn get_mcp23017_b(&self) -> impl Mcp23017 { - Mcp23017Driver::new(MutexDevice::new(&self.i2c_bus), 0b0100001) + fn new_mcp23017_b(&self) -> Result { + Ok(Mcp23017Driver::new(MutexDevice::new(&self.i2c_bus), 0b0100001)) } fn get_battery_voltage(&self) -> Result { diff --git a/flight/src/lib.rs b/flight/src/lib.rs index e0bd209..0c4f2c2 100644 --- a/flight/src/lib.rs +++ b/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_b = hal.get_mcp23017_b(); + let mut mcp23017_a = hal.new_mcp23017_a()?; + 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)?; - mcp23017_a.flush()?; + let running = AtomicBool::new(true); - sleep(Duration::from_secs(1)); + 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)); - mcp23017_b.set_pin(7u8, true)?; - mcp23017_b.flush()?; + Mcp23017Task::new(&mcp23017_a, "mcp23017-a".into()) + .start(scope, &running, 10)?; - sleep(Duration::from_secs(1)); + Mcp23017Task::new(&mcp23017_b, "mcp23017-b".into()) + .start(scope, &running, 10)?; - mcp23017_a.set_pin(7u8, false)?; - mcp23017_a.flush()?; + 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); - sleep(Duration::from_secs(1)); - mcp23017_b.set_pin(7u8, false)?; - mcp23017_b.flush()?; + led_pin_a.set_state(PinState::High); + sleep(Duration::from_secs(1)); + led_pin_b.set_state(PinState::High); + sleep(Duration::from_secs(1)); + + anyhow::Ok(()) + })?; + + info!("Shutting Down"); // 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; diff --git a/flight/src/logger.rs b/flight/src/logger.rs index fe9dfe1..419f54c 100644 --- a/flight/src/logger.rs +++ b/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(""); + 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( diff --git a/flight/src/on_drop.rs b/flight/src/on_drop.rs new file mode 100644 index 0000000..04be51b --- /dev/null +++ b/flight/src/on_drop.rs @@ -0,0 +1,18 @@ +use log::trace; + +pub struct OnDrop { + func: F, +} + +pub fn on_drop(func: F) -> OnDrop { + OnDrop { + func + } +} + +impl Drop for OnDrop { + fn drop(&mut self) { + trace!("OnDrop::drop()"); + (self.func)() + } +}