diff --git a/flight/src/hardware/channelization.rs b/flight/src/hardware/channelization.rs index 629bf50..2404370 100644 --- a/flight/src/hardware/channelization.rs +++ b/flight/src/hardware/channelization.rs @@ -1,7 +1,11 @@ #![allow(dead_code)] -use crate::hardware::mcp23017::{Mcp23017, Mcp23017OutputPin}; +use crate::hardware::pin::{Pin, PinDevice}; use anyhow::Result; +use embedded_hal::digital::PinState; +use log::trace; +use std::fmt::Debug; +use std::time::Instant; pub const RCS5: PinoutChannel = PinoutChannel::ExtA(0); pub const RCS6: PinoutChannel = PinoutChannel::ExtA(1); @@ -42,11 +46,37 @@ pub enum PinoutChannel { ExtB(u8), } -impl PinoutChannel { - pub fn new_output_pin<'a, MCP: Mcp23017>(self, ext_a: &'a MCP, ext_b: &'a MCP) -> Result { +pub struct DevicePin<'a, Device: PinDevice> { + pin: u8, + device: &'a Device, +} + +impl<'a, Device: PinDevice> Pin for DevicePin<'a, Device> { + fn set(&mut self, value: PinState, valid_until: Instant, priority: u8) { + self.device.set_pin(self.pin, value, valid_until, priority); + } +} + +pub enum ChannelPin<'a, A: PinDevice, B: PinDevice> { + ExtA(DevicePin<'a, A>), + ExtB(DevicePin<'a, B>), +} + +impl<'a, A: PinDevice, B: PinDevice> Pin for ChannelPin<'a, A, B> { + fn set(&mut self, value: PinState, valid_until: Instant, priority: u8) { match self { - PinoutChannel::ExtA(pin) => ext_a.new_output_pin(pin), - PinoutChannel::ExtB(pin) => ext_b.new_output_pin(pin), + ChannelPin::ExtA(pin) => pin.set(value, valid_until, priority), + ChannelPin::ExtB(pin) => pin.set(value, valid_until, priority), } } } + +impl PinoutChannel { + pub fn new<'a>(self, ext_a: &'a (impl PinDevice + Debug), ext_b: &'a (impl PinDevice + Debug)) -> Result { + trace!("PinoutChannel::new(self: {self:?}, ext_a: {ext_a:?}, ext_b: {ext_b:?}"); + Ok(match self { + PinoutChannel::ExtA(pin) => ChannelPin::ExtA(DevicePin { pin, device: ext_a }), + PinoutChannel::ExtB(pin) => ChannelPin::ExtB(DevicePin { pin, device: ext_b }), + }) + } +} diff --git a/flight/src/hardware/mcp23017/driver.rs b/flight/src/hardware/mcp23017/driver.rs index cf51464..ca6dd23 100644 --- a/flight/src/hardware/mcp23017/driver.rs +++ b/flight/src/hardware/mcp23017/driver.rs @@ -1,11 +1,10 @@ use crate::hardware::error::WrappingError; -use crate::hardware::mcp23017::pin::{Mcp23017OutputPinDriver, Mcp23017Pins}; -use crate::hardware::mcp23017::{Mcp23017, Mcp23017OutputPin}; +use crate::hardware::mcp23017::Mcp23017; use anyhow::bail; +use embedded_hal::digital::PinState; 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 @@ -17,9 +16,8 @@ where { i2c: Mutex, address: u8, - bank: AtomicU16, - last_flushed: AtomicU16, - in_use: AtomicU16, + bank: u16, + last_flushed: u16, } impl Debug for Mcp23017Driver @@ -46,9 +44,8 @@ where Self { i2c: i2c.into(), address, - bank: AtomicU16::new(0), - last_flushed: AtomicU16::new(0), - in_use: AtomicU16::new(0), + bank: 0, + last_flushed: 0, } } } @@ -62,40 +59,13 @@ where { fn drop(&mut self) { trace!("Mcp23017Driver::drop(self: {self:?})"); + self.bank = 0; // We want all pins to be set back to 0 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, @@ -116,42 +86,31 @@ where Ok(()) } - fn new_output_pin(&self, pin: u8) -> anyhow::Result { - trace!("Mcp23017Driver::new_output_pin(self: {self:?}, pin: {pin})"); + fn set_pin(&mut self, pin: u8, value: PinState) -> anyhow::Result<()> { + trace!("Mcp23017Driver::set_pin(self: {self:?}, pin: {pin}, value: {value:?})"); 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, - }) + self.bank = match value { + PinState::Low => self.bank & !pin_mask, + PinState::High => self.bank | pin_mask, + }; + + Ok(()) } - fn flush(&self) -> anyhow::Result<()> { + fn flush(&mut 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(); + if self.bank != self.last_flushed { + let bytes = self.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(WrappingError)?; self.i2c.clear_poison(); + self.last_flushed = self.bank; } } Ok(()) diff --git a/flight/src/hardware/mcp23017/mod.rs b/flight/src/hardware/mcp23017/mod.rs index 8aa0113..3b26865 100644 --- a/flight/src/hardware/mcp23017/mod.rs +++ b/flight/src/hardware/mcp23017/mod.rs @@ -1,6 +1,5 @@ -mod pin; -mod driver; mod task; +mod driver; use anyhow::Result; use embedded_hal::digital::PinState; @@ -8,15 +7,9 @@ use embedded_hal::digital::PinState; pub trait Mcp23017 { fn init(&mut self) -> Result<()>; - fn new_output_pin(&self, pin: u8) -> Result; + fn set_pin(&mut self, pin: u8, value: PinState) -> Result<()>; - fn flush(&self) -> Result<()>; -} - -pub trait Mcp23017OutputPin { - fn set_state(&mut self, pin_state: PinState); - - fn set_state_on_drop(&mut self, pin_state: PinState); + fn flush(&mut self) -> Result<()>; } pub use driver::Mcp23017Driver; diff --git a/flight/src/hardware/mcp23017/pin.rs b/flight/src/hardware/mcp23017/pin.rs deleted file mode 100644 index 12141f6..0000000 --- a/flight/src/hardware/mcp23017/pin.rs +++ /dev/null @@ -1,44 +0,0 @@ -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 index c687b47..4a4c1c7 100644 --- a/flight/src/hardware/mcp23017/task.rs +++ b/flight/src/hardware/mcp23017/task.rs @@ -1,36 +1,203 @@ use crate::hardware::mcp23017::Mcp23017; +use crate::hardware::pin::PinDevice; use anyhow::Result; +use embedded_hal::digital::PinState; +use log::trace; +use std::fmt::Debug; use std::sync::atomic::{AtomicBool, Ordering}; +use std::sync::mpsc::{channel, Sender}; use std::thread; use std::thread::{sleep, Scope, ScopedJoinHandle}; -use std::time::Duration; +use std::time::{Duration, Instant}; -pub struct Mcp23017Task<'a, M: Mcp23017 + Sync> { - mcp23017: &'a M, - name: String, +#[derive(Clone, Debug)] +pub enum Mcp23017Message { + SetPin { + pin: u8, + value: PinState, + valid_until: Instant, + priority: u8, + }, } -impl<'a, M: Mcp23017 + Sync> Mcp23017Task<'a, M> { - pub fn new( - mcp23017: &'a M, - name: String, - ) -> Self { +#[derive(Clone, Debug)] +pub struct Mcp23017Task { + #[allow(dead_code)] + name: String, + sender: Sender, +} + +impl PinDevice for Mcp23017Task { + fn set_pin(&self, pin: u8, value: PinState, valid_until: Instant, priority: u8) { + trace!("Mcp23017Task::set_pin(self: {self:?}, pin: {pin}, value: {value:?})"); + // This can only fail if the other end is disconnected - which we intentionally want to + // ignore + let _ = self.sender.send(Mcp23017Message::SetPin { + pin, + value, + valid_until, + priority, + }); + } +} + +struct AllPins { + pins: [PinData; 16], +} + +impl AllPins { + fn new() -> Self { Self { - mcp23017, - name, + pins: [PinData::new(); _], + } + } +} + +#[derive(Copy, Clone)] +struct PinData { + state: PinState, + valid_until: Option, + priority: u8, + next_state: PinState, + next_validity: Option, + next_priority: u8, + default: PinState, + changed: bool, +} + +impl PinData { + fn new() -> Self { + Self { + state: PinState::Low, + valid_until: None, + priority: 0, + next_state: PinState::Low, + next_validity: None, + next_priority: 0, + default: PinState::Low, + changed: false, } } - pub fn start(self, scope: &'a Scope<'a, '_>, running: &'a AtomicBool, frequency: u64) -> Result> { + fn get(&mut self, now: Instant) -> PinState { + // Do this twice to check both the current and the current next + // If the current is currently invalid, we'd upgrade the next to current + for _ in 0..2 { + let is_current_valid = self.valid_until.map(|current| current >= now).unwrap_or(false); + if is_current_valid { + return self.state; + } else { + if self.valid_until.is_some() { + self.changed = true; + } + self.state = self.next_state; + self.valid_until = self.next_validity; + self.priority = self.next_priority; + + self.next_validity = None; + self.next_priority = 0; + } + } + + self.default + } + + fn set(&mut self, value: PinState, valid_until: Instant, priority: u8) { + let can_replace_current = self.valid_until.map(|current| current <= valid_until).unwrap_or(true); + let can_replace_next = self.next_validity.map(|next| next <= valid_until).unwrap_or(true); + + if priority >= self.priority { + // This is now the highest priority thing (or most recent of equal priority) + if can_replace_current { + if can_replace_next { + self.next_validity = None; + self.next_priority = 0 + } + } else { + self.next_state = self.state; + self.next_validity = self.valid_until; + self.next_priority = self.priority; + } + self.state = value; + self.valid_until = Some(valid_until); + self.priority = priority; + self.changed = true; + } else { + // This is not the highest priority thing + if self.priority >= self.next_priority { + // Higher priority than the next highest though + self.next_state = value; + self.next_validity = Some(valid_until); + self.next_priority = priority; + self.changed = true; + } else { + // Not high enough priority to remember + } + } + } +} + +impl Mcp23017Task { + pub fn start<'a, M: Mcp23017 + Send + Debug>( + scope: &'a Scope<'a, '_>, + running: &'a AtomicBool, + mut mcp23017: M, + name: String, + frequency: u64, + ) -> Result<(ScopedJoinHandle<'a, ()>, Self)> where + M: 'a, + { + trace!("Mcp23017Task::start(scope, running, mcp23017: {mcp23017:?}, name: {name}, frequency: {frequency})"); + + let (sender, receiver) = channel::(); let period = Duration::from_nanos(1_000_000_000 / frequency); - Ok(thread::Builder::new() - .name(self.name) + let handle = thread::Builder::new() + .name(name.clone()) .spawn_scoped(scope, move || { + let mut pins = AllPins::new(); + let mut cycle_start_time = Instant::now(); while running.load(Ordering::Relaxed) { - let _ = self.mcp23017.flush(); - sleep(period); + let mut changed = false; + + while let Ok(recv) = receiver.try_recv() { + match recv { + Mcp23017Message::SetPin { pin, value, valid_until, priority } => { + if (0u8..16u8).contains(&pin) { + pins.pins[pin as usize].set(value, valid_until, priority); + } + } + } + } + + for pin in 0u8..16u8 { + // This shouldn't be able to fail + // TODO: handle error case + let state = pins.pins[pin as usize].get(cycle_start_time); + if pins.pins[pin as usize].changed { + pins.pins[pin as usize].changed = false; + let _ = mcp23017.set_pin(pin, state); + changed = true; + } + } + if changed { + let _ = mcp23017.flush(); + } + + cycle_start_time += period; + let sleep_duration = cycle_start_time - Instant::now(); + if sleep_duration > Duration::ZERO { + sleep(sleep_duration); + } } - })?) + })?; + + Ok(( + handle, + Self { + name, + sender, + } + )) } } \ No newline at end of file diff --git a/flight/src/hardware/mod.rs b/flight/src/hardware/mod.rs index 0318fe8..4abe141 100644 --- a/flight/src/hardware/mod.rs +++ b/flight/src/hardware/mod.rs @@ -2,9 +2,10 @@ use crate::hardware::mcp23017::Mcp23017; use crate::hardware::mct8316a::Mct8316a; use anyhow::Result; use embedded_hal::pwm::SetDutyCycle; +use std::fmt::Debug; pub trait Hardware { - type Mcp23017<'a>: Mcp23017 + Sync + type Mcp23017<'a>: Mcp23017 + Send + Debug where Self: 'a; type Pwm: SetDutyCycle + Sync; @@ -41,3 +42,4 @@ mod mcp3208; pub mod channelization; pub mod mct8316a; mod sim; +pub mod pin; diff --git a/flight/src/hardware/pin.rs b/flight/src/hardware/pin.rs new file mode 100644 index 0000000..ddfd76d --- /dev/null +++ b/flight/src/hardware/pin.rs @@ -0,0 +1,10 @@ +use embedded_hal::digital::PinState; +use std::time::Instant; + +pub trait PinDevice { + fn set_pin(&self, pin: u8, value: PinState, valid_until: Instant, priority: u8); +} + +pub trait Pin { + fn set(&mut self, value: PinState, valid_until: Instant, priority: u8); +} diff --git a/flight/src/lib.rs b/flight/src/lib.rs index 93dd3a0..cc0ace4 100644 --- a/flight/src/lib.rs +++ b/flight/src/lib.rs @@ -1,8 +1,8 @@ use crate::hardware::channelization::{LED_A, LED_B}; use crate::hardware::initialize; -use crate::hardware::mcp23017::Mcp23017OutputPin; use crate::hardware::mcp23017::{Mcp23017, Mcp23017Task}; use crate::hardware::mct8316a::Mct8316a; +use crate::hardware::pin::Pin; use crate::hardware::Hardware; use crate::on_drop::on_drop; use anyhow::Result; @@ -13,7 +13,7 @@ use std::sync::atomic::{AtomicBool, Ordering}; use std::sync::Arc; use std::thread; use std::thread::sleep; -use std::time::Duration; +use std::time::{Duration, Instant}; mod hardware; @@ -55,36 +55,30 @@ pub fn run() -> Result<()> { // 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)?; + let (_, task_a) = Mcp23017Task::start(scope, &running, mcp23017_a, "mcp23017-a".into(), 10)?; - Mcp23017Task::new(&mcp23017_b, "mcp23017-b".into()) - .start(scope, &running, 10)?; + let (_, task_b) = Mcp23017Task::start(scope, &running, mcp23017_b, "mcp23017-b".into(), 10)?; - let mut led_pin_a = LED_A.new_output_pin(&mcp23017_a, &mcp23017_b)?; - led_pin_a.set_state_on_drop(PinState::Low); - let mut led_pin_b = LED_B.new_output_pin(&mcp23017_a, &mcp23017_b)?; - led_pin_b.set_state_on_drop(PinState::Low); + let mut led_pin_a = LED_A.new(&task_a, &task_b)?; + let mut led_pin_b = LED_B.new(&task_a, &task_b)?; info!("Starting Main Loop"); for _ in 0..2 { debug!("A On"); - led_pin_a.set_state(PinState::High); + led_pin_a.set(PinState::High, Instant::now() + Duration::from_secs(2), 0); sleep(Duration::from_secs(1)); if !running.load(Ordering::Relaxed) { break; }; debug!("B On"); - led_pin_b.set_state(PinState::High); + led_pin_b.set(PinState::High, Instant::now() + Duration::from_secs(2), 0); sleep(Duration::from_secs(1)); if !running.load(Ordering::Relaxed) { break; }; debug!("A Off"); - led_pin_a.set_state(PinState::Low); sleep(Duration::from_secs(1)); if !running.load(Ordering::Relaxed) { break; }; debug!("B Off"); - led_pin_b.set_state(PinState::Low); sleep(Duration::from_secs(1)); if !running.load(Ordering::Relaxed) { break; }; } @@ -96,8 +90,6 @@ pub fn run() -> Result<()> { // Explicitly drop these to allow the borrow checker to be sure that // dropping the hal is safe - drop(mcp23017_a); - drop(mcp23017_b); drop(pwm0); drop(mct8316); @@ -110,3 +102,4 @@ pub fn run() -> Result<()> { mod test_utils; mod data; mod on_drop; +mod rcs; diff --git a/flight/src/rcs/mod.rs b/flight/src/rcs/mod.rs new file mode 100644 index 0000000..3cdfa9a --- /dev/null +++ b/flight/src/rcs/mod.rs @@ -0,0 +1,13 @@ +// use crate::hardware::mcp23017::Mcp23017OutputPin; +// +// struct RcsTask { +// pin: PIN, +// } +// +// impl RcsTask { +// pub fn new(pin: PIN) -> Self { +// Self { +// pin +// } +// } +// }