initial safety biased leased control architecture

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 struct DevicePin<'a, Device: PinDevice> {
-    pub fn new_output_pin<'a, MCP: Mcp23017>(self, ext_a: &'a MCP, ext_b: &'a MCP) -> Result<impl Mcp23017OutputPin + 'a> {
+    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),
+            ChannelPin::ExtA(pin) => pin.set(value, valid_until, priority),
-            PinoutChannel::ExtB(pin) => ext_b.new_output_pin(pin),
+            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<impl Pin> {
+        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 }),
+        })
+    }
+}

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;
-use crate::hardware::mcp23017::{Mcp23017, Mcp23017OutputPin};
 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<I2C>
@@ -17,9 +16,8 @@ where
 {
     i2c: Mutex<I2C>,
     address: u8,
-    bank: AtomicU16,
+    bank: u16,
-    last_flushed: AtomicU16,
+    last_flushed: u16,
-    in_use: AtomicU16,
 }
 
 impl<I2C> Debug for Mcp23017Driver<I2C>
@@ -46,9 +44,8 @@ where
         Self {
             i2c: i2c.into(),
             address,
-            bank: AtomicU16::new(0),
+            bank: 0,
-            last_flushed: AtomicU16::new(0),
+            last_flushed: 0,
-            in_use: AtomicU16::new(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<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,
@@ -116,42 +86,31 @@ where
         Ok(())
     }
 
-    fn new_output_pin(&self, pin: u8) -> anyhow::Result<impl Mcp23017OutputPin> {
+    fn set_pin(&mut self, pin: u8, value: PinState) -> anyhow::Result<()> {
-        trace!("Mcp23017Driver::new_output_pin(self: {self:?}, pin: {pin})");
+        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 {
+        self.bank = match value {
-            mcp23017: self,
+            PinState::Low => self.bank & !pin_mask,
-            pin,
+            PinState::High => self.bank | pin_mask,
-            on_drop: None,
+        };
-        })
+
+        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
+        if self.bank != self.last_flushed {
-        let bank = self.bank.load(Ordering::Relaxed);
+            let bytes = self.bank.to_le_bytes();
-        // 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(WrappingError)?;
                 self.i2c.clear_poison();
+                self.last_flushed = self.bank;
             }
         }
         Ok(())

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<impl Mcp23017OutputPin>;
+    fn set_pin(&mut self, pin: u8, value: PinState) -> Result<()>;
 
-    fn flush(&self) -> Result<()>;
+    fn flush(&mut self) -> Result<()>;
-}
-
-pub trait Mcp23017OutputPin {
-    fn set_state(&mut self, pin_state: PinState);
-
-    fn set_state_on_drop(&mut self, pin_state: PinState);
 }
 
 pub use driver::Mcp23017Driver;

flight/src/hardware/mcp23017/pin.rs

@@ -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<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

@@ -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> {
+#[derive(Clone, Debug)]
-    mcp23017: &'a M,
+pub enum Mcp23017Message {
-    name: String,
+    SetPin {
+        pin: u8,
+        value: PinState,
+        valid_until: Instant,
+        priority: u8,
+    },
 }
 
-impl<'a, M: Mcp23017 + Sync> Mcp23017Task<'a, M> {
+#[derive(Clone, Debug)]
-    pub fn new(
+pub struct Mcp23017Task {
-        mcp23017: &'a M,
+    #[allow(dead_code)]
-        name: String,
+    name: String,
-    ) -> Self {
+    sender: Sender<Mcp23017Message>,
+}
+
+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,
+            pins: [PinData::new(); _],
-            name,
+        }
+    }
+}
+
+#[derive(Copy, Clone)]
+struct PinData {
+    state: PinState,
+    valid_until: Option<Instant>,
+    priority: u8,
+    next_state: PinState,
+    next_validity: Option<Instant>,
+    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<ScopedJoinHandle<'a, ()>> {
+    fn get(&mut self, now: Instant) -> PinState {
-        let period = Duration::from_nanos(1_000_000_000 / frequency);
+        // 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
-        Ok(thread::Builder::new()
+        for _ in 0..2 {
-            .name(self.name)
+            let is_current_valid = self.valid_until.map(|current| current >= now).unwrap_or(false);
-            .spawn_scoped(scope, move || {
+            if is_current_valid {
-                while running.load(Ordering::Relaxed) {
+                return self.state;
-                    let _ = self.mcp23017.flush();
+            } else {
-                    sleep(period);
+                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::<Mcp23017Message>();
+        let period = Duration::from_nanos(1_000_000_000 / frequency);
+
+        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 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,
+            }
+        ))
     }
 }

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<Error: std::error::Error + Sync + Send> + Sync;
@@ -41,3 +42,4 @@ mod mcp3208;
 pub mod channelization;
 pub mod mct8316a;
 mod sim;
+pub mod pin;

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);
+}

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())
+        let (_, task_a) = Mcp23017Task::start(scope, &running, mcp23017_a, "mcp23017-a".into(), 10)?;
-            .start(scope, &running, 10)?;
 
-        Mcp23017Task::new(&mcp23017_b, "mcp23017-b".into())
+        let (_, task_b) = Mcp23017Task::start(scope, &running, mcp23017_b, "mcp23017-b".into(), 10)?;
-            .start(scope, &running, 10)?;
 
-        let mut led_pin_a = LED_A.new_output_pin(&mcp23017_a, &mcp23017_b)?;
+        let mut led_pin_a = LED_A.new(&task_a, &task_b)?;
-        led_pin_a.set_state_on_drop(PinState::Low);
+        let mut led_pin_b = LED_B.new(&task_a, &task_b)?;
-        let mut led_pin_b = LED_B.new_output_pin(&mcp23017_a, &mcp23017_b)?;
-        led_pin_b.set_state_on_drop(PinState::Low);
 
         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;

flight/src/rcs/mod.rs

@@ -0,0 +1,13 @@
+// use crate::hardware::mcp23017::Mcp23017OutputPin;
+//
+// struct RcsTask<PIN: Mcp23017OutputPin> {
+//     pin: PIN,
+// }
+//
+// impl<PIN: Mcp23017OutputPin> RcsTask<PIN> {
+//     pub fn new(pin: PIN) -> Self {
+//         Self {
+//             pin
+//         }
+//     }
+// }