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

improved integration with telem viz

Browse Source
This commit is contained in:
Sergey Savelyev
2026-01-03 21:26:46 -05:00
parent 275cb07c4c
commit 252db5993d
12 changed files with 216 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files

57
flight/src/comms/mod.rs
View File

@@ -1,8 +1,9 @@
use crate::scheduler::{CyclicTask, TaskHandle};
use crate::state_vector::{SectionIdentifier, SectionWriter, StateVector};
use anyhow::{Result, ensure};
use log::{error, trace, warn};
use nautilus_common::command::{Command, CommandHeader};
use nautilus_common::telemetry::{Telemetry, TelemetryMessage};
pub(crate) use nautilus_common::telemetry::{CommsState, Telemetry, TelemetryMessage};
use nautilus_common::udp::{UdpRecvPostcardError, UdpSocketExt};
use std::any::type_name;
use std::collections::HashMap;
@@ -11,7 +12,7 @@ use std::net::{IpAddr, Ipv4Addr, SocketAddr, ToSocketAddrs, UdpSocket};
use std::sync::mpsc::Receiver;
use std::time::Instant;
pub type TelemetrySender = TaskHandle<Telemetry, ()>;
pub type TelemetrySender = TaskHandle<Telemetry, SectionIdentifier>;
impl TelemetrySender {
pub fn send(&self, telemetry_message: TelemetryMessage) {
@@ -33,6 +34,7 @@ where
udp: UdpSocket,
ground_address: A,
command_callbacks: HashMap<String, CommandCallback<'a>>,
section_writer: SectionWriter<'a, CommsState>,
}
impl<A> Debug for CommsTask<'_, A>
@@ -52,7 +54,7 @@ impl<'a, A> CommsTask<'a, A>
where
A: ToSocketAddrs + Debug,
{
pub fn new(local_port: u16, ground_address: A) -> Result<Self> {
pub fn new(local_port: u16, ground_address: A, state_vector: &'a StateVector) -> Result<Self> {
trace!(
"CommsTask::new<A={}>(local_port: {local_port}, ground_address: {ground_address:?})",
type_name::<A>()
@@ -61,10 +63,13 @@ where
// let bind_addr = SocketAddr::new(IpAddr::V6(Ipv6Addr::UNSPECIFIED), local_port);
let udp = UdpSocket::bind(bind_addr)?;
udp.set_nonblocking(true)?;
let section_writer = state_vector.create_section(CommsState::default());
Ok(Self {
udp,
ground_address,
command_callbacks: HashMap::new(),
section_writer,
})
}
@@ -100,13 +105,15 @@ where
A: ToSocketAddrs + Debug,
{
type Message = Telemetry;
type Data = ();
type Data = SectionIdentifier;
fn get_data(&self) -> Self::Data {
trace!(
"CommsTask<A={}>::get_data(self: {self:?})",
type_name::<A>()
);
self.section_writer.get_identifier()
}
fn step(&mut self, receiver: &Receiver<Self::Message>, step_time: Instant) {
@@ -117,29 +124,51 @@ where
let mut buffer = [0u8; 512];
match self.udp.recv_postcard::<CommandHeader>(&mut buffer) {
Ok((cmd, _)) => match self.command_callbacks.get(cmd.name) {
Some(handler) => {
if let Err(e) = handler(cmd.data) {
error!("Command Error: {e}");
Ok((cmd, _, size)) => {
self.section_writer.update(|state| {
state.rx_packets = state.rx_packets.wrapping_add(1);
state.rx_bytes = state
.rx_bytes
.wrapping_add(u32::try_from(size % (u32::MAX as usize)).unwrap_or(0));
});
match self.command_callbacks.get(cmd.name) {
Some(handler) => {
if let Err(e) = handler(cmd.data) {
error!("Command Error: {e}");
}
}
None => {
warn!("Unknown Command: {}", cmd.name);
}
}
None => {
warn!("Unknown Command: {}", cmd.name);
}
},
}
Err(UdpRecvPostcardError::NoData) => {}
Err(err) => {
error!("Rx error: {err}");
self.section_writer.update(|state| {
state.rx_errors = state.rx_errors.wrapping_add(1);
});
}
}
// Intentionally ignore Err case
while let Ok(tlm) = receiver.try_recv() {
if let Err(err) = self
match self
.udp
.send_postcard(&tlm, &mut buffer, &self.ground_address)
{
error!("Tx Error: {err}");
Ok(bytes_sent) => self.section_writer.update(|state| {
state.tx_packets = state.tx_packets.wrapping_add(1);
state.tx_bytes = state
.tx_bytes
.wrapping_add(u32::try_from(bytes_sent % (u32::MAX as usize)).unwrap_or(0));
}),
Err(err) => {
error!("Tx Error: {err}");
self.section_writer.update(|state| {
state.tx_errors = state.tx_errors.wrapping_add(1);
});
}
}
}
}

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

@@ -44,7 +44,7 @@ pub struct Mcp23017Data {
impl Mcp23017Data {
pub fn get_id(&self) -> SectionIdentifier {
self.id.clone()
self.id
}
}
@@ -216,9 +216,10 @@ impl<M: Mcp23017 + Debug> CyclicTask for Mcp23017Task<'_, M> {
}
for pin in 0u8..16u8 {
let state = self.pins.pins[pin as usize].value;
if self.pins.pins[pin as usize].changed {
self.pins.pins[pin as usize].changed = false;
let current_pin = &mut self.pins.pins[pin as usize];
let state = current_pin.value;
if current_pin.changed {
current_pin.changed = false;
// This shouldn't be able to fail
// TODO: handle error case
let _ = self.mcp23017.set_pin(pin, state);

17
flight/src/lib.rs
View File

@@ -1,5 +1,6 @@
#![warn(clippy::all, clippy::pedantic)]
use crate::comms::CommsTask;
use crate::comms::{CommsState, CommsTask};
use crate::hardware::Hardware;
use crate::hardware::initialize;
use crate::hardware::mcp23017::{Mcp23017, Mcp23017State, Mcp23017Task};
@@ -10,12 +11,12 @@ use crate::state_vector::StateVector;
use anyhow::Result;
use embedded_hal::pwm::SetDutyCycle;
use log::info;
use nautilus_common::add_ctrlc_handler_arc;
use nautilus_common::telemetry::{SwitchBank, TelemetryMessage};
use std::sync::Arc;
use std::sync::atomic::{AtomicBool, Ordering};
use std::thread::sleep;
use std::time::Duration;
use nautilus_common::add_ctrlc_handler_arc;
mod hardware;
@@ -69,30 +70,34 @@ pub fn run() -> Result<()> {
)?;
let b_id = task_b.get_id();
let mut comms = CommsTask::new(15000, "nautilus-ground:14000")?;
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())?;
let comms = s.run_cyclic("comms-task", comms, 10)?;
let comms_id = *comms;
let sv = &state_vector;
s.run_cyclic(
"telemetry-producer",
move || {
sv.access_section(&a_id, |state: &Mcp23017State| {
sv.access_section(a_id, |state: &Mcp23017State| {
comms.send(TelemetryMessage::SwitchState {
bank: SwitchBank::A,
switches: state.pins,
});
});
sv.access_section(&b_id, |state: &Mcp23017State| {
sv.access_section(b_id, |state: &Mcp23017State| {
comms.send(TelemetryMessage::SwitchState {
bank: SwitchBank::B,
switches: state.pins,
});
});
sv.access_section(comms_id, |state: &CommsState| {
comms.send(state.clone().into());
});
},
10,
1,
)?;
info!("Starting Main Loop");

16
flight/src/state_vector/mod.rs
View File

@@ -12,7 +12,7 @@ pub struct StateVector {
sections: RwLock<HashMap<SectionIdentifier, Box<RwLock<dyn Any + Send + Sync>>>>,
}
#[derive(Clone, Eq, PartialEq, Hash, Debug)]
#[derive(Clone, Copy, Eq, PartialEq, Hash, Debug)]
pub struct SectionIdentifier(usize);
pub struct SectionWriter<'a, T> {
@@ -39,7 +39,7 @@ impl<T: 'static> SectionWriter<'_, T> {
"SectionWriter<T={}>::get_identifier(self: {self:?})",
type_name::<T>()
);
self.id.clone()
self.id
}
pub fn update<F, R>(&self, f: F) -> R
@@ -81,9 +81,7 @@ impl StateVector {
self.sections.clear_poison();
let mut sections = self.sections.write().unwrap();
if !sections.contains_key(&id) {
sections.insert(id.clone(), lock);
}
sections.entry(id).or_insert(lock);
drop(sections);
@@ -94,7 +92,7 @@ impl StateVector {
}
}
pub fn access_section<T, F, R>(&self, id: &SectionIdentifier, f: F) -> Option<R>
pub fn access_section<T, F, R>(&self, id: SectionIdentifier, f: F) -> Option<R>
where
T: 'static,
F: FnOnce(&T) -> R,
@@ -109,7 +107,7 @@ impl StateVector {
let Ok(sections) = self.sections.read() else {
return None;
};
let section = sections.get(id)?;
let section = sections.get(&id)?;
section.clear_poison();
let Ok(data) = section.read() else {
return None;
@@ -148,14 +146,14 @@ mod tests {
let id_1 = section_1.get_identifier();
state_vector.access_section(&id_1, |s: &TestType| {
state_vector.access_section(id_1, |s: &TestType| {
assert_eq!(1, s.value1);
assert_eq!(2, s.value2);
});
let id_2 = section_2.get_identifier();
state_vector.access_section(&id_2, |s: &TestType| {
state_vector.access_section(id_2, |s: &TestType| {
assert_eq!(3, s.value1);
assert_eq!(4, s.value2);
});