initial state vector implementation

flight/src/hardware/mcp23017/mod.rs

@@ -13,4 +13,5 @@ pub trait Mcp23017 {
 }
 
 pub use driver::Mcp23017Driver;
+pub use task::Mcp23017State;
 pub use task::Mcp23017Task;

flight/src/hardware/mcp23017/task.rs

@@ -1,6 +1,7 @@
 use crate::hardware::mcp23017::Mcp23017;
 use crate::hardware::pin::PinDevice;
 use crate::scheduler::{CyclicTask, TaskHandle};
+use crate::state_vector::{SectionIdentifier, SectionWriter, StateVector};
 use embedded_hal::digital::PinState;
 use log::trace;
 use std::fmt::{Debug, Formatter};
@@ -31,12 +32,18 @@ impl PinDevice for TaskHandle<Mcp23017Message> {
     }
 }
 
-pub struct Mcp23017Task<M: Mcp23017> {
-    mcp23017: M,
-    pins: AllPins,
+#[derive(Default)]
+pub struct Mcp23017State {
+    pub pins: [bool; 16],
 }
 
-impl<M: Mcp23017 + Debug> Debug for Mcp23017Task<M> {
+pub struct Mcp23017Task<'a, M: Mcp23017> {
+    mcp23017: M,
+    pins: AllPins,
+    state: SectionWriter<'a, Mcp23017State>,
+}
+
+impl<M: Mcp23017 + Debug> Debug for Mcp23017Task<'_, M> {
     fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
         write!(f, "Mcp23017Task {{ mcp23017: {:?} }}", self.mcp23017)
     }
@@ -64,6 +71,7 @@ struct PinData {
     next_priority: u8,
     default: PinState,
     changed: bool,
+    value: PinState,
 }
 
 impl PinData {
@@ -77,16 +85,18 @@ impl PinData {
             next_priority: 0,
             default: PinState::Low,
             changed: false,
+            value: PinState::Low,
         }
     }
 
-    fn get(&mut self, now: Instant) -> PinState {
+    fn evaluate(&mut self, now: Instant) {
         // 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;
+                self.value = self.state;
+                return;
             } else {
                 if self.valid_until.is_some() {
                     self.changed = true;
@@ -100,7 +110,7 @@ impl PinData {
             }
         }
 
-        self.default
+        self.value = self.default;
     }
 
     fn set(&mut self, value: PinState, valid_until: Instant, priority: u8) {
@@ -123,6 +133,7 @@ impl PinData {
             self.valid_until = Some(valid_until);
             self.priority = priority;
             self.changed = true;
+            self.value = value;
         } else {
             // This is not the highest priority thing
             if self.priority >= self.next_priority {
@@ -138,17 +149,22 @@ impl PinData {
     }
 }
 
-impl<M: Mcp23017 + Debug> Mcp23017Task<M> {
-    pub fn new(mcp23017: M) -> Self {
+impl<'a, M: Mcp23017 + Debug> Mcp23017Task<'a, M> {
+    pub fn new(mcp23017: M, state_vector: &'a StateVector) -> Self {
         trace!("Mcp23017Task::new(mcp23017: {mcp23017:?})");
         Self {
             mcp23017,
             pins: AllPins::new(),
+            state: state_vector.create_section(Mcp23017State::default()),
         }
     }
+
+    pub fn get_state(&self) -> SectionIdentifier {
+        self.state.get_identifier()
+    }
 }
 
-impl<M: Mcp23017> CyclicTask for Mcp23017Task<M> {
+impl<M: Mcp23017> CyclicTask for Mcp23017Task<'_, M> {
     type Message = Mcp23017Message;
 
     fn step(
@@ -158,6 +174,10 @@ impl<M: Mcp23017> CyclicTask for Mcp23017Task<M> {
     ) {
         let mut changed = false;
 
+        for pin in 0u8..16u8 {
+            self.pins.pins[pin as usize].evaluate(step_time);
+        }
+
         while let Ok(recv) = receiver.try_recv() {
             match recv {
                 Mcp23017Message::SetPin { pin, value, valid_until, priority } => {
@@ -169,17 +189,20 @@ impl<M: Mcp23017> CyclicTask for Mcp23017Task<M> {
         }
 
         for pin in 0u8..16u8 {
-            // This shouldn't be able to fail
-            // TODO: handle error case
-            let state = self.pins.pins[pin as usize].get(step_time);
+            let state = self.pins.pins[pin as usize].value;
             if self.pins.pins[pin as usize].changed {
                 self.pins.pins[pin as usize].changed = false;
+                // This shouldn't be able to fail
+                // TODO: handle error case
                 let _ = self.mcp23017.set_pin(pin, state);
                 changed = true;
             }
         }
         if changed {
             let _ = self.mcp23017.flush();
+            self.state.update(|s| {
+                s.pins = self.pins.pins.map(|pin| pin.value == PinState::High);
+            });
         }
     }
 }