Skip to content

Commit

Permalink
WIP: Stuff 3 messages into a single packet.
Browse files Browse the repository at this point in the history
  • Loading branch information
@NoahSprenger
NoahSprenger committed May 11, 2024
1 parent beef059 commit 2a22612
Show file tree
Hide file tree
Showing 3 changed files with 31 additions and 116 deletions.
13 changes: 4 additions & 9 deletions boards/communication/src/communication.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -120,17 +120,15 @@ 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"));

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"));
Expand All @@ -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"));
Expand Down Expand Up @@ -266,9 +263,7 @@ impl RadioManager {
mav_sequence: 0,
}
}
pub fn send_message(&mut self, m: Message) -> Result<(), HydraError> {
let payload: Vec<u8, 255> = postcard::to_vec(&m)?;

pub fn send_message(&mut self, payload: Vec<u8, 255>) -> Result<(), HydraError> {
let mav_header = mavlink::MavHeader {
system_id: 1,
component_id: 1,
Expand Down
104 changes: 15 additions & 89 deletions boards/communication/src/data_manager.rs
View file
Original file line number Diff line number Diff line change
@@ -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<Message>,
pub ekf_nav_1: Option<Message>,
pub ekf_nav_2: Option<Message>,
pub ekf_nav_acc: Option<Message>,
pub ekf_quat: Option<Message>,
pub imu_1: Option<Message>,
pub imu_2: Option<Message>,
pub utc_time: Option<Message>,
pub gps_vel: Option<Message>,
pub gps_vel_acc: Option<Message>,
pub gps_pos_1: Option<Message>,
pub gps_pos_2: Option<Message>,
pub gps_pos_acc: Option<Message>,
pub state: Option<StateData>,
pub message_queue: HistoryBuffer<Message, 32>,

Check failure on line 10 in boards/communication/src/data_manager.rs

View workflow job for this annotation

GitHub Actions / All 

the trait bound `HistoryBuffer<messages::Message, 32>: Clone` is not satisfied

Check failure on line 10 in boards/communication/src/data_manager.rs

View workflow job for this annotation

GitHub Actions / clippy

the trait bound `heapless::HistoryBuffer<messages::Message, 32>: core::clone::Clone` is not satisfied 

error[E0277]: the trait bound `heapless::HistoryBuffer<messages::Message, 32>: core::clone::Clone` is not satisfied
  --> boards/communication/src/data_manager.rs:10:5
   |
8  | #[derive(Clone)]
   |          ----- in this derive macro expansion
9  | pub struct DataManager {
10 |     pub message_queue: HistoryBuffer<Message, 32>,
   |     ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ the trait `core::clone::Clone` is not implemented for `heapless::HistoryBuffer<messages::Message, 32>`
   |
   = note: this error originates in the derive macro `Clone` (in Nightly builds, run with -Z macro-backtrace for more info)
pub logging_rate: Option<RadioRate>,
}

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.
}
}
Expand All @@ -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<Message>; 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<Vec<u8, 255>> {
let mut bytes: Vec<u8, 255> = Vec::new();
for el in self.message_queue.oldest_ordered() {
bytes.extend_from_slice(el.to_bytes())

Check failure on line 35 in boards/communication/src/data_manager.rs

View workflow job for this annotation

GitHub Actions / All 

no method named `to_bytes` found for reference `&messages::Message` in the current scope

Check failure on line 35 in boards/communication/src/data_manager.rs

View workflow job for this annotation

GitHub Actions / All 

mismatched types

Check failure on line 35 in boards/communication/src/data_manager.rs

View workflow job for this annotation

GitHub Actions / clippy

mismatched types 

error[E0308]: mismatched types
  --> boards/communication/src/data_manager.rs:35:13
   |
35 |             bytes.extend_from_slice(el.to_bytes())
   |             ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ expected `()`, found `Result<(), ()>`
   |
   = note: expected unit type `()`
                   found enum `core::result::Result<(), ()>`
help: consider using a semicolon here
   |
35 |             bytes.extend_from_slice(el.to_bytes());
   |                                                   +
help: consider using `Result::expect` to unwrap the `core::result::Result<(), ()>` value, panicking if the value is a `Result::Err`
   |
35 |             bytes.extend_from_slice(el.to_bytes()).expect("REASON")
   |                                                   +++++++++++++++++

Check failure on line 35 in boards/communication/src/data_manager.rs

View workflow job for this annotation

GitHub Actions / clippy

no method named `to_bytes` found for reference `&messages::Message` in the current scope 

error[E0599]: no method named `to_bytes` found for reference `&messages::Message` in the current scope
  --> boards/communication/src/data_manager.rs:35:40
   |
35 |             bytes.extend_from_slice(el.to_bytes())
   |                                        ^^^^^^^^ method not found in `&Message`
}
if bytes.len() > 0 {
return Some(bytes);
}
None
}

pub fn clone_states(&self) -> [Option<StateData>; 1] {
[self.state.clone()]

Check failure on line 44 in boards/communication/src/data_manager.rs

View workflow job for this annotation

GitHub Actions / All 

no field `state` on type `&data_manager::DataManager`

Check failure on line 44 in boards/communication/src/data_manager.rs

View workflow job for this annotation

GitHub Actions / clippy

no field `state` on type `&data_manager::DataManager` 

error[E0609]: no field `state` on type `&data_manager::DataManager`
  --> boards/communication/src/data_manager.rs:44:15
   |
44 |         [self.state.clone()]
   |               ^^^^^ unknown field
   |
   = note: available fields are: `message_queue`, `logging_rate`
}
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 {

Check failure on line 49 in boards/communication/src/data_manager.rs

View workflow job for this annotation

GitHub Actions / clippy

use of moved value 

error[E0382]: use of moved value
  --> boards/communication/src/data_manager.rs:49:37
   |
46 |     pub fn handle_data(&mut self, data: Message) {
   |                                   ---- move occurs because `data` has type `messages::Message`, which does not implement the `Copy` trait
47 |         self.message_queue.write(data);
   |                                  ---- value moved here
48 |         match data.data {
49 |             messages::Data::Command(command) => match command.data {
   |                                     ^^^^^^^ value used here after move
messages::command::CommandData::RadioRateChange(command_data) => {
self.logging_rate = Some(command_data.rate);
Expand Down
30 changes: 12 additions & 18 deletions boards/communication/src/main.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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<u8, 255>) {
cx.shared.radio_manager.lock(|radio_manager| {
cx.shared.em.run(|| {
radio_manager.send_message(m)?;
Expand All @@ -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<u8, 255>) {
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()?;

Check failure on line 248 in boards/communication/src/main.rs

View workflow job for this annotation

GitHub Actions / All 

`?` couldn't convert the error to `common_arm::error::hydra_error::HydraErrorType`

Check failure on line 248 in boards/communication/src/main.rs

View workflow job for this annotation

GitHub Actions / clippy

`?` couldn't convert the error to `common_arm::error::hydra_error::HydraErrorType` 

error[E0277]: `?` couldn't convert the error to `common_arm::error::hydra_error::HydraErrorType`
   --> boards/communication/src/main.rs:248:52
    |
248 |             let mut buf: [u8; 255] = m.into_array()?;
    |                                                    ^ the trait `core::convert::From<heapless::Vec<u8, 255>>` is not implemented for `common_arm::error::hydra_error::HydraErrorType`, which is required by `core::result::Result<(), common_arm::HydraError>: core::ops::FromResidual<core::result::Result<core::convert::Infallible, heapless::Vec<u8, 255>>>`
    |
    = note: the question mark operation (`?`) implicitly performs a conversion on the error value using the `From` trait
    = help: the following other types implement trait `core::convert::From<T>`:
              <common_arm::error::hydra_error::HydraErrorType as core::convert::From<atsamd_hal::prelude::nb::Error<common_arm::mcan::tx_buffers::Error>>>
              <common_arm::error::hydra_error::HydraErrorType as core::convert::From<postcard::Error>>
              <common_arm::error::hydra_error::HydraErrorType as core::convert::From<embedded_sdmmc::Error<embedded_sdmmc::sdmmc::Error>>>
              <common_arm::error::hydra_error::HydraErrorType as core::convert::From<messages::mavlink::error::MessageReadError>>
              <common_arm::error::hydra_error::HydraErrorType as core::convert::From<messages::mavlink::error::MessageWriteError>>
              <common_arm::error::hydra_error::HydraErrorType as core::convert::From<common_arm::mcan::message::TooMuchData>>
              <common_arm::error::hydra_error::HydraErrorType as core::convert::From<core::convert::Infallible>>
              <common_arm::error::hydra_error::HydraErrorType as core::convert::From<&'static str>>
    = note: required for `heapless::Vec<u8, 255>` to implement `core::convert::Into<common_arm::error::hydra_error::HydraErrorType>`
    = note: required for `common_arm::HydraError` to implement `core::convert::From<heapless::Vec<u8, 255>>`
    = note: required for `core::result::Result<(), common_arm::HydraError>` to implement `core::ops::FromResidual<core::result::Result<core::convert::Infallible, heapless::Vec<u8, 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)?;
Expand All @@ -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())?;

Check failure on line 274 in boards/communication/src/main.rs

View workflow job for this annotation

GitHub Actions / All 

mismatched types

Check failure on line 274 in boards/communication/src/main.rs

View workflow job for this annotation

GitHub Actions / clippy

mismatched types 

error[E0308]: mismatched types
   --> boards/communication/src/main.rs:274:29
    |
274 |             spawn!(send_gs, sensors.clone())?;
    |             ----------------^^^^^^^^^^^^^^^-
    |             |               |
    |             |               expected `Vec<u8, 255>`, found `Option<Vec<u8, 255>>`
    |             arguments to this function are incorrect
    |
    = note: expected struct `heapless::Vec<_, _>`
                 found enum `core::option::Option<heapless::Vec<_, _>>`
note: function defined here
   --> boards/communication/src/main.rs:44:1
    |
44  | #[rtic::app(device = hal::pac, peripherals = true, dispatchers = [EVSYS_0, EVSYS_1, EVSYS_2])]
    | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
    = note: this error originates in the attribute macro `rtic::app` (in Nightly builds, run with -Z macro-backtrace for more info)
help: consider using `Option::expect` to unwrap the `core::option::Option<heapless::Vec<u8, 255>>` value, panicking if the value is an `Option::None`
    |
274 |             spawn!(send_gs, sensors.clone().expect("REASON"))?;
    |                                            +++++++++++++++++
spawn!(sd_dump, sensors)?;

Check failure on line 275 in boards/communication/src/main.rs

View workflow job for this annotation

GitHub Actions / All 

mismatched types

Check failure on line 275 in boards/communication/src/main.rs

View workflow job for this annotation

GitHub Actions / clippy

mismatched types 

error[E0308]: mismatched types
   --> boards/communication/src/main.rs:275:29
    |
275 |             spawn!(sd_dump, sensors)?;
    |             ----------------^^^^^^^-
    |             |               |
    |             |               expected `Vec<u8, 255>`, found `Option<Vec<u8, 255>>`
    |             arguments to this function are incorrect
    |
    = note: expected struct `heapless::Vec<_, _>`
                 found enum `core::option::Option<heapless::Vec<_, _>>`
note: function defined here
   --> boards/communication/src/main.rs:44:1
    |
44  | #[rtic::app(device = hal::pac, peripherals = true, dispatchers = [EVSYS_0, EVSYS_1, EVSYS_2])]
    | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
    = note: this error originates in the attribute macro `rtic::app` (in Nightly builds, run with -Z macro-backtrace for more info)
help: consider using `Option::expect` to unwrap the `core::option::Option<heapless::Vec<u8, 255>>` value, panicking if the value is an `Option::None`
    |
275 |             spawn!(sd_dump, sensors.expect("REASON"))?;
    |                                    +++++++++++++++++
Ok(())
});
match logging_rate {
Expand Down

0 comments on commit 2a22612

Please sign in to comment.