initial comms

flight/Cargo.toml

@@ -5,19 +5,16 @@ edition = "2024"
 
 [dependencies]
 anyhow = "1.0.100"
-fern = { version = "0.7.1", features = ["colored"] }
 log = { version = "0.4.28", features = ["max_level_trace", "release_max_level_debug"] }
 chrono = "0.4.42"
 embedded-hal = "1.0.0"
 embedded-hal-bus = { version = "0.3.0", features = ["std"] }
 embedded-hal-mock = { version = "0.11.1", optional = true }
 rpi-pal = { version = "0.22.2", features = ["hal"], optional = true }
-nalgebra = "0.34.1"
 hex = "0.4.3"
-thiserror = "2.0.17"
-num-traits = "0.2.19"
 crc = "3.3.0"
-ctrlc = { version = "3.5" }
+nautilus_common = { path = "../common" }
+ciborium = { version = "0.2.2" }
 
 [dev-dependencies]
 embedded-hal-mock = { version = "0.11.1" }

flight/src/comms/mod.rs

@@ -0,0 +1,64 @@
+use crate::scheduler::CyclicTask;
+use anyhow::Result;
+use log::{error, trace};
+use nautilus_common::command::Command;
+use nautilus_common::telemetry::Telemetry;
+use nautilus_common::udp::{UdpRecvCborError, UdpSocketExt};
+use std::fmt::Debug;
+use std::io::Cursor;
+use std::net::{IpAddr, Ipv4Addr, SocketAddr, ToSocketAddrs, UdpSocket};
+use std::sync::atomic::{AtomicBool, Ordering};
+use std::sync::mpsc::Receiver;
+use std::sync::Arc;
+use std::time::Instant;
+
+#[derive(Debug)]
+pub struct CommsTask<A: ToSocketAddrs> {
+    udp: UdpSocket,
+    ground_address: A,
+    running: Arc<AtomicBool>,
+}
+
+impl<A: ToSocketAddrs + Debug> CommsTask<A> {
+    pub fn new(
+        local_port: u16,
+        ground_address: A,
+        running: Arc<AtomicBool>,
+    ) -> Result<Self> {
+        trace!("CommsTask::new(local_port: {local_port}, ground_address: {ground_address:?})");
+        let bind_addr = SocketAddr::new(IpAddr::V4(Ipv4Addr::UNSPECIFIED), local_port);
+        // let bind_addr = SocketAddr::new(IpAddr::V6(Ipv6Addr::UNSPECIFIED), local_port);
+        let udp = UdpSocket::bind(bind_addr)?;
+        udp.set_nonblocking(true)?;
+        Ok(Self {
+            udp,
+            ground_address,
+            running,
+        })
+    }
+}
+
+impl<A: ToSocketAddrs> CyclicTask for CommsTask<A> {
+    type Message = ();
+
+    fn step(&mut self, _receiver: &Receiver<Self::Message>, _step_time: Instant) {
+        let mut buffer = Cursor::new([0u8; 512]);
+
+        match self.udp.recv_cbor::<Command, _>(&mut buffer) {
+            Ok((cmd, _)) => {
+                match cmd {
+                    Command::Shutdown => self.running.store(false, Ordering::Relaxed),
+                }
+            }
+            Err(UdpRecvCborError::NoData) => {}
+            Err(err) => {
+                error!("Rx error: {err}");
+            }
+        }
+
+        let tlm = Telemetry::Timestamp(chrono::Utc::now());
+        if let Err(err) = self.udp.send_cbor(&tlm, &mut buffer, &self.ground_address) {
+            error!("Tx Error: {err}");
+        }
+    }
+}

flight/src/hardware/mcp23017/task.rs

@@ -1,14 +1,11 @@
 use crate::hardware::mcp23017::Mcp23017;
 use crate::hardware::pin::PinDevice;
-use anyhow::Result;
+use crate::scheduler::{CyclicTask, TaskHandle};
 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, Instant};
