From 2a2261229b223d45ce9f8742ddbda50bda88ca28 Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Sat, 11 May 2024 13:30:02 -0400 Subject: [PATCH] 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 {