adds initial rcs implementation

flight/Cargo.toml

@@ -14,9 +14,11 @@ log = { workspace = true, features = ["max_level_trace", "release_max_level_debu
 nautilus_common = { workspace = true }
 postcard = { workspace = true }
 rpi-pal = { workspace = true, features = ["hal"], optional = true }
+good_lp = { workspace = true, features = ["microlp"] }
 
 [dev-dependencies]
 embedded-hal-mock = { workspace = true }
+env_logger = { workspace = true }
 
 [features]
 raspi = ["dep:rpi-pal"]

flight/src/commanded_state.rs

@@ -0,0 +1,297 @@
+use log::trace;
+use std::any::type_name;
+use std::fmt::Debug;
+use std::ops::Deref;
+use std::time::Instant;
+
+#[derive(Debug, Clone)]
+struct StateEntry<T> {
+    state: T,
+    valid_until: Instant,
+    priority: u8,
+}
+
+#[derive(Debug, Clone)]
+pub struct CommandedState<T, const N: usize = 2> {
+    entries: [Option<StateEntry<T>>; N],
+    default: T,
+    changed: bool,
+}
+
+impl<T, const N: usize> CommandedState<T, N>
+where
+    T: Debug,
+{
+    pub fn new(default: T) -> Self {
+        trace!(
+            "CommandedState::<{}>::new(default: {default:?})",
+            type_name::<T>()
+        );
+        Self {
+            entries: [const { None }; N],
+            default,
+            changed: false,
+        }
+    }
+
+    pub fn consume_changed(&mut self) -> bool {
+        trace!(
+            "CommandedState::<{}>::consume_changed(self: {self:?})",
+            type_name::<T>()
+        );
+        let res = self.changed;
+        self.changed = false;
+        res
+    }
+
+    fn evaluate_change_if<F>(&mut self, current_time: Instant, f: F) -> &T
+    where
+        F: FnOnce(&T, &T) -> bool,
+    {
+        trace!(
+            "CommandedState::<{}>::evaluate_change_if(self: {self:?}, current_time: {current_time:?}, f: {})",
+            type_name::<T>(),
+            type_name::<F>()
+        );
+        let mut original_value: Option<StateEntry<T>> = None;
+        for i in 0..N {
+            if let Some(entry) = &self.entries[0] {
+                if entry.valid_until >= current_time {
+                    if let Some(original_value) = original_value {
+                        self.changed |= f(&original_value.state, &entry.state);
+                    }
+                    // SAFETY: we just checked that this index is Some
+                    // Unfortunately we can't use entry in order to satisfy the borrow checker
+                    return self.entries[0].as_ref().map(|x| &x.state).unwrap();
+                }
+                // An expired entry - we need to remove it
+
+                // Empty out this entry (keeping it in original value in case it is needed)
+                if i == 0 {
+                    original_value = self.entries[0].take();
+                } else {
+                    self.entries[0] = None;
+                }
+                // Bump up all future entries
+                self.entries.rotate_left(1);
+            } else {
+                break;
+            }
+        }
+        if let Some(original_value) = original_value {
+            self.changed |= f(&original_value.state, &self.default);
+        }
+        &self.default
+    }
+
+    fn insert_change_if<F>(&mut self, state: T, valid_until: Instant, priority: u8, f: F)
+    where
+        F: FnOnce(&T, &T) -> bool,
+    {
+        trace!(
+            "CommandedState::<{}>::insert_change_if(self: {self:?}, state: {state:?}, valid_until: {valid_until:?}, priority: {priority}, f: {})",
+            type_name::<T>(),
+            type_name::<F>()
+        );
+        for i in 0..N {
+            if let Some(entry) = &mut self.entries[i] {
+                // The current entry exists - let's find out it replacing it is an option
+                if priority >= entry.priority {
+                    // We have enough priority to modify this entry
+                    if priority == entry.priority || entry.valid_until <= valid_until {
+                        // If we are same priority (always replace) or
+                        // we will be valid for longer then we can just replace this entry
+                        if i == 0 {
+                            self.changed |= f(&state, &entry.state);
+                        }
+                        *entry = StateEntry {
+                            state,
+                            valid_until,
+                            priority,
+                        };
+                        // Remove entries which will expire before this one
+                        for j in (i + 1)..N {
+                            if let Some(later_entry) = &self.entries[j] {
+                                // If the later entry exists and expires before this one
+                                if later_entry.valid_until <= valid_until {
+                                    self.entries[j] = None;
+                                    self.entries[j..].rotate_left(1);
+                                }
+                            }
+                        }
+                        return;
+                    }
+                    // We want to add a higher priority entry, but we will expire first
+                    if i == 0 {
+                        self.changed |= f(&state, &entry.state);
+                    }
+                    self.entries[i..].rotate_right(1);
+                    self.entries[i] = Some(StateEntry {
+                        state,
+                        valid_until,
+                        priority,
+                    });
+                    return;
+                }
+            } else {
+                // No entry exists so we can just put ourselves there
+                if i == 0 {
+                    self.changed |= f(&state, &self.default);
+                }
+                self.entries[i] = Some(StateEntry {
+                    state,
+                    valid_until,
+                    priority,
+                });
+                return;
+            }
+        }
+        // If we got here then we didn't find a valid place to insert it
+    }
+
+    #[allow(unused)]
+    #[inline]
+    pub fn evaluate_changed(&mut self, current_time: Instant) -> &T {
+        self.evaluate_change_if(current_time, |_, _| true)
+    }
+
+    #[allow(unused)]
+    #[inline]
+    pub fn insert_changed(&mut self, state: T, valid_until: Instant, priority: u8) {
+        self.insert_change_if(state, valid_until, priority, |_, _| true);
+    }
+}
+
+impl<T, const N: usize> CommandedState<T, N>
+where
+    T: PartialEq + Debug,
+{
+    #[inline]
+    pub fn evaluate(&mut self, current_time: Instant) -> &T {
+        self.evaluate_change_if(current_time, T::ne)
+    }
+
+    #[inline]
+    pub fn insert(&mut self, state: T, valid_until: Instant, priority: u8) {
+        self.insert_change_if(state, valid_until, priority, T::ne);
+    }
+}
+
+impl<T, const N: usize> Deref for CommandedState<T, N> {
+    type Target = T;
+
+    fn deref(&self) -> &Self::Target {
+        for i in 0..N {
+            if let Some(entry) = &self.entries[i] {
+                return &entry.state;
+            }
+        }
+        &self.default
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use log::LevelFilter;
+    use std::time::Duration;
+
+    #[test]
+    fn commanded_state() {
+        let _ = env_logger::builder()
+            .is_test(true)
+            .filter_level(LevelFilter::Trace)
+            .try_init();
+
+        let mut state = CommandedState::<_, 2>::new(4);
+        let now = Instant::now();
+        assert!(!state.consume_changed());
+        assert_eq!(4, *state.evaluate(now));
+        assert!(!state.consume_changed());
+
+        state.insert(5, now, 0);
+        assert!(state.consume_changed());
+        assert!(!state.consume_changed());
+        assert_eq!(5, *state.evaluate(now));
+        assert!(!state.consume_changed());
+
+        let now = now + Duration::from_secs(1);
+        assert_eq!(4, *state.evaluate(now));
+        assert!(state.consume_changed());
+
+        state.insert(6, now + Duration::from_secs(2), 0);
+        assert!(state.consume_changed());
+        assert_eq!(6, *state.evaluate(now));
+        assert!(!state.consume_changed());
+
+        state.insert(7, now + Duration::from_secs(1), 1);
+        assert!(state.consume_changed());
+        assert_eq!(7, *state);
+
+        state.insert(8, now + Duration::from_secs(3), 0);
+        assert!(!state.consume_changed());
+        assert_eq!(7, *state);
+
+        state.insert(9, now + Duration::from_secs(2), 1);
+        assert!(state.consume_changed());
+        assert_eq!(9, *state);
+
+        assert_eq!(9, *state.evaluate(now));
+        assert!(!state.consume_changed());
+
+        let now = now + Duration::from_secs(2);
+        assert_eq!(9, *state.evaluate(now));
+        assert!(!state.consume_changed());
+
+        let now = now + Duration::from_secs(1);
+        assert_eq!(8, *state.evaluate(now));
+        assert!(state.consume_changed());
+
+        let now = now + Duration::from_secs(1);
+        assert_eq!(4, *state.evaluate(now));
+        assert!(state.consume_changed());
+
+        state.insert(10, now + Duration::from_secs(1), 0);
+        state.insert(11, now, 1);
+        state.insert(12, now + Duration::from_secs(1), 1);
+        assert!(state.consume_changed());
+        assert_eq!(12, *state);
+
+        let now = now + Duration::from_secs(2);
+        assert_eq!(4, *state.evaluate(now));
+        assert!(state.consume_changed());
+
+        state.insert(13, now + Duration::from_secs(1), 0);
+        state.insert(14, now, 1);
+        assert_eq!(14, *state);
+        let now = now + Duration::from_secs(2);
+        assert_eq!(4, *state.evaluate(now));
+        assert!(state.consume_changed());
+    }
+
+    #[test]
+    fn changed() {
+        let _ = env_logger::builder()
+            .is_test(true)
+            .filter_level(LevelFilter::Trace)
+            .try_init();
+
+        let mut state = CommandedState::<_, 2>::new(4);
+        let now = Instant::now();
+        assert!(!state.consume_changed());
+        assert_eq!(4, *state);
+
+        state.insert(4, now + Duration::from_secs(1), 0);
+        assert!(!state.consume_changed());
+
+        let now = Instant::now() + Duration::from_secs(2);
+        assert_eq!(4, *state.evaluate_changed(now));
+        assert!(state.consume_changed());
+
+        state.insert_changed(4, now + Duration::from_secs(1), 0);
+        assert!(state.consume_changed());
+
+        let now = Instant::now() + Duration::from_secs(2);
+        assert_eq!(4, *state.evaluate(now));
+    }
+}

flight/src/comms/command.rs

@@ -0,0 +1,42 @@
+use nautilus_common::command::valid_priority_command::ValidPriorityCommand;
+use std::fmt::Debug;
+use std::ops::{Deref, DerefMut};
+use std::time::Instant;
+
+#[derive(Clone, Debug)]
+pub struct InstantPriorityCommand<T>
+where
+    T: Clone + Debug,
+{
+    pub inner: T,
+    pub valid_until: Instant,
+    pub priority: u8,
+}
+
+impl<T> From<ValidPriorityCommand<T>> for InstantPriorityCommand<T>
+where
+    T: Clone + Debug,
+{
+    fn from(value: ValidPriorityCommand<T>) -> Self {
+        let valid_until = value.get_valid_until_instant();
+        Self {
+            inner: value.inner,
+            valid_until,
+            priority: value.priority,
+        }
+    }
+}
+
+impl<T: Clone + Debug> Deref for InstantPriorityCommand<T> {
+    type Target = T;
+
+    fn deref(&self) -> &Self::Target {
+        &self.inner
+    }
+}
+
+impl<T: Clone + Debug> DerefMut for InstantPriorityCommand<T> {
+    fn deref_mut(&mut self) -> &mut Self::Target {
+        &mut self.inner
+    }
+}

flight/src/comms/mod.rs

@@ -1,3 +1,5 @@
+pub mod command;
+
 use crate::scheduler::{CyclicTask, TaskHandle};
 use crate::state_vector::{SectionIdentifier, SectionWriter, StateVector};
 use anyhow::{Result, ensure};
@@ -54,7 +56,11 @@ impl<'a, A> CommsTask<'a, A>
 where
     A: ToSocketAddrs + Debug,
 {
-    pub fn new(local_port: u16, ground_address: A, state_vector: &'a StateVector) -> Result<Self> {
+    pub fn new<'b: 'a>(
+        local_port: u16,
+        ground_address: A,
+        state_vector: &'b StateVector,
+    ) -> Result<Self> {
         trace!(
             "CommsTask::new<A={}>(local_port: {local_port}, ground_address: {ground_address:?})",
             type_name::<A>()

flight/src/hardware/channelization.rs

@@ -46,12 +46,12 @@ pub enum PinoutChannel {
     ExtB(u8),
 }
 
-pub struct DevicePin<'a, Device: PinDevice> {
+pub struct DevicePin<Device: PinDevice> {
     pin: u8,
-    device: &'a Device,
+    device: Device,
 }
 
-impl<Device: PinDevice> Pin for DevicePin<'_, Device> {
+impl<Device: PinDevice> Pin for DevicePin<Device> {
     fn set(&mut self, value: PinState, valid_until: Instant, priority: u8) {
         trace!(
             "ChannelPin<Device={}>::set(self, value: {value:?}, valid_until: {valid_until:?}, priority: {priority})",
@@ -61,12 +61,12 @@ impl<Device: PinDevice> Pin for DevicePin<'_, Device> {
     }
 }
 
-pub enum ChannelPin<'a, A: PinDevice, B: PinDevice> {
-    ExtA(DevicePin<'a, A>),
-    ExtB(DevicePin<'a, B>),
+pub enum ChannelPin<A: PinDevice, B: PinDevice> {
+    ExtA(DevicePin<A>),
+    ExtB(DevicePin<B>),
 }
 
-impl<A: PinDevice, B: PinDevice> Pin for ChannelPin<'_, A, B> {
+impl<A: PinDevice, B: PinDevice> Pin for ChannelPin<A, B> {
     fn set(&mut self, value: PinState, valid_until: Instant, priority: u8) {
         trace!(
             "ChannelPin<A={}, B={}>::set(self, value: {value:?}, valid_until: {valid_until:?}, priority: {priority})",
@@ -81,11 +81,11 @@ impl<A: PinDevice, B: PinDevice> Pin for ChannelPin<'_, A, B> {
 }
 
 impl PinoutChannel {
-    pub fn get_pin<'a>(
-        self,
-        ext_a: &'a (impl PinDevice + Debug),
-        ext_b: &'a (impl PinDevice + Debug),
-    ) -> impl Pin {
+    pub fn get_pin<A, B>(self, ext_a: A, ext_b: B) -> ChannelPin<A, B>
+    where
+        A: PinDevice + Debug,
+        B: PinDevice + Debug,
+    {
         trace!("PinoutChannel::get_pin(self: {self:?}, ext_a: {ext_a:?}, ext_b: {ext_b:?}");
         match self {
             PinoutChannel::ExtA(pin) => ChannelPin::ExtA(DevicePin { pin, device: ext_a }),

flight/src/hardware/mcp23017/task.rs

@@ -1,9 +1,11 @@
+use crate::commanded_state::CommandedState;
 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::array;
 use std::fmt::{Debug, Formatter};
 use std::sync::mpsc::Receiver;
 use std::time::Instant;
@@ -20,7 +22,9 @@ pub enum Mcp23017Message {
 
 impl<D: Debug> PinDevice for TaskHandle<Mcp23017Message, D> {
     fn set_pin(&self, pin: u8, value: PinState, valid_until: Instant, priority: u8) {
-        trace!("Mcp23017Task::set_pin(self: {self:?}, pin: {pin}, value: {value:?})");
+        trace!(
+            "TaskHandle<Mcp23017Message, D>::set_pin(self: {self:?}, pin: {pin}, value: {value:?}, valid_until: {valid_until:?}, priority: {priority})"
+        );
         // This can only fail if the other end is disconnected - which we intentionally want to
         // ignore
         let _ = self.sender.send(Mcp23017Message::SetPin {
@@ -68,107 +72,12 @@ impl AllPins {
     fn new() -> Self {
         trace!("AllPins::new()");
         Self {
-            pins: [PinData::new(); _],
+            pins: array::repeat(PinData::new(PinState::Low)),
         }
     }
 }
 
-#[derive(Copy, Clone, Debug)]
-struct PinData {
-    state: PinState,
-    valid_until: Option<Instant>,
-    priority: u8,
-    next_state: PinState,
-    next_validity: Option<Instant>,
-    next_priority: u8,
-    default: PinState,
-    changed: bool,
-    value: PinState,
-}
-
-impl PinData {
-    fn new() -> Self {
-        trace!("PinData::new()");
-        Self {
-            state: PinState::Low,
-            valid_until: None,
-            priority: 0,
-            next_state: PinState::Low,
-            next_validity: None,
-            next_priority: 0,
-            default: PinState::Low,
-            changed: false,
-            value: PinState::Low,
-        }
-    }
-
-    fn evaluate(&mut self, now: Instant) {
-        trace!("PinData::evaluate(self: {self:?}, now: {now:?})");
-        // 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.is_some_and(|current| current >= now);
-            if is_current_valid {
-                self.value = self.state;
-                return;
-            }
-
-            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.value = self.default;
-    }
-
-    fn set(&mut self, value: PinState, valid_until: Instant, priority: u8) {
-        trace!(
-            "PinData::set(self: {self:?}, value: {value:?}, valid_until: {valid_until:?}, priority: {priority})"
-        );
-        let can_replace_current = self
-            .valid_until
-            .is_none_or(|current| current <= valid_until)
-            || self.priority == priority;
-        let can_replace_next = self.next_validity.is_none_or(|next| next <= valid_until)
-            || self.next_priority == priority;
-
-        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;
-            self.value = value;
-        } 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
-            }
-        }
-    }
-}
+type PinData = CommandedState<PinState>;
 
 impl<'a, M: Mcp23017 + Debug> Mcp23017Task<'a, M> {
     pub fn new(mcp23017: M, state_vector: &'a StateVector) -> Self {
@@ -209,7 +118,7 @@ impl<M: Mcp23017 + Debug> CyclicTask for Mcp23017Task<'_, M> {
                     priority,
                 } => {
                     if (0u8..16u8).contains(&pin) {
-                        self.pins.pins[pin as usize].set(value, valid_until, priority);
+                        self.pins.pins[pin as usize].insert(value, valid_until, priority);
                     }
                 }
             }
@@ -217,9 +126,8 @@ impl<M: Mcp23017 + Debug> CyclicTask for Mcp23017Task<'_, M> {
 
         for pin in 0u8..16u8 {
             let current_pin = &mut self.pins.pins[pin as usize];
-            let state = current_pin.value;
-            if current_pin.changed {
-                current_pin.changed = false;
+            let state = **current_pin;
+            if current_pin.consume_changed() {
                 // This shouldn't be able to fail
                 // TODO: handle error case
                 let _ = self.mcp23017.set_pin(pin, state);
@@ -229,7 +137,7 @@ impl<M: Mcp23017 + Debug> CyclicTask for Mcp23017Task<'_, M> {
         if changed {
             let _ = self.mcp23017.flush();
             self.state.update(|s| {
-                s.pins = self.pins.pins.map(|pin| pin.value == PinState::High);
+                s.pins = array::from_fn(|i| *self.pins.pins[i] == PinState::High);
             });
         }
     }

flight/src/hardware/pin.rs

@@ -1,5 +1,6 @@
 use embedded_hal::digital::PinState;
-use nautilus_common::command::{SetPin, ValidPriorityCommand};
+use nautilus_common::command::set_pin::SetPin;
+use nautilus_common::command::valid_priority_command::ValidPriorityCommand;
 use std::time::Instant;
 
 pub trait PinDevice {

flight/src/lib.rs

@@ -6,6 +6,7 @@ use crate::hardware::initialize;
 use crate::hardware::mcp23017::{Mcp23017, Mcp23017State, Mcp23017Task};
 use crate::hardware::mct8316a::Mct8316a;
 use crate::hardware::pin::PinDevice;
+use crate::rcs::RcsTask;
 use crate::scheduler::Scheduler;
 use crate::state_vector::StateVector;
 use anyhow::Result;
@@ -70,10 +71,13 @@ pub fn run() -> Result<()> {
         )?;
         let b_id = task_b.get_id();
 
+        let rcs = s.run_cyclic("rcs-task", RcsTask::new(&task_a, &task_b), 10)?;
+
         let mut comms = CommsTask::new(15000, "nautilus-ground:14000", &state_vector)?;
         comms.add_command_handler("/shutdown", new_shutdown_handler(&running))?;
         comms.add_command_handler("/mcp23017a/set", task_a.new_set_pin_callback())?;
         comms.add_command_handler("/mcp23017b/set", task_b.new_set_pin_callback())?;
+        comms.add_command_handler("/rcs/set", rcs.new_set_rcs_callback())?;
         let comms = s.run_cyclic("comms-task", comms, 10)?;
         let comms_id = *comms;
 
@@ -122,6 +126,7 @@ pub fn run() -> Result<()> {
     Ok(())
 }
 
+mod commanded_state;
 mod comms;
 mod data;
 mod rcs;

flight/src/rcs/mod.rs

@@ -1,13 +1,153 @@
-// use crate::hardware::mcp23017::Mcp23017OutputPin;
-//
-// struct RcsTask<PIN: Mcp23017OutputPin> {
-//     pin: PIN,
-// }
-//
-// impl<PIN: Mcp23017OutputPin> RcsTask<PIN> {
-//     pub fn new(pin: PIN) -> Self {
-//         Self {
-//             pin
-//         }
-//     }
-// }
+mod thruster_balancing;
+
+use crate::commanded_state::CommandedState;
+use crate::comms::command::InstantPriorityCommand;
+use crate::hardware::channelization::{
+    ChannelPin, PinoutChannel, RCS0, RCS1, RCS2, RCS3, RCS4, RCS5, RCS6, RCS7, RCS8, RCS9,
+};
+use crate::hardware::pin::{Pin, PinDevice};
+use crate::rcs::thruster_balancing::BalancingConstants;
+use crate::scheduler::{CyclicTask, TaskHandle};
+use embedded_hal::digital::PinState;
+use log::trace;
+use nautilus_common::command::set_rcs::SetRcs;
+use nautilus_common::command::valid_priority_command::ValidPriorityCommand;
+use nautilus_common::math::Vector;
+use std::fmt::Debug;
+use std::iter::zip;
+use std::sync::mpsc::Receiver;
+use std::time::{Duration, Instant};
+
+const PIN_PRIORITY: u8 = 0;
+
+struct Thruster {
+    channel: PinoutChannel,
+    position: Vector,
+    direction: Vector,
+    force: f64,
+}
+
+impl Thruster {
+    const fn new(channel: PinoutChannel, position: Vector, direction: Vector, force: f64) -> Self {
+        Self {
+            channel,
+            position,
+            direction,
+            force,
+        }
+    }
+}
+
+const RCS_THRUSTERS: [Thruster; 10] = [
+    Thruster::new(RCS0, Vector::ZERO, Vector::ZERO, 1.0),
+    Thruster::new(RCS1, Vector::ZERO, Vector::ZERO, 1.0),
+    Thruster::new(RCS2, Vector::ZERO, Vector::ZERO, 1.0),
+    Thruster::new(RCS3, Vector::ZERO, Vector::ZERO, 1.0),
+    Thruster::new(RCS4, Vector::ZERO, Vector::ZERO, 1.0),
+    Thruster::new(RCS5, Vector::ZERO, Vector::ZERO, 1.0),
+    Thruster::new(RCS6, Vector::ZERO, Vector::ZERO, 1.0),
+    Thruster::new(RCS7, Vector::ZERO, Vector::ZERO, 1.0),
+    Thruster::new(RCS8, Vector::ZERO, Vector::ZERO, 1.0),
+    Thruster::new(RCS9, Vector::ZERO, Vector::ZERO, 1.0),
+];
+
+#[derive(Clone, Debug)]
+pub enum RcsMessage {
+    SetRcs(InstantPriorityCommand<SetRcs>),
+}
+
+impl<D: Debug> TaskHandle<RcsMessage, D> {
+    pub fn set_rcs(&self, cmd: InstantPriorityCommand<SetRcs>) {
+        trace!("TaskHandle<RcsMessage, D>::set_rcs(self: {self:?}, cmd: {cmd:?})");
+        // This can only fail if the other end is disconnected - which we intentionally want to
+        // ignore
+        let _ = self.sender.send(RcsMessage::SetRcs(cmd));
+    }
+
+    pub fn new_set_rcs_callback<'a>(&self) -> impl Fn(ValidPriorityCommand<SetRcs>) + 'a
+    where
+        Self: Sized + Clone + 'a,
+    {
+        let this = self.clone();
+        move |cmd| {
+            this.set_rcs(cmd.into());
+        }
+    }
+}
+
+#[derive(Clone)]
+pub struct RcsTask<P> {
+    rcs_pins: [P; RCS_THRUSTERS.len()],
+    command: CommandedState<(Vector, Vector)>,
+}
+
+impl<A, B> RcsTask<ChannelPin<A, B>>
+where
+    A: PinDevice + Debug + Clone,
+    B: PinDevice + Debug + Clone,
+{
+    pub fn new(a: &A, b: &B) -> Self {
+        let rcs_pins = RCS_THRUSTERS.map(|thruster| thruster.channel.get_pin(a.clone(), b.clone()));
+        Self {
+            rcs_pins,
+            command: CommandedState::new((Vector::ZERO, Vector::ZERO)),
+        }
+    }
+}
+
+impl<P> CyclicTask for RcsTask<P>
+where
+    P: Pin,
+{
+    type Message = RcsMessage;
+    type Data = ();
+
+    fn get_data(&self) -> Self::Data {
+        trace!("RcsTask::get_data(self)");
+    }
+
+    fn step(&mut self, receiver: &Receiver<Self::Message>, step_time: Instant) {
+        trace!("RcsTask::step(self, receiver, step_time: {step_time:?})");
+
+        self.command.evaluate(step_time);
+
+        while let Ok(msg) = receiver.try_recv() {
+            match msg {
+                RcsMessage::SetRcs(InstantPriorityCommand {
+                    inner:
+                        SetRcs {
+                            translation,
+                            rotation,
+                        },
+                    valid_until,
+                    priority,
+                }) => {
+                    self.command
+                        .insert((translation, rotation), valid_until, priority);
+                }
+            }
+        }
+
+        let (translation, rotation) = *self.command;
+
+        let throttle = thruster_balancing::balance_thrust(
+            translation,
+            rotation,
+            &RCS_THRUSTERS,
+            BalancingConstants::default(),
+        );
+
+        for (pin, throttle) in zip(&mut self.rcs_pins, throttle) {
+            pin.set(
+                if throttle >= 0.5 {
+                    PinState::High
+                } else {
+                    PinState::Low
+                },
+                // This signal is valid for the next half second
+                step_time + Duration::from_secs(1),
+                PIN_PRIORITY,
+            );
+        }
+    }
+}

flight/src/rcs/thruster_balancing.rs

@@ -0,0 +1,302 @@
+use crate::rcs::Thruster;
+use good_lp::{
+    Expression, IntoAffineExpression, ProblemVariables, Solution, SolverModel, microlp, variable,
+};
+use log::error;
+use nautilus_common::math::Vector;
+use std::ops::Add;
+
+#[derive(Copy, Clone, Debug)]
+pub(super) struct BalancingConstants {
+    /// Should be smaller than the other two constants
+    throttle: f64,
+    force: f64,
+    torque: f64,
+}
+
+impl Default for BalancingConstants {
+    fn default() -> Self {
+        Self {
+            throttle: 1e-3,
+            force: 1.0,
+            torque: 1.0,
+        }
+    }
+}
+
+#[allow(clippy::similar_names)]
+pub(super) fn balance_thrust<const N: usize>(
+    desired_force: Vector,
+    desired_torque: Vector,
+    thrusters: &[Thruster; N],
+    constants: BalancingConstants,
+) -> [f64; N] {
+    let mut problem = ProblemVariables::new();
+    let throttles: [_; N] = std::array::from_fn(|_| problem.add(variable().min(0).max(1)));
+
+    let force_error_x_1 = problem.add(variable().min(0));
+    let force_error_x_2 = problem.add(variable().min(0));
+    let force_error_x = force_error_x_1 + force_error_x_2;
+    let force_delta_x = force_error_x_1 - force_error_x_2;
+    let force_error_y_1 = problem.add(variable().min(0));
+    let force_error_y_2 = problem.add(variable().min(0));
+    let force_error_y = force_error_y_1 + force_error_y_2;
+    let force_delta_y = force_error_y_1 - force_error_y_2;
+    let force_error_z_1 = problem.add(variable().min(0));
+    let force_error_z_2 = problem.add(variable().min(0));
+    let force_error_z = force_error_z_1 + force_error_z_2;
+    let force_delta_z = force_error_z_1 - force_error_z_2;
+    let force_error = force_error_x + force_error_y + force_error_z;
+
+    let torque_error_x_1 = problem.add(variable().min(0));
+    let torque_error_x_2 = problem.add(variable().min(0));
+    let torque_error_x = torque_error_x_1 + torque_error_x_2;
+    let torque_delta_x = torque_error_x_1 - torque_error_x_2;
+    let torque_error_y_1 = problem.add(variable().min(0));
+    let torque_error_y_2 = problem.add(variable().min(0));
+    let torque_error_y = torque_error_y_1 + torque_error_y_2;
+    let torque_delta_y = torque_error_y_1 - torque_error_y_2;
+    let torque_error_z_1 = problem.add(variable().min(0));
+    let torque_error_z_2 = problem.add(variable().min(0));
+    let torque_error_z = torque_error_z_1 + torque_error_z_2;
+    let torque_delta_z = torque_error_z_1 - torque_error_z_2;
+    let torque_error = torque_error_x + torque_error_y + torque_error_z;
+
+    let total_throttle = throttles.iter().fold(0.into_expression(), Expression::add);
+
+    let generated_force_x = (0..N)
+        .map(|i| thrusters[i].direction.x * thrusters[i].force * throttles[i])
+        .fold(0.into_expression(), Expression::add);
+    let generated_force_y = (0..N)
+        .map(|i| thrusters[i].direction.y * thrusters[i].force * throttles[i])
+        .fold(0.into_expression(), Expression::add);
+    let generated_force_z = (0..N)
+        .map(|i| thrusters[i].direction.z * thrusters[i].force * throttles[i])
+        .fold(0.into_expression(), Expression::add);
+
+    let torque_directions: [_; N] = std::array::from_fn(|i| {
+        let cross = thrusters[i].position.cross_product(thrusters[i].direction);
+        if cross == Vector::ZERO {
+            Vector::ZERO
+        } else {
+            cross.normalize()
+        }
+    });
+    let moment_arms: [f64; N] = std::array::from_fn(|i| thrusters[i].position.length());
+
+    let generated_torque_x = (0..N)
+        .map(|i| torque_directions[i].x * thrusters[i].force * throttles[i] * moment_arms[i])
+        .fold(0.into_expression(), Expression::add);
+    let generated_torque_y = (0..N)
+        .map(|i| torque_directions[i].y * thrusters[i].force * throttles[i] * moment_arms[i])
+        .fold(0.into_expression(), Expression::add);
+    let generated_torque_z = (0..N)
+        .map(|i| torque_directions[i].z * thrusters[i].force * throttles[i] * moment_arms[i])
+        .fold(0.into_expression(), Expression::add);
+
+    let to_minimize = (total_throttle.clone() * constants.throttle)
+        + (force_error.clone() * constants.force)
+        + (torque_error.clone() * constants.torque);
+
+    let solution = problem
+        .minimise(to_minimize)
+        .using(microlp)
+        .with(force_delta_x.eq(generated_force_x - desired_force.x))
+        .with(force_delta_y.eq(generated_force_y - desired_force.y))
+        .with(force_delta_z.eq(generated_force_z - desired_force.z))
+        .with(torque_delta_x.eq(generated_torque_x - desired_torque.x))
+        .with(torque_delta_y.eq(generated_torque_y - desired_torque.y))
+        .with(torque_delta_z.eq(generated_torque_z - desired_torque.z))
+        .solve();
+
+    match solution {
+        Ok(solution) => std::array::from_fn(|i| solution.value(throttles[i])),
+        Err(err) => {
+            error!("Failed to balance thrusters {err}");
+            std::array::repeat(0.0)
+        }
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use crate::hardware::channelization::PinoutChannel;
+    use crate::rcs::Thruster;
+    use crate::rcs::thruster_balancing::{BalancingConstants, balance_thrust};
+    use nautilus_common::math::Vector;
+
+    const EPSILON: f64 = 1e-6;
+
+    #[test]
+    fn test_thruster_balancing() {
+        // We can't fire the one thruster we have because it will induce a torque
+        assert_eq!(
+            [0.0],
+            balance_thrust(
+                Vector::X,
+                Vector::ZERO,
+                &[Thruster::new(
+                    PinoutChannel::ExtA(0),
+                    Vector::new(0.0, 1.0, 0.0),
+                    Vector::X,
+                    1.0
+                ),],
+                BalancingConstants::default(),
+            )
+        );
+
+        // A single perfectly aligned thruster
+        assert_eq!(
+            [1.0],
+            balance_thrust(
+                Vector::X,
+                Vector::ZERO,
+                &[Thruster::new(
+                    PinoutChannel::ExtA(0),
+                    Vector::ZERO,
+                    Vector::X,
+                    1.0
+                ),],
+                BalancingConstants::default(),
+            )
+        );
+
+        // Two balanced thrusters should both be fired
+        assert_eq!(
+            [1.0, 1.0],
+            balance_thrust(
+                Vector::X,
+                Vector::ZERO,
+                &[
+                    Thruster::new(
+                        PinoutChannel::ExtA(0),
+                        Vector::new(0.0, 1.0, 0.0),
+                        Vector::X,
+                        0.5
+                    ),
+                    Thruster::new(
+                        PinoutChannel::ExtA(0),
+                        Vector::new(0.0, -1.0, 0.0),
+                        Vector::X,
+                        0.5
+                    ),
+                ],
+                BalancingConstants::default(),
+            )
+        );
+
+        // Two thrusters with different moment arms
+        // Despite desiring a lot of force, we can't generate too much without inducing a torque
+        assert!(
+            balance_thrust(
+                Vector::X * 10.0,
+                Vector::ZERO,
+                &[
+                    Thruster::new(
+                        PinoutChannel::ExtA(0),
+                        Vector::new(0.0, 1.0, 0.0),
+                        Vector::X,
+                        0.5
+                    ),
+                    Thruster::new(
+                        PinoutChannel::ExtA(0),
+                        Vector::new(0.0, -2.0, 0.0),
+                        Vector::X,
+                        0.5
+                    ),
+                ],
+                BalancingConstants::default(),
+            )
+            .into_iter()
+            .zip([1.0, 0.5])
+            .map(|(a, b)| a - b)
+            .sum::<f64>()
+            .abs()
+                < EPSILON
+        );
+
+        // Four perfectly balanced thrusters
+        assert!(
+            balance_thrust(
+                Vector::ZERO,
+                Vector::Z * 2.0,
+                &[
+                    Thruster::new(
+                        PinoutChannel::ExtA(0),
+                        Vector::new(0.0, 1.0, 0.0),
+                        Vector::X,
+                        1.0
+                    ),
+                    Thruster::new(
+                        PinoutChannel::ExtA(0),
+                        Vector::new(0.0, 1.0, 0.0),
+                        -Vector::X,
+                        1.0
+                    ),
+                    Thruster::new(
+                        PinoutChannel::ExtA(0),
+                        Vector::new(0.0, -1.0, 0.0),
+                        Vector::X,
+                        1.0
+                    ),
+                    Thruster::new(
+                        PinoutChannel::ExtA(0),
+                        Vector::new(0.0, -1.0, 0.0),
+                        -Vector::X,
+                        1.0
+                    ),
+                ],
+                BalancingConstants::default(),
+            )
+            .into_iter()
+            .zip([0.0, 1.0, 1.0, 0.0])
+            .map(|(a, b)| a - b)
+            .sum::<f64>()
+            .abs()
+                < EPSILON
+        );
+
+        // Four perfectly balanced thrusters
+        assert!(
+            balance_thrust(
+                Vector::X * 2.0,
+                Vector::Z * 2.0,
+                &[
+                    Thruster::new(
+                        PinoutChannel::ExtA(0),
+                        Vector::new(0.0, 1.0, 0.0),
+                        Vector::X,
+                        1.0
+                    ),
+                    Thruster::new(
+                        PinoutChannel::ExtA(0),
+                        Vector::new(0.0, 1.0, 0.0),
+                        -Vector::X,
+                        1.0
+                    ),
+                    Thruster::new(
+                        PinoutChannel::ExtA(0),
+                        Vector::new(0.0, -1.0, 0.0),
+                        Vector::X,
+                        1.0
+                    ),
+                    Thruster::new(
+                        PinoutChannel::ExtA(0),
+                        Vector::new(0.0, -1.0, 0.0),
+                        -Vector::X,
+                        1.0
+                    ),
+                ],
+                BalancingConstants::default(),
+            )
+            .into_iter()
+            // Only the one thruster should fire - we can't do any better than that
+            // any more thrusters we fire won't give us any more thrust
+            .zip([0.0, 0.0, 1.0, 0.0])
+            .map(|(a, b)| a - b)
+            .sum::<f64>()
+            .abs()
+                < EPSILON
+        );
+    }
+}

flight/src/scheduler/mod.rs

@@ -13,8 +13,6 @@ use std::time::{Duration, Instant};
 
 #[derive(Clone, Debug)]
 pub struct TaskHandle<Message, Data> {
-    #[allow(dead_code)]
-    pub name: String,
     pub sender: Sender<Message>,
     data: Data,
 }
@@ -46,7 +44,7 @@ pub trait CyclicTask {
 
 impl<F> CyclicTask for F
 where
-    F: Fn(),
+    F: FnMut(),
 {
     type Message = ();
     type Data = ();
@@ -58,7 +56,10 @@ where
     }
 }
 
-pub struct Scheduler<'s, 'e> {
+pub struct Scheduler<'s, 'e>
+where
+    'e: 's,
+{
     scope: &'s Scope<'s, 'e>,
     running: Arc<AtomicBool>,
 }
@@ -68,7 +69,7 @@ impl<'s> Scheduler<'s, '_> {
     where
         F: FnOnce(Scheduler<'_, 'env>) -> R,
     {
-        trace!("Scheduler::new(running: {running:?}, f)");
+        trace!("Scheduler::scope(running: {running:?}, f)");
         thread::scope(|scope: &Scope| {
             let running_result = running.clone();
             // This will automatically set running to false when it drops
@@ -83,16 +84,29 @@ impl<'s> Scheduler<'s, '_> {
         })
     }
 
+    // pub fn inner_scope<'env, F, R>(&self, f: F) -> R
+    // where
+    //     F: FnOnce(Scheduler<'_, 'env>) -> R,
+    //     's: 'env,
+    // {
+    //     trace!("Scheduler::inner_scope(self)");
+    //     thread::scope(|scope: &Scope<'_, 'env>| {
+    //         f(Scheduler {
+    //             scope,
+    //             running: self.running.clone(),
+    //         })
+    //     })
+    // }
+
     #[allow(dead_code)]
-    pub fn run<'t, T>(
+    pub fn run<T>(
         &self,
         name: impl Into<String>,
         task: T,
     ) -> Result<TaskHandle<T::Message, T::Data>>
     where
-        T: Task + Send + Debug + 't,
+        T: Task + Send + Debug + 's,
         T::Message: Send,
-        't: 's,
     {
         let name = name.into();
         trace!(
@@ -107,19 +121,18 @@ impl<'s> Scheduler<'s, '_> {
             .spawn_scoped(self.scope, move || {
                 task.run(receiver, running);
             })?;
-        Ok(TaskHandle { name, sender, data })
+        Ok(TaskHandle { sender, data })
     }
 
-    pub fn run_cyclic<'t, T>(
+    pub fn run_cyclic<T>(
         &self,
         name: impl Into<String>,
         mut task: T,
         frequency: u64,
     ) -> Result<TaskHandle<T::Message, T::Data>>
     where
-        T: CyclicTask + Send + 't,
+        T: CyclicTask + Send + 's,
         T::Message: Send,
-        't: 's,
     {
         let name = name.into();
         trace!(
@@ -144,6 +157,6 @@ impl<'s> Scheduler<'s, '_> {
                     }
                 }
             })?;
-        Ok(TaskHandle { name, sender, data })
+        Ok(TaskHandle { sender, data })
     }
 }