+use std::fmt::{Debug, Formatter};
+use std::sync::mpsc::Receiver;
+use std::time::Instant;
 
 #[derive(Clone, Debug)]
 pub enum Mcp23017Message {
@@ -20,14 +17,7 @@ pub enum Mcp23017Message {
     },
 }
 
-#[derive(Clone, Debug)]
-pub struct Mcp23017Task {
-    #[allow(dead_code)]
-    name: String,
-    sender: Sender<Mcp23017Message>,
-}
-
-impl PinDevice for Mcp23017Task {
+impl PinDevice for TaskHandle<Mcp23017Message> {
     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
@@ -41,6 +31,17 @@ impl PinDevice for Mcp23017Task {
     }
 }
 
+pub struct Mcp23017Task<M: Mcp23017> {
+    mcp23017: M,
+    pins: AllPins,
+}
+
+impl<M: Mcp23017 + Debug> Debug for Mcp23017Task<M> {
+    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
+        write!(f, "Mcp23017Task {{ mcp23017: {:?} }}", self.mcp23017)
+    }
+}
+
 struct AllPins {
     pins: [PinData; 16],
 }
@@ -137,67 +138,48 @@ impl PinData {
     }
 }
 
-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})");
+impl<M: Mcp23017 + Debug> Mcp23017Task<M> {
+    pub fn new(mcp23017: M) -> Self {
+        trace!("Mcp23017Task::new(mcp23017: {mcp23017:?})");
+        Self {
+            mcp23017,
+            pins: AllPins::new(),
+        }
+    }
+}
 
-        let (sender, receiver) = channel::<Mcp23017Message>();
-        let period = Duration::from_nanos(1_000_000_000 / frequency);
+impl<M: Mcp23017> CyclicTask for Mcp23017Task<M> {
+    type Message = Mcp23017Message;
 
-        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;
+    fn step(
+        &mut self,
+        receiver: &Receiver<Self::Message>,
+        step_time: Instant,
+    ) {
+        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);
+        while let Ok(recv) = receiver.try_recv() {
+            match recv {
+                Mcp23017Message::SetPin { pin, value, valid_until, priority } => {
+                    if (0u8..16u8).contains(&pin) {
+                        self.pins.pins[pin as usize].set(value, valid_until, priority);
                     }
                 }
-            })?;
-
-        Ok((
-            handle,
-            Self {
-                name,
-                sender,
             }
-        ))
+        }
+
+        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);
+            if self.pins.pins[pin as usize].changed {
+                self.pins.pins[pin as usize].changed = false;
+                let _ = self.mcp23017.set_pin(pin, state);
+                changed = true;
+            }
+        }
+        if changed {
+            let _ = self.mcp23017.flush();
+        }
     }
-}
+}

flight/src/hardware/raspi/mod.rs

@@ -44,6 +44,7 @@ impl RaspiHardware {
                 Mode::Mode0,
             )?), 3.3f64).into(),
             mct8316a: SimMct8316a::new().into(),
+            // mct8316a: SimMct8316a::new().into(),
         })
     }
 }

flight/src/lib.rs

@@ -1,30 +1,23 @@
+use crate::comms::CommsTask;
 use crate::hardware::channelization::{LED_A, LED_B};
 use crate::hardware::initialize;
 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 crate::scheduler::Scheduler;
 use anyhow::Result;
 use embedded_hal::digital::PinState;
 use embedded_hal::pwm::SetDutyCycle;
 use log::{debug, info};
+use nautilus_common::add_ctrlc_handler;
 use std::sync::atomic::{AtomicBool, Ordering};
 use std::sync::Arc;
-use std::thread;
 use std::thread::sleep;
 use std::time::{Duration, Instant};
 
 mod hardware;
 
