From d53d78434c79791955dae5405af8663dbf2f591d Mon Sep 17 00:00:00 2001 From: Sergey Savelyev Date: Sun, 30 Nov 2025 08:02:39 -0800 Subject: [PATCH] increase command flexibility --- Cargo.lock | 96 ++++++++------------- Cargo.toml | 10 +++ common/Cargo.toml | 16 ++-- common/src/command/mod.rs | 76 +++++++++++++++- common/src/lib.rs | 5 +- common/src/logger.rs | 2 +- common/src/udp.rs | 98 ++++++++++++++------- flight/Cargo.toml | 8 +- flight/src/comms/mod.rs | 115 +++++++++++++++++++++---- flight/src/hardware/mcp23017/task.rs | 12 ++- flight/src/hardware/mct8316a/driver.rs | 1 - flight/src/hardware/mct8316a/eeprom.rs | 52 ++++++----- flight/src/hardware/pin.rs | 15 ++++ flight/src/lib.rs | 52 +++-------- flight/src/scheduler/mod.rs | 3 +- ground/Cargo.toml | 9 +- ground/src/lib.rs | 72 +++++++++++++--- 17 files changed, 422 insertions(+), 220 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 8d17aff..557c7c5 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -71,30 +71,12 @@ dependencies = [ ] [[package]] -name = "ciborium" -version = "0.2.2" +name = "cobs" +version = "0.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "42e69ffd6f0917f5c029256a24d0161db17cea3997d185db0d35926308770f0e" +checksum = "0fa961b519f0b462e3a3b4a34b64d119eeaca1d59af726fe450bbba07a9fc0a1" dependencies = [ - "ciborium-io", - "ciborium-ll", - "serde", -] - -[[package]] -name = "ciborium-io" -version = "0.2.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "05afea1e0a06c9be33d539b876f1ce3692f4afea2cb41f740e7743225ed1c757" - -[[package]] -name = "ciborium-ll" -version = "0.2.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "57663b653d948a338bfb3eeba9bb2fd5fcfaecb9e199e87e1eda4d9e8b240fd9" -dependencies = [ - "ciborium-io", - "half", + "thiserror", ] [[package]] @@ -134,12 +116,6 @@ version = "1.2.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "790eea4361631c5e7d22598ecd5723ff611904e3344ce8720784c93e3d83d40b" -[[package]] -name = "crunchy" -version = "0.2.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "460fbee9c2c2f33933d720630a6a0bac33ba7053db5344fac858d4b8952d77d5" - [[package]] name = "ctrlc" version = "3.5.0" @@ -207,6 +183,18 @@ dependencies = [ "nb 1.1.0", ] +[[package]] +name = "embedded-io" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ef1a6892d9eef45c8fa6b9e0086428a2cca8491aca8f787c534a3d6d0bcb3ced" + +[[package]] +name = "embedded-io" +version = "0.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "edd0f118536f44f5ccd48bcb8b111bdc3de888b58c74639dfb034a357d0f206d" + [[package]] name = "embedded-time" version = "0.12.1" @@ -226,17 +214,6 @@ dependencies = [ "log", ] -[[package]] -name = "half" -version = "2.7.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6ea2d84b969582b4b1864a92dc5d27cd2b77b622a8d79306834f1be5ba20d84b" -dependencies = [ - "cfg-if", - "crunchy", - "zerocopy", -] - [[package]] name = "hex" version = "0.4.3" @@ -301,10 +278,10 @@ version = "0.1.0" dependencies = [ "anyhow", "chrono", - "ciborium", "ctrlc", "fern", "log", + "postcard", "serde", "thiserror", ] @@ -315,7 +292,6 @@ version = "0.0.1" dependencies = [ "anyhow", "chrono", - "ciborium", "crc", "embedded-hal 1.0.0", "embedded-hal-bus", @@ -323,6 +299,7 @@ dependencies = [ "hex", "log", "nautilus_common", + "postcard", "rpi-pal", ] @@ -331,9 +308,12 @@ name = "nautilus_ground" version = "0.1.0" dependencies = [ "anyhow", - "ciborium", + "chrono", + "hex", "log", "nautilus_common", + "postcard", + "serde", ] [[package]] @@ -431,6 +411,18 @@ version = "1.21.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "42f5e15c9953c5e4ccceeb2e7382a716482c34515315f7b03532b8b4e8393d2d" +[[package]] +name = "postcard" +version = "1.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6764c3b5dd454e283a30e6dfe78e9b31096d9e32036b5d1eaac7a6119ccb9a24" +dependencies = [ + "cobs", + "embedded-io 0.4.0", + "embedded-io 0.6.1", + "serde", +] + [[package]] name = "proc-macro2" version = "1.0.94" @@ -712,23 +704,3 @@ name = "windows_x86_64_msvc" version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "589f6da84c646204747d1270a2a5661ea66ed1cced2631d546fdfb155959f9ec" - -[[package]] -name = "zerocopy" -version = "0.8.27" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0894878a5fa3edfd6da3f88c4805f4c8558e2b996227a3d864f47fe11e38282c" -dependencies = [ - "zerocopy-derive", -] - -[[package]] -name = "zerocopy-derive" -version = "0.8.27" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "88d2b8d9c68ad2b9e4340d7832716a4d21a22a1154777ad56ea55c51a9cf3831" -dependencies = [ - "proc-macro2", - "quote", - "syn", -] diff --git a/Cargo.toml b/Cargo.toml index a3bfcf4..0b7682b 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,3 +1,13 @@ [workspace] resolver = "3" members = ["common", "ground", "flight"] + +[workspace.dependencies] +anyhow = "1.0.100" +fern = { version = "0.7.1", features = ["colored"] } +log = { version = "0.4.28" } +chrono = { version = "0.4.42", features = ["serde"] } +ctrlc = "3.5.0" +serde = { version = "1.0.228", features = ["derive"], default-features = false } +postcard = { version = "1.1.3", default-features = false, features = ["alloc"] } +thiserror = "2.0.17" diff --git a/common/Cargo.toml b/common/Cargo.toml index f3988af..28e7ff7 100644 --- a/common/Cargo.toml +++ b/common/Cargo.toml @@ -4,11 +4,11 @@ version = "0.1.0" 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 = { version = "0.4.42", features = ["serde"] } -ctrlc = "3.5.0" -serde = { version = "1.0.228", features = ["derive"], default-features = false } -ciborium = { version = "0.2.2" } -thiserror = "2.0.17" +anyhow = { workspace = true } +fern = { workspace = true } +log = { workspace = true } +chrono = { workspace = true } +ctrlc = { workspace = true } +serde = { workspace = true } +postcard = { workspace = true } +thiserror = { workspace = true } diff --git a/common/src/command/mod.rs b/common/src/command/mod.rs index 4bd9b55..f32205a 100644 --- a/common/src/command/mod.rs +++ b/common/src/command/mod.rs @@ -1,6 +1,78 @@ +use chrono::serde::ts_nanoseconds; +use chrono::{DateTime, TimeDelta, Utc}; +use serde::de::DeserializeOwned; use serde::{Deserialize, Serialize}; +use std::fmt::Debug; +use std::ops::{Deref, DerefMut}; +use std::time::Instant; #[derive(Clone, Debug, Serialize, Deserialize)] -pub enum Command { - Shutdown, +pub struct SetPin { + pub pin: u8, + pub value: bool, +} + +#[derive(Clone, Debug, Serialize, Deserialize)] +pub struct ValidPriorityCommand +where + T: Clone + Debug, +{ + pub inner: T, + #[serde(with = "ts_nanoseconds")] + pub valid_until: DateTime, + pub priority: u8, +} + +impl ValidPriorityCommand +where + T: Clone + Debug, +{ + /// Get the valid until time as an Instant + /// + /// # Panics + /// While this theoretically could panic, there are checks to prevent this. + pub fn get_valid_until_instant(&self) -> Instant { + let delta = self.valid_until.signed_duration_since(Utc::now()); + let now = Instant::now(); + if delta >= TimeDelta::zero() { + // Unwrap is safe because we checked that it is not negative + now + delta.to_std().unwrap() + } else { + // Unwrap is safe because we converted the negative to a positive + now.checked_sub((-delta).to_std().unwrap()).unwrap_or(now) + } + } +} + +#[derive(Clone, Debug, Serialize, Deserialize)] +pub struct CommandHeader<'a> { + pub name: &'a str, + pub data: &'a [u8], +} + +pub trait Command: Serialize + DeserializeOwned {} + +impl Command for () {} + +impl Command for SetPin {} + +impl Command + for ValidPriorityCommand +{ +} + +impl Deref for ValidPriorityCommand { + type Target = T; + + fn deref(&self) -> &Self::Target { + &self.inner + } +} + +impl DerefMut + for ValidPriorityCommand +{ + fn deref_mut(&mut self) -> &mut Self::Target { + &mut self.inner + } } diff --git a/common/src/lib.rs b/common/src/lib.rs index 42a4ee3..b4cb991 100644 --- a/common/src/lib.rs +++ b/common/src/lib.rs @@ -1,7 +1,4 @@ -#![warn( - clippy::all, - clippy::pedantic, -)] +#![warn(clippy::all, clippy::pedantic)] use log::info; use std::sync::Arc; use std::sync::atomic::{AtomicBool, Ordering}; diff --git a/common/src/logger.rs b/common/src/logger.rs index 1adbcf7..aa43845 100644 --- a/common/src/logger.rs +++ b/common/src/logger.rs @@ -6,7 +6,7 @@ use std::str::FromStr; use std::{env, thread}; /// Set up the logger with a given package name -/// +/// /// # Errors /// If an error occurred while trying to set up the logger pub fn setup_logger(package_name: &'static str) -> Result<()> { diff --git a/common/src/udp.rs b/common/src/udp.rs index 752b626..eacab4f 100644 --- a/common/src/udp.rs +++ b/common/src/udp.rs @@ -1,85 +1,92 @@ -use crate::udp::UdpSendCborError::LengthMismatch; +use crate::command::{Command, CommandHeader}; +use crate::udp::UdpSendPostcardError::LengthMismatch; use log::error; -use serde::Serialize; -use serde::de::DeserializeOwned; -use std::io::{Cursor, ErrorKind}; +use serde::{Deserialize, Serialize}; +use std::io::ErrorKind; use std::net::{SocketAddr, ToSocketAddrs, UdpSocket}; use thiserror::Error; #[derive(Error, Debug)] -pub enum UdpRecvCborError { +pub enum UdpRecvPostcardError { #[error("IO Error")] Io(#[from] std::io::Error), #[error("Deserialization Error")] - Deserialization(#[from] ciborium::de::Error), + Deserialization(#[from] postcard::Error), #[error("No Data")] NoData, } #[derive(Error, Debug)] -pub enum UdpSendCborError { +pub enum UdpSendPostcardError { #[error("IO Error")] Io(#[from] std::io::Error), #[error("Serialization Error")] - Serialization(#[from] ciborium::ser::Error), + Serialization(#[from] postcard::Error), #[error("Length Mismatch")] LengthMismatch { expected: usize, actual: usize }, } pub trait UdpSocketExt { - /// Receive a CBOR encoded message from this UDP Socket /// /// # Errors /// An error that could have occurred while trying to receive this message. /// If no data was received a `UdpRecvCborError::NoData` error would be returned. - fn recv_cbor( + fn recv_postcard<'de, T: Deserialize<'de>>( &self, - buffer: &mut Cursor<[u8; N]>, - ) -> Result<(T, SocketAddr), UdpRecvCborError>; + buffer: &'de mut [u8], + ) -> Result<(T, SocketAddr), UdpRecvPostcardError>; /// Send a CBOR encoded message to an address using this socket /// /// # Errors /// An error that could have occurred while trying to send this message - fn send_cbor( + fn send_postcard( &self, data: &T, - buffer: &mut Cursor<[u8; N]>, + buffer: &mut [u8], addr: A, - ) -> Result<(), UdpSendCborError>; + ) -> Result<(), UdpSendPostcardError>; + + /// Send a command message to an address using this socket + /// + /// # Errors + /// An error that could have occurred while trying to send this message + fn send_command( + &self, + name: &str, + data: &T, + addr: A, + ) -> Result<(), UdpSendPostcardError>; } impl UdpSocketExt for UdpSocket { - fn recv_cbor( + fn recv_postcard<'de, T: Deserialize<'de>>( &self, - buffer: &mut Cursor<[u8; N]>, - ) -> Result<(T, SocketAddr), UdpRecvCborError> { - buffer.set_position(0); - match self.recv_from(buffer.get_mut()) { - Ok((size, addr)) => match ciborium::from_reader::(&buffer.get_ref()[..size]) { + buffer: &'de mut [u8], + ) -> Result<(T, SocketAddr), UdpRecvPostcardError> { + match self.recv_from(buffer) { + Ok((size, addr)) => match postcard::from_bytes::(&buffer[..size]) { Ok(res) => Ok((res, addr)), Err(err) => Err(err.into()), }, Err(err) => match err.kind() { - ErrorKind::WouldBlock | ErrorKind::TimedOut => Err(UdpRecvCborError::NoData), + ErrorKind::WouldBlock | ErrorKind::TimedOut => Err(UdpRecvPostcardError::NoData), _ => Err(err.into()), }, } } - fn send_cbor( + fn send_postcard( &self, data: &T, - mut buffer: &mut Cursor<[u8; N]>, + buffer: &mut [u8], addr: A, - ) -> Result<(), UdpSendCborError> { - buffer.set_position(0); - match ciborium::into_writer(data, &mut buffer) { - Ok(()) => { - let size_encoded = usize::try_from(buffer.position()) - .expect("Values greater than u32 are not expected anyway"); - match self.send_to(&buffer.get_ref()[..size_encoded], addr) { + ) -> Result<(), UdpSendPostcardError> { + match postcard::to_slice(data, buffer) { + Ok(result) => { + let size_encoded = result.len(); + match self.send_to(result, addr) { Ok(size_sent) => { if size_encoded != size_sent { return Err(LengthMismatch { @@ -91,8 +98,35 @@ impl UdpSocketExt for UdpSocket { } Err(e) => Err(e.into()), } - }, + } Err(e) => Err(e.into()), } } + + fn send_command( + &self, + name: &str, + data: &T, + addr: A, + ) -> Result<(), UdpSendPostcardError> { + let mut inner_buffer = [0u8; 512]; + let inner_buffer = postcard::to_slice(data, &mut inner_buffer)?; + let mut buffer = [0u8; 512]; + let buffer = postcard::to_slice( + &CommandHeader { + name, + data: inner_buffer, + }, + &mut buffer, + )?; + let size_encoded = buffer.len(); + let size_sent = self.send_to(buffer, addr)?; + if size_encoded != size_sent { + return Err(LengthMismatch { + expected: size_encoded, + actual: size_sent, + }); + } + Ok(()) + } } diff --git a/flight/Cargo.toml b/flight/Cargo.toml index f2177fc..7b4fcce 100644 --- a/flight/Cargo.toml +++ b/flight/Cargo.toml @@ -4,9 +4,9 @@ version = "0.0.1" edition = "2024" [dependencies] -anyhow = "1.0.100" -log = { version = "0.4.28", features = ["max_level_trace", "release_max_level_debug"] } -chrono = "0.4.42" +anyhow = { workspace = true } +log = { workspace = true, features = ["max_level_trace", "release_max_level_debug"] } +chrono = { workspace = true } embedded-hal = "1.0.0" embedded-hal-bus = { version = "0.3.0", features = ["std"] } embedded-hal-mock = { version = "0.11.1", optional = true } @@ -14,7 +14,7 @@ rpi-pal = { version = "0.22.2", features = ["hal"], optional = true } hex = "0.4.3" crc = "3.3.0" nautilus_common = { path = "../common" } -ciborium = { version = "0.2.2" } +postcard = { workspace = true } [dev-dependencies] embedded-hal-mock = { version = "0.11.1" } diff --git a/flight/src/comms/mod.rs b/flight/src/comms/mod.rs index fe64044..447ee14 100644 --- a/flight/src/comms/mod.rs +++ b/flight/src/comms/mod.rs @@ -1,12 +1,11 @@ use crate::scheduler::{CyclicTask, TaskHandle}; use anyhow::Result; -use log::{error, trace}; -use nautilus_common::command::Command; +use log::{error, trace, warn}; +use nautilus_common::command::{CommandHeader, SetPin, ValidPriorityCommand}; use nautilus_common::telemetry::{Telemetry, TelemetryMessage}; -use nautilus_common::udp::{UdpRecvCborError, UdpSocketExt}; +use nautilus_common::udp::{UdpRecvPostcardError, UdpSocketExt}; use std::any::type_name; -use std::fmt::Debug; -use std::io::Cursor; +use std::fmt::{Debug, Formatter}; use std::net::{IpAddr, Ipv4Addr, SocketAddr, ToSocketAddrs, UdpSocket}; use std::sync::Arc; use std::sync::atomic::{AtomicBool, Ordering}; @@ -26,15 +25,47 @@ impl TelemetrySender { } } -#[derive(Debug)] -pub struct CommsTask { +pub struct CommsTask +where + A: ToSocketAddrs + Debug, + SetPinA: Fn(ValidPriorityCommand) + Send + Sync, + SetPinB: Fn(ValidPriorityCommand) + Send + Sync, +{ udp: UdpSocket, ground_address: A, running: Arc, + set_pin_a: SetPinA, + set_pin_b: SetPinB, } -impl CommsTask { - pub fn new(local_port: u16, ground_address: A, running: Arc) -> Result { +impl Debug for CommsTask +where + A: ToSocketAddrs + Debug, + SetPinA: Fn(ValidPriorityCommand) + Send + Sync, + SetPinB: Fn(ValidPriorityCommand) + Send + Sync, +{ + fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result { + write!( + f, + "CommsTask {{ udp: {:?}, ground_address: {:?}, running: {:?} }}", + self.udp, self.ground_address, self.running + ) + } +} + +impl CommsTask +where + A: ToSocketAddrs + Debug, + SetPinA: Fn(ValidPriorityCommand) + Send + Sync, + SetPinB: Fn(ValidPriorityCommand) + Send + Sync, +{ + pub fn new( + local_port: u16, + ground_address: A, + running: Arc, + set_pin_a: SetPinA, + set_pin_b: SetPinB, + ) -> Result { trace!( "CommsTask::new(local_port: {local_port}, ground_address: {ground_address:?})", type_name::() @@ -47,11 +78,18 @@ impl CommsTask { udp, ground_address, running, + set_pin_a, + set_pin_b, }) } } -impl CyclicTask for CommsTask { +impl CyclicTask for CommsTask +where + A: ToSocketAddrs + Debug, + SetPinA: Fn(ValidPriorityCommand) + Send + Sync, + SetPinB: Fn(ValidPriorityCommand) + Send + Sync, +{ type Message = Telemetry; type Data = (); @@ -67,13 +105,55 @@ impl CyclicTask for CommsTask { "CommsTask::step(self: {self:?}, receiver: {receiver:?}, step_time: {step_time:?})", type_name::() ); - let mut buffer = Cursor::new([0u8; 512]); + let mut buffer = [0u8; 512]; - match self.udp.recv_cbor::(&mut buffer) { - Ok((cmd, _)) => match cmd { - Command::Shutdown => self.running.store(false, Ordering::Relaxed), + match self.udp.recv_postcard::(&mut buffer) { + Ok((cmd, _)) => match cmd.name { + "/shutdown" => match postcard::take_from_bytes::<()>(cmd.data) { + Ok(((), remainder)) => { + if remainder.is_empty() { + self.running.store(false, Ordering::Relaxed); + } else { + error!("shutdown has extra data"); + } + } + Err(e) => { + error!("Failed to parse ShutdownCommand {e}"); + } + }, + "/mcp23017a/set" => { + match postcard::take_from_bytes::>(cmd.data) { + Ok((set_pin, remainder)) => { + if remainder.is_empty() { + (self.set_pin_a)(set_pin); + } else { + error!("set pin has extra data"); + } + } + Err(e) => { + error!("Failed to parse SetPin {e}"); + } + } + } + "/mcp23017b/set" => { + match postcard::take_from_bytes::>(cmd.data) { + Ok((set_pin, remainder)) => { + if remainder.is_empty() { + (self.set_pin_b)(set_pin); + } else { + error!("set pin has extra data"); + } + } + Err(e) => { + error!("Failed to parse SetPin {e}"); + } + } + } + _ => { + warn!("Unknown Command: {}", cmd.name); + } }, - Err(UdpRecvCborError::NoData) => {} + Err(UdpRecvPostcardError::NoData) => {} Err(err) => { error!("Rx error: {err}"); } @@ -81,7 +161,10 @@ impl CyclicTask for CommsTask { // Intentionally ignore Err case while let Ok(tlm) = receiver.try_recv() { - if let Err(err) = self.udp.send_cbor(&tlm, &mut buffer, &self.ground_address) { + if let Err(err) = self + .udp + .send_postcard(&tlm, &mut buffer, &self.ground_address) + { error!("Tx Error: {err}"); } } diff --git a/flight/src/hardware/mcp23017/task.rs b/flight/src/hardware/mcp23017/task.rs index 2909838..4db46b1 100644 --- a/flight/src/hardware/mcp23017/task.rs +++ b/flight/src/hardware/mcp23017/task.rs @@ -107,9 +107,7 @@ impl PinData { // 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); + let is_current_valid = self.valid_until.is_some_and(|current| current >= now); if is_current_valid { self.value = self.state; return; @@ -135,10 +133,10 @@ impl PinData { ); let can_replace_current = self .valid_until - .is_none_or(|current| current <= valid_until); - let can_replace_next = self - .next_validity - .is_none_or(|next| next <= 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) diff --git a/flight/src/hardware/mct8316a/driver.rs b/flight/src/hardware/mct8316a/driver.rs index 6dc6de3..98e7873 100644 --- a/flight/src/hardware/mct8316a/driver.rs +++ b/flight/src/hardware/mct8316a/driver.rs @@ -158,7 +158,6 @@ where } } - #[allow(clippy::identity_op)] pub(super) fn write(&self, address: u32, data: Mct8316AVData) -> Result<()> { trace!("Mct8316AVDriver::write(self: {self:?}, address: {address:06x}, data: {data})"); diff --git a/flight/src/hardware/mct8316a/eeprom.rs b/flight/src/hardware/mct8316a/eeprom.rs index dc5732e..d113166 100644 --- a/flight/src/hardware/mct8316a/eeprom.rs +++ b/flight/src/hardware/mct8316a/eeprom.rs @@ -78,33 +78,31 @@ where pub fn set_isd_config(self, isd_config: IsdConfig) -> Result { trace!("Mct8316AVEeprom::set_isd_config(self: {self:?}, isd_config: {isd_config:?})"); - let expected_value = if isd_config.enable_isd { 0x4000_0000 } else { 0 } - | if isd_config.enable_brake { - 0x2000_0000 - } else { - 0 - } - | if isd_config.enable_high_impedance { - 0x1000_0000 - } else { - 0 - } - | if isd_config.enable_reverse_drive { - 0x0800_0000 - } else { - 0 - } - | if isd_config.enable_resynchronization { - 0x0400_0000 - } else { - 0 - } - | if isd_config.enable_stationary_brake { - 0x0200_0000 - } else { - 0 - } - | (((isd_config.stationary_detect_threshold as u32) & 0x7) << 22) + let expected_value = if isd_config.enable_isd { + 0x4000_0000 + } else { + 0 + } | if isd_config.enable_brake { + 0x2000_0000 + } else { + 0 + } | if isd_config.enable_high_impedance { + 0x1000_0000 + } else { + 0 + } | if isd_config.enable_reverse_drive { + 0x0800_0000 + } else { + 0 + } | if isd_config.enable_resynchronization { + 0x0400_0000 + } else { + 0 + } | if isd_config.enable_stationary_brake { + 0x0200_0000 + } else { + 0 + } | (((isd_config.stationary_detect_threshold as u32) & 0x7) << 22) | ((isd_config.brake_mode as u32) & 0x1) << 21 | ((isd_config.brake_config as u32) & 0x1) << 20 | ((isd_config.brake_current_threshold as u32) & 0x7) << 17 diff --git a/flight/src/hardware/pin.rs b/flight/src/hardware/pin.rs index ddfd76d..1ba6545 100644 --- a/flight/src/hardware/pin.rs +++ b/flight/src/hardware/pin.rs @@ -1,8 +1,23 @@ use embedded_hal::digital::PinState; +use nautilus_common::command::{SetPin, ValidPriorityCommand}; use std::time::Instant; pub trait PinDevice { fn set_pin(&self, pin: u8, value: PinState, valid_until: Instant, priority: u8); + + fn new_pinset_callback(self) -> impl Fn(ValidPriorityCommand) + where + Self: Sized, + { + move |cmd| { + self.set_pin( + cmd.pin, + cmd.value.into(), + cmd.get_valid_until_instant(), + cmd.priority, + ) + } + } } pub trait Pin { diff --git a/flight/src/lib.rs b/flight/src/lib.rs index 5158a17..abcd120 100644 --- a/flight/src/lib.rs +++ b/flight/src/lib.rs @@ -1,26 +1,21 @@ -#![warn( - clippy::all, - clippy::pedantic, -)] +#![warn(clippy::all, clippy::pedantic)] use crate::comms::CommsTask; use crate::hardware::Hardware; -use crate::hardware::channelization::{LED_A, LED_B}; use crate::hardware::initialize; use crate::hardware::mcp23017::{Mcp23017, Mcp23017State, Mcp23017Task}; use crate::hardware::mct8316a::Mct8316a; -use crate::hardware::pin::Pin; +use crate::hardware::pin::PinDevice; use crate::scheduler::Scheduler; use crate::state_vector::StateVector; use anyhow::Result; -use embedded_hal::digital::PinState; use embedded_hal::pwm::SetDutyCycle; -use log::{debug, info}; +use log::info; use nautilus_common::add_ctrlc_handler; use nautilus_common::telemetry::{SwitchBank, TelemetryMessage}; use std::sync::Arc; use std::sync::atomic::{AtomicBool, Ordering}; use std::thread::sleep; -use std::time::{Duration, Instant}; +use std::time::Duration; mod hardware; @@ -71,7 +66,13 @@ pub fn run() -> Result<()> { let comms = s.run_cyclic( "comms-task", - CommsTask::new(15000, "nautilus-ground:14000", running.clone())?, + CommsTask::new( + 15000, + "nautilus-ground:14000", + running.clone(), + task_a.clone().new_pinset_callback(), + task_b.clone().new_pinset_callback(), + )?, 10, )?; @@ -95,36 +96,9 @@ pub fn run() -> Result<()> { 1, )?; - let mut led_pin_a = LED_A.get_pin(&task_a, &task_b); - let mut led_pin_b = LED_B.get_pin(&task_a, &task_b); - info!("Starting Main Loop"); - loop { - debug!("A On"); - 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(PinState::High, Instant::now() + Duration::from_secs(2), 0); - sleep(Duration::from_secs(1)); - if !running.load(Ordering::Relaxed) { - break; - } - - debug!("A Off"); - sleep(Duration::from_secs(1)); - if !running.load(Ordering::Relaxed) { - break; - } - - debug!("B Off"); - sleep(Duration::from_secs(1)); - if !running.load(Ordering::Relaxed) { - break; - } + while running.load(Ordering::Relaxed) { + sleep(Duration::from_millis(100)); } anyhow::Ok(()) diff --git a/flight/src/scheduler/mod.rs b/flight/src/scheduler/mod.rs index 3da0466..6294b4e 100644 --- a/flight/src/scheduler/mod.rs +++ b/flight/src/scheduler/mod.rs @@ -51,8 +51,7 @@ where type Message = (); type Data = (); - fn get_data(&self) -> Self::Data { - } + fn get_data(&self) -> Self::Data {} fn step(&mut self, _receiver: &Receiver, _step_time: Instant) { self(); diff --git a/ground/Cargo.toml b/ground/Cargo.toml index 725c0ea..99eba98 100644 --- a/ground/Cargo.toml +++ b/ground/Cargo.toml @@ -4,7 +4,10 @@ version = "0.1.0" edition = "2024" [dependencies] -anyhow = "1.0.100" +anyhow = { workspace = true } nautilus_common = { path = "../common" } -log = "0.4.28" -ciborium = { version = "0.2.2" } +log = { workspace = true } +chrono = { workspace = true } +postcard = { workspace = true } +hex = "0.4.3" +serde = { workspace = true } diff --git a/ground/src/lib.rs b/ground/src/lib.rs index 40f355b..bd00a63 100644 --- a/ground/src/lib.rs +++ b/ground/src/lib.rs @@ -1,14 +1,11 @@ -#![warn( - clippy::all, - clippy::pedantic, -)] +#![warn(clippy::all, clippy::pedantic)] use anyhow::Result; +use chrono::{TimeDelta, Utc}; use log::{error, info}; use nautilus_common::add_ctrlc_handler; -use nautilus_common::command::Command; +use nautilus_common::command::{SetPin, ValidPriorityCommand}; use nautilus_common::telemetry::Telemetry; -use nautilus_common::udp::{UdpRecvCborError, UdpSocketExt}; -use std::io::Cursor; +use nautilus_common::udp::{UdpRecvPostcardError, UdpSocketExt}; use std::net::{IpAddr, Ipv4Addr, SocketAddr, UdpSocket}; use std::sync::Arc; use std::sync::atomic::{AtomicBool, Ordering}; @@ -33,14 +30,33 @@ pub fn run() -> Result<()> { let udp = UdpSocket::bind(bind_addr)?; udp.set_read_timeout(Some(Duration::from_millis(100)))?; - let mut buffer = Cursor::new([0u8; 512]); + let mut buffer = [0u8; 512]; + + let mut do_once = true; + while running.load(Ordering::Relaxed) { - match udp.recv_cbor::(&mut buffer) { + match udp.recv_postcard::(&mut buffer) { Ok((tlm, addr)) => { flight_addr = Some(addr); info!("{tlm:?}"); + + if do_once { + do_once = false; + udp.send_command( + "/mcp23017a/set", + &ValidPriorityCommand { + inner: SetPin { + pin: 0, + value: true, + }, + valid_until: Utc::now() + TimeDelta::seconds(5), + priority: 0, + }, + addr, + )?; + } } - Err(UdpRecvCborError::NoData) => { + Err(UdpRecvPostcardError::NoData) => { // NoOp } Err(err) => { @@ -50,8 +66,40 @@ pub fn run() -> Result<()> { } if let Some(flight_addr) = flight_addr { - let cmd = Command::Shutdown; - udp.send_cbor(&cmd, &mut buffer, flight_addr)?; + udp.send_command("/shutdown", &(), flight_addr)?; + + // let cmd_data = CommandData::Shutdown; + // udp.send_postcard(&cmd_data, &mut buffer, flight_addr)?; + + // let cmd_data = SetPin { + // pin: 4, + // value: true, + // valid_until: Utc::now(), + // priority: 120, + // }; + // let cmd_data = postcard::to_allocvec(&cmd_data)?; + // + // let cmd = CommandHeader { + // name: "/shutdown", + // data: &cmd_data, + // }; + // + // let encoded = postcard::to_allocvec(&cmd)?; + // println!("{}", hex::encode(&encoded)); + // + // let decoded = postcard::from_bytes::(&encoded)?; + // println!("{decoded:?}"); + // + // let (decoded, remainder) = postcard::take_from_bytes::(decoded.data)?; + // ensure!(remainder.is_empty(), "Not all command bytes consumed"); + // println!("{decoded:?}"); + + // let mut another_buffer = Cursor::new([0u8; 512]); + // // ciborium::into_writer(&cmd, &mut another_buffer)?; + // let _ = Serializer::writer(&cmd, &mut another_buffer)?; + // let size_encoded = usize::try_from(another_buffer.position())?; + + // let _ = test(&another_buffer.get_ref()[0..size_encoded])?; } Ok(())