sergeysav/ProjectNautilus
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

initial safety biased leased control architecture

Browse Source
This commit is contained in:
Sergey Savelyev
2025-10-22 22:20:28 -07:00
parent 26271fcb17
commit fd63bdc0c9
9 changed files with 276 additions and 153 deletions
Download Patch File Download Diff File Expand all files Collapse all files

40
flight/src/hardware/channelization.rs
View File

@@ -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<impl Mcp23017OutputPin + 'a> {
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<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 }),
})
}
}

81
flight/src/hardware/mcp23017/driver.rs
View File

@@ -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<I2C>
@@ -17,9 +16,8 @@ where
{
i2c: Mutex<I2C>,
address: u8,
bank: AtomicU16,
last_flushed: AtomicU16,
in_use: AtomicU16,
bank: u16,
last_flushed: u16,
}
impl<I2C> Debug for Mcp23017Driver<I2C>
@@ -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<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> {
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");
self.bank = match value {
PinState::Low => self.bank & !pin_mask,
PinState::High => self.bank | pin_mask,
};
Ok(())
}
Ok(Mcp23017OutputPinDriver {
mcp23017: self,
pin,
on_drop: None,
})
}
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(())

13
flight/src/hardware/mcp23017/mod.rs
View File

@@ -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<()>;
}
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;

44
flight/src/hardware/mcp23017/pin.rs
View File

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

201
flight/src/hardware/mcp23017/task.rs
View File

@@ -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,
#[derive(Clone, Debug)]
pub struct Mcp23017Task {
#[allow(dead_code)]
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,
name,
pins: [PinData::new(); _],
}
}
}
pub fn start(self, scope: &'a Scope<'a, '_>, running: &'a AtomicBool, frequency: u64) -> Result<ScopedJoinHandle<'a, ()>> {
#[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,
}
}
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::<Mcp23017Message>();
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,
}
))
}
}

4
flight/src/hardware/mod.rs
View File

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

10
flight/src/hardware/pin.rs Normal file
View File

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

25
flight/src/lib.rs
View File

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

13
flight/src/rcs/mod.rs Normal file
View File

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