-fn add_ctrlc_handler(flag: Arc<AtomicBool>) -> Result<()> {
-    ctrlc::set_handler(move || {
-        info!("Shutdown Requested");
-        flag.store(false, Ordering::Relaxed);
-    })?;
-    Ok(())
-}
-
 pub fn run() -> Result<()> {
     info!(
         "Project Nautilus Flight Software {}",
@@ -49,21 +42,17 @@ pub fn run() -> Result<()> {
     mcp23017_b.init()?;
     mct8316.init()?;
 
-    thread::scope(|scope| {
-        // This will automatically set running to false when it drops
-        // This means that if the main thread exits this scope, we will
-        // shut down any side branches
-        let _shutdown_threads = on_drop(|| running.store(false, Ordering::Relaxed));
+    Scheduler::new(running.clone(), |s| {
+        let task_a = s.run_cyclic("mcp23017-a", Mcp23017Task::new(mcp23017_a), 10)?;
+        let task_b = s.run_cyclic("mcp23017-b", Mcp23017Task::new(mcp23017_b), 10)?;
 
-        let (_, task_a) = Mcp23017Task::start(scope, &running, mcp23017_a, "mcp23017-a".into(), 10)?;
-
-        let (_, task_b) = Mcp23017Task::start(scope, &running, mcp23017_b, "mcp23017-b".into(), 10)?;
+        let _comms = s.run_cyclic("comms", CommsTask::new(15000, "192.168.50.157:14000", running.clone())?, 1)?;
 
         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 {
+        loop {
             debug!("A On");
             led_pin_a.set(PinState::High, Instant::now() + Duration::from_secs(2), 0);
             sleep(Duration::from_secs(1));
@@ -103,3 +92,5 @@ mod test_utils;
 mod data;
 mod on_drop;
 mod rcs;
+mod comms;
+mod scheduler;

flight/src/logger.rs

@@ -1,52 +0,0 @@
-use anyhow::Result;
-use fern::colors::{Color, ColoredLevelConfig};
-use log::debug;
-use std::fs::create_dir_all;
-use std::str::FromStr;
-use std::{env, thread};
-
-pub fn setup_logger() -> Result<()> {
-    let log_file = env::var("LOG_FILE").or_else(|_| {
-        create_dir_all("logs/")?;
-
-        anyhow::Ok(format!(
-            "logs/{}_{}.log",
-            env!("CARGO_PKG_NAME"),
-            chrono::Local::now().format("%Y%m%d_%H%M%S")
-        ))
-    })?;
-    let log_level = match env::var("LOG_LEVEL") {
-        Ok(log_level) => log::LevelFilter::from_str(&log_level).unwrap_or(log::LevelFilter::Info),
-        Err(_) => log::LevelFilter::Info,
-    };
-
-    let colors = ColoredLevelConfig::new()
-        .error(Color::Red)
-        .warn(Color::Yellow)
-        .info(Color::White)
-        .debug(Color::White)
-        .trace(Color::BrightBlack);
-
-    fern::Dispatch::new()
-        .format(move |out, message, record| {
-            let binding = thread::current();
-            let thread_name = binding.name().unwrap_or("<unnamed>");
-
-            out.finish(format_args!(
-                "[{time} {level} {thread_name} {target}] {message}",
-                level = colors.color(record.level()),
-                time = chrono::Local::now().format("%Y-%m-%dT%H:%M:%S%.9f"),
-                target = record.target(),
-            ))
-        })
-        .chain(
-            fern::Dispatch::new()
-                .level(log_level)
-                .chain(std::io::stdout()),
-        )
-        .chain(fern::log_file(log_file.clone())?)
-        .apply()?;
-
-    debug!("Logging to {} at level {}", log_file, log_level);
-    Ok(())
-}

flight/src/main.rs

@@ -1,11 +1,10 @@
-use crate::logger::setup_logger;
 use log::error;
+use nautilus_common::logger::setup_logger;
 use nautilus_flight::run;
 
-mod logger;
 
 fn main() {
-    setup_logger().expect("Failed to setup logger");
+    setup_logger(env!("CARGO_PKG_NAME")).expect("Failed to setup logger");
     match run() {
         Ok(_) => {}
         Err(err) => {

flight/src/scheduler/mod.rs

@@ -0,0 +1,114 @@
+use crate::on_drop::on_drop;
+use anyhow::Result;
+use log::trace;
+use std::fmt::Debug;
+use std::sync::atomic::{AtomicBool, Ordering};
+use std::sync::mpsc::{channel, Receiver, Sender};
+use std::sync::Arc;
+use std::thread;
+use std::thread::{sleep, Scope};
+use std::time::{Duration, Instant};
+
+#[derive(Clone, Debug)]
+pub struct TaskHandle<Message> {
+    #[allow(dead_code)]
+    pub name: String,
+    pub sender: Sender<Message>,
+}
+
+#[allow(dead_code)]
+pub trait Task {
+    type Message;
+
+    fn run(self, receiver: Receiver<Self::Message>, running: Arc<AtomicBool>);
+}
+
+pub trait CyclicTask {
+    type Message;
+
+    fn step(&mut self, receiver: &Receiver<Self::Message>, step_time: Instant);
+}
+
+pub struct Scheduler<'s, 'e>
+{
+    scope: &'s Scope<'s, 'e>,
+    running: Arc<AtomicBool>,
+}
+
+impl<'s, 'e> Scheduler<'s, 'e> {
+    pub fn new<'env, F, R>(running: Arc<AtomicBool>, f: F) -> R
+    where
+        F: FnOnce(Scheduler<'_, 'env>) -> R,
+    {
+        trace!("Scheduler::new(running: {running:?}, f)");
+        thread::scope(|scope: &Scope| {
+            // This will automatically set running to false when it drops
+            // This means that if the function returns any side branches
+            // checking running will shut down
+            let _shutdown_threads = on_drop(|| running.store(false, Ordering::Relaxed));
+
+            f(Scheduler {
+                scope,
+                running: running.clone(),
+            })
+        })
+    }
+
+    #[allow(dead_code)]
+    pub fn run<T>(
+        &self,
+        name: impl Into<String>,
+        task: T,
+    ) -> Result<TaskHandle<T::Message>> where
+        T: Task + Send + Debug + 's,
+        T::Message: Send,
+    {
+        let name = name.into();
+        trace!("Scheduler::run(name: {name}, task: {task:?})");
+        let running = self.running.clone();
+        let (sender, receiver) = channel::<T::Message>();
+        let _ = thread::Builder::new()
+            .name(name.clone())
+            .spawn_scoped(self.scope, move || {
+                task.run(receiver, running);
+            })?;
+        Ok(TaskHandle {
+            name,
+            sender,
+        })
+    }
+
+    pub fn run_cyclic<T>(
+        &self,
+        name: impl Into<String>,
+        mut task: T,
+        frequency: u64,
+    ) -> Result<TaskHandle<T::Message>> where
+        T: CyclicTask + Send + Debug + 's,
+        T::Message: Send,
+    {
+        let name = name.into();
+        trace!("Scheduler::run_cyclic(name: {name}, task: {task:?}, frequency: {frequency})");
+        let running = self.running.clone();
+        let (sender, receiver) = channel::<T::Message>();
+        let _ = thread::Builder::new()
+            .name(name.clone())
+            .spawn_scoped(self.scope, move || {
+                let period = Duration::from_nanos(1_000_000_000 / frequency);
+                let mut cycle_start_time = Instant::now();
+                while running.load(Ordering::Relaxed) {
+                    task.step(&receiver, cycle_start_time);
+
+                    cycle_start_time += period;
+                    let sleep_duration = cycle_start_time - Instant::now();
+                    if sleep_duration > Duration::ZERO {
+                        sleep(sleep_duration);
+                    }
+                }
+            })?;
+        Ok(TaskHandle {
+            name,
+            sender,
+        })
+    }
+}