From 2a2261229b223d45ce9f8742ddbda50bda88ca28 Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Sat, 11 May 2024 13:30:02 -0400 Subject: [PATCH 1/5] WIP: Stuff 3 messages into a single packet. --- boards/communication/src/communication.rs | 13 +-- boards/communication/src/data_manager.rs | 104 ++++------------------ boards/communication/src/main.rs | 30 +++---- 3 files changed, 31 insertions(+), 116 deletions(-) diff --git a/boards/communication/src/communication.rs b/boards/communication/src/communication.rs index 6af79cb..cb9aaa5 100644 --- a/boards/communication/src/communication.rs +++ b/boards/communication/src/communication.rs @@ -120,8 +120,7 @@ impl CanDevice0 { can.filters_standard() .push(Filter::Classic { action: Action::StoreFifo0, - filter: ecan::StandardId::new(messages::node::Node::RecoveryBoard.into()) - .unwrap(), + filter: ecan::StandardId::new(messages::node::Node::RecoveryBoard.into()).unwrap(), mask: ecan::StandardId::ZERO, }) .unwrap_or_else(|_| panic!("Recovery filter")); @@ -129,8 +128,7 @@ impl CanDevice0 { can.filters_standard() .push(Filter::Classic { action: Action::StoreFifo1, - filter: ecan::StandardId::new(messages::node::Node::SensorBoard.into()) - .unwrap(), + filter: ecan::StandardId::new(messages::node::Node::SensorBoard.into()).unwrap(), mask: ecan::StandardId::ZERO, }) .unwrap_or_else(|_| panic!("Sensor filter")); @@ -146,8 +144,7 @@ impl CanDevice0 { can.filters_standard() .push(Filter::Classic { action: Action::StoreFifo0, - filter: ecan::StandardId::new(messages::node::Node::GroundStation.into()) - .unwrap(), + filter: ecan::StandardId::new(messages::node::Node::GroundStation.into()).unwrap(), mask: ecan::StandardId::ZERO, }) .unwrap_or_else(|_| panic!("Ground Station filter")); @@ -266,9 +263,7 @@ impl RadioManager { mav_sequence: 0, } } - pub fn send_message(&mut self, m: Message) -> Result<(), HydraError> { - let payload: Vec = postcard::to_vec(&m)?; - + pub fn send_message(&mut self, payload: Vec) -> Result<(), HydraError> { let mav_header = mavlink::MavHeader { system_id: 1, component_id: 1, diff --git a/boards/communication/src/data_manager.rs b/boards/communication/src/data_manager.rs index 6d60069..466a901 100644 --- a/boards/communication/src/data_manager.rs +++ b/boards/communication/src/data_manager.rs @@ -1,43 +1,20 @@ +use heapless::{HistoryBuffer, Vec}; use messages::command::RadioRate; use messages::state::StateData; use messages::Message; +const MAX_RADIO_MSG: u8 = 255; + #[derive(Clone)] pub struct DataManager { - pub air: Option, - pub ekf_nav_1: Option, - pub ekf_nav_2: Option, - pub ekf_nav_acc: Option, - pub ekf_quat: Option, - pub imu_1: Option, - pub imu_2: Option, - pub utc_time: Option, - pub gps_vel: Option, - pub gps_vel_acc: Option, - pub gps_pos_1: Option, - pub gps_pos_2: Option, - pub gps_pos_acc: Option, - pub state: Option, + pub message_queue: HistoryBuffer, pub logging_rate: Option, } impl DataManager { pub fn new() -> Self { Self { - air: None, - ekf_nav_1: None, - ekf_nav_2: None, - ekf_nav_acc: None, - ekf_quat: None, - imu_1: None, - imu_2: None, - utc_time: None, - gps_vel: None, - gps_vel_acc: None, - gps_pos_1: None, - gps_pos_2: None, - gps_pos_acc: None, - state: None, + message_queue: HistoryBuffer::new(), logging_rate: Some(RadioRate::Slow), // start slow. } } @@ -52,74 +29,23 @@ impl DataManager { return RadioRate::Slow; } - /// Do not clone instead take to reduce CPU load. - pub fn take_sensors(&mut self) -> [Option; 13] { - [ - self.air.take(), - self.ekf_nav_1.take(), - self.ekf_nav_2.take(), - self.ekf_nav_acc.take(), - self.ekf_quat.take(), - self.imu_1.take(), - self.imu_2.take(), - self.utc_time.take(), - self.gps_vel.take(), - self.gps_vel_acc.take(), - self.gps_pos_1.take(), - self.gps_pos_2.take(), - self.gps_pos_acc.take(), - ] + pub fn stuff_messages(&mut self) -> Option> { + let mut bytes: Vec = Vec::new(); + for el in self.message_queue.oldest_ordered() { + bytes.extend_from_slice(el.to_bytes()) + } + if bytes.len() > 0 { + return Some(bytes); + } + None } pub fn clone_states(&self) -> [Option; 1] { [self.state.clone()] } pub fn handle_data(&mut self, data: Message) { + self.message_queue.write(data); match data.data { - messages::Data::Sensor(ref sensor) => match sensor.data { - messages::sensor::SensorData::EkfNavAcc(_) => { - self.ekf_nav_acc = Some(data); - } - messages::sensor::SensorData::GpsPosAcc(_) => { - self.gps_pos_acc = Some(data); - } - messages::sensor::SensorData::Air(_) => { - self.air = Some(data); - } - messages::sensor::SensorData::EkfNav1(_) => { - self.ekf_nav_1 = Some(data); - } - messages::sensor::SensorData::EkfNav2(_) => { - self.ekf_nav_2 = Some(data); - } - messages::sensor::SensorData::EkfQuat(_) => { - self.ekf_quat = Some(data); - } - messages::sensor::SensorData::GpsVel(_) => { - self.gps_vel = Some(data); - } - messages::sensor::SensorData::GpsVelAcc(_) => { - self.gps_vel_acc = Some(data); - } - messages::sensor::SensorData::Imu1(_) => { - self.imu_1 = Some(data); - } - messages::sensor::SensorData::Imu2(_) => { - self.imu_2 = Some(data); - } - messages::sensor::SensorData::UtcTime(_) => { - self.utc_time = Some(data); - } - messages::sensor::SensorData::GpsPos1(_) => { - self.gps_pos_1 = Some(data); - } - messages::sensor::SensorData::GpsPos2(_) => { - self.gps_pos_2 = Some(data); - } - }, - messages::Data::State(state) => { - self.state = Some(state.data); - } messages::Data::Command(command) => match command.data { messages::command::CommandData::RadioRateChange(command_data) => { self.logging_rate = Some(command_data.rate); diff --git a/boards/communication/src/main.rs b/boards/communication/src/main.rs index dcd1ea9..29faeb3 100644 --- a/boards/communication/src/main.rs +++ b/boards/communication/src/main.rs @@ -25,6 +25,7 @@ use hal::gpio::{ use hal::prelude::*; use hal::sercom::{spi, spi::Config, spi::Duplex, spi::Pads, spi::Spi, IoSet1, Sercom4}; use health::HealthMonitorChannelsCommunication; +use heapless::Vec; use mcan::messageram::SharedMemory; use messages::command::RadioRate; use messages::health::Health; @@ -231,7 +232,7 @@ mod app { * Sends a message to the radio over UART. */ #[task(capacity = 10, shared = [&em, radio_manager])] - fn send_gs(mut cx: send_gs::Context, m: Message) { + fn send_gs(mut cx: send_gs::Context, m: Vec) { cx.shared.radio_manager.lock(|radio_manager| { cx.shared.em.run(|| { radio_manager.send_message(m)?; @@ -241,10 +242,10 @@ mod app { } #[task(capacity = 10, local = [sd_manager], shared = [&em])] - fn sd_dump(cx: sd_dump::Context, m: Message) { + fn sd_dump(cx: sd_dump::Context, m: Vec) { let manager = cx.local.sd_manager; cx.shared.em.run(|| { - let mut buf: [u8; 255] = [0; 255]; + let mut buf: [u8; 255] = m.into_array()?; let msg_ser = postcard::to_slice_cobs(&m, &mut buf)?; if let Some(mut file) = manager.file.take() { manager.write(&mut file, &msg_ser)?; @@ -262,23 +263,16 @@ mod app { */ #[task(shared = [data_manager, &em])] fn sensor_send(mut cx: sensor_send::Context) { - let (sensors, logging_rate) = cx - .shared - .data_manager - .lock(|data_manager| (data_manager.take_sensors(), data_manager.get_logging_rate())); + let (sensors, logging_rate) = cx.shared.data_manager.lock(|data_manager| { + ( + data_manager.stuff_messages(), + data_manager.get_logging_rate(), + ) + }); cx.shared.em.run(|| { - for msg in sensors { - match msg { - Some(x) => { - spawn!(send_gs, x.clone())?; - spawn!(sd_dump, x)?; - } - None => { - continue; - } - } - } + spawn!(send_gs, sensors.clone())?; + spawn!(sd_dump, sensors)?; Ok(()) }); match logging_rate { From f83838dde44e1a1f7a93f3ed5f1d1ffb2606ff1a Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Tue, 14 May 2024 15:32:49 -0400 Subject: [PATCH 2/5] Fix: Build Errors --- boards/communication/src/data_manager.rs | 59 ++++++++++++++++++++---- boards/communication/src/main.rs | 20 ++++---- 2 files changed, 60 insertions(+), 19 deletions(-) diff --git a/boards/communication/src/data_manager.rs b/boards/communication/src/data_manager.rs index 466a901..11be630 100644 --- a/boards/communication/src/data_manager.rs +++ b/boards/communication/src/data_manager.rs @@ -2,13 +2,17 @@ use heapless::{HistoryBuffer, Vec}; use messages::command::RadioRate; use messages::state::StateData; use messages::Message; +use messages::{ + MAX_COMMAND_SIZE, MAX_HEALTH_SIZE, MAX_LOG_SIZE, MAX_SENSOR_SIZE, MAX_SIZE, MAX_STATE_SIZE, +}; +use postcard; const MAX_RADIO_MSG: u8 = 255; -#[derive(Clone)] pub struct DataManager { pub message_queue: HistoryBuffer, pub logging_rate: Option, + pub state: Option, } impl DataManager { @@ -16,6 +20,7 @@ impl DataManager { Self { message_queue: HistoryBuffer::new(), logging_rate: Some(RadioRate::Slow), // start slow. + state: None, } } @@ -29,22 +34,54 @@ impl DataManager { return RadioRate::Slow; } - pub fn stuff_messages(&mut self) -> Option> { + pub fn stuff_messages(&mut self) -> Result, postcard::Error> { let mut bytes: Vec = Vec::new(); for el in self.message_queue.oldest_ordered() { - bytes.extend_from_slice(el.to_bytes()) + match el.data { + messages::Data::Command(_) => { + if bytes.len() + MAX_COMMAND_SIZE <= MAX_SIZE { + bytes.extend(postcard::to_vec::(el)?); + } else { + break; + } + } + messages::Data::Health(_) => { + if bytes.len() + MAX_HEALTH_SIZE <= MAX_SIZE { + bytes.extend(postcard::to_vec::(el)?); + } else { + break; + } + } + messages::Data::Sensor(_) => { + if bytes.len() + MAX_SENSOR_SIZE <= MAX_SIZE { + bytes.extend(postcard::to_vec::(el)?); + } else { + break; + } + } + messages::Data::State(_) => { + if bytes.len() + MAX_STATE_SIZE <= MAX_SIZE { + bytes.extend(postcard::to_vec::(el)?); + } else { + break; + } + } + messages::Data::Log(_) => { + if bytes.len() + MAX_LOG_SIZE <= MAX_SIZE { + bytes.extend(postcard::to_vec::(el)?); + } else { + break; + } + } + } } if bytes.len() > 0 { - return Some(bytes); + return Ok(bytes); } - None + return Err(postcard::Error::WontImplement); } - pub fn clone_states(&self) -> [Option; 1] { - [self.state.clone()] - } pub fn handle_data(&mut self, data: Message) { - self.message_queue.write(data); match data.data { messages::Data::Command(command) => match command.data { messages::command::CommandData::RadioRateChange(command_data) => { @@ -54,7 +91,9 @@ impl DataManager { messages::command::CommandData::DeployMain(_) => {} messages::command::CommandData::PowerDown(_) => {} }, - _ => {} + _ => { + self.message_queue.write(data); + } } } } diff --git a/boards/communication/src/main.rs b/boards/communication/src/main.rs index 29faeb3..db03f29 100644 --- a/boards/communication/src/main.rs +++ b/boards/communication/src/main.rs @@ -221,11 +221,10 @@ mod app { }); } - /// Receives a log message from the custom logger so that it can be sent over the radio. pub fn queue_gs_message(d: impl Into) { let message = Message::new(&monotonics::now(), COM_ID, d.into()); - - send_gs::spawn(message).ok(); + let bytes = postcard::to_vec(&message).unwrap(); + spawn!(send_gs, bytes).ok(); } /** @@ -245,7 +244,7 @@ mod app { fn sd_dump(cx: sd_dump::Context, m: Vec) { let manager = cx.local.sd_manager; cx.shared.em.run(|| { - let mut buf: [u8; 255] = m.into_array()?; + let mut buf: [u8; 255] = [0; 255]; let msg_ser = postcard::to_slice_cobs(&m, &mut buf)?; if let Some(mut file) = manager.file.take() { manager.write(&mut file, &msg_ser)?; @@ -263,7 +262,7 @@ mod app { */ #[task(shared = [data_manager, &em])] fn sensor_send(mut cx: sensor_send::Context) { - let (sensors, logging_rate) = cx.shared.data_manager.lock(|data_manager| { + let (stuffed_messages, logging_rate) = cx.shared.data_manager.lock(|data_manager| { ( data_manager.stuff_messages(), data_manager.get_logging_rate(), @@ -271,8 +270,9 @@ mod app { }); cx.shared.em.run(|| { - spawn!(send_gs, sensors.clone())?; - spawn!(sd_dump, sensors)?; + let bytes = postcard::to_vec(&stuffed_messages?)?; + spawn!(send_gs, bytes.clone())?; + spawn!(sd_dump, bytes)?; Ok(()) }); match logging_rate { @@ -294,7 +294,8 @@ mod app { cx.shared.em.run(|| { if let Some(x) = state_data { let message = Message::new(&monotonics::now(), COM_ID, State::new(x)); - spawn!(send_gs, message)?; + let bytes = postcard::to_vec(&message).unwrap(); + spawn!(send_gs, bytes)?; } // if there is none we still return since we simply don't have data yet. Ok(()) }); @@ -315,7 +316,8 @@ mod app { Health::new(health_manager.monitor.data.clone(), state), ) }); - spawn!(send_gs, msg)?; + let bytes = postcard::to_vec(&msg).unwrap(); + spawn!(send_gs, bytes)?; spawn_after!(report_health, ExtU64::secs(5))?; Ok(()) }); From 3e3125d3e0e6e1cd8e952864f15caf77fa6a95da Mon Sep 17 00:00:00 2001 From: Eilay Katsnelson Date: Sat, 1 Jun 2024 11:20:09 -0400 Subject: [PATCH 3/5] important changes --- boards/communication/src/data_manager.rs | 21 +++++++++++---------- boards/communication/src/main.rs | 12 +++++++----- 2 files changed, 18 insertions(+), 15 deletions(-) diff --git a/boards/communication/src/data_manager.rs b/boards/communication/src/data_manager.rs index 11be630..95625e1 100644 --- a/boards/communication/src/data_manager.rs +++ b/boards/communication/src/data_manager.rs @@ -1,16 +1,17 @@ +use common_arm::HydraError; use heapless::{HistoryBuffer, Vec}; use messages::command::RadioRate; use messages::state::StateData; use messages::Message; use messages::{ - MAX_COMMAND_SIZE, MAX_HEALTH_SIZE, MAX_LOG_SIZE, MAX_SENSOR_SIZE, MAX_SIZE, MAX_STATE_SIZE, + MAX_COMMAND_SIZE, MAX_HEALTH_SIZE, MAX_LOG_SIZE, MAX_SENSOR_SIZE, MAX_STATE_SIZE, }; use postcard; -const MAX_RADIO_MSG: u8 = 255; +const MAX_RADIO_MSG: usize = 255; pub struct DataManager { - pub message_queue: HistoryBuffer, + pub message_queue: HistoryBuffer, pub logging_rate: Option, pub state: Option, } @@ -34,40 +35,40 @@ impl DataManager { return RadioRate::Slow; } - pub fn stuff_messages(&mut self) -> Result, postcard::Error> { + pub fn stuff_messages(&mut self) -> Result, HydraError> { let mut bytes: Vec = Vec::new(); for el in self.message_queue.oldest_ordered() { match el.data { messages::Data::Command(_) => { - if bytes.len() + MAX_COMMAND_SIZE <= MAX_SIZE { + if bytes.len() + MAX_COMMAND_SIZE <= MAX_RADIO_MSG { bytes.extend(postcard::to_vec::(el)?); } else { break; } } messages::Data::Health(_) => { - if bytes.len() + MAX_HEALTH_SIZE <= MAX_SIZE { + if bytes.len() + MAX_HEALTH_SIZE <= MAX_RADIO_MSG { bytes.extend(postcard::to_vec::(el)?); } else { break; } } messages::Data::Sensor(_) => { - if bytes.len() + MAX_SENSOR_SIZE <= MAX_SIZE { + if bytes.len() + MAX_SENSOR_SIZE <= MAX_RADIO_MSG { bytes.extend(postcard::to_vec::(el)?); } else { break; } } messages::Data::State(_) => { - if bytes.len() + MAX_STATE_SIZE <= MAX_SIZE { + if bytes.len() + MAX_STATE_SIZE <= MAX_RADIO_MSG { bytes.extend(postcard::to_vec::(el)?); } else { break; } } messages::Data::Log(_) => { - if bytes.len() + MAX_LOG_SIZE <= MAX_SIZE { + if bytes.len() + MAX_LOG_SIZE <= MAX_RADIO_MSG { bytes.extend(postcard::to_vec::(el)?); } else { break; @@ -78,7 +79,7 @@ impl DataManager { if bytes.len() > 0 { return Ok(bytes); } - return Err(postcard::Error::WontImplement); + return Err(HydraError::from("No messages to send")); } pub fn handle_data(&mut self, data: Message) { diff --git a/boards/communication/src/main.rs b/boards/communication/src/main.rs index db03f29..917856b 100644 --- a/boards/communication/src/main.rs +++ b/boards/communication/src/main.rs @@ -221,10 +221,12 @@ mod app { }); } - pub fn queue_gs_message(d: impl Into) { + pub fn queue_gs_message(d: impl Into){ let message = Message::new(&monotonics::now(), COM_ID, d.into()); - let bytes = postcard::to_vec(&message).unwrap(); - spawn!(send_gs, bytes).ok(); + let bytes = postcard::to_vec(&message); + if let Ok(bytes) = bytes { + send_gs::spawn(bytes).ok(); + } } /** @@ -294,7 +296,7 @@ mod app { cx.shared.em.run(|| { if let Some(x) = state_data { let message = Message::new(&monotonics::now(), COM_ID, State::new(x)); - let bytes = postcard::to_vec(&message).unwrap(); + let bytes = postcard::to_vec(&message)?; spawn!(send_gs, bytes)?; } // if there is none we still return since we simply don't have data yet. Ok(()) @@ -316,7 +318,7 @@ mod app { Health::new(health_manager.monitor.data.clone(), state), ) }); - let bytes = postcard::to_vec(&msg).unwrap(); + let bytes = postcard::to_vec(&msg)?; spawn!(send_gs, bytes)?; spawn_after!(report_health, ExtU64::secs(5))?; Ok(()) From 4f0656fa53bdc3f0d7f2bd1a4e7a1654ffe780e6 Mon Sep 17 00:00:00 2001 From: Eilay Katsnelson Date: Sat, 1 Jun 2024 11:28:13 -0400 Subject: [PATCH 4/5] fix linter --- boards/communication/src/data_manager.rs | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/boards/communication/src/data_manager.rs b/boards/communication/src/data_manager.rs index 95625e1..3101437 100644 --- a/boards/communication/src/data_manager.rs +++ b/boards/communication/src/data_manager.rs @@ -3,9 +3,7 @@ use heapless::{HistoryBuffer, Vec}; use messages::command::RadioRate; use messages::state::StateData; use messages::Message; -use messages::{ - MAX_COMMAND_SIZE, MAX_HEALTH_SIZE, MAX_LOG_SIZE, MAX_SENSOR_SIZE, MAX_STATE_SIZE, -}; +use messages::{MAX_COMMAND_SIZE, MAX_HEALTH_SIZE, MAX_LOG_SIZE, MAX_SENSOR_SIZE, MAX_STATE_SIZE}; use postcard; const MAX_RADIO_MSG: usize = 255; From cd6282b2f736ac6fb1e3bbe7e6ebf94f576f1352 Mon Sep 17 00:00:00 2001 From: Eilay Katsnelson Date: Sat, 1 Jun 2024 11:43:45 -0400 Subject: [PATCH 5/5] fix linting 2 --- boards/communication/src/main.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/boards/communication/src/main.rs b/boards/communication/src/main.rs index 917856b..c2624e0 100644 --- a/boards/communication/src/main.rs +++ b/boards/communication/src/main.rs @@ -221,7 +221,7 @@ mod app { }); } - pub fn queue_gs_message(d: impl Into){ + pub fn queue_gs_message(d: impl Into) { let message = Message::new(&monotonics::now(), COM_ID, d.into()); let bytes = postcard::to_vec(&message); if let Ok(bytes) = bytes {