diff --git a/Cargo.lock b/Cargo.lock index f5024d0..bf0f868 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -624,30 +624,6 @@ dependencies = [ "serde", ] -[[package]] -name = "pressure" -version = "0.1.0" -dependencies = [ - "ads126x", - "chrono", - "common-arm", - "cortex-m", - "cortex-m-rt", - "defmt", - "defmt-rtt", - "defmt-test", - "embedded-alloc", - "fdcan", - "heapless 0.7.17", - "messages", - "panic-probe", - "postcard", - "rtic", - "rtic-monotonics", - "rtic-sync", - "stm32h7xx-hal", -] - [[package]] name = "proc-macro-error" version = "1.0.4" @@ -958,30 +934,6 @@ dependencies = [ "void", ] -[[package]] -name = "strain" -version = "0.1.0" -dependencies = [ - "ads126x", - "chrono", - "common-arm", - "cortex-m", - "cortex-m-rt", - "defmt", - "defmt-rtt", - "defmt-test", - "embedded-alloc", - "fdcan", - "heapless 0.7.17", - "messages", - "panic-probe", - "postcard", - "rtic", - "rtic-monotonics", - "rtic-sync", - "stm32h7xx-hal", -] - [[package]] name = "syn" version = "1.0.109" diff --git a/boards/pressure/Cargo.toml b/boards/pressure/Cargo.toml deleted file mode 100644 index d157ffc..0000000 --- a/boards/pressure/Cargo.toml +++ /dev/null @@ -1,36 +0,0 @@ -[package] -name = "pressure" -version = "0.1.0" -edition = "2021" - -# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html - -[dependencies] -cortex-m = { workspace = true } -cortex-m-rt = { workspace = true } -rtic = { workspace = true } -rtic-monotonics = { workspace = true } -common-arm = { path = "../../crates/common-arm" } -stm32h7xx-hal = { workspace = true } -postcard = { workspace = true } -defmt = { workspace = true } -fdcan = { workspace = true } -embedded-alloc = {workspace = true} -heapless = {workspace = true} -rtic-sync = { workspace = true } -defmt-rtt = { workspace = true } -panic-probe = { workspace = true } -chrono = { workspace = true } -messages = {workspace = true} -ads126x = {path = "../../crates/ads126x"} - -[dev-dependencies] -defmt-test = { workspace = true } - -[[test]] -name = "sd" -harness = false - -[[bin]] -name = "pressure" -harness = false \ No newline at end of file diff --git a/boards/pressure/src/adc_manager.rs b/boards/pressure/src/adc_manager.rs deleted file mode 100644 index 73e8d1f..0000000 --- a/boards/pressure/src/adc_manager.rs +++ /dev/null @@ -1,81 +0,0 @@ -use ads126x::{ - register::{DataRate, Mode1Register, Mode2Register}, - ADCCommand, Ads126x, -}; - -use common_arm::spawn; -use stm32h7xx_hal::{ - gpio::{Output, Pin, PushPull}, - spi::Spi, -}; - -use crate::app::delay; - -// There is an option to use interrupts using the data ready pins, but for now we will poll. -pub struct AdcManager { - pub spi: Spi, - pub adc1: Ads126x>>, - pub adc2: Ads126x>>, - pub adc1_cs: Pin<'C', 10, Output>, - pub adc2_cs: Pin<'D', 2, Output>, -} - -impl AdcManager { - pub fn new( - spi: Spi, - adc1_rst: Pin<'C', 11, Output>, - adc2_rst: Pin<'E', 0, Output>, - adc1_cs: Pin<'C', 10, Output>, - adc2_cs: Pin<'D', 2, Output>, - ) -> Self { - Self { - spi, - adc1: Ads126x::new(adc1_rst), - adc2: Ads126x::new(adc2_rst), - adc1_cs, - adc2_cs, - } - } - - pub fn init_adc1(&mut self) -> Result<(), ads126x::error::ADS126xError> { - self.adc2_cs.set_high(); - self.adc1_cs.set_low(); - self.adc1.reset()?; - - spawn!(delay, 1000); - - let mut mode1_cfg = Mode1Register::default(); - mode1_cfg.set_filter(ads126x::register::DigitalFilter::Sinc1); - self.adc1.set_mode1(&mode1_cfg, &mut self.spi)?; - - let mut mode2_cfg = Mode2Register::default(); - mode2_cfg.set_dr(DataRate::SPS1200); - self.adc1.set_mode2(&mode2_cfg, &mut self.spi)?; - - self.adc1.send_command(ADCCommand::START1, &mut self.spi)?; - self.adc1.send_command(ADCCommand::START2, &mut self.spi)?; - - Ok(()) - } - - pub fn init_adc2(&mut self) -> Result<(), ads126x::error::ADS126xError> { - self.adc1_cs.set_high(); - self.adc2_cs.set_low(); - self.adc2.reset()?; - - spawn!(delay, 1000); - - let mut mode1_cfg = Mode1Register::default(); - mode1_cfg.set_filter(ads126x::register::DigitalFilter::Sinc1); - self.adc1.set_mode1(&mode1_cfg, &mut self.spi)?; - - let mut mode2_cfg = Mode2Register::default(); - mode2_cfg.set_dr(DataRate::SPS1200); - self.adc1.set_mode2(&mode2_cfg, &mut self.spi)?; - - self.adc1.send_command(ADCCommand::START1, &mut self.spi)?; - self.adc1.send_command(ADCCommand::START2, &mut self.spi)?; - - Ok(()) - } -} diff --git a/boards/pressure/src/communication.rs b/boards/pressure/src/communication.rs deleted file mode 100644 index 212bdcd..0000000 --- a/boards/pressure/src/communication.rs +++ /dev/null @@ -1,194 +0,0 @@ -use crate::data_manager::DataManager; -use crate::types::COM_ID; -use common_arm::HydraError; -use defmt::{error, info}; -use fdcan::{ - frame::{FrameFormat, TxFrameHeader}, - id::StandardId, -}; -use mavlink::peek_reader::PeekReader; -use messages::mavlink::uorocketry::MavMessage; -use messages::mavlink::{self}; -use messages::Message; -use postcard::from_bytes; - -/// Clock configuration is out of scope for this builder -/// easiest way to avoid alloc is to use no generics -pub struct CanCommandManager { - can: fdcan::FdCan< - stm32h7xx_hal::can::Can, - fdcan::NormalOperationMode, - >, -} - -impl CanCommandManager { - pub fn new( - can: fdcan::FdCan< - stm32h7xx_hal::can::Can, - fdcan::NormalOperationMode, - >, - ) -> Self { - Self { can } - } - pub fn send_message(&mut self, m: Message) -> Result<(), HydraError> { - let mut buf = [0u8; 64]; - let payload = postcard::to_slice(&m, &mut buf)?; - let header = TxFrameHeader { - len: payload.len() as u8, // switch to const as this never changes or swtich on message type of known size - id: StandardId::new(COM_ID.into()).unwrap().into(), - frame_format: FrameFormat::Standard, - bit_rate_switching: false, - marker: None, - }; - self.can.transmit(header, payload)?; - Ok(()) - } - pub fn process_data(&mut self, data_manager: &mut DataManager) -> Result<(), HydraError> { - let mut buf = [0u8; 64]; - while self.can.receive0(&mut buf).is_ok() { - if let Ok(data) = from_bytes::(&buf) { - info!("Received message {}", data.clone()); - data_manager.handle_command(data)?; - } else { - info!("Error: {:?}", from_bytes::(&buf).unwrap_err()); - } - } - Ok(()) - } -} - -/// Clock configuration is out of scope for this builder -/// easiest way to avoid alloc is to use no generics -pub struct CanDataManager { - can: fdcan::FdCan< - stm32h7xx_hal::can::Can, - fdcan::NormalOperationMode, - >, -} - -impl CanDataManager { - pub fn new( - can: fdcan::FdCan< - stm32h7xx_hal::can::Can, - fdcan::NormalOperationMode, - >, - ) -> Self { - Self { can } - } - pub fn send_message(&mut self, m: Message) -> Result<(), HydraError> { - let mut buf = [0u8; 64]; - let payload = postcard::to_slice(&m, &mut buf)?; - let header = TxFrameHeader { - len: payload.len() as u8, // switch to const as this never changes or swtich on message type of known size - id: StandardId::new(COM_ID.into()).unwrap().into(), - frame_format: FrameFormat::Fdcan, - bit_rate_switching: false, - marker: None, - }; - // self.can.abort(fdcan::Mailbox::_2); // this is needed if boards are not in sync (if they are not in sync that is a bigger problem) - - stm32h7xx_hal::nb::block!(self.can.transmit(header, payload))?; - - Ok(()) - } - pub fn process_data(&mut self) -> Result<(), HydraError> { - let mut buf = [0u8; 64]; - while self.can.receive0(&mut buf).is_ok() { - if let Ok(data) = from_bytes::(&buf) { - info!("Received message {}", data.clone()); - crate::app::send_gs::spawn(data).ok(); - } else if let Err(e) = from_bytes::(&buf) { - info!("Error: {:?}", e); - } - } - self.can - .clear_interrupt(fdcan::interrupt::Interrupt::RxFifo0NewMsg); - Ok(()) - } -} - -pub struct RadioDevice { - transmitter: stm32h7xx_hal::serial::Tx, - pub receiver: PeekReader>, -} - -impl RadioDevice { - pub fn new(uart: stm32h7xx_hal::serial::Serial) -> Self { - let (tx, mut rx) = uart.split(); - - rx.listen(); - // setup interrupts - - RadioDevice { - transmitter: tx, - receiver: PeekReader::new(rx), - } - } -} - -pub struct RadioManager { - pub radio: RadioDevice, - mav_sequence: u8, -} - -impl RadioManager { - pub fn new(radio: RadioDevice) -> Self { - RadioManager { - radio, - mav_sequence: 0, - } - } - pub fn send_message(&mut self, payload: &[u8]) -> Result<(), HydraError> { - let mav_header = mavlink::MavHeader { - system_id: 1, - component_id: 1, - sequence: self.increment_mav_sequence(), - }; - // Create a fixed-size array and copy the payload into it - let mut fixed_payload = [0u8; 255]; - let len = payload.len().min(255); - fixed_payload[..len].copy_from_slice(&payload[..len]); - - let mav_message = mavlink::uorocketry::MavMessage::POSTCARD_MESSAGE( - mavlink::uorocketry::POSTCARD_MESSAGE_DATA { - message: fixed_payload, - }, - ); - mavlink::write_versioned_msg( - &mut self.radio.transmitter, - mavlink::MavlinkVersion::V2, - mav_header, - &mav_message, - )?; - Ok(()) - } - pub fn increment_mav_sequence(&mut self) -> u8 { - self.mav_sequence = self.mav_sequence.wrapping_add(1); - self.mav_sequence - } - pub fn receive_message(&mut self) -> Result { - let (_header, msg): (_, MavMessage) = - mavlink::read_versioned_msg(&mut self.radio.receiver, mavlink::MavlinkVersion::V2)?; - - // info!("{:?}", ); - // Do we need the header? - match msg { - mavlink::uorocketry::MavMessage::POSTCARD_MESSAGE(msg) => { - Ok(postcard::from_bytes::(&msg.message)?) - // weird Ok syntax to coerce to hydra error type. - } - mavlink::uorocketry::MavMessage::COMMAND_MESSAGE(command) => { - info!("{}", command.command); - Ok(postcard::from_bytes::(&command.command)?) - } - mavlink::uorocketry::MavMessage::HEARTBEAT(_) => { - info!("Heartbeat"); - Err(mavlink::error::MessageReadError::Io.into()) - } - _ => { - error!("Error, ErrorContext::UnkownPostcardMessage"); - Err(mavlink::error::MessageReadError::Io.into()) - } - } - } -} diff --git a/boards/pressure/src/data_manager.rs b/boards/pressure/src/data_manager.rs deleted file mode 100644 index 67b5834..0000000 --- a/boards/pressure/src/data_manager.rs +++ /dev/null @@ -1,186 +0,0 @@ -use common_arm::HydraError; -use messages::command::RadioRate; -use messages::state::StateData; -use messages::Message; -use stm32h7xx_hal::rcc::ResetReason; -#[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 reset_reason: Option, - pub logging_rate: Option, - pub recovery_sensing: Option, - pub nav_pos_l1h: 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, - reset_reason: None, - logging_rate: Some(RadioRate::Slow), // start slow. - recovery_sensing: None, - nav_pos_l1h: None, - } - } - - pub fn get_logging_rate(&mut self) -> RadioRate { - if let Some(rate) = self.logging_rate.take() { - let rate_cln = rate.clone(); - self.logging_rate = Some(rate); - return rate_cln; - } - self.logging_rate = Some(RadioRate::Slow); - RadioRate::Slow - } - - /// Do not clone instead take to reduce CPU load. - pub fn take_sensors(&mut self) -> [Option; 15] { - [ - 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(), - self.nav_pos_l1h.take(), - self.recovery_sensing.take(), - ] - } - - pub fn clone_states(&self) -> [Option; 1] { - [self.state.clone()] - } - - pub fn clone_reset_reason(&self) -> Option { - self.reset_reason - } - - pub fn set_reset_reason(&mut self, reset: ResetReason) { - self.reset_reason = Some(reset); - } - - pub fn handle_command(&mut self, data: Message) -> Result<(), HydraError> { - match data.data { - messages::Data::Command(command) => match command.data { - messages::command::CommandData::PowerDown(_) => { - crate::app::sleep_system::spawn().ok(); - } - messages::command::CommandData::RadioRateChange(command_data) => { - self.logging_rate = Some(command_data.rate); - } - _ => { - // We don't care atm about these other commands. - } - }, - _ => { - // we can disregard all other messages for now. - } - } - Ok(()) - } - pub fn handle_data(&mut self, data: Message) { - match data.data { - messages::Data::Sensor(ref sensor) => match sensor.data { - messages::sensor::SensorData::SbgData(ref sbg_data) => match sbg_data { - messages::sensor::SbgData::EkfNavAcc(_) => { - self.ekf_nav_acc = Some(data); - } - messages::sensor::SbgData::GpsPosAcc(_) => { - self.gps_pos_acc = Some(data); - } - messages::sensor::SbgData::Air(_) => { - self.air = Some(data); - } - messages::sensor::SbgData::EkfNav1(_) => { - self.ekf_nav_1 = Some(data); - } - messages::sensor::SbgData::EkfNav2(_) => { - self.ekf_nav_2 = Some(data); - } - messages::sensor::SbgData::EkfQuat(_) => { - self.ekf_quat = Some(data); - } - messages::sensor::SbgData::GpsVel(_) => { - self.gps_vel = Some(data); - } - messages::sensor::SbgData::GpsVelAcc(_) => { - self.gps_vel_acc = Some(data); - } - messages::sensor::SbgData::Imu1(_) => { - self.imu_1 = Some(data); - } - messages::sensor::SbgData::Imu2(_) => { - self.imu_2 = Some(data); - } - messages::sensor::SbgData::UtcTime(_) => { - self.utc_time = Some(data); - } - messages::sensor::SbgData::GpsPos1(_) => { - self.gps_pos_1 = Some(data); - } - messages::sensor::SbgData::GpsPos2(_) => { - self.gps_pos_2 = Some(data); - } - }, - messages::sensor::SensorData::RecoverySensing(_) => { - self.recovery_sensing = Some(data); - } - messages::sensor::SensorData::NavPosLlh(_) => { - self.nav_pos_l1h = Some(data); - } - messages::sensor::SensorData::ResetReason(_) => {} - }, - 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); - // } - // messages::command::CommandData::DeployDrogue(_) => {} - // messages::command::CommandData::DeployMain(_) => {} - // messages::command::CommandData::PowerDown(_) => {} - // }, - _ => {} - } - } -} - -impl Default for DataManager { - fn default() -> Self { - Self::new() - } -} diff --git a/boards/pressure/src/main.rs b/boards/pressure/src/main.rs deleted file mode 100644 index b320b16..0000000 --- a/boards/pressure/src/main.rs +++ /dev/null @@ -1,586 +0,0 @@ -#![no_std] -#![no_main] - -mod communication; -mod data_manager; -mod adc_manager; -mod types; - -use chrono::NaiveDate; -use common_arm::*; -use communication::{CanCommandManager, CanDataManager}; -use communication::{RadioDevice, RadioManager}; -use core::num::{NonZeroU16, NonZeroU8}; -use data_manager::DataManager; -use defmt::info; -use fdcan::{ - config::NominalBitTiming, - filter::{StandardFilter, StandardFilterSlot}, -}; -use messages::command::RadioRate; -use messages::{sensor, Data}; -use panic_probe as _; -use rtic_monotonics::systick::prelude::*; -use rtic_sync::{channel::*, make_channel}; -use stm32h7xx_hal::gpio::gpioa::{PA2, PA3}; -use stm32h7xx_hal::gpio::gpiob::PB4; -use stm32h7xx_hal::gpio::Speed; -use stm32h7xx_hal::gpio::{Output, PushPull}; -use stm32h7xx_hal::prelude::*; -use stm32h7xx_hal::rtc; -use stm32h7xx_hal::{rcc, rcc::rec}; -use types::COM_ID; // global logger - -const DATA_CHANNEL_CAPACITY: usize = 10; - -systick_monotonic!(Mono, 500); - -#[inline(never)] -#[defmt::panic_handler] -fn panic() -> ! { - // stm32h7xx_hal::pac::SCB::sys_reset() - cortex_m::asm::udf() -} - -#[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, dispatchers = [EXTI0, EXTI1, EXTI2, SPI3, SPI2])] -mod app { - - use messages::Message; - use stm32h7xx_hal::gpio::{Alternate, Pin}; - - use super::*; - - #[shared] - struct SharedResources { - data_manager: DataManager, - em: ErrorManager, - // sd_manager: SdManager< - // stm32h7xx_hal::spi::Spi, - // PA4>, - // >, - radio_manager: RadioManager, - can_command_manager: CanCommandManager, - can_data_manager: CanDataManager, - sbg_power: PB4>, - rtc: rtc::Rtc, - } - #[local] - struct LocalResources { - led_red: PA2>, - led_green: PA3>, - buzzer: stm32h7xx_hal::pwm::Pwm< - stm32h7xx_hal::pac::TIM12, - 0, - stm32h7xx_hal::pwm::ComplementaryImpossible, - >, - } - - #[init] - fn init(ctx: init::Context) -> (SharedResources, LocalResources) { - // channel setup - let (_s, r) = make_channel!(Message, DATA_CHANNEL_CAPACITY); - - let core = ctx.core; - - /* Logging Setup */ - HydraLogging::set_ground_station_callback(queue_gs_message); - - let pwr = ctx.device.PWR.constrain(); - // We could use smps, but the board is not designed for it - // let pwrcfg = example_power!(pwr).freeze(); - let mut pwrcfg = pwr.freeze(); - - info!("Power enabled"); - let backup = pwrcfg.backup().unwrap(); - info!("Backup domain enabled"); - // RCC - let mut rcc = ctx.device.RCC.constrain(); - let reset = rcc.get_reset_reason(); - let fdcan_prec_unsafe = unsafe { rcc.steal_peripheral_rec() } - .FDCAN - .kernel_clk_mux(rec::FdcanClkSel::Pll1Q); - - let ccdr = rcc - .use_hse(48.MHz()) // check the clock hardware - .sys_ck(200.MHz()) - .pll1_strategy(rcc::PllConfigStrategy::Iterative) - .pll1_q_ck(32.MHz()) - .freeze(pwrcfg, &ctx.device.SYSCFG); - info!("RCC configured"); - let fdcan_prec = ccdr - .peripheral - .FDCAN - .kernel_clk_mux(rec::FdcanClkSel::Pll1Q); - - let btr = NominalBitTiming { - prescaler: NonZeroU16::new(10).unwrap(), - seg1: NonZeroU8::new(13).unwrap(), - seg2: NonZeroU8::new(2).unwrap(), - sync_jump_width: NonZeroU8::new(1).unwrap(), - }; - - // let data_bit_timing = DataBitTiming { - // prescaler: NonZeroU8::new(10).unwrap(), - // seg1: NonZeroU8::new(13).unwrap(), - // seg2: NonZeroU8::new(2).unwrap(), - // sync_jump_width: NonZeroU8::new(4).unwrap(), - // transceiver_delay_compensation: true, - // }; - - info!("CAN enabled"); - // GPIO - let gpioa = ctx.device.GPIOA.split(ccdr.peripheral.GPIOA); - let gpiod = ctx.device.GPIOD.split(ccdr.peripheral.GPIOD); - let gpiob = ctx.device.GPIOB.split(ccdr.peripheral.GPIOB); - - let pins = gpiob.pb14.into_alternate(); - let mut c0 = ctx - .device - .TIM12 - .pwm(pins, 4.kHz(), ccdr.peripheral.TIM12, &ccdr.clocks); - - c0.set_duty(c0.get_max_duty() / 4); - // PWM outputs are disabled by default - // c0.enable(); - - info!("PWM enabled"); - // assert_eq!(ccdr.clocks.pll1_q_ck().unwrap().raw(), 32_000_000); - info!("PLL1Q:"); - // https://github.com/stm32-rs/stm32h7xx-hal/issues/369 This needs to be stolen. Grrr I hate the imaturity of the stm32-hal - let can2: fdcan::FdCan< - stm32h7xx_hal::can::Can, - fdcan::ConfigMode, - > = { - let rx = gpiob.pb12.into_alternate().speed(Speed::VeryHigh); - let tx = gpiob.pb13.into_alternate().speed(Speed::VeryHigh); - ctx.device.FDCAN2.fdcan(tx, rx, fdcan_prec) - }; - - let mut can_data = can2; - can_data.set_protocol_exception_handling(false); - - can_data.set_nominal_bit_timing(btr); - - // can_data.set_automatic_retransmit(false); // data can be dropped due to its volume. - - // can_command.set_data_bit_timing(data_bit_timing); - - can_data.set_standard_filter( - StandardFilterSlot::_0, - StandardFilter::accept_all_into_fifo0(), - ); - - can_data.set_standard_filter( - StandardFilterSlot::_1, - StandardFilter::accept_all_into_fifo0(), - ); - - can_data.set_standard_filter( - StandardFilterSlot::_2, - StandardFilter::accept_all_into_fifo0(), - ); - - can_data.enable_interrupt(fdcan::interrupt::Interrupt::RxFifo0NewMsg); - - can_data.enable_interrupt_line(fdcan::interrupt::InterruptLine::_0, true); - - let config = can_data - .get_config() - .set_frame_transmit(fdcan::config::FrameTransmissionConfig::AllowFdCanAndBRS); - can_data.apply_config(config); - - let can_data_manager = CanDataManager::new(can_data.into_normal()); - - let can1: fdcan::FdCan< - stm32h7xx_hal::can::Can, - fdcan::ConfigMode, - > = { - let rx = gpioa.pa11.into_alternate().speed(Speed::VeryHigh); - let tx = gpioa.pa12.into_alternate().speed(Speed::VeryHigh); - ctx.device.FDCAN1.fdcan(tx, rx, fdcan_prec_unsafe) - }; - - let mut can_command = can1; - can_command.set_protocol_exception_handling(false); - - can_command.set_nominal_bit_timing(btr); - can_command.set_standard_filter( - StandardFilterSlot::_0, - StandardFilter::accept_all_into_fifo0(), - ); - - can_command.set_standard_filter( - StandardFilterSlot::_1, - StandardFilter::accept_all_into_fifo0(), - ); - - can_command.set_standard_filter( - StandardFilterSlot::_2, - StandardFilter::accept_all_into_fifo0(), - ); - - // can_data.set_data_bit_timing(data_bit_timing); - can_command.enable_interrupt(fdcan::interrupt::Interrupt::RxFifo0NewMsg); - - can_command.enable_interrupt_line(fdcan::interrupt::InterruptLine::_0, true); - - let config = can_command - .get_config() - .set_frame_transmit(fdcan::config::FrameTransmissionConfig::AllowFdCanAndBRS); // check this maybe don't bit switch allow. - can_command.apply_config(config); - - let can_command_manager = CanCommandManager::new(can_command.into_normal()); - - // let spi_sd: stm32h7xx_hal::spi::Spi< - // stm32h7xx_hal::stm32::SPI1, - // stm32h7xx_hal::spi::Enabled, - // u8, - // > = ctx.device.SPI1.spi( - // ( - // gpioa.pa5.into_alternate::<5>(), - // gpioa.pa6.into_alternate(), - // gpioa.pa7.into_alternate(), - // ), - // spi::Config::new(spi::MODE_0), - // 16.MHz(), - // ccdr.peripheral.SPI1, - // &ccdr.clocks, - // ); - - // let cs_sd = gpioa.pa4.into_push_pull_output(); - - // let sd_manager = SdManager::new(spi_sd, cs_sd); - - // leds - let led_red = gpioa.pa2.into_push_pull_output(); - let led_green = gpioa.pa3.into_push_pull_output(); - - // sbg power pin - let mut sbg_power = gpiob.pb4.into_push_pull_output(); - sbg_power.set_high(); - - // UART for sbg - let tx: Pin<'D', 1, Alternate<8>> = gpiod.pd1.into_alternate(); - let rx: Pin<'D', 0, Alternate<8>> = gpiod.pd0.into_alternate(); - - // let stream_tuple = StreamsTuple::new(ctx.device.DMA1, ccdr.peripheral.DMA1); - let uart_radio = ctx - .device - .UART4 - .serial((tx, rx), 57600.bps(), ccdr.peripheral.UART4, &ccdr.clocks) - .unwrap(); - // let mut sbg_manager = sbg_manager::SBGManager::new(uart_sbg, stream_tuple); - - let radio = RadioDevice::new(uart_radio); - - let radio_manager = RadioManager::new(radio); - - let mut rtc = stm32h7xx_hal::rtc::Rtc::open_or_init( - ctx.device.RTC, - backup.RTC, - stm32h7xx_hal::rtc::RtcClock::Lsi, - &ccdr.clocks, - ); - - // TODO: Get current time from some source - let now = NaiveDate::from_ymd_opt(2001, 1, 1) - .unwrap() - .and_hms_opt(0, 0, 0) - .unwrap(); - - rtc.set_date_time(now); - - /* Monotonic clock */ - Mono::start(core.SYST, 200_000_000); - - let mut data_manager = DataManager::new(); - data_manager.set_reset_reason(reset); - let em = ErrorManager::new(); - blink::spawn().ok(); - send_data_internal::spawn(r).ok(); - reset_reason_send::spawn().ok(); - state_send::spawn().ok(); - // generate_random_messages::spawn().ok(); - // sensor_send::spawn().ok(); - info!("Online"); - - ( - SharedResources { - data_manager, - em, - // sd_manager, - radio_manager, - can_command_manager, - can_data_manager, - sbg_power, - rtc, - }, - LocalResources { - led_red, - led_green, - buzzer: c0, - }, - ) - } - - #[task(priority = 3)] - async fn delay(_cx: delay::Context, delay: u32) { - Mono::delay(delay.millis()).await; - } - - #[task(priority = 3, shared = [&em, rtc])] - async fn generate_random_messages(mut cx: generate_random_messages::Context) { - loop { - cx.shared.em.run(|| { - let message = Message::new( - cx.shared - .rtc - .lock(|rtc| messages::FormattedNaiveDateTime(rtc.date_time().unwrap())), - COM_ID, - messages::state::State::new(messages::state::StateData::Initializing), - ); - spawn!(send_gs, message.clone())?; - // spawn!(send_data_internal, message)?; - Ok(()) - }); - Mono::delay(1.secs()).await; - } - } - - #[task(priority = 3, shared = [data_manager, &em, rtc])] - async fn reset_reason_send(mut cx: reset_reason_send::Context) { - let reason = cx - .shared - .data_manager - .lock(|data_manager| data_manager.clone_reset_reason()); - match reason { - Some(reason) => { - let x = match reason { - stm32h7xx_hal::rcc::ResetReason::BrownoutReset => sensor::ResetReason::BrownoutReset, - stm32h7xx_hal::rcc::ResetReason::CpuReset => sensor::ResetReason::CpuReset, - stm32h7xx_hal::rcc::ResetReason::D1EntersDStandbyErroneouslyOrCpuEntersCStopErroneously => sensor::ResetReason::D1EntersDStandbyErroneouslyOrCpuEntersCStopErroneously, - stm32h7xx_hal::rcc::ResetReason::D1ExitsDStandbyMode => sensor::ResetReason::D1ExitsDStandbyMode, - stm32h7xx_hal::rcc::ResetReason::D2ExitsDStandbyMode => sensor::ResetReason::D2ExitsDStandbyMode, - stm32h7xx_hal::rcc::ResetReason::GenericWatchdogReset => sensor::ResetReason::GenericWatchdogReset, - stm32h7xx_hal::rcc::ResetReason::IndependentWatchdogReset => sensor::ResetReason::IndependentWatchdogReset, - stm32h7xx_hal::rcc::ResetReason::PinReset => sensor::ResetReason::PinReset, - stm32h7xx_hal::rcc::ResetReason::PowerOnReset => sensor::ResetReason::PowerOnReset, - stm32h7xx_hal::rcc::ResetReason::SystemReset => sensor::ResetReason::SystemReset, - stm32h7xx_hal::rcc::ResetReason::Unknown { rcc_rsr } => sensor::ResetReason::Unknown { rcc_rsr }, - stm32h7xx_hal::rcc::ResetReason::WindowWatchdogReset => sensor::ResetReason::WindowWatchdogReset, - }; - let message = messages::Message::new( - cx.shared - .rtc - .lock(|rtc| messages::FormattedNaiveDateTime(rtc.date_time().unwrap())), - COM_ID, - sensor::Sensor::new(x), - ); - - cx.shared.em.run(|| { - spawn!(send_gs, message)?; - Ok(()) - }) - } - None => return, - } - } - - #[task(shared = [data_manager, &em, rtc])] - async fn state_send(mut cx: state_send::Context) { - let state_data = cx - .shared - .data_manager - .lock(|data_manager| data_manager.state.clone()); - cx.shared.em.run(|| { - if let Some(x) = state_data { - let message = Message::new( - cx.shared - .rtc - .lock(|rtc| messages::FormattedNaiveDateTime(rtc.date_time().unwrap())), - COM_ID, - messages::state::State::new(x), - ); - spawn!(send_gs, message)?; - } // if there is none we still return since we simply don't have data yet. - Ok(()) - }); - Mono::delay(5.secs()).await; - // spawn_after!(state_send, ExtU64::secs(5)).ok(); - } - - /** - * Sends information about the sensors. - */ - #[task(priority = 3, shared = [data_manager, &em])] - async fn sensor_send(mut cx: sensor_send::Context) { - loop { - let (sensors, logging_rate) = cx.shared.data_manager.lock(|data_manager| { - (data_manager.take_sensors(), data_manager.get_logging_rate()) - }); - - cx.shared.em.run(|| { - for msg in sensors { - match msg { - Some(x) => { - // info!("Sending sensor data {}", x.clone()); - spawn!(send_gs, x)?; - // spawn!(sd_dump, x)?; - } - None => { - info!("No sensor data to send"); - continue; - } - } - } - - Ok(()) - }); - match logging_rate { - RadioRate::Fast => { - Mono::delay(100.millis()).await; - } - RadioRate::Slow => { - Mono::delay(250.millis()).await; - } - } - } - } - - /// 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) { - info!("Queueing message"); - send_gs_intermediate::spawn(d.into()).ok(); - } - - #[task(priority = 3, shared = [rtc, &em])] - async fn send_gs_intermediate(mut cx: send_gs_intermediate::Context, m: Data) { - cx.shared.em.run(|| { - cx.shared.rtc.lock(|rtc| { - let message = messages::Message::new( - messages::FormattedNaiveDateTime(rtc.date_time().unwrap()), - COM_ID, - m, - ); - spawn!(send_gs, message)?; - Ok(()) - }) - }); - } - - #[task(priority = 2, binds = FDCAN1_IT0, shared = [can_command_manager, data_manager, &em])] - fn can_command(mut cx: can_command::Context) { - // info!("CAN Command"); - cx.shared.can_command_manager.lock(|can| { - cx.shared - .data_manager - .lock(|data_manager| cx.shared.em.run(|| can.process_data(data_manager))); - }) - } - - #[task(priority = 3, shared = [sbg_power])] - async fn sbg_power_on(mut cx: sbg_power_on::Context) { - loop { - cx.shared.sbg_power.lock(|sbg| { - sbg.set_high(); - }); - Mono::delay(10000.millis()).await; - } - } - - /** - * Sends a message to the radio over UART. - */ - #[task(priority = 3, shared = [&em, radio_manager])] - async fn send_gs(mut cx: send_gs::Context, m: Message) { - // info!("{}", m.clone()); - - cx.shared.radio_manager.lock(|radio_manager| { - cx.shared.em.run(|| { - // info!("Sending message {}", m); - let mut buf = [0; 255]; - let data = postcard::to_slice(&m, &mut buf)?; - radio_manager.send_message(data)?; - Ok(()) - }) - }); - } - - #[task( priority = 3, binds = FDCAN2_IT0, shared = [&em, can_data_manager, data_manager])] - fn can_data(mut cx: can_data::Context) { - cx.shared.can_data_manager.lock(|can| { - { - cx.shared.em.run(|| { - can.process_data()?; - Ok(()) - }) - } - }); - } - - #[task(priority = 2, shared = [&em, can_data_manager, data_manager])] - async fn send_data_internal( - mut cx: send_data_internal::Context, - mut receiver: Receiver<'static, Message, DATA_CHANNEL_CAPACITY>, - ) { - loop { - if let Ok(m) = receiver.recv().await { - cx.shared.can_data_manager.lock(|can| { - cx.shared.em.run(|| { - can.send_message(m)?; - Ok(()) - }) - }); - } - } - } - - #[task(priority = 2, shared = [&em, can_command_manager, data_manager])] - async fn send_command_internal(mut cx: send_command_internal::Context, m: Message) { - // while let Ok(m) = receiver.recv().await { - cx.shared.can_command_manager.lock(|can| { - cx.shared.em.run(|| { - can.send_message(m)?; - Ok(()) - }) - }); - // } - } - - #[task(priority = 1, local = [led_red, led_green, buzzer, buzzed: bool = false], shared = [&em])] - async fn blink(cx: blink::Context) { - loop { - if cx.shared.em.has_error() { - cx.local.led_red.toggle(); - if *cx.local.buzzed { - cx.local.buzzer.set_duty(0); - *cx.local.buzzed = false; - } else { - let duty = cx.local.buzzer.get_max_duty() / 4; - cx.local.buzzer.set_duty(duty); - *cx.local.buzzed = true; - } - Mono::delay(500.millis()).await; - } else { - cx.local.led_green.toggle(); - if *cx.local.buzzed { - cx.local.buzzer.set_duty(0); - *cx.local.buzzed = false; - } else { - let duty = cx.local.buzzer.get_max_duty() / 4; - cx.local.buzzer.set_duty(duty); - *cx.local.buzzed = true; - } - Mono::delay(2000.millis()).await; - } - } - } - - #[task(priority = 3, shared = [&em, sbg_power])] - async fn sleep_system(mut cx: sleep_system::Context) { - // Turn off the SBG and CAN, also start a timer to wake up the system. Put the chip in sleep mode. - cx.shared.sbg_power.lock(|sbg| { - sbg.set_low(); - }); - } -} diff --git a/boards/pressure/src/types.rs b/boards/pressure/src/types.rs deleted file mode 100644 index 1c9a889..0000000 --- a/boards/pressure/src/types.rs +++ /dev/null @@ -1,3 +0,0 @@ -use messages::node::{Node, Node::PressureBoard}; - -pub static COM_ID: Node = PressureBoard; diff --git a/boards/pressure/tests/sd.rs b/boards/pressure/tests/sd.rs deleted file mode 100644 index 9c4f1db..0000000 --- a/boards/pressure/tests/sd.rs +++ /dev/null @@ -1,85 +0,0 @@ -#![no_std] -#![no_main] - -use common_arm::SdManager; -use defmt::info; -use panic_probe as _; -use stm32h7xx_hal::gpio::{Output, PushPull, PA4}; -use stm32h7xx_hal::pac; -use stm32h7xx_hal::prelude::*; -use stm32h7xx_hal::spi; - -struct State { - sd_manager: SdManager< - stm32h7xx_hal::spi::Spi, - PA4>, - >, -} - -#[defmt_test::tests] -mod tests { - use super::*; - - #[init] - fn init() -> State { - let _cp = cortex_m::Peripherals::take().unwrap(); - let dp = pac::Peripherals::take().unwrap(); - - let pwr = dp.PWR.constrain(); - let pwrcfg = pwr.freeze(); - - info!("Power enabled"); - // RCC - let mut rcc = dp.RCC.constrain(); - let reset = rcc.get_reset_reason(); - - info!("Reset reason: {:?}", reset); - - let ccdr = rcc - .use_hse(48.MHz()) // check the clock hardware - .sys_ck(200.MHz()) - .freeze(pwrcfg, &dp.SYSCFG); - info!("RCC configured"); - - let gpioa = dp.GPIOA.split(ccdr.peripheral.GPIOA); - - let spi_sd: stm32h7xx_hal::spi::Spi< - stm32h7xx_hal::stm32::SPI1, - stm32h7xx_hal::spi::Enabled, - u8, - > = dp.SPI1.spi( - ( - gpioa.pa5.into_alternate::<5>(), - gpioa.pa6.into_alternate(), - gpioa.pa7.into_alternate(), - ), - spi::Config::new(spi::MODE_0), - 16.MHz(), - ccdr.peripheral.SPI1, - &ccdr.clocks, - ); - - let cs_sd = gpioa.pa4.into_push_pull_output(); - - let sd_manager = SdManager::new(spi_sd, cs_sd); - State { sd_manager } - } - - #[test] - fn writing_file(state: &mut State) { - let sd_manager = &mut state.sd_manager; - - let mut test_file = sd_manager - .open_file("testing.txt") - .expect("Cannot open file"); - sd_manager - .write(&mut test_file, b"Hello this is a test!") - .expect("Could not write file."); - sd_manager - .write_str(&mut test_file, "Testing Strings") - .expect("Could not write string"); - sd_manager - .close_file(test_file) - .expect("Could not close file."); // we are done with the file so destroy it - } -} diff --git a/boards/strain/Cargo.toml b/boards/strain/Cargo.toml deleted file mode 100644 index f12215c..0000000 --- a/boards/strain/Cargo.toml +++ /dev/null @@ -1,36 +0,0 @@ -[package] -name = "strain" -version = "0.1.0" -edition = "2021" - -# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html - -[dependencies] -cortex-m = { workspace = true } -cortex-m-rt = { workspace = true } -rtic = { workspace = true } -rtic-monotonics = { workspace = true } -common-arm = { path = "../../crates/common-arm" } -stm32h7xx-hal = { workspace = true } -postcard = { workspace = true } -defmt = { workspace = true } -fdcan = { workspace = true } -embedded-alloc = {workspace = true} -heapless = {workspace = true} -rtic-sync = { workspace = true } -defmt-rtt = { workspace = true } -panic-probe = { workspace = true } -chrono = { workspace = true } -messages = {workspace = true} -ads126x = {path = "../../crates/ads126x"} - -[dev-dependencies] -defmt-test = { workspace = true } - -[[test]] -name = "sd" -harness = false - -[[bin]] -name = "strain" -harness = false \ No newline at end of file diff --git a/boards/strain/src/adc_manager.rs b/boards/strain/src/adc_manager.rs deleted file mode 100644 index 73e8d1f..0000000 --- a/boards/strain/src/adc_manager.rs +++ /dev/null @@ -1,81 +0,0 @@ -use ads126x::{ - register::{DataRate, Mode1Register, Mode2Register}, - ADCCommand, Ads126x, -}; - -use common_arm::spawn; -use stm32h7xx_hal::{ - gpio::{Output, Pin, PushPull}, - spi::Spi, -}; - -use crate::app::delay; - -// There is an option to use interrupts using the data ready pins, but for now we will poll. -pub struct AdcManager { - pub spi: Spi, - pub adc1: Ads126x>>, - pub adc2: Ads126x>>, - pub adc1_cs: Pin<'C', 10, Output>, - pub adc2_cs: Pin<'D', 2, Output>, -} - -impl AdcManager { - pub fn new( - spi: Spi, - adc1_rst: Pin<'C', 11, Output>, - adc2_rst: Pin<'E', 0, Output>, - adc1_cs: Pin<'C', 10, Output>, - adc2_cs: Pin<'D', 2, Output>, - ) -> Self { - Self { - spi, - adc1: Ads126x::new(adc1_rst), - adc2: Ads126x::new(adc2_rst), - adc1_cs, - adc2_cs, - } - } - - pub fn init_adc1(&mut self) -> Result<(), ads126x::error::ADS126xError> { - self.adc2_cs.set_high(); - self.adc1_cs.set_low(); - self.adc1.reset()?; - - spawn!(delay, 1000); - - let mut mode1_cfg = Mode1Register::default(); - mode1_cfg.set_filter(ads126x::register::DigitalFilter::Sinc1); - self.adc1.set_mode1(&mode1_cfg, &mut self.spi)?; - - let mut mode2_cfg = Mode2Register::default(); - mode2_cfg.set_dr(DataRate::SPS1200); - self.adc1.set_mode2(&mode2_cfg, &mut self.spi)?; - - self.adc1.send_command(ADCCommand::START1, &mut self.spi)?; - self.adc1.send_command(ADCCommand::START2, &mut self.spi)?; - - Ok(()) - } - - pub fn init_adc2(&mut self) -> Result<(), ads126x::error::ADS126xError> { - self.adc1_cs.set_high(); - self.adc2_cs.set_low(); - self.adc2.reset()?; - - spawn!(delay, 1000); - - let mut mode1_cfg = Mode1Register::default(); - mode1_cfg.set_filter(ads126x::register::DigitalFilter::Sinc1); - self.adc1.set_mode1(&mode1_cfg, &mut self.spi)?; - - let mut mode2_cfg = Mode2Register::default(); - mode2_cfg.set_dr(DataRate::SPS1200); - self.adc1.set_mode2(&mode2_cfg, &mut self.spi)?; - - self.adc1.send_command(ADCCommand::START1, &mut self.spi)?; - self.adc1.send_command(ADCCommand::START2, &mut self.spi)?; - - Ok(()) - } -} diff --git a/boards/strain/src/communication.rs b/boards/strain/src/communication.rs deleted file mode 100644 index 212bdcd..0000000 --- a/boards/strain/src/communication.rs +++ /dev/null @@ -1,194 +0,0 @@ -use crate::data_manager::DataManager; -use crate::types::COM_ID; -use common_arm::HydraError; -use defmt::{error, info}; -use fdcan::{ - frame::{FrameFormat, TxFrameHeader}, - id::StandardId, -}; -use mavlink::peek_reader::PeekReader; -use messages::mavlink::uorocketry::MavMessage; -use messages::mavlink::{self}; -use messages::Message; -use postcard::from_bytes; - -/// Clock configuration is out of scope for this builder -/// easiest way to avoid alloc is to use no generics -pub struct CanCommandManager { - can: fdcan::FdCan< - stm32h7xx_hal::can::Can, - fdcan::NormalOperationMode, - >, -} - -impl CanCommandManager { - pub fn new( - can: fdcan::FdCan< - stm32h7xx_hal::can::Can, - fdcan::NormalOperationMode, - >, - ) -> Self { - Self { can } - } - pub fn send_message(&mut self, m: Message) -> Result<(), HydraError> { - let mut buf = [0u8; 64]; - let payload = postcard::to_slice(&m, &mut buf)?; - let header = TxFrameHeader { - len: payload.len() as u8, // switch to const as this never changes or swtich on message type of known size - id: StandardId::new(COM_ID.into()).unwrap().into(), - frame_format: FrameFormat::Standard, - bit_rate_switching: false, - marker: None, - }; - self.can.transmit(header, payload)?; - Ok(()) - } - pub fn process_data(&mut self, data_manager: &mut DataManager) -> Result<(), HydraError> { - let mut buf = [0u8; 64]; - while self.can.receive0(&mut buf).is_ok() { - if let Ok(data) = from_bytes::(&buf) { - info!("Received message {}", data.clone()); - data_manager.handle_command(data)?; - } else { - info!("Error: {:?}", from_bytes::(&buf).unwrap_err()); - } - } - Ok(()) - } -} - -/// Clock configuration is out of scope for this builder -/// easiest way to avoid alloc is to use no generics -pub struct CanDataManager { - can: fdcan::FdCan< - stm32h7xx_hal::can::Can, - fdcan::NormalOperationMode, - >, -} - -impl CanDataManager { - pub fn new( - can: fdcan::FdCan< - stm32h7xx_hal::can::Can, - fdcan::NormalOperationMode, - >, - ) -> Self { - Self { can } - } - pub fn send_message(&mut self, m: Message) -> Result<(), HydraError> { - let mut buf = [0u8; 64]; - let payload = postcard::to_slice(&m, &mut buf)?; - let header = TxFrameHeader { - len: payload.len() as u8, // switch to const as this never changes or swtich on message type of known size - id: StandardId::new(COM_ID.into()).unwrap().into(), - frame_format: FrameFormat::Fdcan, - bit_rate_switching: false, - marker: None, - }; - // self.can.abort(fdcan::Mailbox::_2); // this is needed if boards are not in sync (if they are not in sync that is a bigger problem) - - stm32h7xx_hal::nb::block!(self.can.transmit(header, payload))?; - - Ok(()) - } - pub fn process_data(&mut self) -> Result<(), HydraError> { - let mut buf = [0u8; 64]; - while self.can.receive0(&mut buf).is_ok() { - if let Ok(data) = from_bytes::(&buf) { - info!("Received message {}", data.clone()); - crate::app::send_gs::spawn(data).ok(); - } else if let Err(e) = from_bytes::(&buf) { - info!("Error: {:?}", e); - } - } - self.can - .clear_interrupt(fdcan::interrupt::Interrupt::RxFifo0NewMsg); - Ok(()) - } -} - -pub struct RadioDevice { - transmitter: stm32h7xx_hal::serial::Tx, - pub receiver: PeekReader>, -} - -impl RadioDevice { - pub fn new(uart: stm32h7xx_hal::serial::Serial) -> Self { - let (tx, mut rx) = uart.split(); - - rx.listen(); - // setup interrupts - - RadioDevice { - transmitter: tx, - receiver: PeekReader::new(rx), - } - } -} - -pub struct RadioManager { - pub radio: RadioDevice, - mav_sequence: u8, -} - -impl RadioManager { - pub fn new(radio: RadioDevice) -> Self { - RadioManager { - radio, - mav_sequence: 0, - } - } - pub fn send_message(&mut self, payload: &[u8]) -> Result<(), HydraError> { - let mav_header = mavlink::MavHeader { - system_id: 1, - component_id: 1, - sequence: self.increment_mav_sequence(), - }; - // Create a fixed-size array and copy the payload into it - let mut fixed_payload = [0u8; 255]; - let len = payload.len().min(255); - fixed_payload[..len].copy_from_slice(&payload[..len]); - - let mav_message = mavlink::uorocketry::MavMessage::POSTCARD_MESSAGE( - mavlink::uorocketry::POSTCARD_MESSAGE_DATA { - message: fixed_payload, - }, - ); - mavlink::write_versioned_msg( - &mut self.radio.transmitter, - mavlink::MavlinkVersion::V2, - mav_header, - &mav_message, - )?; - Ok(()) - } - pub fn increment_mav_sequence(&mut self) -> u8 { - self.mav_sequence = self.mav_sequence.wrapping_add(1); - self.mav_sequence - } - pub fn receive_message(&mut self) -> Result { - let (_header, msg): (_, MavMessage) = - mavlink::read_versioned_msg(&mut self.radio.receiver, mavlink::MavlinkVersion::V2)?; - - // info!("{:?}", ); - // Do we need the header? - match msg { - mavlink::uorocketry::MavMessage::POSTCARD_MESSAGE(msg) => { - Ok(postcard::from_bytes::(&msg.message)?) - // weird Ok syntax to coerce to hydra error type. - } - mavlink::uorocketry::MavMessage::COMMAND_MESSAGE(command) => { - info!("{}", command.command); - Ok(postcard::from_bytes::(&command.command)?) - } - mavlink::uorocketry::MavMessage::HEARTBEAT(_) => { - info!("Heartbeat"); - Err(mavlink::error::MessageReadError::Io.into()) - } - _ => { - error!("Error, ErrorContext::UnkownPostcardMessage"); - Err(mavlink::error::MessageReadError::Io.into()) - } - } - } -} diff --git a/boards/strain/src/data_manager.rs b/boards/strain/src/data_manager.rs deleted file mode 100644 index 67b5834..0000000 --- a/boards/strain/src/data_manager.rs +++ /dev/null @@ -1,186 +0,0 @@ -use common_arm::HydraError; -use messages::command::RadioRate; -use messages::state::StateData; -use messages::Message; -use stm32h7xx_hal::rcc::ResetReason; -#[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 reset_reason: Option, - pub logging_rate: Option, - pub recovery_sensing: Option, - pub nav_pos_l1h: 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, - reset_reason: None, - logging_rate: Some(RadioRate::Slow), // start slow. - recovery_sensing: None, - nav_pos_l1h: None, - } - } - - pub fn get_logging_rate(&mut self) -> RadioRate { - if let Some(rate) = self.logging_rate.take() { - let rate_cln = rate.clone(); - self.logging_rate = Some(rate); - return rate_cln; - } - self.logging_rate = Some(RadioRate::Slow); - RadioRate::Slow - } - - /// Do not clone instead take to reduce CPU load. - pub fn take_sensors(&mut self) -> [Option; 15] { - [ - 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(), - self.nav_pos_l1h.take(), - self.recovery_sensing.take(), - ] - } - - pub fn clone_states(&self) -> [Option; 1] { - [self.state.clone()] - } - - pub fn clone_reset_reason(&self) -> Option { - self.reset_reason - } - - pub fn set_reset_reason(&mut self, reset: ResetReason) { - self.reset_reason = Some(reset); - } - - pub fn handle_command(&mut self, data: Message) -> Result<(), HydraError> { - match data.data { - messages::Data::Command(command) => match command.data { - messages::command::CommandData::PowerDown(_) => { - crate::app::sleep_system::spawn().ok(); - } - messages::command::CommandData::RadioRateChange(command_data) => { - self.logging_rate = Some(command_data.rate); - } - _ => { - // We don't care atm about these other commands. - } - }, - _ => { - // we can disregard all other messages for now. - } - } - Ok(()) - } - pub fn handle_data(&mut self, data: Message) { - match data.data { - messages::Data::Sensor(ref sensor) => match sensor.data { - messages::sensor::SensorData::SbgData(ref sbg_data) => match sbg_data { - messages::sensor::SbgData::EkfNavAcc(_) => { - self.ekf_nav_acc = Some(data); - } - messages::sensor::SbgData::GpsPosAcc(_) => { - self.gps_pos_acc = Some(data); - } - messages::sensor::SbgData::Air(_) => { - self.air = Some(data); - } - messages::sensor::SbgData::EkfNav1(_) => { - self.ekf_nav_1 = Some(data); - } - messages::sensor::SbgData::EkfNav2(_) => { - self.ekf_nav_2 = Some(data); - } - messages::sensor::SbgData::EkfQuat(_) => { - self.ekf_quat = Some(data); - } - messages::sensor::SbgData::GpsVel(_) => { - self.gps_vel = Some(data); - } - messages::sensor::SbgData::GpsVelAcc(_) => { - self.gps_vel_acc = Some(data); - } - messages::sensor::SbgData::Imu1(_) => { - self.imu_1 = Some(data); - } - messages::sensor::SbgData::Imu2(_) => { - self.imu_2 = Some(data); - } - messages::sensor::SbgData::UtcTime(_) => { - self.utc_time = Some(data); - } - messages::sensor::SbgData::GpsPos1(_) => { - self.gps_pos_1 = Some(data); - } - messages::sensor::SbgData::GpsPos2(_) => { - self.gps_pos_2 = Some(data); - } - }, - messages::sensor::SensorData::RecoverySensing(_) => { - self.recovery_sensing = Some(data); - } - messages::sensor::SensorData::NavPosLlh(_) => { - self.nav_pos_l1h = Some(data); - } - messages::sensor::SensorData::ResetReason(_) => {} - }, - 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); - // } - // messages::command::CommandData::DeployDrogue(_) => {} - // messages::command::CommandData::DeployMain(_) => {} - // messages::command::CommandData::PowerDown(_) => {} - // }, - _ => {} - } - } -} - -impl Default for DataManager { - fn default() -> Self { - Self::new() - } -} diff --git a/boards/strain/src/main.rs b/boards/strain/src/main.rs deleted file mode 100644 index b320b16..0000000 --- a/boards/strain/src/main.rs +++ /dev/null @@ -1,586 +0,0 @@ -#![no_std] -#![no_main] - -mod communication; -mod data_manager; -mod adc_manager; -mod types; - -use chrono::NaiveDate; -use common_arm::*; -use communication::{CanCommandManager, CanDataManager}; -use communication::{RadioDevice, RadioManager}; -use core::num::{NonZeroU16, NonZeroU8}; -use data_manager::DataManager; -use defmt::info; -use fdcan::{ - config::NominalBitTiming, - filter::{StandardFilter, StandardFilterSlot}, -}; -use messages::command::RadioRate; -use messages::{sensor, Data}; -use panic_probe as _; -use rtic_monotonics::systick::prelude::*; -use rtic_sync::{channel::*, make_channel}; -use stm32h7xx_hal::gpio::gpioa::{PA2, PA3}; -use stm32h7xx_hal::gpio::gpiob::PB4; -use stm32h7xx_hal::gpio::Speed; -use stm32h7xx_hal::gpio::{Output, PushPull}; -use stm32h7xx_hal::prelude::*; -use stm32h7xx_hal::rtc; -use stm32h7xx_hal::{rcc, rcc::rec}; -use types::COM_ID; // global logger - -const DATA_CHANNEL_CAPACITY: usize = 10; - -systick_monotonic!(Mono, 500); - -#[inline(never)] -#[defmt::panic_handler] -fn panic() -> ! { - // stm32h7xx_hal::pac::SCB::sys_reset() - cortex_m::asm::udf() -} - -#[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, dispatchers = [EXTI0, EXTI1, EXTI2, SPI3, SPI2])] -mod app { - - use messages::Message; - use stm32h7xx_hal::gpio::{Alternate, Pin}; - - use super::*; - - #[shared] - struct SharedResources { - data_manager: DataManager, - em: ErrorManager, - // sd_manager: SdManager< - // stm32h7xx_hal::spi::Spi, - // PA4>, - // >, - radio_manager: RadioManager, - can_command_manager: CanCommandManager, - can_data_manager: CanDataManager, - sbg_power: PB4>, - rtc: rtc::Rtc, - } - #[local] - struct LocalResources { - led_red: PA2>, - led_green: PA3>, - buzzer: stm32h7xx_hal::pwm::Pwm< - stm32h7xx_hal::pac::TIM12, - 0, - stm32h7xx_hal::pwm::ComplementaryImpossible, - >, - } - - #[init] - fn init(ctx: init::Context) -> (SharedResources, LocalResources) { - // channel setup - let (_s, r) = make_channel!(Message, DATA_CHANNEL_CAPACITY); - - let core = ctx.core; - - /* Logging Setup */ - HydraLogging::set_ground_station_callback(queue_gs_message); - - let pwr = ctx.device.PWR.constrain(); - // We could use smps, but the board is not designed for it - // let pwrcfg = example_power!(pwr).freeze(); - let mut pwrcfg = pwr.freeze(); - - info!("Power enabled"); - let backup = pwrcfg.backup().unwrap(); - info!("Backup domain enabled"); - // RCC - let mut rcc = ctx.device.RCC.constrain(); - let reset = rcc.get_reset_reason(); - let fdcan_prec_unsafe = unsafe { rcc.steal_peripheral_rec() } - .FDCAN - .kernel_clk_mux(rec::FdcanClkSel::Pll1Q); - - let ccdr = rcc - .use_hse(48.MHz()) // check the clock hardware - .sys_ck(200.MHz()) - .pll1_strategy(rcc::PllConfigStrategy::Iterative) - .pll1_q_ck(32.MHz()) - .freeze(pwrcfg, &ctx.device.SYSCFG); - info!("RCC configured"); - let fdcan_prec = ccdr - .peripheral - .FDCAN - .kernel_clk_mux(rec::FdcanClkSel::Pll1Q); - - let btr = NominalBitTiming { - prescaler: NonZeroU16::new(10).unwrap(), - seg1: NonZeroU8::new(13).unwrap(), - seg2: NonZeroU8::new(2).unwrap(), - sync_jump_width: NonZeroU8::new(1).unwrap(), - }; - - // let data_bit_timing = DataBitTiming { - // prescaler: NonZeroU8::new(10).unwrap(), - // seg1: NonZeroU8::new(13).unwrap(), - // seg2: NonZeroU8::new(2).unwrap(), - // sync_jump_width: NonZeroU8::new(4).unwrap(), - // transceiver_delay_compensation: true, - // }; - - info!("CAN enabled"); - // GPIO - let gpioa = ctx.device.GPIOA.split(ccdr.peripheral.GPIOA); - let gpiod = ctx.device.GPIOD.split(ccdr.peripheral.GPIOD); - let gpiob = ctx.device.GPIOB.split(ccdr.peripheral.GPIOB); - - let pins = gpiob.pb14.into_alternate(); - let mut c0 = ctx - .device - .TIM12 - .pwm(pins, 4.kHz(), ccdr.peripheral.TIM12, &ccdr.clocks); - - c0.set_duty(c0.get_max_duty() / 4); - // PWM outputs are disabled by default - // c0.enable(); - - info!("PWM enabled"); - // assert_eq!(ccdr.clocks.pll1_q_ck().unwrap().raw(), 32_000_000); - info!("PLL1Q:"); - // https://github.com/stm32-rs/stm32h7xx-hal/issues/369 This needs to be stolen. Grrr I hate the imaturity of the stm32-hal - let can2: fdcan::FdCan< - stm32h7xx_hal::can::Can, - fdcan::ConfigMode, - > = { - let rx = gpiob.pb12.into_alternate().speed(Speed::VeryHigh); - let tx = gpiob.pb13.into_alternate().speed(Speed::VeryHigh); - ctx.device.FDCAN2.fdcan(tx, rx, fdcan_prec) - }; - - let mut can_data = can2; - can_data.set_protocol_exception_handling(false); - - can_data.set_nominal_bit_timing(btr); - - // can_data.set_automatic_retransmit(false); // data can be dropped due to its volume. - - // can_command.set_data_bit_timing(data_bit_timing); - - can_data.set_standard_filter( - StandardFilterSlot::_0, - StandardFilter::accept_all_into_fifo0(), - ); - - can_data.set_standard_filter( - StandardFilterSlot::_1, - StandardFilter::accept_all_into_fifo0(), - ); - - can_data.set_standard_filter( - StandardFilterSlot::_2, - StandardFilter::accept_all_into_fifo0(), - ); - - can_data.enable_interrupt(fdcan::interrupt::Interrupt::RxFifo0NewMsg); - - can_data.enable_interrupt_line(fdcan::interrupt::InterruptLine::_0, true); - - let config = can_data - .get_config() - .set_frame_transmit(fdcan::config::FrameTransmissionConfig::AllowFdCanAndBRS); - can_data.apply_config(config); - - let can_data_manager = CanDataManager::new(can_data.into_normal()); - - let can1: fdcan::FdCan< - stm32h7xx_hal::can::Can, - fdcan::ConfigMode, - > = { - let rx = gpioa.pa11.into_alternate().speed(Speed::VeryHigh); - let tx = gpioa.pa12.into_alternate().speed(Speed::VeryHigh); - ctx.device.FDCAN1.fdcan(tx, rx, fdcan_prec_unsafe) - }; - - let mut can_command = can1; - can_command.set_protocol_exception_handling(false); - - can_command.set_nominal_bit_timing(btr); - can_command.set_standard_filter( - StandardFilterSlot::_0, - StandardFilter::accept_all_into_fifo0(), - ); - - can_command.set_standard_filter( - StandardFilterSlot::_1, - StandardFilter::accept_all_into_fifo0(), - ); - - can_command.set_standard_filter( - StandardFilterSlot::_2, - StandardFilter::accept_all_into_fifo0(), - ); - - // can_data.set_data_bit_timing(data_bit_timing); - can_command.enable_interrupt(fdcan::interrupt::Interrupt::RxFifo0NewMsg); - - can_command.enable_interrupt_line(fdcan::interrupt::InterruptLine::_0, true); - - let config = can_command - .get_config() - .set_frame_transmit(fdcan::config::FrameTransmissionConfig::AllowFdCanAndBRS); // check this maybe don't bit switch allow. - can_command.apply_config(config); - - let can_command_manager = CanCommandManager::new(can_command.into_normal()); - - // let spi_sd: stm32h7xx_hal::spi::Spi< - // stm32h7xx_hal::stm32::SPI1, - // stm32h7xx_hal::spi::Enabled, - // u8, - // > = ctx.device.SPI1.spi( - // ( - // gpioa.pa5.into_alternate::<5>(), - // gpioa.pa6.into_alternate(), - // gpioa.pa7.into_alternate(), - // ), - // spi::Config::new(spi::MODE_0), - // 16.MHz(), - // ccdr.peripheral.SPI1, - // &ccdr.clocks, - // ); - - // let cs_sd = gpioa.pa4.into_push_pull_output(); - - // let sd_manager = SdManager::new(spi_sd, cs_sd); - - // leds - let led_red = gpioa.pa2.into_push_pull_output(); - let led_green = gpioa.pa3.into_push_pull_output(); - - // sbg power pin - let mut sbg_power = gpiob.pb4.into_push_pull_output(); - sbg_power.set_high(); - - // UART for sbg - let tx: Pin<'D', 1, Alternate<8>> = gpiod.pd1.into_alternate(); - let rx: Pin<'D', 0, Alternate<8>> = gpiod.pd0.into_alternate(); - - // let stream_tuple = StreamsTuple::new(ctx.device.DMA1, ccdr.peripheral.DMA1); - let uart_radio = ctx - .device - .UART4 - .serial((tx, rx), 57600.bps(), ccdr.peripheral.UART4, &ccdr.clocks) - .unwrap(); - // let mut sbg_manager = sbg_manager::SBGManager::new(uart_sbg, stream_tuple); - - let radio = RadioDevice::new(uart_radio); - - let radio_manager = RadioManager::new(radio); - - let mut rtc = stm32h7xx_hal::rtc::Rtc::open_or_init( - ctx.device.RTC, - backup.RTC, - stm32h7xx_hal::rtc::RtcClock::Lsi, - &ccdr.clocks, - ); - - // TODO: Get current time from some source - let now = NaiveDate::from_ymd_opt(2001, 1, 1) - .unwrap() - .and_hms_opt(0, 0, 0) - .unwrap(); - - rtc.set_date_time(now); - - /* Monotonic clock */ - Mono::start(core.SYST, 200_000_000); - - let mut data_manager = DataManager::new(); - data_manager.set_reset_reason(reset); - let em = ErrorManager::new(); - blink::spawn().ok(); - send_data_internal::spawn(r).ok(); - reset_reason_send::spawn().ok(); - state_send::spawn().ok(); - // generate_random_messages::spawn().ok(); - // sensor_send::spawn().ok(); - info!("Online"); - - ( - SharedResources { - data_manager, - em, - // sd_manager, - radio_manager, - can_command_manager, - can_data_manager, - sbg_power, - rtc, - }, - LocalResources { - led_red, - led_green, - buzzer: c0, - }, - ) - } - - #[task(priority = 3)] - async fn delay(_cx: delay::Context, delay: u32) { - Mono::delay(delay.millis()).await; - } - - #[task(priority = 3, shared = [&em, rtc])] - async fn generate_random_messages(mut cx: generate_random_messages::Context) { - loop { - cx.shared.em.run(|| { - let message = Message::new( - cx.shared - .rtc - .lock(|rtc| messages::FormattedNaiveDateTime(rtc.date_time().unwrap())), - COM_ID, - messages::state::State::new(messages::state::StateData::Initializing), - ); - spawn!(send_gs, message.clone())?; - // spawn!(send_data_internal, message)?; - Ok(()) - }); - Mono::delay(1.secs()).await; - } - } - - #[task(priority = 3, shared = [data_manager, &em, rtc])] - async fn reset_reason_send(mut cx: reset_reason_send::Context) { - let reason = cx - .shared - .data_manager - .lock(|data_manager| data_manager.clone_reset_reason()); - match reason { - Some(reason) => { - let x = match reason { - stm32h7xx_hal::rcc::ResetReason::BrownoutReset => sensor::ResetReason::BrownoutReset, - stm32h7xx_hal::rcc::ResetReason::CpuReset => sensor::ResetReason::CpuReset, - stm32h7xx_hal::rcc::ResetReason::D1EntersDStandbyErroneouslyOrCpuEntersCStopErroneously => sensor::ResetReason::D1EntersDStandbyErroneouslyOrCpuEntersCStopErroneously, - stm32h7xx_hal::rcc::ResetReason::D1ExitsDStandbyMode => sensor::ResetReason::D1ExitsDStandbyMode, - stm32h7xx_hal::rcc::ResetReason::D2ExitsDStandbyMode => sensor::ResetReason::D2ExitsDStandbyMode, - stm32h7xx_hal::rcc::ResetReason::GenericWatchdogReset => sensor::ResetReason::GenericWatchdogReset, - stm32h7xx_hal::rcc::ResetReason::IndependentWatchdogReset => sensor::ResetReason::IndependentWatchdogReset, - stm32h7xx_hal::rcc::ResetReason::PinReset => sensor::ResetReason::PinReset, - stm32h7xx_hal::rcc::ResetReason::PowerOnReset => sensor::ResetReason::PowerOnReset, - stm32h7xx_hal::rcc::ResetReason::SystemReset => sensor::ResetReason::SystemReset, - stm32h7xx_hal::rcc::ResetReason::Unknown { rcc_rsr } => sensor::ResetReason::Unknown { rcc_rsr }, - stm32h7xx_hal::rcc::ResetReason::WindowWatchdogReset => sensor::ResetReason::WindowWatchdogReset, - }; - let message = messages::Message::new( - cx.shared - .rtc - .lock(|rtc| messages::FormattedNaiveDateTime(rtc.date_time().unwrap())), - COM_ID, - sensor::Sensor::new(x), - ); - - cx.shared.em.run(|| { - spawn!(send_gs, message)?; - Ok(()) - }) - } - None => return, - } - } - - #[task(shared = [data_manager, &em, rtc])] - async fn state_send(mut cx: state_send::Context) { - let state_data = cx - .shared - .data_manager - .lock(|data_manager| data_manager.state.clone()); - cx.shared.em.run(|| { - if let Some(x) = state_data { - let message = Message::new( - cx.shared - .rtc - .lock(|rtc| messages::FormattedNaiveDateTime(rtc.date_time().unwrap())), - COM_ID, - messages::state::State::new(x), - ); - spawn!(send_gs, message)?; - } // if there is none we still return since we simply don't have data yet. - Ok(()) - }); - Mono::delay(5.secs()).await; - // spawn_after!(state_send, ExtU64::secs(5)).ok(); - } - - /** - * Sends information about the sensors. - */ - #[task(priority = 3, shared = [data_manager, &em])] - async fn sensor_send(mut cx: sensor_send::Context) { - loop { - let (sensors, logging_rate) = cx.shared.data_manager.lock(|data_manager| { - (data_manager.take_sensors(), data_manager.get_logging_rate()) - }); - - cx.shared.em.run(|| { - for msg in sensors { - match msg { - Some(x) => { - // info!("Sending sensor data {}", x.clone()); - spawn!(send_gs, x)?; - // spawn!(sd_dump, x)?; - } - None => { - info!("No sensor data to send"); - continue; - } - } - } - - Ok(()) - }); - match logging_rate { - RadioRate::Fast => { - Mono::delay(100.millis()).await; - } - RadioRate::Slow => { - Mono::delay(250.millis()).await; - } - } - } - } - - /// 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) { - info!("Queueing message"); - send_gs_intermediate::spawn(d.into()).ok(); - } - - #[task(priority = 3, shared = [rtc, &em])] - async fn send_gs_intermediate(mut cx: send_gs_intermediate::Context, m: Data) { - cx.shared.em.run(|| { - cx.shared.rtc.lock(|rtc| { - let message = messages::Message::new( - messages::FormattedNaiveDateTime(rtc.date_time().unwrap()), - COM_ID, - m, - ); - spawn!(send_gs, message)?; - Ok(()) - }) - }); - } - - #[task(priority = 2, binds = FDCAN1_IT0, shared = [can_command_manager, data_manager, &em])] - fn can_command(mut cx: can_command::Context) { - // info!("CAN Command"); - cx.shared.can_command_manager.lock(|can| { - cx.shared - .data_manager - .lock(|data_manager| cx.shared.em.run(|| can.process_data(data_manager))); - }) - } - - #[task(priority = 3, shared = [sbg_power])] - async fn sbg_power_on(mut cx: sbg_power_on::Context) { - loop { - cx.shared.sbg_power.lock(|sbg| { - sbg.set_high(); - }); - Mono::delay(10000.millis()).await; - } - } - - /** - * Sends a message to the radio over UART. - */ - #[task(priority = 3, shared = [&em, radio_manager])] - async fn send_gs(mut cx: send_gs::Context, m: Message) { - // info!("{}", m.clone()); - - cx.shared.radio_manager.lock(|radio_manager| { - cx.shared.em.run(|| { - // info!("Sending message {}", m); - let mut buf = [0; 255]; - let data = postcard::to_slice(&m, &mut buf)?; - radio_manager.send_message(data)?; - Ok(()) - }) - }); - } - - #[task( priority = 3, binds = FDCAN2_IT0, shared = [&em, can_data_manager, data_manager])] - fn can_data(mut cx: can_data::Context) { - cx.shared.can_data_manager.lock(|can| { - { - cx.shared.em.run(|| { - can.process_data()?; - Ok(()) - }) - } - }); - } - - #[task(priority = 2, shared = [&em, can_data_manager, data_manager])] - async fn send_data_internal( - mut cx: send_data_internal::Context, - mut receiver: Receiver<'static, Message, DATA_CHANNEL_CAPACITY>, - ) { - loop { - if let Ok(m) = receiver.recv().await { - cx.shared.can_data_manager.lock(|can| { - cx.shared.em.run(|| { - can.send_message(m)?; - Ok(()) - }) - }); - } - } - } - - #[task(priority = 2, shared = [&em, can_command_manager, data_manager])] - async fn send_command_internal(mut cx: send_command_internal::Context, m: Message) { - // while let Ok(m) = receiver.recv().await { - cx.shared.can_command_manager.lock(|can| { - cx.shared.em.run(|| { - can.send_message(m)?; - Ok(()) - }) - }); - // } - } - - #[task(priority = 1, local = [led_red, led_green, buzzer, buzzed: bool = false], shared = [&em])] - async fn blink(cx: blink::Context) { - loop { - if cx.shared.em.has_error() { - cx.local.led_red.toggle(); - if *cx.local.buzzed { - cx.local.buzzer.set_duty(0); - *cx.local.buzzed = false; - } else { - let duty = cx.local.buzzer.get_max_duty() / 4; - cx.local.buzzer.set_duty(duty); - *cx.local.buzzed = true; - } - Mono::delay(500.millis()).await; - } else { - cx.local.led_green.toggle(); - if *cx.local.buzzed { - cx.local.buzzer.set_duty(0); - *cx.local.buzzed = false; - } else { - let duty = cx.local.buzzer.get_max_duty() / 4; - cx.local.buzzer.set_duty(duty); - *cx.local.buzzed = true; - } - Mono::delay(2000.millis()).await; - } - } - } - - #[task(priority = 3, shared = [&em, sbg_power])] - async fn sleep_system(mut cx: sleep_system::Context) { - // Turn off the SBG and CAN, also start a timer to wake up the system. Put the chip in sleep mode. - cx.shared.sbg_power.lock(|sbg| { - sbg.set_low(); - }); - } -} diff --git a/boards/strain/src/types.rs b/boards/strain/src/types.rs deleted file mode 100644 index 7ebc58c..0000000 --- a/boards/strain/src/types.rs +++ /dev/null @@ -1,3 +0,0 @@ -use messages::node::{Node, Node::StrainBoard}; - -pub static COM_ID: Node = StrainBoard; diff --git a/boards/strain/tests/sd.rs b/boards/strain/tests/sd.rs deleted file mode 100644 index 9c4f1db..0000000 --- a/boards/strain/tests/sd.rs +++ /dev/null @@ -1,85 +0,0 @@ -#![no_std] -#![no_main] - -use common_arm::SdManager; -use defmt::info; -use panic_probe as _; -use stm32h7xx_hal::gpio::{Output, PushPull, PA4}; -use stm32h7xx_hal::pac; -use stm32h7xx_hal::prelude::*; -use stm32h7xx_hal::spi; - -struct State { - sd_manager: SdManager< - stm32h7xx_hal::spi::Spi, - PA4>, - >, -} - -#[defmt_test::tests] -mod tests { - use super::*; - - #[init] - fn init() -> State { - let _cp = cortex_m::Peripherals::take().unwrap(); - let dp = pac::Peripherals::take().unwrap(); - - let pwr = dp.PWR.constrain(); - let pwrcfg = pwr.freeze(); - - info!("Power enabled"); - // RCC - let mut rcc = dp.RCC.constrain(); - let reset = rcc.get_reset_reason(); - - info!("Reset reason: {:?}", reset); - - let ccdr = rcc - .use_hse(48.MHz()) // check the clock hardware - .sys_ck(200.MHz()) - .freeze(pwrcfg, &dp.SYSCFG); - info!("RCC configured"); - - let gpioa = dp.GPIOA.split(ccdr.peripheral.GPIOA); - - let spi_sd: stm32h7xx_hal::spi::Spi< - stm32h7xx_hal::stm32::SPI1, - stm32h7xx_hal::spi::Enabled, - u8, - > = dp.SPI1.spi( - ( - gpioa.pa5.into_alternate::<5>(), - gpioa.pa6.into_alternate(), - gpioa.pa7.into_alternate(), - ), - spi::Config::new(spi::MODE_0), - 16.MHz(), - ccdr.peripheral.SPI1, - &ccdr.clocks, - ); - - let cs_sd = gpioa.pa4.into_push_pull_output(); - - let sd_manager = SdManager::new(spi_sd, cs_sd); - State { sd_manager } - } - - #[test] - fn writing_file(state: &mut State) { - let sd_manager = &mut state.sd_manager; - - let mut test_file = sd_manager - .open_file("testing.txt") - .expect("Cannot open file"); - sd_manager - .write(&mut test_file, b"Hello this is a test!") - .expect("Could not write file."); - sd_manager - .write_str(&mut test_file, "Testing Strings") - .expect("Could not write string"); - sd_manager - .close_file(test_file) - .expect("Could not close file."); // we are done with the file so destroy it - } -} diff --git a/boards/temperature/src/adc_manager.rs b/boards/temperature/src/adc_manager.rs index 73e8d1f..428f9e7 100644 --- a/boards/temperature/src/adc_manager.rs +++ b/boards/temperature/src/adc_manager.rs @@ -42,7 +42,10 @@ impl AdcManager { self.adc1_cs.set_low(); self.adc1.reset()?; - spawn!(delay, 1000); + match spawn!(delay, 1000) { + Ok(_) => (), + Err(_) => panic!("Failed ADC 1 init."), + } let mut mode1_cfg = Mode1Register::default(); mode1_cfg.set_filter(ads126x::register::DigitalFilter::Sinc1); @@ -63,7 +66,10 @@ impl AdcManager { self.adc2_cs.set_low(); self.adc2.reset()?; - spawn!(delay, 1000); + match spawn!(delay, 1000) { + Ok(_) => (), + Err(_) => panic!("Failed ADC 2 init."), + } let mut mode1_cfg = Mode1Register::default(); mode1_cfg.set_filter(ads126x::register::DigitalFilter::Sinc1); diff --git a/boards/temperature/src/communication.rs b/boards/temperature/src/communication.rs index 212bdcd..31080ab 100644 --- a/boards/temperature/src/communication.rs +++ b/boards/temperature/src/communication.rs @@ -1,34 +1,76 @@ +use core::num::{NonZeroU16, NonZeroU8}; + use crate::data_manager::DataManager; use crate::types::COM_ID; use common_arm::HydraError; -use defmt::{error, info}; +use defmt::info; +use fdcan::Instance; use fdcan::{ + config::NominalBitTiming, + filter::{StandardFilter, StandardFilterSlot}, frame::{FrameFormat, TxFrameHeader}, id::StandardId, }; -use mavlink::peek_reader::PeekReader; -use messages::mavlink::uorocketry::MavMessage; -use messages::mavlink::{self}; use messages::Message; use postcard::from_bytes; /// Clock configuration is out of scope for this builder /// easiest way to avoid alloc is to use no generics -pub struct CanCommandManager { - can: fdcan::FdCan< - stm32h7xx_hal::can::Can, - fdcan::NormalOperationMode, - >, +pub struct CanManager { + can: fdcan::FdCan, } -impl CanCommandManager { - pub fn new( - can: fdcan::FdCan< - stm32h7xx_hal::can::Can, - fdcan::NormalOperationMode, - >, - ) -> Self { - Self { can } +impl CanManager { + pub fn new(mut can: fdcan::FdCan) -> Self { + // let data_bit_timing = DataBitTiming { + // prescaler: NonZeroU8::new(10).unwrap(), + // seg1: NonZeroU8::new(13).unwrap(), + // seg2: NonZeroU8::new(2).unwrap(), + // sync_jump_width: NonZeroU8::new(4).unwrap(), + // transceiver_delay_compensation: true, + // }; + // can_data.set_automatic_retransmit(false); // data can be dropped due to its volume. + + // can_command.set_data_bit_timing(data_bit_timing); + let btr = NominalBitTiming { + prescaler: NonZeroU16::new(10).unwrap(), + seg1: NonZeroU8::new(13).unwrap(), + seg2: NonZeroU8::new(2).unwrap(), + sync_jump_width: NonZeroU8::new(1).unwrap(), + }; + + // Why? + can.set_protocol_exception_handling(false); + + can.set_nominal_bit_timing(btr); + can.set_standard_filter( + StandardFilterSlot::_0, + StandardFilter::accept_all_into_fifo0(), + ); + + can.set_standard_filter( + StandardFilterSlot::_1, + StandardFilter::accept_all_into_fifo0(), + ); + + can.set_standard_filter( + StandardFilterSlot::_2, + StandardFilter::accept_all_into_fifo0(), + ); + + can.enable_interrupt(fdcan::interrupt::Interrupt::RxFifo0NewMsg); + + can.enable_interrupt_line(fdcan::interrupt::InterruptLine::_0, true); + + let config = can + .get_config() + .set_frame_transmit(fdcan::config::FrameTransmissionConfig::AllowFdCanAndBRS); + + can.apply_config(config); + + Self { + can: can.into_normal(), + } } pub fn send_message(&mut self, m: Message) -> Result<(), HydraError> { let mut buf = [0u8; 64]; @@ -56,139 +98,3 @@ impl CanCommandManager { Ok(()) } } - -/// Clock configuration is out of scope for this builder -/// easiest way to avoid alloc is to use no generics -pub struct CanDataManager { - can: fdcan::FdCan< - stm32h7xx_hal::can::Can, - fdcan::NormalOperationMode, - >, -} - -impl CanDataManager { - pub fn new( - can: fdcan::FdCan< - stm32h7xx_hal::can::Can, - fdcan::NormalOperationMode, - >, - ) -> Self { - Self { can } - } - pub fn send_message(&mut self, m: Message) -> Result<(), HydraError> { - let mut buf = [0u8; 64]; - let payload = postcard::to_slice(&m, &mut buf)?; - let header = TxFrameHeader { - len: payload.len() as u8, // switch to const as this never changes or swtich on message type of known size - id: StandardId::new(COM_ID.into()).unwrap().into(), - frame_format: FrameFormat::Fdcan, - bit_rate_switching: false, - marker: None, - }; - // self.can.abort(fdcan::Mailbox::_2); // this is needed if boards are not in sync (if they are not in sync that is a bigger problem) - - stm32h7xx_hal::nb::block!(self.can.transmit(header, payload))?; - - Ok(()) - } - pub fn process_data(&mut self) -> Result<(), HydraError> { - let mut buf = [0u8; 64]; - while self.can.receive0(&mut buf).is_ok() { - if let Ok(data) = from_bytes::(&buf) { - info!("Received message {}", data.clone()); - crate::app::send_gs::spawn(data).ok(); - } else if let Err(e) = from_bytes::(&buf) { - info!("Error: {:?}", e); - } - } - self.can - .clear_interrupt(fdcan::interrupt::Interrupt::RxFifo0NewMsg); - Ok(()) - } -} - -pub struct RadioDevice { - transmitter: stm32h7xx_hal::serial::Tx, - pub receiver: PeekReader>, -} - -impl RadioDevice { - pub fn new(uart: stm32h7xx_hal::serial::Serial) -> Self { - let (tx, mut rx) = uart.split(); - - rx.listen(); - // setup interrupts - - RadioDevice { - transmitter: tx, - receiver: PeekReader::new(rx), - } - } -} - -pub struct RadioManager { - pub radio: RadioDevice, - mav_sequence: u8, -} - -impl RadioManager { - pub fn new(radio: RadioDevice) -> Self { - RadioManager { - radio, - mav_sequence: 0, - } - } - pub fn send_message(&mut self, payload: &[u8]) -> Result<(), HydraError> { - let mav_header = mavlink::MavHeader { - system_id: 1, - component_id: 1, - sequence: self.increment_mav_sequence(), - }; - // Create a fixed-size array and copy the payload into it - let mut fixed_payload = [0u8; 255]; - let len = payload.len().min(255); - fixed_payload[..len].copy_from_slice(&payload[..len]); - - let mav_message = mavlink::uorocketry::MavMessage::POSTCARD_MESSAGE( - mavlink::uorocketry::POSTCARD_MESSAGE_DATA { - message: fixed_payload, - }, - ); - mavlink::write_versioned_msg( - &mut self.radio.transmitter, - mavlink::MavlinkVersion::V2, - mav_header, - &mav_message, - )?; - Ok(()) - } - pub fn increment_mav_sequence(&mut self) -> u8 { - self.mav_sequence = self.mav_sequence.wrapping_add(1); - self.mav_sequence - } - pub fn receive_message(&mut self) -> Result { - let (_header, msg): (_, MavMessage) = - mavlink::read_versioned_msg(&mut self.radio.receiver, mavlink::MavlinkVersion::V2)?; - - // info!("{:?}", ); - // Do we need the header? - match msg { - mavlink::uorocketry::MavMessage::POSTCARD_MESSAGE(msg) => { - Ok(postcard::from_bytes::(&msg.message)?) - // weird Ok syntax to coerce to hydra error type. - } - mavlink::uorocketry::MavMessage::COMMAND_MESSAGE(command) => { - info!("{}", command.command); - Ok(postcard::from_bytes::(&command.command)?) - } - mavlink::uorocketry::MavMessage::HEARTBEAT(_) => { - info!("Heartbeat"); - Err(mavlink::error::MessageReadError::Io.into()) - } - _ => { - error!("Error, ErrorContext::UnkownPostcardMessage"); - Err(mavlink::error::MessageReadError::Io.into()) - } - } - } -} diff --git a/boards/temperature/src/main.rs b/boards/temperature/src/main.rs index 74e2136..a0da2ad 100644 --- a/boards/temperature/src/main.rs +++ b/boards/temperature/src/main.rs @@ -6,36 +6,26 @@ mod communication; mod data_manager; mod types; +use adc_manager::AdcManager; use chrono::NaiveDate; use common_arm::*; -use communication::{CanCommandManager, CanDataManager}; -use communication::{RadioDevice, RadioManager}; -use core::num::{NonZeroU16, NonZeroU8}; +use communication::CanManager; use data_manager::DataManager; use defmt::info; -use fdcan::{ - config::NominalBitTiming, - filter::{StandardFilter, StandardFilterSlot}, -}; -use messages::command::RadioRate; use messages::Message; use messages::{sensor, Data}; use panic_probe as _; use rtic_monotonics::systick::prelude::*; use rtic_sync::{channel::*, make_channel}; use stm32h7xx_hal::gpio::gpioa::{PA2, PA3}; -use stm32h7xx_hal::gpio::gpiob::PB4; use stm32h7xx_hal::gpio::Speed; +use stm32h7xx_hal::gpio::PA4; use stm32h7xx_hal::gpio::{Output, PushPull}; +use stm32h7xx_hal::hal::spi; use stm32h7xx_hal::prelude::*; use stm32h7xx_hal::rtc; -use stm32h7xx_hal::{ - gpio::{Alternate, Pin}, - hal::spi, -}; use stm32h7xx_hal::{rcc, rcc::rec}; use types::COM_ID; // global logger -use adc_manager::AdcManager; const DATA_CHANNEL_CAPACITY: usize = 10; @@ -50,43 +40,38 @@ fn panic() -> ! { #[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, dispatchers = [EXTI0, EXTI1, EXTI2, SPI3, SPI2])] mod app { + use super::*; #[shared] struct SharedResources { data_manager: DataManager, em: ErrorManager, - // sd_manager: SdManager< - // stm32h7xx_hal::spi::Spi, - // PA4>, - // >, - radio_manager: RadioManager, - can_command_manager: CanCommandManager, - can_data_manager: CanDataManager, - sbg_power: PB4>, + sd_manager: SdManager< + stm32h7xx_hal::spi::Spi, + PA4>, + >, + can_command_manager: CanManager>, + can_data_manager: CanManager>, rtc: rtc::Rtc, adc_manager: AdcManager, } #[local] struct LocalResources { + can_sender: Sender<'static, Message, DATA_CHANNEL_CAPACITY>, led_red: PA2>, led_green: PA3>, - buzzer: stm32h7xx_hal::pwm::Pwm< - stm32h7xx_hal::pac::TIM12, - 0, - stm32h7xx_hal::pwm::ComplementaryImpossible, - >, } #[init] fn init(ctx: init::Context) -> (SharedResources, LocalResources) { // channel setup - let (_s, r) = make_channel!(Message, DATA_CHANNEL_CAPACITY); + let (can_sender, r) = make_channel!(Message, DATA_CHANNEL_CAPACITY); let core = ctx.core; /* Logging Setup */ - HydraLogging::set_ground_station_callback(queue_gs_message); + HydraLogging::set_ground_station_callback(queue_log_message); let pwr = ctx.device.PWR.constrain(); // We could use smps, but the board is not designed for it @@ -115,22 +100,6 @@ mod app { .FDCAN .kernel_clk_mux(rec::FdcanClkSel::Pll1Q); - let btr = NominalBitTiming { - prescaler: NonZeroU16::new(10).unwrap(), - seg1: NonZeroU8::new(13).unwrap(), - seg2: NonZeroU8::new(2).unwrap(), - sync_jump_width: NonZeroU8::new(1).unwrap(), - }; - - // let data_bit_timing = DataBitTiming { - // prescaler: NonZeroU8::new(10).unwrap(), - // seg1: NonZeroU8::new(13).unwrap(), - // seg2: NonZeroU8::new(2).unwrap(), - // sync_jump_width: NonZeroU8::new(4).unwrap(), - // transceiver_delay_compensation: true, - // }; - - info!("CAN enabled"); // GPIO let gpioa = ctx.device.GPIOA.split(ccdr.peripheral.GPIOA); let gpiod = ctx.device.GPIOD.split(ccdr.peripheral.GPIOD); @@ -138,16 +107,6 @@ mod app { let gpiob = ctx.device.GPIOB.split(ccdr.peripheral.GPIOB); let gpioe = ctx.device.GPIOE.split(ccdr.peripheral.GPIOE); - let pins = gpiob.pb14.into_alternate(); - let mut c0 = ctx - .device - .TIM12 - .pwm(pins, 4.kHz(), ccdr.peripheral.TIM12, &ccdr.clocks); - - c0.set_duty(c0.get_max_duty() / 4); - // PWM outputs are disabled by default - // c0.enable(); - info!("PWM enabled"); // assert_eq!(ccdr.clocks.pll1_q_ck().unwrap().raw(), 32_000_000); info!("PLL1Q:"); @@ -161,40 +120,7 @@ mod app { ctx.device.FDCAN2.fdcan(tx, rx, fdcan_prec) }; - let mut can_data = can2; - can_data.set_protocol_exception_handling(false); - - can_data.set_nominal_bit_timing(btr); - - // can_data.set_automatic_retransmit(false); // data can be dropped due to its volume. - - // can_command.set_data_bit_timing(data_bit_timing); - - can_data.set_standard_filter( - StandardFilterSlot::_0, - StandardFilter::accept_all_into_fifo0(), - ); - - can_data.set_standard_filter( - StandardFilterSlot::_1, - StandardFilter::accept_all_into_fifo0(), - ); - - can_data.set_standard_filter( - StandardFilterSlot::_2, - StandardFilter::accept_all_into_fifo0(), - ); - - can_data.enable_interrupt(fdcan::interrupt::Interrupt::RxFifo0NewMsg); - - can_data.enable_interrupt_line(fdcan::interrupt::InterruptLine::_0, true); - - let config = can_data - .get_config() - .set_frame_transmit(fdcan::config::FrameTransmissionConfig::AllowFdCanAndBRS); - can_data.apply_config(config); - - let can_data_manager = CanDataManager::new(can_data.into_normal()); + let can_data_manager = CanManager::new(can2); let can1: fdcan::FdCan< stm32h7xx_hal::can::Can, @@ -205,56 +131,27 @@ mod app { ctx.device.FDCAN1.fdcan(tx, rx, fdcan_prec_unsafe) }; - let mut can_command = can1; - can_command.set_protocol_exception_handling(false); + let can_command_manager = CanManager::new(can1); - can_command.set_nominal_bit_timing(btr); - can_command.set_standard_filter( - StandardFilterSlot::_0, - StandardFilter::accept_all_into_fifo0(), - ); - - can_command.set_standard_filter( - StandardFilterSlot::_1, - StandardFilter::accept_all_into_fifo0(), - ); - - can_command.set_standard_filter( - StandardFilterSlot::_2, - StandardFilter::accept_all_into_fifo0(), + let spi_sd: stm32h7xx_hal::spi::Spi< + stm32h7xx_hal::stm32::SPI1, + stm32h7xx_hal::spi::Enabled, + u8, + > = ctx.device.SPI1.spi( + ( + gpioa.pa5.into_alternate::<5>(), + gpioa.pa6.into_alternate(), + gpioa.pa7.into_alternate(), + ), + stm32h7xx_hal::spi::Config::new(spi::MODE_0), + 16.MHz(), + ccdr.peripheral.SPI1, + &ccdr.clocks, ); - // can_data.set_data_bit_timing(data_bit_timing); - can_command.enable_interrupt(fdcan::interrupt::Interrupt::RxFifo0NewMsg); + let cs_sd = gpioa.pa4.into_push_pull_output(); - can_command.enable_interrupt_line(fdcan::interrupt::InterruptLine::_0, true); - - let config = can_command - .get_config() - .set_frame_transmit(fdcan::config::FrameTransmissionConfig::AllowFdCanAndBRS); // check this maybe don't bit switch allow. - can_command.apply_config(config); - - let can_command_manager = CanCommandManager::new(can_command.into_normal()); - - // let spi_sd: stm32h7xx_hal::spi::Spi< - // stm32h7xx_hal::stm32::SPI1, - // stm32h7xx_hal::spi::Enabled, - // u8, - // > = ctx.device.SPI1.spi( - // ( - // gpioa.pa5.into_alternate::<5>(), - // gpioa.pa6.into_alternate(), - // gpioa.pa7.into_alternate(), - // ), - // spi::Config::new(spi::MODE_0), - // 16.MHz(), - // ccdr.peripheral.SPI1, - // &ccdr.clocks, - // ); - - // let cs_sd = gpioa.pa4.into_push_pull_output(); - - // let sd_manager = SdManager::new(spi_sd, cs_sd); + let sd_manager = SdManager::new(spi_sd, cs_sd); // ADC setup let adc_spi: stm32h7xx_hal::spi::Spi< @@ -285,26 +182,6 @@ mod app { let led_red = gpioa.pa2.into_push_pull_output(); let led_green = gpioa.pa3.into_push_pull_output(); - // sbg power pin - let mut sbg_power = gpiob.pb4.into_push_pull_output(); - sbg_power.set_high(); - - // UART for sbg - let tx: Pin<'D', 1, Alternate<8>> = gpiod.pd1.into_alternate(); - let rx: Pin<'D', 0, Alternate<8>> = gpiod.pd0.into_alternate(); - - // let stream_tuple = StreamsTuple::new(ctx.device.DMA1, ccdr.peripheral.DMA1); - let uart_radio = ctx - .device - .UART4 - .serial((tx, rx), 57600.bps(), ccdr.peripheral.UART4, &ccdr.clocks) - .unwrap(); - // let mut sbg_manager = sbg_manager::SBGManager::new(uart_sbg, stream_tuple); - - let radio = RadioDevice::new(uart_radio); - - let radio_manager = RadioManager::new(radio); - let mut rtc = stm32h7xx_hal::rtc::Rtc::open_or_init( ctx.device.RTC, backup.RTC, @@ -312,7 +189,7 @@ mod app { &ccdr.clocks, ); - // TODO: Get current time from some source + // TODO: Get current time from some source, this should be the responsibility of pheonix to sync the boards with GPS time. let now = NaiveDate::from_ymd_opt(2001, 1, 1) .unwrap() .and_hms_opt(0, 0, 0) @@ -330,49 +207,26 @@ mod app { send_data_internal::spawn(r).ok(); reset_reason_send::spawn().ok(); state_send::spawn().ok(); - // generate_random_messages::spawn().ok(); - // sensor_send::spawn().ok(); info!("Online"); ( SharedResources { data_manager, em, - // sd_manager, - radio_manager, + sd_manager, can_command_manager, can_data_manager, - sbg_power, rtc, adc_manager, }, LocalResources { + can_sender, led_red, led_green, - buzzer: c0, }, ) } - #[task(priority = 3, shared = [&em, rtc])] - async fn generate_random_messages(mut cx: generate_random_messages::Context) { - loop { - cx.shared.em.run(|| { - let message = Message::new( - cx.shared - .rtc - .lock(|rtc| messages::FormattedNaiveDateTime(rtc.date_time().unwrap())), - COM_ID, - messages::state::State::new(messages::state::StateData::Initializing), - ); - spawn!(send_gs, message.clone())?; - // spawn!(send_data_internal, message)?; - Ok(()) - }); - Mono::delay(1.secs()).await; - } - } - #[task(priority = 3, shared = [data_manager, &em, rtc])] async fn reset_reason_send(mut cx: reset_reason_send::Context) { let reason = cx @@ -404,9 +258,9 @@ mod app { ); cx.shared.em.run(|| { - spawn!(send_gs, message)?; + spawn!(queue_data_internal, message)?; Ok(()) - }) + }); } None => return, } @@ -432,7 +286,10 @@ mod app { COM_ID, messages::state::State::new(x), ); - spawn!(send_gs, message)?; + cx.shared.em.run(|| { + spawn!(queue_data_internal, message)?; + Ok(()) + }); } // if there is none we still return since we simply don't have data yet. Ok(()) }); @@ -445,47 +302,46 @@ mod app { */ #[task(priority = 3, shared = [data_manager, &em])] async fn sensor_send(mut cx: sensor_send::Context) { - loop { - let (sensors, logging_rate) = cx.shared.data_manager.lock(|data_manager| { - (data_manager.take_sensors(), data_manager.get_logging_rate()) - }); + let sensors = cx + .shared + .data_manager + .lock(|data_manager| data_manager.take_sensors()); - cx.shared.em.run(|| { - for msg in sensors { - match msg { - Some(x) => { - // info!("Sending sensor data {}", x.clone()); - spawn!(send_gs, x)?; - // spawn!(sd_dump, x)?; - } - None => { - info!("No sensor data to send"); - continue; - } + cx.shared.em.run(|| { + for msg in sensors { + match msg { + Some(x) => { + spawn!(queue_data_internal, x)?; + } + None => { + info!("No sensor data to send"); + continue; } - } - - Ok(()) - }); - match logging_rate { - RadioRate::Fast => { - Mono::delay(100.millis()).await; - } - RadioRate::Slow => { - Mono::delay(250.millis()).await; } } - } + + Ok(()) + }); + } + + /// Callback for our logging library to access the needed resources. + pub fn queue_log_message(d: impl Into) { + send_log_intermediate::spawn(d.into()).ok(); } - /// 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) { - info!("Queueing message"); - send_gs_intermediate::spawn(d.into()).ok(); + #[task(priority = 3, local = [can_sender], shared = [&em])] + async fn queue_data_internal(cx: queue_data_internal::Context, m: Message) { + match cx.local.can_sender.send(m).await { + // Preferably clean this up to be handled by the error manager. + Ok(_) => {} + Err(_) => { + info!("Failed to send data"); + } + } } #[task(priority = 3, shared = [rtc, &em])] - async fn send_gs_intermediate(mut cx: send_gs_intermediate::Context, m: Data) { + async fn send_log_intermediate(mut cx: send_log_intermediate::Context, m: Data) { cx.shared.em.run(|| { cx.shared.rtc.lock(|rtc| { let message = messages::Message::new( @@ -493,7 +349,8 @@ mod app { COM_ID, m, ); - spawn!(send_gs, message)?; + + spawn!(queue_data_internal, message)?; Ok(()) }) }); @@ -509,41 +366,15 @@ mod app { }) } - #[task(priority = 3, shared = [sbg_power])] - async fn sbg_power_on(mut cx: sbg_power_on::Context) { - loop { - cx.shared.sbg_power.lock(|sbg| { - sbg.set_high(); - }); - Mono::delay(10000.millis()).await; - } - } - - /** - * Sends a message to the radio over UART. - */ - #[task(priority = 3, shared = [&em, radio_manager])] - async fn send_gs(mut cx: send_gs::Context, m: Message) { - // info!("{}", m.clone()); - - cx.shared.radio_manager.lock(|radio_manager| { - cx.shared.em.run(|| { - // info!("Sending message {}", m); - let mut buf = [0; 255]; - let data = postcard::to_slice(&m, &mut buf)?; - radio_manager.send_message(data)?; - Ok(()) - }) - }); - } - #[task( priority = 3, binds = FDCAN2_IT0, shared = [&em, can_data_manager, data_manager])] fn can_data(mut cx: can_data::Context) { cx.shared.can_data_manager.lock(|can| { { - cx.shared.em.run(|| { - can.process_data()?; - Ok(()) + cx.shared.data_manager.lock(|data_manager| { + cx.shared.em.run(|| { + can.process_data(data_manager)?; + Ok(()) + }) }) } }); @@ -568,50 +399,29 @@ mod app { #[task(priority = 2, shared = [&em, can_command_manager, data_manager])] async fn send_command_internal(mut cx: send_command_internal::Context, m: Message) { - // while let Ok(m) = receiver.recv().await { cx.shared.can_command_manager.lock(|can| { cx.shared.em.run(|| { can.send_message(m)?; Ok(()) }) }); - // } } - #[task(priority = 1, local = [led_red, led_green, buzzer, buzzed: bool = false], shared = [&em])] + #[task(priority = 1, local = [led_red, led_green], shared = [&em])] async fn blink(cx: blink::Context) { loop { if cx.shared.em.has_error() { cx.local.led_red.toggle(); - if *cx.local.buzzed { - cx.local.buzzer.set_duty(0); - *cx.local.buzzed = false; - } else { - let duty = cx.local.buzzer.get_max_duty() / 4; - cx.local.buzzer.set_duty(duty); - *cx.local.buzzed = true; - } Mono::delay(500.millis()).await; } else { cx.local.led_green.toggle(); - if *cx.local.buzzed { - cx.local.buzzer.set_duty(0); - *cx.local.buzzed = false; - } else { - let duty = cx.local.buzzer.get_max_duty() / 4; - cx.local.buzzer.set_duty(duty); - *cx.local.buzzed = true; - } Mono::delay(2000.millis()).await; } } } - #[task(priority = 3, shared = [&em, sbg_power])] - async fn sleep_system(mut cx: sleep_system::Context) { - // Turn off the SBG and CAN, also start a timer to wake up the system. Put the chip in sleep mode. - cx.shared.sbg_power.lock(|sbg| { - sbg.set_low(); - }); + #[task(priority = 3, shared = [&em])] + async fn sleep_system(_cx: sleep_system::Context) { + // in here we can stop the ADCs. } } diff --git a/crates/thermocouple-converter/src/lib.rs b/crates/thermocouple-converter/src/lib.rs index 5c2e9df..f88c34e 100644 --- a/crates/thermocouple-converter/src/lib.rs +++ b/crates/thermocouple-converter/src/lib.rs @@ -3,13 +3,56 @@ //! This crate contains code to convert type k thermocouple voltages to temperatures. //! +///Ranges where polynomial coefficients are for +pub const ENERGY_RANGES: [[f64; 2]; 3] = [[-5.891, 0.0], [0.0, 20.644], [20.644, 54.886]]; + +///Type K thermocouple coefficients for polynomial calculation +pub const TYPE_K_COEF: [[f64; 10]; 3] = [ + [ + 0.0, + 25.173462, + -1.1662878, + -1.0833638, + -0.89773540, + -0.37342377, + -0.086632643, + -0.010450598, + -0.00051920577, + 0.0, + ], + [ + 0.0, + 25.08355, + 0.07860106, + -0.2503131, + 0.08315270, + -0.01228034, + 0.0009804036, + -0.00004413030, + 0.000001057734, + -0.00000001052755, + ], + [ + -131.8058, + 48.30222, + -1.646031, + 0.05464731, + -0.0009650715, + 0.000008802193, + -0.00000003110810, + 0.0, + 0.0, + 0.0, + ], +]; + ///function for power /// in: base: f64, exp: i32 /// out: result: f64 fn pow(base: f64, exp: i32) -> f64 { let mut result = 1.0; if exp < 0 { - result = 1.0/pow(base, -exp); + result = 1.0 / pow(base, -exp); } else { for _ in 0..exp { result *= base; @@ -22,36 +65,21 @@ fn pow(base: f64, exp: i32) -> f64 { ///in: voltage: f64 ///out: celsius: f64 pub fn voltage_to_celsius(voltage: f64) -> f64 { - - ///Ranges where polynomial coefficients are for - pub const ENERGY_RANGES: [[f64;2];3] = [ - [-5.891, 0.0], - [0.0, 20.644], - [20.644, 54.886] - ]; - - ///Type K thermocouple coefficients for polynomial calculation - pub const TYPE_K_COEF: [[f64; 10]; 3] = [ - [0.0,25.173462,-1.1662878,-1.0833638,-0.89773540,-0.37342377,-0.086632643,-0.010450598,-0.00051920577,0.0], - [0.0,25.08355,0.07860106,-0.2503131,0.08315270,-0.01228034,0.0009804036,-0.00004413030,0.000001057734,-0.00000001052755], - [-131.8058,48.30222,-1.646031,0.05464731,-0.0009650715,0.000008802193,-0.00000003110810,0.0,0.0,0.0] - ]; - //define variables let mut result = 0.0; let mut i = 0; //goes through the different ranges - while i < ENERGY_RANGES.len(){ + while i < ENERGY_RANGES.len() { if voltage >= ENERGY_RANGES[i][0] && voltage <= ENERGY_RANGES[i][1] { //calculates the result for k in 0..TYPE_K_COEF[i].len() { result += TYPE_K_COEF[i][k] * pow(voltage, k as i32); } return result; - }else{ + } else { //if the voltage is not in the range, it goes to the next range - i+=1; + i += 1; } } @@ -66,23 +94,23 @@ mod tests { #[test] fn voltage_to_celsius_test1() { //println!("Test 1: {}", voltage_to_celsius(20.644)); - let result:f64 = voltage_to_celsius(20.644); + let result: f64 = voltage_to_celsius(20.644); assert!(499.97 <= result && 500.0 >= result); // println!("Test 2: {}", voltage_to_celsius(6.138)); - let result:f64 = voltage_to_celsius(6.138); + let result: f64 = voltage_to_celsius(6.138); assert!(150.01 <= result && 150.03 >= result); // println!("Test 3: {}", voltage_to_celsius(0.039)); - let result:f64 = voltage_to_celsius(0.039); + let result: f64 = voltage_to_celsius(0.039); assert!(0.97 <= result && 0.98 >= result); // println!("Test 4: {}", voltage_to_celsius(-0.778)); - let result:f64 = voltage_to_celsius(-0.778); + let result: f64 = voltage_to_celsius(-0.778); assert!(-20.03 <= result && -20.01 >= result); - + // println!("Test 5: {}", voltage_to_celsius(10.0)); - let result:f64 = voltage_to_celsius(10.0); + let result: f64 = voltage_to_celsius(10.0); assert!(246.1 <= result && 246.3 >= result); } -} \ No newline at end of file +}