From 98f64d399156b22f85866dd9082a0ae16f48a601 Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Tue, 19 Mar 2024 10:37:08 -0400 Subject: [PATCH 01/47] Add blink task and rename sensor_v2 to nav --- Cargo.lock | 39 ++++--- boards/{sensor_v2 => nav}/Cargo.toml | 4 +- .../{sensor_v2 => nav}/src/communication.rs | 0 boards/{sensor_v2 => nav}/src/data_manager.rs | 0 boards/nav/src/main.rs | 107 ++++++++++++++++++ boards/{sensor_v2 => nav}/src/sbg_manager.rs | 0 boards/{sensor_v2 => nav}/src/types.rs | 0 boards/sensor_v2/src/main.rs | 71 ------------ 8 files changed, 135 insertions(+), 86 deletions(-) rename boards/{sensor_v2 => nav}/Cargo.toml (88%) rename boards/{sensor_v2 => nav}/src/communication.rs (100%) rename boards/{sensor_v2 => nav}/src/data_manager.rs (100%) create mode 100644 boards/nav/src/main.rs rename boards/{sensor_v2 => nav}/src/sbg_manager.rs (100%) rename boards/{sensor_v2 => nav}/src/types.rs (100%) delete mode 100644 boards/sensor_v2/src/main.rs diff --git a/Cargo.lock b/Cargo.lock index 9023e0c6..b213cffb 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -777,6 +777,7 @@ dependencies = [ "fugit", "heapless", "mavlink", + "messages-proc-macros-lib", "postcard", "proptest", "proptest-derive", @@ -784,6 +785,14 @@ dependencies = [ "ts-rs", ] +[[package]] +name = "messages-proc-macros-lib" +version = "0.1.0" +dependencies = [ + "quote 1.0.35", + "serde", +] + [[package]] name = "modular-bitfield" version = "0.11.2" @@ -805,6 +814,22 @@ dependencies = [ "syn 1.0.109", ] +[[package]] +name = "nav" +version = "0.1.0" +dependencies = [ + "common-arm", + "common-arm-stm32h7", + "cortex-m", + "cortex-m-rt", + "cortex-m-rtic", + "defmt", + "messages", + "postcard", + "stm32h7xx-hal", + "systick-monotonic", +] + [[package]] name = "nb" version = "0.1.3" @@ -1307,20 +1332,6 @@ dependencies = [ "typenum", ] -[[package]] -name = "sensor_v2" -version = "0.1.0" -dependencies = [ - "common-arm", - "common-arm-stm32h7", - "cortex-m", - "cortex-m-rt", - "cortex-m-rtic", - "messages", - "postcard", - "stm32h7xx-hal", -] - [[package]] name = "seq-macro" version = "0.3.5" diff --git a/boards/sensor_v2/Cargo.toml b/boards/nav/Cargo.toml similarity index 88% rename from boards/sensor_v2/Cargo.toml rename to boards/nav/Cargo.toml index 4c0710e9..c6c13b4c 100644 --- a/boards/sensor_v2/Cargo.toml +++ b/boards/nav/Cargo.toml @@ -1,5 +1,5 @@ [package] -name = "sensor_v2" +name = "nav" version = "0.1.0" edition = "2021" @@ -14,3 +14,5 @@ common-arm = {path = "../../libraries/common-arm"} stm32h7xx-hal = { workspace = true } postcard = "1.0.2" messages = { path = "../../libraries/messages" } +systick-monotonic = "1.0.1" +defmt = "0.3.2" diff --git a/boards/sensor_v2/src/communication.rs b/boards/nav/src/communication.rs similarity index 100% rename from boards/sensor_v2/src/communication.rs rename to boards/nav/src/communication.rs diff --git a/boards/sensor_v2/src/data_manager.rs b/boards/nav/src/data_manager.rs similarity index 100% rename from boards/sensor_v2/src/data_manager.rs rename to boards/nav/src/data_manager.rs diff --git a/boards/nav/src/main.rs b/boards/nav/src/main.rs new file mode 100644 index 00000000..21dd83db --- /dev/null +++ b/boards/nav/src/main.rs @@ -0,0 +1,107 @@ +#![no_std] +#![no_main] + +mod data_manager; + +use common_arm::*; +use data_manager::DataManager; +use defmt::info; +use stm32h7xx_hal::gpio::gpioa::{PA1, PA2}; +use stm32h7xx_hal::gpio::gpioc::{PC13, PC3}; +use stm32h7xx_hal::gpio::Input; +use stm32h7xx_hal::gpio::{Output, PushPull}; +use stm32h7xx_hal::prelude::*; +use systick_monotonic::*; + +/// Custom panic handler. +/// Reset the system if a panic occurs. +#[panic_handler] +fn panic(info: &core::panic::PanicInfo) -> ! { + stm32h7xx_hal::pac::SCB::sys_reset(); +} + +#[rtic::app(device = stm32h7xx_hal::stm32, dispatchers = [EXTI0, EXTI1])] +mod app { + use super::*; + + #[shared] + struct SharedResources { + data_manager: DataManager, + em: ErrorManager, + } + #[local] + struct LocalResources { + led_red: PA1>, + led_green: PA2>, + } + + #[monotonic(binds = SysTick, default = true)] + type SysMono = Systick<100>; // 100 Hz / 10 ms granularity + + #[init] + fn init(mut ctx: init::Context) -> (SharedResources, LocalResources, init::Monotonics) { + let core = ctx.core; + + 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 pwrcfg = pwr.freeze(); + + // RCC + let rcc = ctx.device.RCC.constrain(); + let ccdr = rcc + .use_hse(48.MHz()) + .sys_ck(300.MHz()) + .freeze(pwrcfg, &ctx.device.SYSCFG); + + // GPIO + let gpioc = ctx.device.GPIOC.split(ccdr.peripheral.GPIOC); + let gpioa = ctx.device.GPIOA.split(ccdr.peripheral.GPIOA); + + // leds + let mut led_red = gpioa.pa1.into_push_pull_output(); + let mut led_green = gpioa.pa2.into_push_pull_output(); + + blink::spawn().ok(); + + /* Monotonic clock */ + let mono = Systick::new(core.SYST, ccdr.clocks.hclk().raw()); + + info!("Online"); + + ( + SharedResources { + data_manager: DataManager::new(), + em: ErrorManager::new(), + }, + LocalResources { led_red, led_green }, + init::Monotonics(mono), + ) + } + + #[idle] + fn idle(mut ctx: idle::Context) -> ! { + loop { + cortex_m::asm::nop(); // do nothing. just keep the MCU running + } + } + + #[task(local = [led_red, led_green], shared = [&em])] + fn blink(mut cx: blink::Context) { + cx.shared.em.run(|| { + if cx.shared.em.has_error() { + cx.local.led_red.toggle(); + spawn_after!(blink, ExtU64::millis(200))?; + } else { + cx.local.led_green.toggle(); + spawn_after!(blink, ExtU64::secs(1))?; + } + Ok(()) + }); + } + + #[task(shared = [&em])] + fn sleep_system(mut cx: sleep_system::Context) { + // Turn off the SBG and CAN + } +} diff --git a/boards/sensor_v2/src/sbg_manager.rs b/boards/nav/src/sbg_manager.rs similarity index 100% rename from boards/sensor_v2/src/sbg_manager.rs rename to boards/nav/src/sbg_manager.rs diff --git a/boards/sensor_v2/src/types.rs b/boards/nav/src/types.rs similarity index 100% rename from boards/sensor_v2/src/types.rs rename to boards/nav/src/types.rs diff --git a/boards/sensor_v2/src/main.rs b/boards/sensor_v2/src/main.rs deleted file mode 100644 index c1e00f58..00000000 --- a/boards/sensor_v2/src/main.rs +++ /dev/null @@ -1,71 +0,0 @@ -#![no_std] -#![no_main] - -mod data_manager; - -use data_manager::DataManager; - -use stm32h7xx_hal::gpio::gpioc::{PC13, PC3}; -use stm32h7xx_hal::gpio::Input; -use stm32h7xx_hal::gpio::{Output, PushPull}; -use stm32h7xx_hal::prelude::*; - -/// Custom panic handler. -/// Reset the system if a panic occurs. -#[panic_handler] -fn panic(info: &core::panic::PanicInfo) -> ! { - stm32h7xx_hal::pac::SCB::sys_reset(); -} - -#[rtic::app(device = stm32h7xx_hal::stm32, dispatchers = [EXTI0, EXTI1])] -mod app { - use super::*; - - #[shared] - struct SharedResources { - data_manager: DataManager, - } - #[local] - struct LocalResources { - button: PC13, - led: PC3>, - } - - #[init] - fn init(mut ctx: init::Context) -> (SharedResources, LocalResources, init::Monotonics) { - 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 pwrcfg = pwr.freeze(); - - // RCC - let rcc = ctx.device.RCC.constrain(); - let ccdr = rcc.sys_ck(100.MHz()).freeze(pwrcfg, &ctx.device.SYSCFG); - - // GPIO - let gpioc = ctx.device.GPIOC.split(ccdr.peripheral.GPIOC); - - // Button - let mut button = gpioc.pc13.into_floating_input(); - ( - SharedResources { - data_manager: DataManager::new(), - }, - LocalResources { - button, - led: gpioc.pc3.into_push_pull_output(), - }, - init::Monotonics(), - ) - } - - #[idle] - fn idle(mut ctx: idle::Context) -> ! { - loop {} - } - - #[task(local = [button, led])] - fn sleep_system(mut cx: sleep_system::Context) { - // Turn off the SBG and CAN - } -} From 675e473a2748ec191a293e7fc69256513c30ace8 Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Mon, 8 Jul 2024 00:37:12 -0400 Subject: [PATCH 02/47] WIP: Test pinout compatibility. --- Cargo.lock | 17 ++++++++ Cargo.toml | 20 +++------ boards/nav/Cargo.toml | 5 ++- boards/nav/src/main.rs | 59 ++++++++++++++++++++++++++ libraries/common-arm-atsame/Cargo.toml | 4 +- libraries/common-arm-atsame/src/lib.rs | 2 + libraries/common-arm/Cargo.toml | 2 +- libraries/common-arm/src/lib.rs | 2 - 8 files changed, 92 insertions(+), 19 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index b213cffb..4aa63196 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -257,6 +257,7 @@ dependencies = [ "atsamd-hal", "common-arm", "embedded-hal", + "mcan", ] [[package]] @@ -580,6 +581,20 @@ version = "2.0.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "25cbce373ec4653f1a01a31e8a5e5ec0c622dc27ff9c4e6606eefef5cbbed4a5" +[[package]] +name = "fdcan" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "553fb8420a8ca433ed032a3f284deeb95c12e4d3d25cdc15196669a27b2ae34b" +dependencies = [ + "bitflags 1.3.2", + "nb 1.1.0", + "paste", + "static_assertions", + "vcell", + "volatile-register", +] + [[package]] name = "fnv" version = "1.0.7" @@ -824,6 +839,7 @@ dependencies = [ "cortex-m-rt", "cortex-m-rtic", "defmt", + "fdcan", "messages", "postcard", "stm32h7xx-hal", @@ -1458,6 +1474,7 @@ dependencies = [ "embedded-dma", "embedded-hal", "embedded-storage", + "fdcan", "fugit", "nb 1.1.0", "paste", diff --git a/Cargo.toml b/Cargo.toml index 4984755c..137adc2c 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,26 +1,19 @@ [workspace] resolver = "2" -members = [ - "boards/*", - "examples/*", - "libraries/*" -] +members = ["boards/*", "examples/*", "libraries/*"] # Specify which members to build by default. Some libraries, such as messages, contain dev-dependencies that will give # compile errors if built directly. -default-members = [ - "boards/*", - "examples/*" -] +default-members = ["boards/*", "examples/*"] [workspace.dependencies.embedded-hal] -version="0.2.7" +version = "0.2.7" [workspace.dependencies.stm32h7xx-hal] -git = "https://github.com/stm32-rs/stm32h7xx-hal" +git = "https://github.com/stm32-rs/stm32h7xx-hal" # We use 35 even though we have the 33. -features = ["defmt", "rt", "stm32h735" ] +features = ["defmt", "rt", "stm32h735", "can"] [workspace.dependencies.atsamd-hal] git = "https://github.com/uorocketry/atsamd" @@ -70,4 +63,5 @@ opt-level = 0 #Try to remove this [profile.release.build-override] -#opt-level = 0 \ No newline at end of file +#opt-level = 0 + diff --git a/boards/nav/Cargo.toml b/boards/nav/Cargo.toml index c6c13b4c..9398a12a 100644 --- a/boards/nav/Cargo.toml +++ b/boards/nav/Cargo.toml @@ -9,10 +9,11 @@ edition = "2021" cortex-m = { workspace = true } cortex-m-rt = "^0.7.0" cortex-m-rtic = "1.1.3" -common-arm-stm32h7 = {path = "../../libraries/common-arm-stm32h7"} -common-arm = {path = "../../libraries/common-arm"} +common-arm-stm32h7 = { path = "../../libraries/common-arm-stm32h7" } +common-arm = { path = "../../libraries/common-arm" } stm32h7xx-hal = { workspace = true } postcard = "1.0.2" messages = { path = "../../libraries/messages" } systick-monotonic = "1.0.1" defmt = "0.3.2" +fdcan = "0.2" diff --git a/boards/nav/src/main.rs b/boards/nav/src/main.rs index 21dd83db..138c6d07 100644 --- a/boards/nav/src/main.rs +++ b/boards/nav/src/main.rs @@ -4,13 +4,24 @@ mod data_manager; use common_arm::*; +use core::num::{NonZeroU16, NonZeroU8}; use data_manager::DataManager; use defmt::info; +use fdcan::{ + config::NominalBitTiming, + filter::{StandardFilter, StandardFilterSlot}, + frame::{FrameFormat, TxFrameHeader}, + id::StandardId, +}; use stm32h7xx_hal::gpio::gpioa::{PA1, PA2}; use stm32h7xx_hal::gpio::gpioc::{PC13, PC3}; use stm32h7xx_hal::gpio::Input; +use stm32h7xx_hal::gpio::Speed; use stm32h7xx_hal::gpio::{Output, PushPull}; +use stm32h7xx_hal::pac; use stm32h7xx_hal::prelude::*; +use stm32h7xx_hal::spi; +use stm32h7xx_hal::{rcc, rcc::rec}; use systick_monotonic::*; /// Custom panic handler. @@ -52,16 +63,64 @@ mod app { let ccdr = rcc .use_hse(48.MHz()) .sys_ck(300.MHz()) + .pll1_strategy(rcc::PllConfigStrategy::Iterative) + .pll1_q_ck(24.MHz()) .freeze(pwrcfg, &ctx.device.SYSCFG); + let btr = NominalBitTiming { + prescaler: NonZeroU16::new(12).unwrap(), + seg1: NonZeroU8::new(13).unwrap(), + seg2: NonZeroU8::new(2).unwrap(), + sync_jump_width: NonZeroU8::new(1).unwrap(), + }; + // GPIO let gpioc = ctx.device.GPIOC.split(ccdr.peripheral.GPIOC); 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); + assert_eq!(ccdr.clocks.pll1_q_ck().unwrap().raw(), 24_000_000); + let fdcan_prec = ccdr + .peripheral + .FDCAN + .kernel_clk_mux(rec::FdcanClkSel::Pll1Q); + + let can1 = { + 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 = can1; + can.set_protocol_exception_handling(false); + + can.set_nominal_bit_timing(btr); + + can.set_standard_filter( + StandardFilterSlot::_0, + StandardFilter::accept_all_into_fifo0(), + ); + + let (sck, miso, mosi) = (gpioa.pa5, gpioa.pa6, gpioa.pa7); + + let spi = ctx + .device + .SPI1 + .spi((sck, miso, mosi), spi::MODE_0, 1.MHz(), &ccdr.clocks); // leds let mut led_red = gpioa.pa1.into_push_pull_output(); let mut led_green = gpioa.pa2.into_push_pull_output(); + // UART for sbg + let tx = gpiod.pd1.into_alternate(); + let rx = gpiod.pd0.into_alternate(); + + let serial = ctx + .device + .UART4 + .serial((tx, rx), 9_800.bps(), ccdr.peripheral.UART4, &ccdr.clocks) + .unwrap(); blink::spawn().ok(); /* Monotonic clock */ diff --git a/libraries/common-arm-atsame/Cargo.toml b/libraries/common-arm-atsame/Cargo.toml index eb199ecf..1b98028f 100644 --- a/libraries/common-arm-atsame/Cargo.toml +++ b/libraries/common-arm-atsame/Cargo.toml @@ -8,4 +8,6 @@ edition = "2021" [dependencies] common-arm = { path = "../common-arm" } atsamd-hal = { workspace = true } -embedded-hal = {workspace = true } \ No newline at end of file +embedded-hal = { workspace = true } +mcan = "0.3" + diff --git a/libraries/common-arm-atsame/src/lib.rs b/libraries/common-arm-atsame/src/lib.rs index 701862fe..b2649251 100644 --- a/libraries/common-arm-atsame/src/lib.rs +++ b/libraries/common-arm-atsame/src/lib.rs @@ -4,3 +4,5 @@ //! This crate contains common code for HYDRA. Any code that is not board specific but is ATSAME specific should be put in //! here. //! + +pub use mcan; diff --git a/libraries/common-arm/Cargo.toml b/libraries/common-arm/Cargo.toml index a44ef2f6..276dd48c 100644 --- a/libraries/common-arm/Cargo.toml +++ b/libraries/common-arm/Cargo.toml @@ -17,6 +17,6 @@ derive_more = "0.99.17" cortex-m-rt = "^0.7.0" embedded-sdmmc = "0.3.0" embedded-hal = "0.2.7" -mcan = "0.3" nb = "1.1.0" +mcan = "0.3.0" messages = { path = "../messages" } diff --git a/libraries/common-arm/src/lib.rs b/libraries/common-arm/src/lib.rs index 83adaaf1..8209122f 100644 --- a/libraries/common-arm/src/lib.rs +++ b/libraries/common-arm/src/lib.rs @@ -6,8 +6,6 @@ //! here. //! -pub use mcan; - mod error; mod health; mod logging; From 030349839b71269cbd05d6568d97e110af7c5f85 Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Mon, 8 Jul 2024 01:52:22 -0400 Subject: [PATCH 03/47] Fix: MCAN moved as STM32 introduces new CAN library causing symbol issues. --- boards/camera/src/communication.rs | 4 ++-- boards/camera/src/main.rs | 2 +- boards/communication/src/communication.rs | 6 +++--- boards/communication/src/main.rs | 2 +- boards/nav/src/main.rs | 21 +++++++++++++++------ boards/power/src/communication.rs | 2 +- boards/power/src/main.rs | 2 +- boards/recovery/src/communication.rs | 2 +- boards/recovery/src/main.rs | 2 +- boards/sensor/src/communication.rs | 2 +- boards/sensor/src/main.rs | 2 +- 11 files changed, 28 insertions(+), 19 deletions(-) diff --git a/boards/camera/src/communication.rs b/boards/camera/src/communication.rs index cb30c676..330e5368 100644 --- a/boards/camera/src/communication.rs +++ b/boards/camera/src/communication.rs @@ -8,8 +8,8 @@ use atsamd_hal::clock::v2::Source; use atsamd_hal::gpio::{Alternate, AlternateI, Pin, I, PA22, PA23}; use atsamd_hal::pac::CAN0; use atsamd_hal::typelevel::Increment; -use common_arm::mcan; -use common_arm::mcan::message::{rx, Raw}; +use common_arm_atsame::mcan; +use common_arm_atsame::mcan::message::{rx, Raw}; use defmt::info; use mcan::bus::Can; use mcan::embedded_can as ecan; diff --git a/boards/camera/src/main.rs b/boards/camera/src/main.rs index a99c7e69..49bda653 100644 --- a/boards/camera/src/main.rs +++ b/boards/camera/src/main.rs @@ -13,9 +13,9 @@ use crate::state_machine::{StateMachine, StateMachineContext}; use atsamd_hal as hal; use atsamd_hal::clock::v2::pclk::Pclk; use atsamd_hal::clock::v2::Source; -use common_arm::mcan; use common_arm::ErrorManager; use common_arm::*; +use common_arm_atsame::mcan; use communication::Capacities; use hal::gpio::Pins; use hal::gpio::{Pin, PushPullOutput, PB16, PB17}; diff --git a/boards/communication/src/communication.rs b/boards/communication/src/communication.rs index 1ea183af..d56b5736 100644 --- a/boards/communication/src/communication.rs +++ b/boards/communication/src/communication.rs @@ -17,10 +17,10 @@ use atsamd_hal::sercom::uart; use atsamd_hal::sercom::uart::Uart; use atsamd_hal::sercom::uart::{RxDuplex, TxDuplex}; use atsamd_hal::typelevel::Increment; -use common_arm::mcan; -use common_arm::mcan::message::{rx, Raw}; -use common_arm::mcan::tx_buffers::DynTx; use common_arm::{herror, HydraError}; +use common_arm_atsame::mcan; +use common_arm_atsame::mcan::message::{rx, Raw}; +use common_arm_atsame::mcan::tx_buffers::DynTx; use heapless::HistoryBuffer; use heapless::Vec; use mavlink::embedded::Read; diff --git a/boards/communication/src/main.rs b/boards/communication/src/main.rs index dcd1ea95..f96923a3 100644 --- a/boards/communication/src/main.rs +++ b/boards/communication/src/main.rs @@ -7,11 +7,11 @@ mod health; mod types; use atsamd_hal as hal; -use common_arm::mcan; use common_arm::HealthManager; use common_arm::HealthMonitor; use common_arm::SdManager; use common_arm::*; +use common_arm_atsame::mcan; use communication::Capacities; use communication::{RadioDevice, RadioManager}; use data_manager::DataManager; diff --git a/boards/nav/src/main.rs b/boards/nav/src/main.rs index 138c6d07..14bca5a1 100644 --- a/boards/nav/src/main.rs +++ b/boards/nav/src/main.rs @@ -101,12 +101,21 @@ mod app { StandardFilter::accept_all_into_fifo0(), ); - let (sck, miso, mosi) = (gpioa.pa5, gpioa.pa6, gpioa.pa7); - - let spi = ctx - .device - .SPI1 - .spi((sck, miso, mosi), spi::MODE_0, 1.MHz(), &ccdr.clocks); + let spi: 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), + 1.MHz(), + ccdr.peripheral.SPI1, + &ccdr.clocks, + ); // leds let mut led_red = gpioa.pa1.into_push_pull_output(); diff --git a/boards/power/src/communication.rs b/boards/power/src/communication.rs index 4d42abf2..532bb563 100644 --- a/boards/power/src/communication.rs +++ b/boards/power/src/communication.rs @@ -1,7 +1,7 @@ use crate::data_manager::DataManager; use atsamd_hal as hal; -use common_arm::mcan; use common_arm::HydraError; +use common_arm_atsame::mcan; use defmt::info; use hal::can::Dependencies; use hal::clock::v2::ahb::AhbClk; diff --git a/boards/power/src/main.rs b/boards/power/src/main.rs index 405b1df4..17a62d55 100644 --- a/boards/power/src/main.rs +++ b/boards/power/src/main.rs @@ -7,8 +7,8 @@ mod types; use crate::data_manager::DataManager; use atsamd_hal as hal; -use common_arm::mcan; use common_arm::*; +use common_arm_atsame::mcan; use communication::CanDevice0; use communication::Capacities; use hal::clock::v2::pclk::Pclk; diff --git a/boards/recovery/src/communication.rs b/boards/recovery/src/communication.rs index 4d667b7b..50d2e2a2 100644 --- a/boards/recovery/src/communication.rs +++ b/boards/recovery/src/communication.rs @@ -8,8 +8,8 @@ use atsamd_hal::clock::v2::Source; use atsamd_hal::gpio::{Alternate, AlternateI, Pin, I, PA22, PA23}; use atsamd_hal::pac::CAN0; use atsamd_hal::typelevel::Increment; -use common_arm::mcan; use common_arm::HydraError; +use common_arm_atsame::mcan; use defmt::info; use heapless::Vec; use mcan::bus::Can; diff --git a/boards/recovery/src/main.rs b/boards/recovery/src/main.rs index bd08c8f8..c5018081 100644 --- a/boards/recovery/src/main.rs +++ b/boards/recovery/src/main.rs @@ -12,8 +12,8 @@ use atsamd_hal::clock::v2::pclk::Pclk; use atsamd_hal::clock::v2::Source; use atsamd_hal::dmac::DmaController; use common_arm::hinfo; -use common_arm::mcan; use common_arm::*; +use common_arm_atsame::mcan; use communication::Capacities; use data_manager::DataManager; use gpio_manager::GPIOManager; diff --git a/boards/sensor/src/communication.rs b/boards/sensor/src/communication.rs index 2ef700ca..c2e059a2 100644 --- a/boards/sensor/src/communication.rs +++ b/boards/sensor/src/communication.rs @@ -9,8 +9,8 @@ use atsamd_hal::gpio::{Alternate, AlternateI, Pin, I, PA22, PA23}; use atsamd_hal::pac::CAN0; use atsamd_hal::typelevel::Increment; -use common_arm::mcan; use common_arm::HydraError; +use common_arm_atsame::mcan; use defmt::info; use heapless::Vec; use mcan::bus::Can; diff --git a/boards/sensor/src/main.rs b/boards/sensor/src/main.rs index 96d1eea2..163822ee 100644 --- a/boards/sensor/src/main.rs +++ b/boards/sensor/src/main.rs @@ -10,9 +10,9 @@ use atsamd_hal as hal; use atsamd_hal::clock::v2::pclk::Pclk; use atsamd_hal::clock::v2::Source; use atsamd_hal::dmac::DmaController; -use common_arm::mcan; use common_arm::SdManager; use common_arm::*; +use common_arm_atsame::mcan; use communication::Capacities; use data_manager::DataManager; use defmt::info; From e3a0dca683c84630d852598b34e6a0281f77befd Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Fri, 19 Jul 2024 20:52:34 -0400 Subject: [PATCH 04/47] Update sbg-rs crate. --- boards/nav/src/main.rs | 2 + boards/nav/src/sbg_manager.rs | 207 ++++++++++++++++++++++++++++++++++ boards/nav/src/types.rs | 5 + libraries/sbg-rs/src/sbg.rs | 71 +++++------- 4 files changed, 242 insertions(+), 43 deletions(-) diff --git a/boards/nav/src/main.rs b/boards/nav/src/main.rs index 14bca5a1..ed05aeed 100644 --- a/boards/nav/src/main.rs +++ b/boards/nav/src/main.rs @@ -2,6 +2,7 @@ #![no_main] mod data_manager; +mod types; use common_arm::*; use core::num::{NonZeroU16, NonZeroU8}; @@ -23,6 +24,7 @@ use stm32h7xx_hal::prelude::*; use stm32h7xx_hal::spi; use stm32h7xx_hal::{rcc, rcc::rec}; use systick_monotonic::*; +use types::SBGSerial; /// Custom panic handler. /// Reset the system if a panic occurs. diff --git a/boards/nav/src/sbg_manager.rs b/boards/nav/src/sbg_manager.rs index e69de29b..25b8e259 100644 --- a/boards/nav/src/sbg_manager.rs +++ b/boards/nav/src/sbg_manager.rs @@ -0,0 +1,207 @@ +use crate::types::{ConfigSBG, SBGBuffer, SBGTransfer}; +use atsamd_hal::clock::v2::gclk::Gclk0Id; +use atsamd_hal::clock::v2::pclk::Pclk; +use atsamd_hal::dmac; +use atsamd_hal::dmac::Transfer; +use atsamd_hal::gpio::{Pin, Reset, PB02, PB03}; +use atsamd_hal::pac::{MCLK, RTC}; +use atsamd_hal::sercom::IoSet6; +// use atsamd_hal::prelude::_atsamd21_hal_time_U32Ext; +use atsamd_hal::rtc::Rtc; +use core::alloc::{GlobalAlloc, Layout}; +use core::ffi::c_void; +use core::mem::size_of; +use core::ptr; +// use atsamd_hal::time::*; +use crate::app::sbg_handle_data; +use crate::app::sbg_sd_task as sbg_sd; +use atsamd_hal::prelude::*; +use atsamd_hal::sercom::{uart, Sercom, Sercom5}; +use common_arm::spawn; +use defmt::info; +use embedded_alloc::Heap; +use rtic::Mutex; +use sbg_rs::sbg; +use sbg_rs::sbg::{CallbackData, SBG, SBG_BUFFER_SIZE}; + +pub static mut BUF_DST: SBGBuffer = &mut [0; SBG_BUFFER_SIZE]; + +// Simple heap required by the SBG library +static HEAP: Heap = Heap::empty(); + +pub struct SBGManager { + sbg_device: SBG, + xfer: Option, +} + +impl SBGManager { + pub fn new( + rx: Pin, + tx: Pin, + pclk_sercom5: Pclk, + mclk: &mut MCLK, + sercom5: Sercom5, + rtc: RTC, + mut dma_channel: dmac::Channel, + ) -> Self { + /* Initialize the Heap */ + { + use core::mem::MaybeUninit; + const HEAP_SIZE: usize = 1024; + static mut HEAP_MEM: [MaybeUninit; HEAP_SIZE] = [MaybeUninit::uninit(); HEAP_SIZE]; + unsafe { HEAP.init(HEAP_MEM.as_ptr() as usize, HEAP_SIZE) } + } + + let pads_sbg = uart::Pads::::default().rx(rx).tx(tx); + let uart_sbg = ConfigSBG::new(mclk, sercom5, pads_sbg, pclk_sercom5.freq()) + .baud( + 115200.Hz(), + uart::BaudMode::Fractional(uart::Oversampling::Bits8), + ) + .enable(); + + let (sbg_rx, sbg_tx) = uart_sbg.split(); + + /* DMAC config */ + dma_channel + .as_mut() + .enable_interrupts(dmac::InterruptFlags::new().with_tcmpl(true)); + let xfer = Transfer::new(dma_channel, sbg_rx, unsafe { &mut *BUF_DST }, false) + .expect("DMA err") + .begin(Sercom5::DMA_RX_TRIGGER, dmac::TriggerAction::BURST); + + // There is a bug within the HAL that improperly configures the RTC + // in count32 mode. This is circumvented by first using clock mode then + // converting to count32 mode. + let rtc_temp = Rtc::clock_mode(rtc, 1024.Hz(), mclk); + let mut rtc = rtc_temp.into_count32_mode(); + rtc.set_count32(0); + + let sbg: sbg::SBG = sbg::SBG::new(sbg_tx, rtc, |data| { + sbg_handle_data::spawn(data).ok(); + }); + + SBGManager { + sbg_device: sbg, + xfer: Some(xfer), + } + } +} + +pub fn sbg_handle_data(mut cx: sbg_handle_data::Context, data: CallbackData) { + cx.shared.data_manager.lock(|manager| match data { + CallbackData::UtcTime(x) => manager.utc_time = Some(x), + CallbackData::Air(x) => manager.air = Some(x), + CallbackData::EkfQuat(x) => manager.ekf_quat = Some(x), + CallbackData::EkfNav(x) => manager.ekf_nav = Some(x), + CallbackData::Imu(x) => manager.imu = Some(x), + CallbackData::GpsVel(x) => manager.gps_vel = Some(x), + CallbackData::GpsPos(x) => manager.gps_pos = Some(x), + }); +} + +pub fn sbg_sd_task(mut cx: crate::app::sbg_sd_task::Context, data: [u8; SBG_BUFFER_SIZE]) { + cx.shared.sd_manager.lock(|manager| { + if let Some(mut file) = manager.file.take() { + cx.shared.em.run(|| { + manager.write(&mut file, &data)?; + Ok(()) + }); + manager.file = Some(file); // give the file back after use + } else if let Ok(mut file) = manager.open_file("sbg.txt") { + cx.shared.em.run(|| { + manager.write(&mut file, &data)?; + Ok(()) + }); + manager.file = Some(file); + } + }); +} +/** + * Handles the DMA interrupt. + * Handles the SBG data. + */ +pub fn sbg_dma(cx: crate::app::sbg_dma::Context) { + let sbg = cx.local.sbg_manager; + + match &mut sbg.xfer { + Some(xfer) => { + if xfer.complete() { + let (chan0, source, buf) = sbg.xfer.take().unwrap().stop(); + let mut xfer = dmac::Transfer::new(chan0, source, unsafe { &mut *BUF_DST }, false) + .unwrap() + .begin(Sercom5::DMA_RX_TRIGGER, dmac::TriggerAction::BURST); + let buf_clone = buf.clone(); + sbg.sbg_device.read_data(buf); + unsafe { BUF_DST.copy_from_slice(&[0; SBG_BUFFER_SIZE]) }; + xfer.block_transfer_interrupt(); + sbg.xfer = Some(xfer); + cx.shared.em.run(|| { + spawn!(sbg_sd, buf_clone)?; // this warning isn't right but it's fine + Ok(()) + }); + } + } + None => { + // it should be impossible to reach here. + info!("None"); + } + } +} + +/// Stored right before an allocation. Stores information that is needed to deallocate memory. +#[derive(Copy, Clone)] +struct AllocInfo { + layout: Layout, + ptr: *mut u8, +} + +/// Custom malloc for the SBG library. This uses the HEAP object initialized at the start of the +/// [`SBGManager`]. The [`Layout`] of the allocation is stored right before the returned pointed, +/// which makes it possible to implement [`free`] without any other data structures. +#[no_mangle] +pub extern "C" fn malloc(size: usize) -> *mut c_void { + if size == 0 { + return ptr::null_mut(); + } + + // Get a layout for both the requested size + let header_layout = Layout::new::(); + let requested_layout = Layout::from_size_align(size, 8).unwrap(); + let (layout, offset) = header_layout.extend(requested_layout).unwrap(); + + // Ask the allocator for memory + let orig_ptr = unsafe { HEAP.alloc(layout) }; + if orig_ptr.is_null() { + return orig_ptr as *mut c_void; + } + + // Compute the pointer that we will return + let result_ptr = unsafe { orig_ptr.add(offset) }; + + // Store the allocation information right before the returned pointer + let info_ptr = unsafe { result_ptr.sub(size_of::()) as *mut AllocInfo }; + unsafe { + info_ptr.write_unaligned(AllocInfo { + layout, + ptr: orig_ptr, + }); + } + + result_ptr as *mut c_void +} + +/// Custom free implementation for the SBG library. This uses the stored allocation information +/// right before the pointer to free up the resources. +/// +/// SAFETY: The value passed to ptr must have been obtained from a previous call to [`malloc`]. +#[no_mangle] +pub unsafe extern "C" fn free(ptr: *mut c_void) { + assert!(!ptr.is_null()); + + let info_ptr = unsafe { ptr.sub(size_of::()) as *const AllocInfo }; + let info = unsafe { info_ptr.read_unaligned() }; + unsafe { + HEAP.dealloc(info.ptr, info.layout); + } +} diff --git a/boards/nav/src/types.rs b/boards/nav/src/types.rs index e69de29b..59f32710 100644 --- a/boards/nav/src/types.rs +++ b/boards/nav/src/types.rs @@ -0,0 +1,5 @@ +use hal::pac; +use hal::serial::Serial; +use stm32h7xx_hal as hal; + +pub type SBGSerial = Serial; diff --git a/libraries/sbg-rs/src/sbg.rs b/libraries/sbg-rs/src/sbg.rs index 45922492..ee8bcc50 100644 --- a/libraries/sbg-rs/src/sbg.rs +++ b/libraries/sbg-rs/src/sbg.rs @@ -25,10 +25,6 @@ use hal::sercom::{IoSet1, IoSet6, Sercom5}; use heapless::Deque; use messages::sensor::*; -type Pads = uart::PadsFromIds; -type PadsCDC = uart::PadsFromIds; -type Config = uart::Config; - /** * Max buffer size for SBG messages. */ @@ -45,13 +41,14 @@ static mut BUF: &[u8; SBG_BUFFER_SIZE] = &[0; SBG_BUFFER_SIZE]; static mut DEQ: Deque = Deque::new(); -/** - * Holds the RTC instance. This is used to get the current time. - */ -static mut RTC: Option> = None; - static mut DATA_CALLBACK: Option = None; +static mut SERIAL_WRITE_CALLBACK: Option = None; + +static mut RTC_GET_TIME: Option u32> = None; + +static mut SERIAL_FLUSH_CALLBACK: Option = None; + pub enum CallbackData { UtcTime(UtcTime), Air(Air), @@ -68,7 +65,6 @@ struct UARTSBGInterface { pub struct SBG { UARTSBGInterface: UARTSBGInterface, - serial_device: Uart, handle: _SbgEComHandle, isInitialized: bool, } @@ -79,23 +75,16 @@ impl SBG { * Takes ownership of the serial device and RTC instance. */ pub fn new( - mut serial_device: Uart, - rtc: hal::rtc::Rtc, callback: fn(CallbackData), + serial_write_callback: fn(&[u8]), + rtc_get_time: fn() -> u32, + serial_flush_callback: fn(), ) -> Self { - // SAFETY: We are accessing a static variable. - // This is safe because we are the only ones who have access to it. - // Panic if the RTC instance is already taken, this - // only can happen if the SBG instance is created twice. - if unsafe { RTC.is_some() } { - panic!("RTC instance is already taken!"); - } // SAFETY: We are assigning the RTC instance to a static variable. // This is safe because we are the only ones who have access to it. - unsafe { RTC = Some(rtc) }; let interface = UARTSBGInterface { interface: &mut _SbgInterface { - handle: &mut serial_device as *mut Uart as *mut c_void, + handle: null_mut() as *mut c_void, // SAFEY: No idea what I just did. type_: 0, name: [0; 48], pDestroyFunc: Some(SBG::SbgDestroyFunc), @@ -130,17 +119,22 @@ impl SBG { cmdDefaultTimeOut: 500, }; - unsafe { DATA_CALLBACK = Some(callback) } + unsafe { + DATA_CALLBACK = Some(callback); + SERIAL_WRITE_CALLBACK = Some(serial_write_callback); + RTC_GET_TIME = Some(rtc_get_time); + SERIAL_FLUSH_CALLBACK = Some(serial_flush_callback); + } let isInitialized = false; SBG { UARTSBGInterface: interface, - serial_device, handle: handle, isInitialized, } } + /** * Returns true if the SBG is initialized. */ @@ -323,11 +317,7 @@ impl SBG { if pBuffer.is_null() { return _SbgErrorCode_SBG_NULL_POINTER; } - // SAFETY: We are casting a c_void pointer to a Uart peripheral pointer. - // This is safe because we only have one sbg object and we ensure that - // the peripheral pointer is not accessed during the lifetime of this function. - let serial: *mut Uart = - unsafe { (*pInterface).handle as *mut Uart }; + // SAFETY: We are casting a c_void pointer to a u8 pointer and then creating a slice from it. // This is safe because we ensure pBuffer is valid, pBuffer is not accessed during the lifetime of this function, // and the SBGECom library ensures the buffer given is of the correct size. @@ -339,10 +329,9 @@ impl SBG { } // SAFETY: We are accessing a Uart Peripheral pointer. // This is safe because we ensure that the pointer is not accessed during the lifetime of this function. - let result = unsafe { nb::block!(serial.as_mut().unwrap().write(array[counter])) }; - match result { - Ok(_) => counter += 1, - Err(_) => return _SbgErrorCode_SBG_WRITE_ERROR, + match unsafe { SERIAL_WRITE_CALLBACK } { + Some(callback) => callback(&array[counter..counter + 1]), + None => return _SbgErrorCode_SBG_WRITE_ERROR, } } _SbgErrorCode_SBG_NO_ERROR @@ -413,13 +402,11 @@ impl SBG { // SAFETY: We are casting a c_void pointer to a Uart peripheral pointer. // This is safe because we only have one sbg object and we ensure that // the peripheral pointer is not accessed during the lifetime of this function. - let serial: *mut Uart = - unsafe { (*pInterface).handle as *mut Uart }; - let result = unsafe { serial.as_mut().unwrap().flush() }; - match result { - Ok(_) => return _SbgErrorCode_SBG_NO_ERROR, - Err(_) => return _SbgErrorCode_SBG_READ_ERROR, + match unsafe { SERIAL_FLUSH_CALLBACK } { + Some(callback) => callback(), + None => return _SbgErrorCode_SBG_WRITE_ERROR, } + _SbgErrorCode_SBG_NO_ERROR } /** @@ -494,11 +481,9 @@ pub unsafe extern "C" fn sbgPlatformDebugLogMsg( pub extern "C" fn sbgGetTime() -> u32 { // SAFETY: We are accessing a static mut variable. // This is safe because this is the only place where we access the RTC. - unsafe { - match &RTC { - Some(x) => x.count32(), - None => 0, // bad error handling but we can't panic, maybe we should force the timeout to be zero in the event there is no RTC. - } + match unsafe { RTC_GET_TIME } { + Some(get_time) => get_time(), + None => 0, } } From c1386927a09961c6230febbc207bf81d4deac8c6 Mon Sep 17 00:00:00 2001 From: PX4BuildBot Date: Sat, 20 Jul 2024 15:49:11 -0400 Subject: [PATCH 05/47] Update Cargo.toml mavlink --- Cargo.lock | 381 +++++++++++++++++----------------- libraries/messages/Cargo.toml | 7 +- 2 files changed, 196 insertions(+), 192 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 4aa63196..d9bc72b0 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -77,9 +77,9 @@ dependencies = [ [[package]] name = "autocfg" -version = "1.1.0" +version = "1.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d468802bab17cbc0cc575e9b053f41e72aa36bfa6b7f55e3529ffa43161b97fa" +checksum = "0c4b4d0bd25bd0b74681c0ad21497610ce1b7c91b1022cd21c80c6fbdd9476b0" [[package]] name = "bare-metal" @@ -146,9 +146,9 @@ checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" [[package]] name = "bitflags" -version = "2.4.2" +version = "2.6.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ed570934406eb16438a4e976b1b4500774099c13b8cb96eec99f620f05090ddf" +checksum = "b048fb63fd8b5923fc5aa7b340d8e156aec7ec02f0c78fa8a6ddc2613f6f71de" dependencies = [ "serde", ] @@ -186,12 +186,9 @@ checksum = "37b2a672a2cb129a2e41c10b1224bb368f9f37a2b16b612598138befd7b37eb5" [[package]] name = "cc" -version = "1.0.83" +version = "1.1.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f1174fb0b6ec23863f8b971027804a42614e347eafb0a95bf0b12cdae21fc4d0" -dependencies = [ - "libc", -] +checksum = "2aba8f4e9906c7ce3c73463f62a7f0c65183ada1a2d47e397cc8810827f9694f" [[package]] name = "cfg-if" @@ -201,9 +198,9 @@ checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" [[package]] name = "chrono" -version = "0.4.35" +version = "0.4.38" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8eaf5903dcbc0a39312feb77df2ff4c76387d591b9fc7b04a238dcf8bb62639a" +checksum = "a21f936df1771bf62b77f047b726c4625ff2e8aa607c01ec06e5a05bd8463401" dependencies = [ "num-traits", ] @@ -341,8 +338,8 @@ version = "0.7.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "f0f6f3e36f203cfedbc78b357fb28730aa2c6dc1ab060ee5c2405e843988d3c7" dependencies = [ - "proc-macro2 1.0.78", - "quote 1.0.35", + "proc-macro2 1.0.86", + "quote 1.0.36", "syn 1.0.109", ] @@ -368,8 +365,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "eefb40b1ca901c759d29526e5c8a0a1b246c20caaa5b4cc5d0f0b94debecd4c7" dependencies = [ "proc-macro-error", - "proc-macro2 1.0.78", - "quote 1.0.35", + "proc-macro2 1.0.86", + "quote 1.0.36", "rtic-syntax", "syn 1.0.109", ] @@ -394,9 +391,9 @@ dependencies = [ [[package]] name = "crc-any" -version = "2.4.4" +version = "2.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c01a5e1f881f6fb6099a7bdf949e946719fd4f1fefa56264890574febf0eb6d0" +checksum = "a62ec9ff5f7965e4d7280bd5482acd20aadb50d632cf6c1d74493856b011fa73" [[package]] name = "critical-section" @@ -406,9 +403,9 @@ checksum = "7059fff8937831a9ae6f0fe4d658ffabf58f2ca96aa9dec1c889f936f705f216" [[package]] name = "defmt" -version = "0.3.5" +version = "0.3.8" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a8a2d011b2fee29fb7d659b83c43fce9a2cb4df453e16d441a51448e448f3f98" +checksum = "a99dd22262668b887121d4672af5a64b238f026099f1a2a1b322066c9ecfe9e0" dependencies = [ "bitflags 1.3.2", "defmt-macros", @@ -416,31 +413,31 @@ dependencies = [ [[package]] name = "defmt-macros" -version = "0.3.6" +version = "0.3.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "54f0216f6c5acb5ae1a47050a6645024e6edafc2ee32d421955eccfef12ef92e" +checksum = "e3a9f309eff1f79b3ebdf252954d90ae440599c26c2c553fe87a2d17195f2dcb" dependencies = [ "defmt-parser", "proc-macro-error", - "proc-macro2 1.0.78", - "quote 1.0.35", - "syn 2.0.48", + "proc-macro2 1.0.86", + "quote 1.0.36", + "syn 2.0.71", ] [[package]] name = "defmt-parser" -version = "0.3.3" +version = "0.3.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "269924c02afd7f94bc4cecbfa5c379f6ffcf9766b3408fe63d22c728654eccd0" +checksum = "ff4a5fefe330e8d7f31b16a318f9ce81000d8e35e69b93eae154d16d2278f70f" dependencies = [ "thiserror", ] [[package]] name = "defmt-rtt" -version = "0.4.0" +version = "0.4.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "609923761264dd99ed9c7d209718cda4631c5fe84668e0f0960124cbb844c49f" +checksum = "bab697b3dbbc1750b7c8b821aa6f6e7f2480b47a99bc057a2ed7b170ebef0c51" dependencies = [ "critical-section", "defmt", @@ -448,9 +445,9 @@ dependencies = [ [[package]] name = "defmt-test" -version = "0.3.1" +version = "0.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c629c79c479057fa5b63e6095c2aa94367762524dbe791ecb611ef5a25e08851" +checksum = "290966e8c38f94b11884877242de876280d0eab934900e9642d58868e77c5df1" dependencies = [ "cortex-m-rt", "cortex-m-semihosting", @@ -460,26 +457,26 @@ dependencies = [ [[package]] name = "defmt-test-macros" -version = "0.3.0" +version = "0.3.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "94a0dfea4063d72e1ba20494dfbc4667f67420869328cf3670b5824a38a22dc1" +checksum = "984bc6eca246389726ac2826acc2488ca0fe5fcd6b8d9b48797021951d76a125" dependencies = [ - "proc-macro2 1.0.78", - "quote 1.0.35", - "syn 1.0.109", + "proc-macro2 1.0.86", + "quote 1.0.36", + "syn 2.0.71", ] [[package]] name = "derive_more" -version = "0.99.17" +version = "0.99.18" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4fb810d30a7c1953f91334de7244731fc3f3c10d7fe163338a35b9f640960321" +checksum = "5f33878137e4dafd7fa914ad4e259e18a4e8e532b9617a2d0150262bf53abfce" dependencies = [ "convert_case", - "proc-macro2 1.0.78", - "quote 1.0.35", + "proc-macro2 1.0.86", + "quote 1.0.36", "rustc_version 0.4.0", - "syn 1.0.109", + "syn 2.0.71", ] [[package]] @@ -555,21 +552,21 @@ dependencies = [ [[package]] name = "enum_dispatch" -version = "0.3.12" +version = "0.3.13" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8f33313078bb8d4d05a2733a94ac4c2d8a0df9a2b84424ebf4f33bfc224a890e" +checksum = "aa18ce2bc66555b3218614519ac839ddb759a7d6720732f979ef8d13be147ecd" dependencies = [ "once_cell", - "proc-macro2 1.0.78", - "quote 1.0.35", - "syn 2.0.48", + "proc-macro2 1.0.86", + "quote 1.0.36", + "syn 2.0.71", ] [[package]] name = "errno" -version = "0.3.8" +version = "0.3.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a258e46cdc063eb8519c00b9fc845fc47bcfca4130e2f08e88665ceda8474245" +checksum = "534c5cf6194dfab3db3242765c03bbe257cf92f22b38f6bc0c58d59108a820ba" dependencies = [ "libc", "windows-sys", @@ -577,9 +574,9 @@ dependencies = [ [[package]] name = "fastrand" -version = "2.0.1" +version = "2.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "25cbce373ec4653f1a01a31e8a5e5ec0c622dc27ff9c4e6606eefef5cbbed4a5" +checksum = "9fc0510504f03c51ada170672ac806f1f105a88aa97a5281117e1ddc3368e51a" [[package]] name = "fdcan" @@ -628,9 +625,9 @@ dependencies = [ [[package]] name = "getrandom" -version = "0.2.12" +version = "0.2.15" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "190092ea657667030ac6a35e305e62fc4dd69fd98ac98631e5d3a2b1575a12b5" +checksum = "c4567c8db10ae91089c99af84c68c38da3ec2f087c3f82960bcdbf3656b6f4d7" dependencies = [ "cfg-if", "libc", @@ -687,15 +684,15 @@ dependencies = [ [[package]] name = "lazy_static" -version = "1.4.0" +version = "1.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e2abad23fbc42b3700f2f279844dc832adb2b2eb069b2df918f455c4e18cc646" +checksum = "bbd2bcb4c963f2ddae06a2efc7e9f3591312473c50c6685e1f298068316e66fe" [[package]] name = "libc" -version = "0.2.152" +version = "0.2.155" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "13e3bf6590cbc649f4d1a3eefc9d5d6eb746f5200ffb04e5e142700b8faa56e7" +checksum = "97b3888a4aecf77e811145cadf6eef5901f4782c53886191b2f693f24761847c" [[package]] name = "libm" @@ -711,15 +708,15 @@ checksum = "9afa463f5405ee81cdb9cc2baf37e08ec7e4c8209442b5d72c04cfb2cd6e6286" [[package]] name = "linux-raw-sys" -version = "0.4.13" +version = "0.4.14" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "01cda141df6706de531b6c46c3a33ecca755538219bd484262fa09410c13539c" +checksum = "78b3ae25bc7c8c38cec158d1f2757ee79e9b3740fbc7ccf0e59e4b08d793fa89" [[package]] name = "lock_api" -version = "0.4.11" +version = "0.4.12" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3c168f8615b12bc01f9c17e2eb0cc07dcae1940121185446edc3744920e8ef45" +checksum = "07af8b9cdd281b7915f413fa73f29ebd5d55d0d3f0155584dade1ff18cea1b17" dependencies = [ "autocfg", "scopeguard", @@ -727,28 +724,48 @@ dependencies = [ [[package]] name = "log" -version = "0.4.20" +version = "0.4.22" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b5e6163cb8c49088c2c36f57875e58ccd8c87c7427f7fbd50ea6710b2f3f2e8f" +checksum = "a7a70ba024b9dc04c27ea2f0c0548feb474ec5c54bba33a7f72f873a39d07b24" [[package]] name = "mavlink" -version = "0.11.1" -source = "git+https://github.com/uorocketry/rust-mavlink#2ef8c4cb4fb9d944ddfae538e8e474f12253b782" +version = "0.13.1" +source = "git+https://github.com/uorocketry/rust-mavlink.git#e9f4057deedce85cae542fdbef75c5f56a360c9c" dependencies = [ "bitflags 1.3.2", + "mavlink-bindgen", + "mavlink-core", + "num-derive", + "num-traits", + "serde", + "serde_arrays", +] + +[[package]] +name = "mavlink-bindgen" +version = "0.13.1" +source = "git+https://github.com/uorocketry/rust-mavlink.git#e9f4057deedce85cae542fdbef75c5f56a360c9c" +dependencies = [ + "crc-any", + "lazy_static", + "proc-macro2 1.0.86", + "quick-xml", + "quote 1.0.36", + "thiserror", +] + +[[package]] +name = "mavlink-core" +version = "0.13.1" +source = "git+https://github.com/uorocketry/rust-mavlink.git#e9f4057deedce85cae542fdbef75c5f56a360c9c" +dependencies = [ "byteorder", "crc-any", "embedded-hal", - "heapless", - "lazy_static", "nb 1.1.0", - "num-derive", - "num-traits", - "proc-macro2 1.0.78", - "quick-xml", - "quote 1.0.35", "serde", + "serde_arrays", "serial", ] @@ -778,15 +795,15 @@ dependencies = [ [[package]] name = "memchr" -version = "2.7.1" +version = "2.7.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "523dc4f511e55ab87b694dc30d0f820d60906ef06413f93d4d7a1385599cc149" +checksum = "78ca9ab1a0babb1e7d5695e3530886289c18cf2f87ec19a575a0abdce112e3a3" [[package]] name = "messages" version = "0.1.0" dependencies = [ - "bitflags 2.4.2", + "bitflags 2.6.0", "defmt", "derive_more", "fugit", @@ -804,7 +821,7 @@ dependencies = [ name = "messages-proc-macros-lib" version = "0.1.0" dependencies = [ - "quote 1.0.35", + "quote 1.0.36", "serde", ] @@ -824,8 +841,8 @@ version = "0.11.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "5a7d5f7076603ebc68de2dc6a650ec331a062a13abaa346975be747bbfa4b789" dependencies = [ - "proc-macro2 1.0.78", - "quote 1.0.35", + "proc-macro2 1.0.86", + "quote 1.0.36", "syn 1.0.109", ] @@ -889,8 +906,8 @@ version = "0.3.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "876a53fff98e03a936a674b29568b0e605f06b29372c2489ff4de23f1949743d" dependencies = [ - "proc-macro2 1.0.78", - "quote 1.0.35", + "proc-macro2 1.0.86", + "quote 1.0.36", "syn 1.0.109", ] @@ -905,9 +922,9 @@ dependencies = [ [[package]] name = "num-iter" -version = "0.1.44" +version = "0.1.45" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d869c01cc0c455284163fd0092f1f93835385ccab5a98a0dcc497b2f8bf055a9" +checksum = "1429034a0490724d0075ebb2bc9e875d6503c3cf69e235a8941aa757d83ef5bf" dependencies = [ "autocfg", "num-integer", @@ -927,9 +944,9 @@ dependencies = [ [[package]] name = "num-traits" -version = "0.2.17" +version = "0.2.19" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "39e3200413f237f41ab11ad6d161bc7239c84dcb631773ccd7de3dfe4b5c267c" +checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841" dependencies = [ "autocfg", "libm", @@ -943,9 +960,9 @@ checksum = "3fdb12b2476b595f9358c5161aa467c2438859caa136dec86c26fdd2efe17b92" [[package]] name = "opaque-debug" -version = "0.3.0" +version = "0.3.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "624a8340c38c1b80fd549087862da4ba43e08858af025b236e509b6649fc13d5" +checksum = "c08d65885ee38876c4f86fa503fb49d7b507c2b62552df7c70b2fce627e06381" [[package]] name = "panic-halt" @@ -955,9 +972,9 @@ checksum = "de96540e0ebde571dc55c73d60ef407c653844e6f9a1e2fdbd40c07b9252d812" [[package]] name = "panic-probe" -version = "0.3.1" +version = "0.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "aa6fa5645ef5a760cd340eaa92af9c1ce131c8c09e7f8926d8a24b59d26652b9" +checksum = "4047d9235d1423d66cc97da7d07eddb54d4f154d6c13805c6d0793956f4f25b0" dependencies = [ "cortex-m", "defmt", @@ -965,9 +982,9 @@ dependencies = [ [[package]] name = "paste" -version = "1.0.14" +version = "1.0.15" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "de3145af08024dea9fa9914f381a17b8fc6034dfb00f3a84013f7ff43f29ed4c" +checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a" [[package]] name = "postcard" @@ -1014,8 +1031,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "da25490ff9892aab3fcf7c36f08cfb902dd3e71ca0f9f9517bea02a73a5ce38c" dependencies = [ "proc-macro-error-attr", - "proc-macro2 1.0.78", - "quote 1.0.35", + "proc-macro2 1.0.86", + "quote 1.0.36", "syn 1.0.109", "version_check", ] @@ -1026,8 +1043,8 @@ version = "1.0.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a1be40180e52ecc98ad80b184934baf3d0d29f979574e439af5a55274b35f869" dependencies = [ - "proc-macro2 1.0.78", - "quote 1.0.35", + "proc-macro2 1.0.86", + "quote 1.0.36", "version_check", ] @@ -1042,22 +1059,22 @@ dependencies = [ [[package]] name = "proc-macro2" -version = "1.0.78" +version = "1.0.86" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e2422ad645d89c99f8f3e6b88a9fdeca7fabeac836b1002371c4367c8f984aae" +checksum = "5e719e8df665df0d1c8fbfd238015744736151d4445ec0836b8e628aae103b77" dependencies = [ "unicode-ident", ] [[package]] name = "proptest" -version = "1.4.0" +version = "1.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "31b476131c3c86cb68032fdc5cb6d5a1045e3e42d96b69fa599fd77701e1f5bf" +checksum = "b4c2511913b88df1637da85cc8d96ec8e43a3f8bb8ccb71ee1ac240d6f3df58d" dependencies = [ "bit-set", "bit-vec", - "bitflags 2.4.2", + "bitflags 2.6.0", "lazy_static", "num-traits", "rand", @@ -1106,11 +1123,11 @@ dependencies = [ [[package]] name = "quote" -version = "1.0.35" +version = "1.0.36" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "291ec9ab5efd934aaf503a6466c5d5251535d108ee747472c3977cc5acc868ef" +checksum = "0fa76aaf39101c457836aec0ce2316dbdc3ab723cdda1c6bd4e6ad4208acaca7" dependencies = [ - "proc-macro2 1.0.78", + "proc-macro2 1.0.86", ] [[package]] @@ -1171,26 +1188,17 @@ dependencies = [ "typenum", ] -[[package]] -name = "redox_syscall" -version = "0.4.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4722d768eff46b75989dd134e5c353f0d6296e5aaa3132e776cbdb56be7731aa" -dependencies = [ - "bitflags 1.3.2", -] - [[package]] name = "regex-syntax" -version = "0.8.2" +version = "0.8.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c08c74e62047bb2de4ff487b251e4a92e24f48745648451635cec7d591162d9f" +checksum = "7a66a03ae7c801facd77a29370b4faec201768915ac14a721ba36f20bc9c209b" [[package]] name = "rtcc" -version = "0.3.1" +version = "0.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f4fbd0d5bed2b76e27a7ef872568b34072c1af94c277cd52c17a89d54673b3fe" +checksum = "95973c3a0274adc4f3c5b70d2b5b85618d6de9559a6737d3293ecae9a2fc0839" dependencies = [ "chrono", ] @@ -1214,8 +1222,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "5f5e215601dc467752c2bddc6284a622c6f3d2bab569d992adcd5ab7e4cb9478" dependencies = [ "indexmap", - "proc-macro2 1.0.78", - "quote 1.0.35", + "proc-macro2 1.0.86", + "quote 1.0.36", "syn 1.0.109", ] @@ -1254,16 +1262,16 @@ version = "0.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bfa0f585226d2e68097d4f95d113b15b83a82e819ab25717ec0590d9584ef366" dependencies = [ - "semver 1.0.21", + "semver 1.0.23", ] [[package]] name = "rustix" -version = "0.38.30" +version = "0.38.34" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "322394588aaf33c24007e8bb3238ee3e4c5c09c084ab32bc73890b99ff326bca" +checksum = "70dc5ec042f7a43c4a73241207cecc9873a06d45debb38b329f8541d85c2730f" dependencies = [ - "bitflags 2.4.2", + "bitflags 2.6.0", "errno", "libc", "linux-raw-sys", @@ -1287,7 +1295,7 @@ name = "sbg-rs" version = "0.1.0" dependencies = [ "atsamd-hal", - "bitflags 2.4.2", + "bitflags 2.6.0", "cmake", "common-arm", "cortex-m", @@ -1317,9 +1325,9 @@ dependencies = [ [[package]] name = "semver" -version = "1.0.21" +version = "1.0.23" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b97ed7a9823b74f99c7742f5336af7be5ecd3eeafcb1507d1fa93347b1d589b0" +checksum = "61697e0a1c7e512e84a621326239844a24d8207b4669b41bc18b32ea5cbf988b" [[package]] name = "semver-parser" @@ -1356,22 +1364,31 @@ checksum = "a3f0bf26fd526d2a95683cd0f87bf103b8539e2ca1ef48ce002d67aad59aa0b4" [[package]] name = "serde" -version = "1.0.195" +version = "1.0.204" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "63261df402c67811e9ac6def069e4786148c4563f4b50fd4bf30aa370d626b02" +checksum = "bc76f558e0cbb2a839d37354c575f1dc3fdc6546b5be373ba43d95f231bf7c12" dependencies = [ "serde_derive", ] +[[package]] +name = "serde_arrays" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "38636132857f68ec3d5f3eb121166d2af33cb55174c4d5ff645db6165cbef0fd" +dependencies = [ + "serde", +] + [[package]] name = "serde_derive" -version = "1.0.195" +version = "1.0.204" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "46fe8f8603d81ba86327b23a2e9cdf49e1255fb94a4c5f297f6ee0547178ea2c" +checksum = "e0cd7e117be63d3c3678776753929474f3b04a43a080c744d6b0ae2a8c28e222" dependencies = [ - "proc-macro2 1.0.78", - "quote 1.0.35", - "syn 2.0.48", + "proc-macro2 1.0.86", + "quote 1.0.36", + "syn 2.0.71", ] [[package]] @@ -1464,8 +1481,8 @@ dependencies = [ [[package]] name = "stm32h7xx-hal" -version = "0.15.1" -source = "git+https://github.com/stm32-rs/stm32h7xx-hal#cd7c125cceceada4a3c70cc7387d187b00f9f7d2" +version = "0.16.0" +source = "git+https://github.com/stm32-rs/stm32h7xx-hal#5166da8a5485d51e60a42d3a564d1edae0c8e164" dependencies = [ "bare-metal 1.0.0", "cast", @@ -1528,19 +1545,19 @@ version = "1.0.109" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "72b64191b275b66ffe2469e8af2c1cfe3bafa67b529ead792a6d0160888b4237" dependencies = [ - "proc-macro2 1.0.78", - "quote 1.0.35", + "proc-macro2 1.0.86", + "quote 1.0.36", "unicode-ident", ] [[package]] name = "syn" -version = "2.0.48" +version = "2.0.71" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0f3531638e407dfc0814761abb7c00a5b54992b849452a0646b7f65c9f770f3f" +checksum = "b146dcf730474b4bcd16c311627b31ede9ab149045db4d6088b3becaea046462" dependencies = [ - "proc-macro2 1.0.78", - "quote 1.0.35", + "proc-macro2 1.0.86", + "quote 1.0.36", "unicode-ident", ] @@ -1557,13 +1574,12 @@ dependencies = [ [[package]] name = "tempfile" -version = "3.9.0" +version = "3.10.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "01ce4141aa927a6d1bd34a041795abd0db1cccba5d5f24b009f694bdf3a1f3fa" +checksum = "85b77fafb263dd9d05cbeac119526425676db3784113aa9295c88498cbf8bff1" dependencies = [ "cfg-if", "fastrand", - "redox_syscall", "rustix", "windows-sys", ] @@ -1588,22 +1604,22 @@ dependencies = [ [[package]] name = "thiserror" -version = "1.0.56" +version = "1.0.63" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d54378c645627613241d077a3a79db965db602882668f9136ac42af9ecb730ad" +checksum = "c0342370b38b6a11b6cc11d6a805569958d54cfa061a29969c3b5ce2ea405724" dependencies = [ "thiserror-impl", ] [[package]] name = "thiserror-impl" -version = "1.0.56" +version = "1.0.63" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fa0faa943b50f3db30a20aa7e265dbc66076993efed8463e8de414e5d06d3471" +checksum = "a4558b58466b9ad7ca0f102865eccc95938dca1a74a856f2b57b6629050da261" dependencies = [ - "proc-macro2 1.0.78", - "quote 1.0.35", - "syn 2.0.48", + "proc-macro2 1.0.86", + "quote 1.0.36", + "syn 2.0.71", ] [[package]] @@ -1623,8 +1639,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "9f807fdb3151fee75df7485b901a89624358cd07a67a8fb1a5831bf5a07681ff" dependencies = [ "Inflector", - "proc-macro2 1.0.78", - "quote 1.0.35", + "proc-macro2 1.0.86", + "quote 1.0.36", "syn 1.0.109", "termcolor", ] @@ -1695,37 +1711,15 @@ version = "0.11.0+wasi-snapshot-preview1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "9c8d87e72b64a3b4db28d11ce29237c246188f4f51057d65a7eab63b7987e423" -[[package]] -name = "winapi" -version = "0.3.9" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5c839a674fcd7a98952e593242ea400abe93992746761e38641405d28b00f419" -dependencies = [ - "winapi-i686-pc-windows-gnu", - "winapi-x86_64-pc-windows-gnu", -] - -[[package]] -name = "winapi-i686-pc-windows-gnu" -version = "0.4.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ac3b87c63620426dd9b991e5ce0329eff545bccbbb34f3be09ff6fb6ab51b7b6" - [[package]] name = "winapi-util" -version = "0.1.6" +version = "0.1.8" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f29e6f9198ba0d26b4c9f07dbe6f9ed633e1f3d5b8b414090084349e46a52596" +checksum = "4d4cc384e1e73b93bafa6fb4f1df8c41695c8a91cf9c4c64358067d15a7b6c6b" dependencies = [ - "winapi", + "windows-sys", ] -[[package]] -name = "winapi-x86_64-pc-windows-gnu" -version = "0.4.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "712e227841d057c1ee1cd2fb22fa7e5a5461ae8e48fa2ca79ec42cfc1931183f" - [[package]] name = "windows-sys" version = "0.52.0" @@ -1737,13 +1731,14 @@ dependencies = [ [[package]] name = "windows-targets" -version = "0.52.0" +version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8a18201040b24831fbb9e4eb208f8892e1f50a37feb53cc7ff887feb8f50e7cd" +checksum = "9b724f72796e036ab90c1021d4780d4d3d648aca59e491e6b98e725b84e99973" dependencies = [ "windows_aarch64_gnullvm", "windows_aarch64_msvc", "windows_i686_gnu", + "windows_i686_gnullvm", "windows_i686_msvc", "windows_x86_64_gnu", "windows_x86_64_gnullvm", @@ -1752,42 +1747,48 @@ dependencies = [ [[package]] name = "windows_aarch64_gnullvm" -version = "0.52.0" +version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cb7764e35d4db8a7921e09562a0304bf2f93e0a51bfccee0bd0bb0b666b015ea" +checksum = "32a4622180e7a0ec044bb555404c800bc9fd9ec262ec147edd5989ccd0c02cd3" [[package]] name = "windows_aarch64_msvc" -version = "0.52.0" +version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bbaa0368d4f1d2aaefc55b6fcfee13f41544ddf36801e793edbbfd7d7df075ef" +checksum = "09ec2a7bb152e2252b53fa7803150007879548bc709c039df7627cabbd05d469" [[package]] name = "windows_i686_gnu" -version = "0.52.0" +version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a28637cb1fa3560a16915793afb20081aba2c92ee8af57b4d5f28e4b3e7df313" +checksum = "8e9b5ad5ab802e97eb8e295ac6720e509ee4c243f69d781394014ebfe8bbfa0b" + +[[package]] +name = "windows_i686_gnullvm" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0eee52d38c090b3caa76c563b86c3a4bd71ef1a819287c19d586d7334ae8ed66" [[package]] name = "windows_i686_msvc" -version = "0.52.0" +version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ffe5e8e31046ce6230cc7215707b816e339ff4d4d67c65dffa206fd0f7aa7b9a" +checksum = "240948bc05c5e7c6dabba28bf89d89ffce3e303022809e73deaefe4f6ec56c66" [[package]] name = "windows_x86_64_gnu" -version = "0.52.0" +version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3d6fa32db2bc4a2f5abeacf2b69f7992cd09dca97498da74a151a3132c26befd" +checksum = "147a5c80aabfbf0c7d901cb5895d1de30ef2907eb21fbbab29ca94c5b08b1a78" [[package]] name = "windows_x86_64_gnullvm" -version = "0.52.0" +version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1a657e1e9d3f514745a572a6846d3c7aa7dbe1658c056ed9c3344c4109a6949e" +checksum = "24d5b23dc417412679681396f2b49f3de8c1473deb516bd34410872eff51ed0d" [[package]] name = "windows_x86_64_msvc" -version = "0.52.0" +version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dff9641d1cd4be8d1a070daf9e3773c5f67e78b4d9d42263020c057706765c04" +checksum = "589f6da84c646204747d1270a2a5661ea66ed1cced2631d546fdfb155959f9ec" diff --git a/libraries/messages/Cargo.toml b/libraries/messages/Cargo.toml index 50777345..1c66efb5 100644 --- a/libraries/messages/Cargo.toml +++ b/libraries/messages/Cargo.toml @@ -12,7 +12,10 @@ defmt = "0.3.2" fugit = "0.3.6" heapless = "0.7.16" ts-rs = { version = "6.2.1", optional = true } -mavlink = { git = "https://github.com/uorocketry/rust-mavlink", default-features = false } +mavlink = { git = "https://github.com/uorocketry/rust-mavlink.git", features = [ + "embedded-hal-02", + "uorocketry", +], default-features = false } bitflags = { version = "2.3.1", features = ["serde"] } proptest = { version = "1.2.0", optional = true } proptest-derive = { version = "0.3.0", optional = true } @@ -24,6 +27,6 @@ proptest-derive = "0.3.0" postcard = { version = "1.0.4", features = ["alloc"] } [features] -default = ["mavlink/embedded", "mavlink/uorocketry"] +default = ["mavlink/embedded-hal-02", "mavlink/uorocketry"] std = ["mavlink/default", "dep:proptest", "dep:proptest-derive"] ts = ["std", "dep:ts-rs"] From 0d627fe033ed6f0f4978887fd656f8bb7abd304e Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Tue, 23 Jul 2024 13:40:39 -0400 Subject: [PATCH 06/47] WIP: Fix Mavlink, Add baro (untested). --- .cargo/config | 2 +- Cargo.lock | 28 ++++++--- Cargo.toml | 3 +- boards/communication/src/communication.rs | 19 ++---- boards/communication/src/main.rs | 3 +- boards/nav/src/sbg_manager.rs | 27 ++++----- boards/recovery/Cargo.toml | 5 +- boards/recovery/src/gpio_manager.rs | 10 ++-- boards/recovery/src/main.rs | 58 +++++++++++++++++-- libraries/common-arm/Cargo.toml | 2 + libraries/common-arm/src/error/hydra_error.rs | 6 ++ 11 files changed, 111 insertions(+), 52 deletions(-) diff --git a/.cargo/config b/.cargo/config index bab9d3ed..84f0a007 100644 --- a/.cargo/config +++ b/.cargo/config @@ -1,6 +1,6 @@ [target.'cfg(all(target_arch = "arm", target_os = "none"))'] # This runner needs to be parametric so we can pass in the chip name -runner = "probe-rs run --chip ATSAME51J20A --protocol swd" +runner = "probe-rs run --chip STM32H733VGTx --protocol swd" rustflags = [ "-C", "link-arg=-Tlink.x", diff --git a/Cargo.lock b/Cargo.lock index d9bc72b0..d3ad6188 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -243,6 +243,7 @@ dependencies = [ "heapless", "mcan", "messages", + "ms5611-01ba", "nb 1.1.0", "postcard", ] @@ -421,7 +422,7 @@ dependencies = [ "proc-macro-error", "proc-macro2 1.0.86", "quote 1.0.36", - "syn 2.0.71", + "syn 2.0.72", ] [[package]] @@ -463,7 +464,7 @@ checksum = "984bc6eca246389726ac2826acc2488ca0fe5fcd6b8d9b48797021951d76a125" dependencies = [ "proc-macro2 1.0.86", "quote 1.0.36", - "syn 2.0.71", + "syn 2.0.72", ] [[package]] @@ -476,7 +477,7 @@ dependencies = [ "proc-macro2 1.0.86", "quote 1.0.36", "rustc_version 0.4.0", - "syn 2.0.71", + "syn 2.0.72", ] [[package]] @@ -559,7 +560,7 @@ dependencies = [ "once_cell", "proc-macro2 1.0.86", "quote 1.0.36", - "syn 2.0.71", + "syn 2.0.72", ] [[package]] @@ -846,6 +847,14 @@ dependencies = [ "syn 1.0.109", ] +[[package]] +name = "ms5611-01ba" +version = "0.1.0" +source = "git+https://github.com/NoahSprenger/ms5611-01ba?branch=embedded-hal-02#fdb9b51f953d854c7b99079ae3583f82c6378bbc" +dependencies = [ + "embedded-hal", +] + [[package]] name = "nav" version = "0.1.0" @@ -1180,9 +1189,11 @@ dependencies = [ "cortex-m-rt", "cortex-m-rtic", "defmt", + "embedded-hal", "enum_dispatch", "heapless", "messages", + "ms5611-01ba", "postcard", "systick-monotonic", "typenum", @@ -1388,7 +1399,7 @@ checksum = "e0cd7e117be63d3c3678776753929474f3b04a43a080c744d6b0ae2a8c28e222" dependencies = [ "proc-macro2 1.0.86", "quote 1.0.36", - "syn 2.0.71", + "syn 2.0.72", ] [[package]] @@ -1486,6 +1497,7 @@ source = "git+https://github.com/stm32-rs/stm32h7xx-hal#5166da8a5485d51e60a42d3a dependencies = [ "bare-metal 1.0.0", "cast", + "chrono", "cortex-m", "defmt", "embedded-dma", @@ -1552,9 +1564,9 @@ dependencies = [ [[package]] name = "syn" -version = "2.0.71" +version = "2.0.72" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b146dcf730474b4bcd16c311627b31ede9ab149045db4d6088b3becaea046462" +checksum = "dc4b9b9bf2add8093d3f2c0204471e951b2285580335de42f9d2534f3ae7a8af" dependencies = [ "proc-macro2 1.0.86", "quote 1.0.36", @@ -1619,7 +1631,7 @@ checksum = "a4558b58466b9ad7ca0f102865eccc95938dca1a74a856f2b57b6629050da261" dependencies = [ "proc-macro2 1.0.86", "quote 1.0.36", - "syn 2.0.71", + "syn 2.0.72", ] [[package]] diff --git a/Cargo.toml b/Cargo.toml index 137adc2c..154e847c 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -13,7 +13,7 @@ version = "0.2.7" [workspace.dependencies.stm32h7xx-hal] git = "https://github.com/stm32-rs/stm32h7xx-hal" # We use 35 even though we have the 33. -features = ["defmt", "rt", "stm32h735", "can"] +features = ["defmt", "rt", "stm32h735", "can", "rtc"] [workspace.dependencies.atsamd-hal] git = "https://github.com/uorocketry/atsamd" @@ -64,4 +64,3 @@ opt-level = 0 #Try to remove this [profile.release.build-override] #opt-level = 0 - diff --git a/boards/communication/src/communication.rs b/boards/communication/src/communication.rs index d56b5736..7764de01 100644 --- a/boards/communication/src/communication.rs +++ b/boards/communication/src/communication.rs @@ -11,7 +11,6 @@ use atsamd_hal::gpio::{Alternate, AlternateI, Disabled, Floating, Pin, I, PA22, use atsamd_hal::pac::CAN0; use atsamd_hal::pac::MCLK; use atsamd_hal::pac::SERCOM5; -use atsamd_hal::prelude::_embedded_hal_serial_Read; use atsamd_hal::sercom; use atsamd_hal::sercom::uart; use atsamd_hal::sercom::uart::Uart; @@ -24,6 +23,7 @@ use common_arm_atsame::mcan::tx_buffers::DynTx; use heapless::HistoryBuffer; use heapless::Vec; use mavlink::embedded::Read; +use mavlink::peek_reader::PeekReader; use mcan::bus::Can; use mcan::embedded_can as ecan; use mcan::interrupt::state::EnabledLine0; @@ -213,7 +213,7 @@ impl CanDevice0 { pub struct RadioDevice { transmitter: Uart, - receiver: Uart, + receiver: PeekReader>, } impl RadioDevice { @@ -244,7 +244,7 @@ impl RadioDevice { ( RadioDevice { transmitter: tx, - receiver: rx, + receiver: PeekReader::new(rx), }, gclk0, ) @@ -266,9 +266,7 @@ impl RadioManager { mav_sequence: 0, } } - pub fn send_message(&mut self, m: Message) -> Result<(), HydraError> { - let payload: Vec = postcard::to_vec(&m)?; - + pub fn send_message(&mut self, payload: [u8; 255]) -> Result<(), HydraError> { let mav_header = mavlink::MavHeader { system_id: 1, component_id: 1, @@ -291,7 +289,7 @@ impl RadioManager { self.mav_sequence } pub fn receive_message(&mut self) -> Result { - if let Ok(data) = self.radio.receiver.read() { + if let Ok(data) = self.radio.receiver.read_u8() { // lets add this data to the buffer and see if we can parse it self.buf.write(data); let (_header, msg) = @@ -319,11 +317,4 @@ impl Read for RadioManager { buf[..len].copy_from_slice(&self.buf[..len]); Ok(()) } - fn read_u8(&mut self) -> Result { - if !self.buf.is_empty() { - Ok(self.buf[0]) - } else { - Err(mavlink::error::MessageReadError::Io) - } - } } diff --git a/boards/communication/src/main.rs b/boards/communication/src/main.rs index f96923a3..2ea29f88 100644 --- a/boards/communication/src/main.rs +++ b/boards/communication/src/main.rs @@ -234,7 +234,8 @@ mod app { fn send_gs(mut cx: send_gs::Context, m: Message) { cx.shared.radio_manager.lock(|radio_manager| { cx.shared.em.run(|| { - radio_manager.send_message(m)?; + radio_manager + .send_message(postcard::to_slice(&m, &mut [0; 255])?.try_into().unwrap())?; Ok(()) }) }); diff --git a/boards/nav/src/sbg_manager.rs b/boards/nav/src/sbg_manager.rs index 25b8e259..ab811de2 100644 --- a/boards/nav/src/sbg_manager.rs +++ b/boards/nav/src/sbg_manager.rs @@ -3,8 +3,6 @@ use atsamd_hal::clock::v2::gclk::Gclk0Id; use atsamd_hal::clock::v2::pclk::Pclk; use atsamd_hal::dmac; use atsamd_hal::dmac::Transfer; -use atsamd_hal::gpio::{Pin, Reset, PB02, PB03}; -use atsamd_hal::pac::{MCLK, RTC}; use atsamd_hal::sercom::IoSet6; // use atsamd_hal::prelude::_atsamd21_hal_time_U32Ext; use atsamd_hal::rtc::Rtc; @@ -23,6 +21,9 @@ use embedded_alloc::Heap; use rtic::Mutex; use sbg_rs::sbg; use sbg_rs::sbg::{CallbackData, SBG, SBG_BUFFER_SIZE}; +use stm32h7xx_hal::gpio::Alternate; +use stm32h7xx_hal::gpio::Pin; +use stm32h7xx_hal::rtc::Rtc; pub static mut BUF_DST: SBGBuffer = &mut [0; SBG_BUFFER_SIZE]; @@ -36,12 +37,9 @@ pub struct SBGManager { impl SBGManager { pub fn new( - rx: Pin, - tx: Pin, - pclk_sercom5: Pclk, - mclk: &mut MCLK, - sercom5: Sercom5, - rtc: RTC, + rx: Pin<'D', 0, Alternate<8>>, + tx: Pin<'D', 1, Alternate<8>>, + rtc: Rtc, mut dma_channel: dmac::Channel, ) -> Self { /* Initialize the Heap */ @@ -52,14 +50,11 @@ impl SBGManager { unsafe { HEAP.init(HEAP_MEM.as_ptr() as usize, HEAP_SIZE) } } - let pads_sbg = uart::Pads::::default().rx(rx).tx(tx); - let uart_sbg = ConfigSBG::new(mclk, sercom5, pads_sbg, pclk_sercom5.freq()) - .baud( - 115200.Hz(), - uart::BaudMode::Fractional(uart::Oversampling::Bits8), - ) - .enable(); - + let uart_sbg = ctx + .device + .UART4 + .serial((tx, rx), 9_800.bps(), ccdr.peripheral.UART4, &ccdr.clocks) + .unwrap(); let (sbg_rx, sbg_tx) = uart_sbg.split(); /* DMAC config */ diff --git a/boards/recovery/Cargo.toml b/boards/recovery/Cargo.toml index e5460103..ce71e27e 100644 --- a/boards/recovery/Cargo.toml +++ b/boards/recovery/Cargo.toml @@ -18,4 +18,7 @@ common-arm-atsame = { path = "../../libraries/common-arm-atsame" } atsamd-hal = { workspace = true } messages = { path = "../../libraries/messages" } typenum = "1.16.0" -enum_dispatch = "0.3.11" \ No newline at end of file +enum_dispatch = "0.3.11" +ms5611-01ba = { git = "https://github.com/NoahSprenger/ms5611-01ba", branch = "embedded-hal-02" } + +embedded-hal = "0.2.7" diff --git a/boards/recovery/src/gpio_manager.rs b/boards/recovery/src/gpio_manager.rs index 2857fbdd..54078f30 100644 --- a/boards/recovery/src/gpio_manager.rs +++ b/boards/recovery/src/gpio_manager.rs @@ -1,15 +1,15 @@ -use atsamd_hal::gpio::{Pin, PushPullOutput, PA06, PA09}; +use atsamd_hal::gpio::{Pin, PushPullOutput, PA06, PA09, PB11, PB12}; use atsamd_hal::prelude::*; pub struct GPIOManager { - main_ematch: Pin, - drogue_ematch: Pin, + main_ematch: Pin, + drogue_ematch: Pin, } impl GPIOManager { pub fn new( - mut main_ematch: Pin, - mut drogue_ematch: Pin, + mut main_ematch: Pin, + mut drogue_ematch: Pin, ) -> Self { drogue_ematch.set_low().ok(); main_ematch.set_low().ok(); diff --git a/boards/recovery/src/main.rs b/boards/recovery/src/main.rs index c5018081..b6cbe7fe 100644 --- a/boards/recovery/src/main.rs +++ b/boards/recovery/src/main.rs @@ -15,12 +15,20 @@ use common_arm::hinfo; use common_arm::*; use common_arm_atsame::mcan; use communication::Capacities; +use core::fmt::Debug; use data_manager::DataManager; +use defmt::info; +use embedded_hal::spi::FullDuplex; use gpio_manager::GPIOManager; -use hal::gpio::{Pin, Pins, PushPullOutput, PB16, PB17}; +use hal::gpio::{ + Alternate, Output, Pin, Pins, PushPull, PushPullOutput, C, D, PA04, PA05, PA06, PA07, PA09, + PB16, PB17, +}; use hal::prelude::*; +use hal::sercom::{spi, spi::Config, spi::Duplex, spi::Pads, spi::Spi, IoSet3, Sercom0}; use mcan::messageram::SharedMemory; use messages::*; +use ms5611_01ba::{calibration::OversamplingRatio, MS5611_01BA}; use state_machine::{StateMachine, StateMachineContext}; use systick_monotonic::*; use types::COM_ID; @@ -49,6 +57,20 @@ mod app { led_green: Pin, led_red: Pin, state_machine: StateMachine, + barometer: MS5611_01BA< + Spi< + Config< + Pads< + hal::pac::SERCOM0, + IoSet3, + Pin>, + Pin>, + Pin>, + >, + >, + Duplex, + >, + >, } #[monotonic(binds = SysTick, default = true)] @@ -78,7 +100,7 @@ mod app { // SAFETY: Misusing the PAC API can break the system. // This is safe because we only steal the MCLK. - let (_, _, _, _mclk) = unsafe { clocks.pac.steal() }; + let (_, _, _, mclk) = unsafe { clocks.pac.steal() }; /* CAN config */ let (pclk_can, gclk0) = Pclk::enable(tokens.pclks.can0, gclk0); @@ -98,12 +120,29 @@ mod app { let led_red = pins.pb17.into_push_pull_output(); let gpio = GPIOManager::new( // pins switched from schematic - pins.pa09.into_push_pull_output(), - pins.pa06.into_push_pull_output(), + pins.pb12.into_push_pull_output(), + pins.pb11.into_push_pull_output(), ); /* State Machine config */ let state_machine = StateMachine::new(); + //type pads = hal::sercom::spi::PadsFromIds; + //let pads_spi = pads::Pads::new(pins.pa07, pins.pa04, pins.pa05, pins.pa06); + + let (pclk_sd, gclk0) = Pclk::enable(tokens.pclks.sercom0, gclk0); + let pads_spi = spi::Pads::::default() + .sclk(pins.pa05) + .data_in(pins.pa07) + .data_out(pins.pa04); + + let baro_spi = spi::Config::new(&mclk, peripherals.SERCOM0, pads_spi, pclk_sd.freq()) + .length::() + .bit_order(spi::BitOrder::MsbFirst) + .spi_mode(spi::MODE_0) + .enable(); + + let barometer = MS5611_01BA::new(baro_spi, OversamplingRatio::OSR2048); + /* Spawn tasks */ run_sm::spawn().ok(); state_send::spawn().ok(); @@ -125,6 +164,7 @@ mod app { led_green, led_red, state_machine, + barometer, }, init::Monotonics(mono), ) @@ -136,6 +176,16 @@ mod app { loop {} } + #[task(local = [barometer], shared = [&em])] + fn read_barometer(cx: read_barometer::Context) { + cx.shared.em.run(|| { + let mut barometer = cx.local.barometer; + let (p, t) = barometer.get_data()?; + info!("pressure {} temperature {}", p, t); + Ok(()) + }); + } + #[task(priority = 3, local = [fired: bool = false], shared=[gpio, &em])] fn fire_drogue(mut cx: fire_drogue::Context) { cx.shared.em.run(|| { diff --git a/libraries/common-arm/Cargo.toml b/libraries/common-arm/Cargo.toml index 276dd48c..adac4d99 100644 --- a/libraries/common-arm/Cargo.toml +++ b/libraries/common-arm/Cargo.toml @@ -20,3 +20,5 @@ embedded-hal = "0.2.7" nb = "1.1.0" mcan = "0.3.0" messages = { path = "../messages" } + +ms5611-01ba = { git = "https://github.com/NoahSprenger/ms5611-01ba", branch = "embedded-hal-02" } diff --git a/libraries/common-arm/src/error/hydra_error.rs b/libraries/common-arm/src/error/hydra_error.rs index a6bcbd2f..5aa90833 100644 --- a/libraries/common-arm/src/error/hydra_error.rs +++ b/libraries/common-arm/src/error/hydra_error.rs @@ -4,6 +4,7 @@ use defmt::{write, Format}; use derive_more::From; use embedded_sdmmc as sd; use messages::ErrorContext; +use ms5611_01ba::error::DeviceError; /// Open up atsamd hal errors without including the whole crate. @@ -28,6 +29,8 @@ pub enum HydraErrorType { CanError(nb::Error), /// CAN message build error. CanMessageError(mcan::message::TooMuchData), + /// Error from the MS5611 barometer library. + BarometerError(DeviceError), } impl defmt::Format for HydraErrorType { @@ -51,6 +54,9 @@ impl defmt::Format for HydraErrorType { HydraErrorType::MavlinkReadError(_) => { write!(f, "Mavlink read error!"); } + HydraErrorType::BarometerError(_) => { + write!(f, "Barometer error!"); + } // HydraErrorType::DmaError(_) => { // write!(f, "DMA error!"); // } From 47ee89be4f7ab185399b0cd4432061bfe764b3c5 Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Thu, 25 Jul 2024 23:48:17 -0400 Subject: [PATCH 07/47] WIP: Out of scope. --- .cargo/config | 4 +- Cargo.lock | 182 +++++++++++++++++++-- Cargo.toml | 3 +- boards/communication/src/communication.rs | 15 +- boards/communication/src/health.rs | 85 ---------- boards/communication/src/main.rs | 185 ++++++++++------------ boards/communication/src/types.rs | 11 +- boards/nav/Cargo.toml | 2 + boards/nav/src/data_manager.rs | 5 +- boards/nav/src/main.rs | 39 ++++- boards/nav/src/sbg_manager.rs | 66 +++++--- boards/power/src/main.rs | 22 ++- 12 files changed, 375 insertions(+), 244 deletions(-) delete mode 100644 boards/communication/src/health.rs diff --git a/.cargo/config b/.cargo/config index 84f0a007..890523d3 100644 --- a/.cargo/config +++ b/.cargo/config @@ -1,7 +1,7 @@ [target.'cfg(all(target_arch = "arm", target_os = "none"))'] # This runner needs to be parametric so we can pass in the chip name -runner = "probe-rs run --chip STM32H733VGTx --protocol swd" - +#runner = "probe-rs run --chip STM32H733VGTx --protocol swd" +runner = "probe-rs run --chip ATSAME51J18A --protocol swd" rustflags = [ "-C", "link-arg=-Tlink.x", "-C", "link-arg=-Tdefmt.x", diff --git a/Cargo.lock b/Cargo.lock index d3ad6188..99ea48bf 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -40,16 +40,21 @@ dependencies = [ [[package]] name = "atsamd-hal" -version = "0.16.0" -source = "git+https://github.com/uorocketry/atsamd#5b3e781911d5824b7317a4af0ad3ea97873df430" +version = "0.17.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "84fba26d12e032c93fb0de8c0e0ff34fafea35192f61cd0fc247838d78cda22e" dependencies = [ "aes", + "atsamd-hal-macros", "atsame51j", "bitfield 0.13.2", "bitflags 1.3.2", "cipher", "cortex-m", - "embedded-hal", + "embedded-hal 0.2.7", + "embedded-hal 1.0.0", + "embedded-hal-nb", + "embedded-io 0.6.1", "fugit", "mcan-core", "modular-bitfield", @@ -64,10 +69,24 @@ dependencies = [ "void", ] +[[package]] +name = "atsamd-hal-macros" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b27ccb060a908ff8a40acde574902a6bd283d3420dd4ffcdb6a327225f55e098" +dependencies = [ + "litrs", + "phf", + "phf_codegen", + "serde", + "serde_yaml", +] + [[package]] name = "atsame51j" version = "0.13.0" -source = "git+https://github.com/uorocketry/atsamd#5b3e781911d5824b7317a4af0ad3ea97873df430" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8c295af2278e92dbfb65189097a8858556685456e601c7c689e6a3c0290f10b7" dependencies = [ "cortex-m", "cortex-m-rt", @@ -238,7 +257,7 @@ dependencies = [ "defmt", "defmt-rtt", "derive_more", - "embedded-hal", + "embedded-hal 0.2.7", "embedded-sdmmc", "heapless", "mcan", @@ -254,7 +273,7 @@ version = "0.1.0" dependencies = [ "atsamd-hal", "common-arm", - "embedded-hal", + "embedded-hal 0.2.7", "mcan", ] @@ -320,7 +339,7 @@ dependencies = [ "bare-metal 0.2.5", "bitfield 0.13.2", "critical-section", - "embedded-hal", + "embedded-hal 0.2.7", "volatile-register", ] @@ -518,12 +537,34 @@ dependencies = [ "void", ] +[[package]] +name = "embedded-hal" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "361a90feb7004eca4019fb28352a9465666b24f840f5c3cddf0ff13920590b89" + +[[package]] +name = "embedded-hal-nb" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fba4268c14288c828995299e59b12babdbe170f6c6d73731af1b4648142e8605" +dependencies = [ + "embedded-hal 1.0.0", + "nb 1.1.0", +] + [[package]] name = "embedded-io" version = "0.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ef1a6892d9eef45c8fa6b9e0086428a2cca8491aca8f787c534a3d6d0bcb3ced" +[[package]] +name = "embedded-io" +version = "0.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "edd0f118536f44f5ccd48bcb8b111bdc3de888b58c74639dfb034a357d0f206d" + [[package]] name = "embedded-sdmmc" version = "0.3.0" @@ -531,7 +572,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "6d3bf0a2b5becb87e9a329d9290f131e4d10fec39b56d129926826a7cbea1e7a" dependencies = [ "byteorder", - "embedded-hal", + "embedded-hal 0.2.7", "log", "nb 0.1.3", ] @@ -563,6 +604,12 @@ dependencies = [ "syn 2.0.72", ] +[[package]] +name = "equivalent" +version = "1.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5443807d6dff69373d433ab9ef5378ad8df50ca6298caf15de6e52e24aaf54d5" + [[package]] name = "errno" version = "0.3.9" @@ -650,6 +697,12 @@ version = "0.12.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8a9ee70c43aaf417c914396645a0fa852624801b24ebb7ae78fe8272889ac888" +[[package]] +name = "hashbrown" +version = "0.14.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e5274423e17b7c9fc20b6e7e208532f9b19825d82dfd615708b70edd83df41f1" + [[package]] name = "heapless" version = "0.7.17" @@ -671,7 +724,17 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bd070e393353796e801d209ad339e89596eb4c8d430d18ede6a1cced8fafbd99" dependencies = [ "autocfg", - "hashbrown", + "hashbrown 0.12.3", +] + +[[package]] +name = "indexmap" +version = "2.2.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "168fb715dda47215e360912c096649d23d58bf392ac62f73919e831745e40f26" +dependencies = [ + "equivalent", + "hashbrown 0.14.5", ] [[package]] @@ -683,6 +746,12 @@ dependencies = [ "libc", ] +[[package]] +name = "itoa" +version = "1.0.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "49f1f14873335454500d59611f1cf4a4b0f786f9ac11f4312a78e4cf2566695b" + [[package]] name = "lazy_static" version = "1.5.0" @@ -713,6 +782,15 @@ version = "0.4.14" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "78b3ae25bc7c8c38cec158d1f2757ee79e9b3740fbc7ccf0e59e4b08d793fa89" +[[package]] +name = "litrs" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b4ce301924b7887e9d637144fdade93f9dfff9b60981d4ac161db09720d39aa5" +dependencies = [ + "proc-macro2 1.0.86", +] + [[package]] name = "lock_api" version = "0.4.12" @@ -763,7 +841,7 @@ source = "git+https://github.com/uorocketry/rust-mavlink.git#e9f4057deedce85cae5 dependencies = [ "byteorder", "crc-any", - "embedded-hal", + "embedded-hal 0.2.7", "nb 1.1.0", "serde", "serde_arrays", @@ -852,7 +930,7 @@ name = "ms5611-01ba" version = "0.1.0" source = "git+https://github.com/NoahSprenger/ms5611-01ba?branch=embedded-hal-02#fdb9b51f953d854c7b99079ae3583f82c6378bbc" dependencies = [ - "embedded-hal", + "embedded-hal 0.2.7", ] [[package]] @@ -866,6 +944,7 @@ dependencies = [ "cortex-m-rtic", "defmt", "fdcan", + "heapless", "messages", "postcard", "stm32h7xx-hal", @@ -995,6 +1074,44 @@ version = "1.0.15" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a" +[[package]] +name = "phf" +version = "0.11.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ade2d8b8f33c7333b51bcf0428d37e217e9f32192ae4772156f65063b8ce03dc" +dependencies = [ + "phf_shared", +] + +[[package]] +name = "phf_codegen" +version = "0.11.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e8d39688d359e6b34654d328e262234662d16cc0f60ec8dcbe5e718709342a5a" +dependencies = [ + "phf_generator", + "phf_shared", +] + +[[package]] +name = "phf_generator" +version = "0.11.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "48e4cc64c2ad9ebe670cb8fd69dd50ae301650392e81c05f9bfcb2d5bdbc24b0" +dependencies = [ + "phf_shared", + "rand", +] + +[[package]] +name = "phf_shared" +version = "0.11.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "90fcb95eef784c2ac79119d1dd819e162b5da872ce6f3c3abe1e8ca1c082f72b" +dependencies = [ + "siphasher", +] + [[package]] name = "postcard" version = "1.0.8" @@ -1003,7 +1120,7 @@ checksum = "a55c51ee6c0db07e68448e336cf8ea4131a620edefebf9893e759b2d793420f8" dependencies = [ "cobs", "defmt", - "embedded-io", + "embedded-io 0.4.0", "heapless", "serde", ] @@ -1189,7 +1306,7 @@ dependencies = [ "cortex-m-rt", "cortex-m-rtic", "defmt", - "embedded-hal", + "embedded-hal 0.2.7", "enum_dispatch", "heapless", "messages", @@ -1232,7 +1349,7 @@ version = "1.0.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "5f5e215601dc467752c2bddc6284a622c6f3d2bab569d992adcd5ab7e4cb9478" dependencies = [ - "indexmap", + "indexmap 1.9.3", "proc-macro2 1.0.86", "quote 1.0.36", "syn 1.0.109", @@ -1301,6 +1418,12 @@ dependencies = [ "wait-timeout", ] +[[package]] +name = "ryu" +version = "1.0.18" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f3cb5ba0dc43242ce17de99c180e96db90b235b8a9fdc9543c96d2209116bd9f" + [[package]] name = "sbg-rs" version = "0.1.0" @@ -1312,7 +1435,7 @@ dependencies = [ "cortex-m", "cortex-m-rt", "defmt", - "embedded-hal", + "embedded-hal 0.2.7", "heapless", "messages", "nb 1.1.0", @@ -1402,6 +1525,19 @@ dependencies = [ "syn 2.0.72", ] +[[package]] +name = "serde_yaml" +version = "0.9.34+deprecated" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6a8b1a1a2ebf674015cc02edccce75287f1a0130d394307b36743c2f5d504b47" +dependencies = [ + "indexmap 2.2.6", + "itoa", + "ryu", + "serde", + "unsafe-libyaml", +] + [[package]] name = "serial" version = "0.4.0" @@ -1457,6 +1593,12 @@ dependencies = [ "panic-halt", ] +[[package]] +name = "siphasher" +version = "0.3.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "38b58827f4464d87d377d175e90bf58eb00fd8716ff0a62f80356b5e61555d0d" + [[package]] name = "spin" version = "0.9.8" @@ -1501,7 +1643,7 @@ dependencies = [ "cortex-m", "defmt", "embedded-dma", - "embedded-hal", + "embedded-hal 0.2.7", "embedded-storage", "fdcan", "fugit", @@ -1532,7 +1674,7 @@ dependencies = [ "cast", "cortex-m", "cortex-m-rt", - "embedded-hal", + "embedded-hal 0.2.7", "embedded-time", "nb 1.1.0", "rtcc", @@ -1681,6 +1823,12 @@ version = "0.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "fc72304796d0818e357ead4e000d19c9c174ab23dc11093ac919054d20a6a7fc" +[[package]] +name = "unsafe-libyaml" +version = "0.2.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "673aac59facbab8a9007c7f6108d11f63b603f7cabff99fabf650fea5c32b861" + [[package]] name = "vcell" version = "0.1.3" diff --git a/Cargo.toml b/Cargo.toml index 154e847c..8a0f8b8f 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -16,7 +16,8 @@ git = "https://github.com/stm32-rs/stm32h7xx-hal" features = ["defmt", "rt", "stm32h735", "can", "rtc"] [workspace.dependencies.atsamd-hal] -git = "https://github.com/uorocketry/atsamd" +#git = "https://github.com/uorocketry/atsamd" +version = "0.17.0" features = ["same51j", "same51j-rt", "dma", "can"] [workspace.dependencies.stm32l0xx-hal] diff --git a/boards/communication/src/communication.rs b/boards/communication/src/communication.rs index 7764de01..12ece588 100644 --- a/boards/communication/src/communication.rs +++ b/boards/communication/src/communication.rs @@ -7,9 +7,14 @@ use atsamd_hal::clock::v2::pclk::Pclk; use atsamd_hal::clock::v2::pclk::PclkToken; use atsamd_hal::clock::v2::types::Can0; use atsamd_hal::clock::v2::Source; +use atsamd_hal::gpio::PA08; +use atsamd_hal::gpio::PA09; use atsamd_hal::gpio::{Alternate, AlternateI, Disabled, Floating, Pin, I, PA22, PA23, PB16, PB17}; use atsamd_hal::pac::CAN0; use atsamd_hal::pac::MCLK; +use atsamd_hal::pac::SERCOM0; +use atsamd_hal::pac::SERCOM2; +use atsamd_hal::pac::SERCOM4; use atsamd_hal::pac::SERCOM5; use atsamd_hal::sercom; use atsamd_hal::sercom::uart; @@ -218,18 +223,18 @@ pub struct RadioDevice { impl RadioDevice { pub fn new( - radio_token: PclkToken, + radio_token: PclkToken, mclk: &MCLK, - sercom: SERCOM5, - rx_pin: Pin>, - tx_pin: Pin>, + sercom: SERCOM2, + rx_pin: Pin>, + tx_pin: Pin>, gclk0: S, ) -> (Self, S::Inc) where S: Source + Increment, { let (pclk_radio, gclk0) = Pclk::enable(radio_token, gclk0); - let pads = uart::Pads::::default() + let pads = uart::Pads::::default() .rx(rx_pin) .tx(tx_pin); let uart = GroundStationUartConfig::new(mclk, sercom, pads, pclk_radio.freq()) diff --git a/boards/communication/src/health.rs b/boards/communication/src/health.rs deleted file mode 100644 index 1f5cbd97..00000000 --- a/boards/communication/src/health.rs +++ /dev/null @@ -1,85 +0,0 @@ -//! Health related code -//! Would've liked to have this live in common-arm-atsame but the pins and adc are not standardised -//! for all boards which poses the problem of giving an adc to a wrong pin in a generic way. - -use atsamd_hal::gpio::{Alternate, Pin, B, PB00, PB01, PB02, PB03, PB05, PB06, PB07, PB08, PB09}; -use atsamd_hal::{adc::Adc, ehal::adc::OneShot, pac::ADC0, pac::ADC1}; -use common_arm::HealthMonitorChannels; - -// make sure to define the ADC types in types.rs - -// I don't think this should own the ADC object, but rather when a call to evaluate is invoke it should be taken control -// and then released when the function returns. Refactor this later. -pub struct HealthMonitorChannelsCommunication { - reader: Adc, - reader1: Adc, - pin_3v3: Pin>, - pin_5v: Pin>, - pin_pyro: Pin>, - pin_vcc: Pin>, - pin_ext_3v3: Pin>, - pin_ext_5v: Pin>, - pin_int_5v: Pin>, - pin_int_3v3: Pin>, - pin_failover: Pin>, -} - -impl HealthMonitorChannels for HealthMonitorChannelsCommunication { - fn get_3v3(&mut self) -> Option { - self.reader.read(&mut self.pin_3v3).ok() - } - fn get_5v(&mut self) -> Option { - self.reader.read(&mut self.pin_5v).ok() - } - fn get_pyro(&mut self) -> Option { - self.reader.read(&mut self.pin_pyro).ok() - } - fn get_vcc(&mut self) -> Option { - self.reader.read(&mut self.pin_vcc).ok() - } - fn get_int_5v(&mut self) -> Option { - self.reader.read(&mut self.pin_int_5v).ok() - } - fn get_int_3v3(&mut self) -> Option { - self.reader.read(&mut self.pin_int_3v3).ok() - } - fn get_ext_5v(&mut self) -> Option { - self.reader1.read(&mut self.pin_ext_5v).ok() - } - fn get_ext_3v3(&mut self) -> Option { - self.reader1.read(&mut self.pin_ext_3v3).ok() - } - fn get_failover(&mut self) -> Option { - self.reader1.read(&mut self.pin_failover).ok() - } -} - -impl HealthMonitorChannelsCommunication { - pub fn new( - reader: Adc, - reader1: Adc, - pin_3v3: Pin>, - pin_5v: Pin>, - pin_pyro: Pin>, - pin_vcc: Pin>, - pin_ext_3v3: Pin>, - pin_ext_5v: Pin>, - pin_int_5v: Pin>, - pin_int_3v3: Pin>, - pin_failover: Pin>, - ) -> Self { - HealthMonitorChannelsCommunication { - reader, - reader1, - pin_3v3, - pin_5v, - pin_pyro, - pin_vcc, - pin_ext_3v3, - pin_ext_5v, - pin_int_5v, - pin_int_3v3, - pin_failover, - } - } -} diff --git a/boards/communication/src/main.rs b/boards/communication/src/main.rs index 2ea29f88..7adf8b99 100644 --- a/boards/communication/src/main.rs +++ b/boards/communication/src/main.rs @@ -3,7 +3,6 @@ mod communication; mod data_manager; -mod health; mod types; use atsamd_hal as hal; @@ -15,16 +14,20 @@ use common_arm_atsame::mcan; use communication::Capacities; use communication::{RadioDevice, RadioManager}; use data_manager::DataManager; +use defmt::info; use hal::adc::Adc; use hal::clock::v2::pclk::Pclk; use hal::clock::v2::Source; use hal::gpio::Pins; use hal::gpio::{ - Alternate, Output, Pin, PushPull, PushPullOutput, C, PA05, PB12, PB13, PB14, PB15, + Alternate, Output, Pin, PushPull, PushPullOutput, C, D, PA02, PA04, PA05, PA06, PA07, PB12, + PB13, PB14, PB15, }; use hal::prelude::*; -use hal::sercom::{spi, spi::Config, spi::Duplex, spi::Pads, spi::Spi, IoSet1, Sercom4}; -use health::HealthMonitorChannelsCommunication; +use hal::sercom::uart; +use hal::sercom::{ + spi, spi::Config, spi::Duplex, spi::Pads, spi::Spi, IoSet1, IoSet3, Sercom0, Sercom2, Sercom4, +}; use mcan::messageram::SharedMemory; use messages::command::RadioRate; use messages::health::Health; @@ -49,29 +52,29 @@ mod app { struct Shared { em: ErrorManager, data_manager: DataManager, - health_manager: HealthManager, radio_manager: RadioManager, can0: communication::CanDevice0, } #[local] struct Local { - led: Pin, - sd_manager: SdManager< - Spi< - Config< - Pads< - hal::pac::SERCOM4, - IoSet1, - Pin>, - Pin>, - Pin>, - >, - >, - Duplex, - >, - Pin>, - >, + led: Pin, + gps_uart: GpsUart, + //sd_manager: SdManager< + // Spi< + // Config< + // Pads< + // hal::pac::SERCOM0, + // IoSet3, + // Pin>, + // Pin>, + // Pin>, + // >, + // >, + // Duplex, + // >, + // Pin>, + //>, } #[monotonic(binds = SysTick, default = true)] @@ -121,62 +124,52 @@ mod app { /* Radio config */ let (radio, gclk0) = RadioDevice::new( - tokens.pclks.sercom5, + tokens.pclks.sercom2, &mclk, - peripherals.SERCOM5, - pins.pb17, - pins.pb16, + peripherals.SERCOM2, + pins.pa08, + pins.pa09, gclk0, ); let radio_manager = RadioManager::new(radio); /* SD config */ - let (pclk_sd, gclk0) = Pclk::enable(tokens.pclks.sercom4, gclk0); - let pads_spi = spi::Pads::::default() - .sclk(pins.pb13) - .data_in(pins.pb15) - .data_out(pins.pb12); - let sdmmc_spi = spi::Config::new(&mclk, peripherals.SERCOM4, pads_spi, pclk_sd.freq()) - .length::() - .bit_order(spi::BitOrder::MsbFirst) - .spi_mode(spi::MODE_0) + //let (pclk_sd, gclk0) = Pclk::enable(tokens.pclks.sercom0, gclk0); + //let pads_spi = spi::Pads::::default() + // .sclk(pins.pa05) + // .data_in(pins.pa07) + // .data_out(pins.pa04); + //let sdmmc_spi = spi::Config::new(&mclk, peripherals.SERCOM0, pads_spi, pclk_sd.freq()) + // .length::() + // .bit_order(spi::BitOrder::MsbFirst) + // .spi_mode(spi::MODE_0) + // .enable(); + //let sd_manager = SdManager::new(sdmmc_spi, pins.pa06.into_push_pull_output()); + + let (pclk_gps, gclk0) = Pclk::enable(tokens.pclks.sercom4, gclk0); + //info!("here"); + let pads = hal::sercom::uart::Pads::::default() + .rx(pins.pa13) + .tx(pins.pa12); + + let mut gps_uart = GpsUartConfig::new(&mclk, peripherals.SERCOM4, pads, pclk_gps.freq()) + .baud( + 9600.Hz(), + uart::BaudMode::Fractional(uart::Oversampling::Bits16), + ) .enable(); - let sd_manager = SdManager::new(sdmmc_spi, pins.pb14.into_push_pull_output()); - - /* Setup ADC clocks */ - let (_pclk_adc0, gclk0) = Pclk::enable(tokens.pclks.adc0, gclk0); - let (_pclk_adc1, gclk0) = Pclk::enable(tokens.pclks.adc1, gclk0); - /* Setup ADC */ - let adc0 = Adc::adc0(peripherals.ADC0, &mut mclk); - let adc1 = Adc::adc1(peripherals.ADC1, &mut mclk); - - /* Setup Health Monitor */ - let health_monitor_channels = HealthMonitorChannelsCommunication::new( - adc0, - adc1, - pins.pb01.into(), - pins.pb02.into(), - pins.pb03.into(), - pins.pb00.into(), - pins.pb06.into(), - pins.pb07.into(), - pins.pb08.into(), - pins.pb09.into(), - pins.pb05.into(), - ); - - let health_monitor = HealthMonitor::new(health_monitor_channels, 10000, 5000, 1023); - let health_manager = HealthManager::new(health_monitor); + gps_uart.enable_interrupts(hal::sercom::uart::Flags::RXC); /* Status LED */ - let led = pins.pa05.into_push_pull_output(); - + let led = pins.pa02.into_push_pull_output(); + let mut gps_enable = pins.pb09.into_push_pull_output(); + //gps_enable.set_high().ok(); + gps_enable.set_low().ok(); /* Spawn tasks */ sensor_send::spawn().ok(); state_send::spawn().ok(); blink::spawn().ok(); - report_health::spawn().ok(); /* Monotonic clock */ let mono = Systick::new(core.SYST, gclk0.freq().to_Hz()); @@ -185,11 +178,10 @@ mod app { Shared { em: ErrorManager::new(), data_manager: DataManager::new(), - health_manager, radio_manager, can0, }, - Local { led, sd_manager }, + Local { led, gps_uart }, init::Monotonics(mono), ) } @@ -200,6 +192,15 @@ mod app { loop {} } + #[task(binds = SERCOM2_2, local = [gps_uart], shared = [&em])] + fn gps_rx(mut cx: gps_rx::Context) { + cx.shared.em.run(|| { + let byte = cx.local.gps_uart.read().unwrap(); + info!("GPS: {}", byte); + Ok(()) + }); + } + /// Handles the CAN0 interrupt. #[task(priority = 3, binds = CAN0, shared = [can0, data_manager])] fn can0(mut cx: can0::Context) { @@ -241,22 +242,22 @@ mod app { }); } - #[task(capacity = 10, local = [sd_manager], shared = [&em])] - fn sd_dump(cx: sd_dump::Context, m: Message) { - let manager = cx.local.sd_manager; - cx.shared.em.run(|| { - let mut buf: [u8; 255] = [0; 255]; - let msg_ser = postcard::to_slice_cobs(&m, &mut buf)?; - if let Some(mut file) = manager.file.take() { - manager.write(&mut file, &msg_ser)?; - manager.file = Some(file); - } else if let Ok(mut file) = manager.open_file("log.txt") { - manager.write(&mut file, &msg_ser)?; - manager.file = Some(file); - } - Ok(()) - }); - } + // #[task(capacity = 10, local = [sd_manager], shared = [&em])] + // fn sd_dump(cx: sd_dump::Context, m: Message) { + // let manager = cx.local.sd_manager; + // cx.shared.em.run(|| { + // let mut buf: [u8; 255] = [0; 255]; + // let msg_ser = postcard::to_slice_cobs(&m, &mut buf)?; + // if let Some(mut file) = manager.file.take() { + // manager.write(&mut file, &msg_ser)?; + // manager.file = Some(file); + // } else if let Ok(mut file) = manager.open_file("log.txt") { + // manager.write(&mut file, &msg_ser)?; + // manager.file = Some(file); + // } + // Ok(()) + // }); + //} /** * Sends information about the sensors. @@ -273,7 +274,7 @@ mod app { match msg { Some(x) => { spawn!(send_gs, x.clone())?; - spawn!(sd_dump, x)?; + // spawn!(sd_dump, x)?; } None => { continue; @@ -308,26 +309,6 @@ mod app { spawn_after!(state_send, ExtU64::secs(5)).ok(); } - /** - * Simple health report - */ - #[task(shared = [&em, health_manager])] - fn report_health(mut cx: report_health::Context) { - cx.shared.em.run(|| { - let msg = cx.shared.health_manager.lock(|health_manager| { - let state = health_manager.evaluate(); - Message::new( - &monotonics::now(), - COM_ID, - Health::new(health_manager.monitor.data.clone(), state), - ) - }); - spawn!(send_gs, msg)?; - spawn_after!(report_health, ExtU64::secs(5))?; - Ok(()) - }); - } - #[task(binds = SERCOM5_2, shared = [&em, radio_manager])] fn radio_rx(mut cx: radio_rx::Context) { cx.shared.radio_manager.lock(|radio_manager| { diff --git a/boards/communication/src/types.rs b/boards/communication/src/types.rs index 236121d8..3813ee9a 100644 --- a/boards/communication/src/types.rs +++ b/boards/communication/src/types.rs @@ -1,6 +1,6 @@ use atsamd_hal::gpio::*; use atsamd_hal::sercom::uart::EightBit; -use atsamd_hal::sercom::{uart, IoSet1, Sercom5}; +use atsamd_hal::sercom::{uart, uart::Duplex, IoSet1, IoSet3, Sercom0, Sercom2, Sercom4, Sercom5}; use messages::sender::Sender; use messages::sender::Sender::CommunicationBoard; @@ -12,5 +12,12 @@ pub static COM_ID: Sender = CommunicationBoard; // ------- // Ground Station // ------- -pub type GroundStationPads = uart::PadsFromIds; +pub type GroundStationPads = uart::PadsFromIds; pub type GroundStationUartConfig = uart::Config; + +// ------- +// GPS UART +// ------- +pub type GpsPads = uart::PadsFromIds; +pub type GpsUartConfig = uart::Config; +pub type GpsUart = uart::Uart; diff --git a/boards/nav/Cargo.toml b/boards/nav/Cargo.toml index 9398a12a..76363015 100644 --- a/boards/nav/Cargo.toml +++ b/boards/nav/Cargo.toml @@ -17,3 +17,5 @@ messages = { path = "../../libraries/messages" } systick-monotonic = "1.0.1" defmt = "0.3.2" fdcan = "0.2" + +heapless = "0.7.16" diff --git a/boards/nav/src/data_manager.rs b/boards/nav/src/data_manager.rs index f3e7d695..efc3936f 100644 --- a/boards/nav/src/data_manager.rs +++ b/boards/nav/src/data_manager.rs @@ -1,4 +1,3 @@ -use crate::app::sleep_system; use common_arm::{spawn, HydraError}; use messages::sensor::{ Air, EkfNav1, EkfNav2, EkfQuat, GpsPos1, GpsPos2, GpsVel, Imu1, Imu2, SensorData, UtcTime, @@ -47,9 +46,7 @@ impl DataManager { pub fn handle_data(&mut self, data: Message) -> Result<(), HydraError> { match data.data { messages::Data::Command(command) => match command.data { - messages::command::CommandData::PowerDown(_) => { - spawn!(sleep_system)?; // need proper error handling. could just expect, but that is mal practice. - } + messages::command::CommandData::PowerDown(_) => {} _ => { // We don't care atm about these other commands. } diff --git a/boards/nav/src/main.rs b/boards/nav/src/main.rs index ed05aeed..639c0331 100644 --- a/boards/nav/src/main.rs +++ b/boards/nav/src/main.rs @@ -14,7 +14,8 @@ use fdcan::{ frame::{FrameFormat, TxFrameHeader}, id::StandardId, }; -use stm32h7xx_hal::gpio::gpioa::{PA1, PA2}; +use fugit::RateExtU32; +use stm32h7xx_hal::gpio::gpioa::{PA1, PA2, PA3}; use stm32h7xx_hal::gpio::gpioc::{PC13, PC3}; use stm32h7xx_hal::gpio::Input; use stm32h7xx_hal::gpio::Speed; @@ -41,11 +42,12 @@ mod app { struct SharedResources { data_manager: DataManager, em: ErrorManager, + // rtc: Rtc, } #[local] struct LocalResources { - led_red: PA1>, - led_green: PA2>, + led_red: PA2>, + led_green: PA3>, } #[monotonic(binds = SysTick, default = true)] @@ -81,6 +83,18 @@ mod app { 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, 1.kHz(), ccdr.peripheral.TIM12, &ccdr.clocks); + + c0.set_duty(c0.get_max_duty() / 2); + // PWM outputs are disabled by default + c0.enable(); + assert_eq!(ccdr.clocks.pll1_q_ck().unwrap().raw(), 24_000_000); let fdcan_prec = ccdr .peripheral @@ -120,8 +134,8 @@ mod app { ); // leds - let mut led_red = gpioa.pa1.into_push_pull_output(); - let mut led_green = gpioa.pa2.into_push_pull_output(); + let mut led_red = gpioa.pa2.into_push_pull_output(); + let mut led_green = gpioa.pa3.into_push_pull_output(); // UART for sbg let tx = gpiod.pd1.into_alternate(); @@ -164,6 +178,7 @@ mod app { spawn_after!(blink, ExtU64::millis(200))?; } else { cx.local.led_green.toggle(); + info!("Blinking"); spawn_after!(blink, ExtU64::secs(1))?; } Ok(()) @@ -174,4 +189,18 @@ mod app { fn sleep_system(mut cx: sleep_system::Context) { // Turn off the SBG and CAN } + + //extern "Rust" { + // #[task(capacity = 3, shared = [&em, sd_manager])] + // fn sbg_sd_task(context: sbg_sd_task::Context, data: [u8; SBG_BUFFER_SIZE]); + + //#[task(binds = DMAC_0, shared = [&em], local = [sbg_manager])] + //fn sbg_dma(context: sbg_dma::Context); + + //#[task(capacity = 20, shared = [data_manager])] + //fn sbg_handle_data(context: sbg_handle_data::Context, data: CallbackData); + + //#[task(shared = [rtc, &em])] + //fn sbg_get_time(context: sbg_get_time::Context); + //} } diff --git a/boards/nav/src/sbg_manager.rs b/boards/nav/src/sbg_manager.rs index ab811de2..f4456fcd 100644 --- a/boards/nav/src/sbg_manager.rs +++ b/boards/nav/src/sbg_manager.rs @@ -11,21 +11,31 @@ use core::ffi::c_void; use core::mem::size_of; use core::ptr; // use atsamd_hal::time::*; +use crate::app::sbg_get_time; use crate::app::sbg_handle_data; use crate::app::sbg_sd_task as sbg_sd; use atsamd_hal::prelude::*; use atsamd_hal::sercom::{uart, Sercom, Sercom5}; use common_arm::spawn; +use core::{mem, mem::MaybeUninit}; use defmt::info; use embedded_alloc::Heap; use rtic::Mutex; use sbg_rs::sbg; use sbg_rs::sbg::{CallbackData, SBG, SBG_BUFFER_SIZE}; +use stm32h7xx_hal::dma::{ + dma::{DmaConfig, StreamsTuple}, + PeripheralToMemory, Transfer, +}; use stm32h7xx_hal::gpio::Alternate; use stm32h7xx_hal::gpio::Pin; use stm32h7xx_hal::rtc::Rtc; -pub static mut BUF_DST: SBGBuffer = &mut [0; SBG_BUFFER_SIZE]; +//#[link_section = ".axisram.buffers"] +//static mut SBG_BUFFER: MayberUninit<[u8; SBG_BUFFER_SIZE]> = MaybeUninit::uninit(); + +#[link_section = ".axisram.buffers"] +pub static mut SBG_BUFFER: SBGBuffer = &mut [0; SBG_BUFFER_SIZE]; // Simple heap required by the SBG library static HEAP: Heap = Heap::empty(); @@ -40,12 +50,14 @@ impl SBGManager { rx: Pin<'D', 0, Alternate<8>>, tx: Pin<'D', 1, Alternate<8>>, rtc: Rtc, - mut dma_channel: dmac::Channel, + mut stream_tuple: StreamsTuple, + //mut dma_channel: dmac::Channel, ) -> Self { /* Initialize the Heap */ { use core::mem::MaybeUninit; const HEAP_SIZE: usize = 1024; + // TODO: Could add a link section here to memory. static mut HEAP_MEM: [MaybeUninit; HEAP_SIZE] = [MaybeUninit::uninit(); HEAP_SIZE]; unsafe { HEAP.init(HEAP_MEM.as_ptr() as usize, HEAP_SIZE) } } @@ -57,25 +69,37 @@ impl SBGManager { .unwrap(); let (sbg_rx, sbg_tx) = uart_sbg.split(); - /* DMAC config */ - dma_channel - .as_mut() - .enable_interrupts(dmac::InterruptFlags::new().with_tcmpl(true)); - let xfer = Transfer::new(dma_channel, sbg_rx, unsafe { &mut *BUF_DST }, false) - .expect("DMA err") - .begin(Sercom5::DMA_RX_TRIGGER, dmac::TriggerAction::BURST); - - // There is a bug within the HAL that improperly configures the RTC - // in count32 mode. This is circumvented by first using clock mode then - // converting to count32 mode. - let rtc_temp = Rtc::clock_mode(rtc, 1024.Hz(), mclk); - let mut rtc = rtc_temp.into_count32_mode(); - rtc.set_count32(0); - - let sbg: sbg::SBG = sbg::SBG::new(sbg_tx, rtc, |data| { - sbg_handle_data::spawn(data).ok(); + // TODO: This could be wrong. It's a bit of a guess. + //let sbg_buffer: &'static mut [u8; SBG_BUFFER_SIZE] = { + // let buf: &mut [MaybeUninit; SBG_BUFFER_SIZE] = + // unsafe { &mut *(core::ptr::addr_of_mut!(SBG_BUFFER) as *mut _) }; + // for (i, value) in buf.iter_mut().enumerate() { + // unsafe { value.as_mut_ptr().write(i as u8) }; + // } + // unsafe { SBG_BUFFER.assume_init_mut() } + // }; + + let config = DmaConfig::default().memory_increment(true); + let transfer: Transfer<_, _, _, PeripheralToMemory> = Transfer::init( + stream_tuple.0, + sbg_rx, + unsafe { &mut *SBG_BUFFER }, + None, + config, + ); + + transfer.start(|serial| { + serial.enable_rx_dma(); }); + let sbg: sbg::SBG = sbg::SBG::new( + sbg_tx, + |_| sbg_get_time::spawn().ok(), + |data| { + sbg_handle_data::spawn(data).ok(); + }, + ); + SBGManager { sbg_device: sbg, xfer: Some(xfer), @@ -83,6 +107,10 @@ impl SBGManager { } } +pub fn sbg_get_time(mut cx: sbg_get_time::Context) -> u32 { + cx.shared.rtc.lock(|rtc| rtc.date_time().unwrap()) +} + pub fn sbg_handle_data(mut cx: sbg_handle_data::Context, data: CallbackData) { cx.shared.data_manager.lock(|manager| match data { CallbackData::UtcTime(x) => manager.utc_time = Some(x), diff --git a/boards/power/src/main.rs b/boards/power/src/main.rs index 17a62d55..f8d6fc6d 100644 --- a/boards/power/src/main.rs +++ b/boards/power/src/main.rs @@ -11,9 +11,10 @@ use common_arm::*; use common_arm_atsame::mcan; use communication::CanDevice0; use communication::Capacities; +use defmt::info; use hal::clock::v2::pclk::Pclk; use hal::clock::v2::Source; -use hal::gpio::{Pin, Pins, PushPullOutput, PB16, PB17}; +use hal::gpio::{Pin, Pins, PushPullOutput, PA08, PB16, PB17}; use hal::prelude::*; use mcan::messageram::SharedMemory; use systick_monotonic::*; @@ -40,6 +41,7 @@ mod app { struct Local { led_green: Pin, led_red: Pin, + reg_5v: Pin, } #[monotonic(binds = SysTick, default = true)] @@ -50,6 +52,7 @@ mod app { can_memory: SharedMemory = SharedMemory::new() ])] fn init(cx: init::Context) -> (Shared, Local, init::Monotonics) { + info!("Starting up"); let mut peripherals = cx.device; let core = cx.core; let pins = Pins::new(peripherals.PORT); @@ -89,8 +92,10 @@ mod app { // LEDs let led_green = pins.pb16.into_push_pull_output(); let led_red = pins.pb17.into_push_pull_output(); + let reg_5v = pins.pa08.into_push_pull_output(); blink::spawn().ok(); + reg_5v::spawn().ok(); // adc::spawn().ok(); /* Monotonic clock */ @@ -102,7 +107,11 @@ mod app { data_manager: DataManager::new(), can0, }, - Local { led_green, led_red }, + Local { + led_green, + led_red, + reg_5v, + }, init::Monotonics(mono), ) } @@ -113,6 +122,15 @@ mod app { loop {} } + #[task(local = [reg_5v], shared = [&em])] + fn reg_5v(cx: reg_5v::Context) { + cx.shared.em.run(|| { + info!("Enable 5V regulator"); + cx.local.reg_5v.set_high()?; + Ok(()) + }); + } + // #[task(local = [adc_test, adc_pin], shared = [&em])] // fn adc(cx: adc::Context) { // // test adc for 5v PWR sense From 8a84b44e84685adfc14f85b439745b1318a3b61a Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Sun, 4 Aug 2024 09:37:42 -0400 Subject: [PATCH 08/47] ... --- .cargo/{config => config.toml} | 28 +- .vscode/launch.json | 41 +++ Cargo.lock | 191 ++---------- Cargo.toml | 20 +- boards/communication/Cargo.toml | 7 +- boards/communication/src/communication.rs | 69 ++--- boards/communication/src/main.rs | 293 ++++++++++++------ boards/communication/src/types.rs | 2 +- boards/nav/Cargo.toml | 6 +- boards/nav/src/communication.rs | 53 ++++ boards/nav/src/main.rs | 135 ++++++-- boards/nav/src/sbg_manager.rs | 116 +++---- boards/nav/src/types.rs | 6 + boards/recovery/src/main.rs | 27 +- .../src/state_machine/states/ascent.rs | 6 +- .../state_machine/states/wait_for_recovery.rs | 11 +- boards/sensor/src/sbg_manager.rs | 2 +- examples/rtic/src/main.rs | 4 +- examples/simple/src/main.rs | 14 +- libraries/common-arm/Cargo.toml | 1 - .../common-arm/src/error/error_manager.rs | 4 +- libraries/common-arm/src/error/hydra_error.rs | 6 +- libraries/common-arm/src/sd_manager.rs | 2 + libraries/messages/src/lib.rs | 8 +- 24 files changed, 617 insertions(+), 435 deletions(-) rename .cargo/{config => config.toml} (67%) diff --git a/.cargo/config b/.cargo/config.toml similarity index 67% rename from .cargo/config rename to .cargo/config.toml index 890523d3..4951091d 100644 --- a/.cargo/config +++ b/.cargo/config.toml @@ -1,14 +1,14 @@ -[target.'cfg(all(target_arch = "arm", target_os = "none"))'] -# This runner needs to be parametric so we can pass in the chip name -#runner = "probe-rs run --chip STM32H733VGTx --protocol swd" -runner = "probe-rs run --chip ATSAME51J18A --protocol swd" -rustflags = [ - "-C", "link-arg=-Tlink.x", - "-C", "link-arg=-Tdefmt.x", -] - -[env] -DEFMT_LOG="info" - -[build] -target = "thumbv7em-none-eabihf" # Cortex-M4F and Cortex-M7F (with FPU) +[target.'cfg(all(target_arch = "arm", target_os = "none"))'] +# This runner needs to be parametric so we can pass in the chip name +runner = "probe-rs run --chip STM32H733VGTx --protocol swd" +#runner = "probe-rs run --chip ATSAME51J18A --protocol swd" +rustflags = [ + "-C", "link-arg=-Tlink.x", + "-C", "link-arg=-Tdefmt.x", +] + +[env] +DEFMT_LOG="info" + +[build] +target = "thumbv7em-none-eabihf" # Cortex-M4F and Cortex-M7F (with FPU) \ No newline at end of file diff --git a/.vscode/launch.json b/.vscode/launch.json index d21c5cd1..88fc5d3d 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -4,6 +4,47 @@ // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 "version": "0.2.0", "configurations": [ + { + "type": "probe-rs-debug", + "request": "launch", + "name": "Probe-rs Debug Recovery", + "chip": "ATSAME51J18A", + "coreConfigs": [ + { + "programBinary": "${workspaceFolder}/target/thumbv7em-none-eabihf/debug/recovery", + } + ] + }, + { + "type": "probe-rs-debug", + "request": "launch", + "name": "Probe-rs Debug Link (Release)", + "chip": "ATSAME51J18A", + "flashingConfig": { + "flashingEnabled": true, + "haltAfterReset": false, + "formatOptions": { + //!MODIFY (or remove). Valid values are: 'bin', 'hex', 'elf'(default), 'idf' + "binaryFormat": "elf" + } + }, + "coreConfigs": [ + { + "programBinary": "${workspaceFolder}/target/thumbv7em-none-eabihf/release/communication", + } + ] + }, + { + "type": "probe-rs-debug", + "request": "launch", + "name": "Probe-rs Debug nav (Debug)", + "chip": "STM32H733VGTx", + "coreConfigs": [ + { + "programBinary": "${workspaceFolder}/target/thumbv7em-none-eabihf/debug/nav", + } + ] + }, { "type": "cortex-debug", "request": "launch", diff --git a/Cargo.lock b/Cargo.lock index 99ea48bf..f2a269ee 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -40,21 +40,17 @@ dependencies = [ [[package]] name = "atsamd-hal" -version = "0.17.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "84fba26d12e032c93fb0de8c0e0ff34fafea35192f61cd0fc247838d78cda22e" +version = "0.16.0" +source = "git+https://github.com/uorocketry/atsamd#5b3e781911d5824b7317a4af0ad3ea97873df430" dependencies = [ "aes", - "atsamd-hal-macros", "atsame51j", "bitfield 0.13.2", "bitflags 1.3.2", "cipher", "cortex-m", - "embedded-hal 0.2.7", - "embedded-hal 1.0.0", - "embedded-hal-nb", - "embedded-io 0.6.1", + "defmt", + "embedded-hal", "fugit", "mcan-core", "modular-bitfield", @@ -69,24 +65,10 @@ dependencies = [ "void", ] -[[package]] -name = "atsamd-hal-macros" -version = "0.1.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b27ccb060a908ff8a40acde574902a6bd283d3420dd4ffcdb6a327225f55e098" -dependencies = [ - "litrs", - "phf", - "phf_codegen", - "serde", - "serde_yaml", -] - [[package]] name = "atsame51j" version = "0.13.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8c295af2278e92dbfb65189097a8858556685456e601c7c689e6a3c0290f10b7" +source = "git+https://github.com/uorocketry/atsamd#5b3e781911d5824b7317a4af0ad3ea97873df430" dependencies = [ "cortex-m", "cortex-m-rt", @@ -257,7 +239,7 @@ dependencies = [ "defmt", "defmt-rtt", "derive_more", - "embedded-hal 0.2.7", + "embedded-hal", "embedded-sdmmc", "heapless", "mcan", @@ -273,7 +255,7 @@ version = "0.1.0" dependencies = [ "atsamd-hal", "common-arm", - "embedded-hal 0.2.7", + "embedded-hal", "mcan", ] @@ -316,9 +298,11 @@ dependencies = [ "cortex-m-rt", "cortex-m-rtic", "defmt", + "defmt-rtt", "embedded-sdmmc", "heapless", "messages", + "panic-probe", "postcard", "systick-monotonic", "typenum", @@ -339,7 +323,7 @@ dependencies = [ "bare-metal 0.2.5", "bitfield 0.13.2", "critical-section", - "embedded-hal 0.2.7", + "embedded-hal", "volatile-register", ] @@ -537,34 +521,12 @@ dependencies = [ "void", ] -[[package]] -name = "embedded-hal" -version = "1.0.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "361a90feb7004eca4019fb28352a9465666b24f840f5c3cddf0ff13920590b89" - -[[package]] -name = "embedded-hal-nb" -version = "1.0.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fba4268c14288c828995299e59b12babdbe170f6c6d73731af1b4648142e8605" -dependencies = [ - "embedded-hal 1.0.0", - "nb 1.1.0", -] - [[package]] name = "embedded-io" version = "0.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ef1a6892d9eef45c8fa6b9e0086428a2cca8491aca8f787c534a3d6d0bcb3ced" -[[package]] -name = "embedded-io" -version = "0.6.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "edd0f118536f44f5ccd48bcb8b111bdc3de888b58c74639dfb034a357d0f206d" - [[package]] name = "embedded-sdmmc" version = "0.3.0" @@ -572,7 +534,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "6d3bf0a2b5becb87e9a329d9290f131e4d10fec39b56d129926826a7cbea1e7a" dependencies = [ "byteorder", - "embedded-hal 0.2.7", + "embedded-hal", "log", "nb 0.1.3", ] @@ -604,12 +566,6 @@ dependencies = [ "syn 2.0.72", ] -[[package]] -name = "equivalent" -version = "1.0.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5443807d6dff69373d433ab9ef5378ad8df50ca6298caf15de6e52e24aaf54d5" - [[package]] name = "errno" version = "0.3.9" @@ -697,12 +653,6 @@ version = "0.12.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8a9ee70c43aaf417c914396645a0fa852624801b24ebb7ae78fe8272889ac888" -[[package]] -name = "hashbrown" -version = "0.14.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e5274423e17b7c9fc20b6e7e208532f9b19825d82dfd615708b70edd83df41f1" - [[package]] name = "heapless" version = "0.7.17" @@ -724,17 +674,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bd070e393353796e801d209ad339e89596eb4c8d430d18ede6a1cced8fafbd99" dependencies = [ "autocfg", - "hashbrown 0.12.3", -] - -[[package]] -name = "indexmap" -version = "2.2.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "168fb715dda47215e360912c096649d23d58bf392ac62f73919e831745e40f26" -dependencies = [ - "equivalent", - "hashbrown 0.14.5", + "hashbrown", ] [[package]] @@ -746,12 +686,6 @@ dependencies = [ "libc", ] -[[package]] -name = "itoa" -version = "1.0.11" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "49f1f14873335454500d59611f1cf4a4b0f786f9ac11f4312a78e4cf2566695b" - [[package]] name = "lazy_static" version = "1.5.0" @@ -782,15 +716,6 @@ version = "0.4.14" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "78b3ae25bc7c8c38cec158d1f2757ee79e9b3740fbc7ccf0e59e4b08d793fa89" -[[package]] -name = "litrs" -version = "0.4.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b4ce301924b7887e9d637144fdade93f9dfff9b60981d4ac161db09720d39aa5" -dependencies = [ - "proc-macro2 1.0.86", -] - [[package]] name = "lock_api" version = "0.4.12" @@ -841,7 +766,7 @@ source = "git+https://github.com/uorocketry/rust-mavlink.git#e9f4057deedce85cae5 dependencies = [ "byteorder", "crc-any", - "embedded-hal 0.2.7", + "embedded-hal", "nb 1.1.0", "serde", "serde_arrays", @@ -930,23 +855,26 @@ name = "ms5611-01ba" version = "0.1.0" source = "git+https://github.com/NoahSprenger/ms5611-01ba?branch=embedded-hal-02#fdb9b51f953d854c7b99079ae3583f82c6378bbc" dependencies = [ - "embedded-hal 0.2.7", + "embedded-hal", ] [[package]] name = "nav" version = "0.1.0" dependencies = [ + "chrono", "common-arm", "common-arm-stm32h7", "cortex-m", "cortex-m-rt", "cortex-m-rtic", "defmt", + "embedded-alloc", "fdcan", "heapless", "messages", "postcard", + "sbg-rs", "stm32h7xx-hal", "systick-monotonic", ] @@ -1074,44 +1002,6 @@ version = "1.0.15" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a" -[[package]] -name = "phf" -version = "0.11.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ade2d8b8f33c7333b51bcf0428d37e217e9f32192ae4772156f65063b8ce03dc" -dependencies = [ - "phf_shared", -] - -[[package]] -name = "phf_codegen" -version = "0.11.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e8d39688d359e6b34654d328e262234662d16cc0f60ec8dcbe5e718709342a5a" -dependencies = [ - "phf_generator", - "phf_shared", -] - -[[package]] -name = "phf_generator" -version = "0.11.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "48e4cc64c2ad9ebe670cb8fd69dd50ae301650392e81c05f9bfcb2d5bdbc24b0" -dependencies = [ - "phf_shared", - "rand", -] - -[[package]] -name = "phf_shared" -version = "0.11.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "90fcb95eef784c2ac79119d1dd819e162b5da872ce6f3c3abe1e8ca1c082f72b" -dependencies = [ - "siphasher", -] - [[package]] name = "postcard" version = "1.0.8" @@ -1120,7 +1010,7 @@ checksum = "a55c51ee6c0db07e68448e336cf8ea4131a620edefebf9893e759b2d793420f8" dependencies = [ "cobs", "defmt", - "embedded-io 0.4.0", + "embedded-io", "heapless", "serde", ] @@ -1306,7 +1196,7 @@ dependencies = [ "cortex-m-rt", "cortex-m-rtic", "defmt", - "embedded-hal 0.2.7", + "embedded-hal", "enum_dispatch", "heapless", "messages", @@ -1349,7 +1239,7 @@ version = "1.0.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "5f5e215601dc467752c2bddc6284a622c6f3d2bab569d992adcd5ab7e4cb9478" dependencies = [ - "indexmap 1.9.3", + "indexmap", "proc-macro2 1.0.86", "quote 1.0.36", "syn 1.0.109", @@ -1418,12 +1308,6 @@ dependencies = [ "wait-timeout", ] -[[package]] -name = "ryu" -version = "1.0.18" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f3cb5ba0dc43242ce17de99c180e96db90b235b8a9fdc9543c96d2209116bd9f" - [[package]] name = "sbg-rs" version = "0.1.0" @@ -1435,7 +1319,7 @@ dependencies = [ "cortex-m", "cortex-m-rt", "defmt", - "embedded-hal 0.2.7", + "embedded-hal", "heapless", "messages", "nb 1.1.0", @@ -1525,19 +1409,6 @@ dependencies = [ "syn 2.0.72", ] -[[package]] -name = "serde_yaml" -version = "0.9.34+deprecated" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6a8b1a1a2ebf674015cc02edccce75287f1a0130d394307b36743c2f5d504b47" -dependencies = [ - "indexmap 2.2.6", - "itoa", - "ryu", - "serde", - "unsafe-libyaml", -] - [[package]] name = "serial" version = "0.4.0" @@ -1593,12 +1464,6 @@ dependencies = [ "panic-halt", ] -[[package]] -name = "siphasher" -version = "0.3.11" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "38b58827f4464d87d377d175e90bf58eb00fd8716ff0a62f80356b5e61555d0d" - [[package]] name = "spin" version = "0.9.8" @@ -1643,7 +1508,7 @@ dependencies = [ "cortex-m", "defmt", "embedded-dma", - "embedded-hal 0.2.7", + "embedded-hal", "embedded-storage", "fdcan", "fugit", @@ -1674,7 +1539,7 @@ dependencies = [ "cast", "cortex-m", "cortex-m-rt", - "embedded-hal 0.2.7", + "embedded-hal", "embedded-time", "nb 1.1.0", "rtcc", @@ -1823,12 +1688,6 @@ version = "0.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "fc72304796d0818e357ead4e000d19c9c174ab23dc11093ac919054d20a6a7fc" -[[package]] -name = "unsafe-libyaml" -version = "0.2.11" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "673aac59facbab8a9007c7f6108d11f63b603f7cabff99fabf650fea5c32b861" - [[package]] name = "vcell" version = "0.1.3" @@ -1837,9 +1696,9 @@ checksum = "77439c1b53d2303b20d9459b1ade71a83c716e3f9c34f3228c00e6f185d6c002" [[package]] name = "version_check" -version = "0.9.4" +version = "0.9.5" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "49874b5167b65d7193b8aba1567f5c7d93d001cafc34600cee003eda787e483f" +checksum = "0b928f33d975fc6ad9f86c8f283853ad26bdd5b10b7f1542aa2fa15e2289105a" [[package]] name = "void" diff --git a/Cargo.toml b/Cargo.toml index 8a0f8b8f..a93172d4 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -16,9 +16,10 @@ git = "https://github.com/stm32-rs/stm32h7xx-hal" features = ["defmt", "rt", "stm32h735", "can", "rtc"] [workspace.dependencies.atsamd-hal] -#git = "https://github.com/uorocketry/atsamd" -version = "0.17.0" -features = ["same51j", "same51j-rt", "dma", "can"] +git = "https://github.com/uorocketry/atsamd" +#git = "https://github.com/atsamd-rs/atsamd" +#version = "0.17.0" +features = ["defmt", "same51j", "same51j-rt", "dma", "can"] [workspace.dependencies.stm32l0xx-hal] git = "https://github.com/stm32-rs/stm32l0xx-hal" @@ -37,6 +38,7 @@ features = ["critical-section-single-core"] [profile.dev] # Using LTO causes issues with GDB. lto = false +panic = 'abort' # Only optimize dependencies for size in debug, keeping the top crate debug friendly [profile.dev.package."*"] @@ -52,11 +54,13 @@ debug = true debug = true [profile.release] -# symbols are nice and they don't increase the size on Flash -#debug = true -debug = 1 -#lto = true There is an issue with this where it says the interrupts symbol is defined multiple times. Only happens for the STM32H7XX. -opt-level = 1 +codegen-units = 1 +debug = 2 +debug-assertions = false # <- +incremental = false +lto = 'fat' +opt-level = 3 # <- +overflow-checks = false # <- [profile.release.package.sbg-rs] debug = true diff --git a/boards/communication/Cargo.toml b/boards/communication/Cargo.toml index fd97f157..cbb5dada 100644 --- a/boards/communication/Cargo.toml +++ b/boards/communication/Cargo.toml @@ -10,7 +10,6 @@ cortex-m = { workspace = true } cortex-m-rt = "^0.7.0" cortex-m-rtic = "1.1.3" systick-monotonic = "1.0.1" -defmt = "0.3.2" postcard = "1.0.2" heapless = "0.7.16" common-arm-atsame = { path = "../../libraries/common-arm-atsame" } @@ -18,4 +17,8 @@ common-arm = { path = "../../libraries/common-arm" } atsamd-hal = { workspace = true } messages = { path = "../../libraries/messages" } typenum = "1.16.0" -embedded-sdmmc = "0.3.0" \ No newline at end of file +embedded-sdmmc = "0.3.0" +#panic-halt = "0.2.0" +defmt = "0.3" +defmt-rtt = "0.4" +panic-probe = { version = "0.3", features = ["print-defmt"] } \ No newline at end of file diff --git a/boards/communication/src/communication.rs b/boards/communication/src/communication.rs index 12ece588..4825ed05 100644 --- a/boards/communication/src/communication.rs +++ b/boards/communication/src/communication.rs @@ -2,14 +2,16 @@ use crate::data_manager::DataManager; use crate::types::*; use atsamd_hal::can::Dependencies; use atsamd_hal::clock::v2::ahb::AhbClk; -use atsamd_hal::clock::v2::gclk::Gclk0Id; +use atsamd_hal::clock::v2::gclk::{Gclk0Id, Gclk1Id}; use atsamd_hal::clock::v2::pclk::Pclk; use atsamd_hal::clock::v2::pclk::PclkToken; use atsamd_hal::clock::v2::types::Can0; use atsamd_hal::clock::v2::Source; use atsamd_hal::gpio::PA08; use atsamd_hal::gpio::PA09; -use atsamd_hal::gpio::{Alternate, AlternateI, Disabled, Floating, Pin, I, PA22, PA23, PB16, PB17}; +use atsamd_hal::gpio::{ + Alternate, AlternateI, Disabled, Floating, Pin, I, PA12, PA13, PA22, PA23, PB16, PB17, +}; use atsamd_hal::pac::CAN0; use atsamd_hal::pac::MCLK; use atsamd_hal::pac::SERCOM0; @@ -21,7 +23,7 @@ use atsamd_hal::sercom::uart; use atsamd_hal::sercom::uart::Uart; use atsamd_hal::sercom::uart::{RxDuplex, TxDuplex}; use atsamd_hal::typelevel::Increment; -use common_arm::{herror, HydraError}; +use common_arm:: HydraError; use common_arm_atsame::mcan; use common_arm_atsame::mcan::message::{rx, Raw}; use common_arm_atsame::mcan::tx_buffers::DynTx; @@ -43,9 +45,8 @@ use messages::mavlink; use messages::ErrorContext; use messages::Message; use postcard::from_bytes; -use systick_monotonic::fugit::RateExtU32; +use systick_monotonic::*; use typenum::{U0, U128, U32, U64}; - pub struct Capacities; impl mcan::messageram::Capacities for Capacities { @@ -91,10 +92,10 @@ impl CanDevice0 { Dependencies::new(gclk0, pclk_can, ahb_clock, can_rx, can_tx, peripheral); let mut can = - mcan::bus::CanConfigurable::new(200.kHz(), can_dependencies, can_memory).unwrap(); + mcan::bus::CanConfigurable::new(fugit::RateExtU32::kHz(200), can_dependencies, can_memory).unwrap(); can.config().mode = Mode::Fd { allow_bit_rate_switching: false, - data_phase_timing: BitTiming::new(500.kHz()), + data_phase_timing: BitTiming::new(fugit::RateExtU32::kHz(500)), }; if loopback { @@ -156,8 +157,10 @@ impl CanDevice0 { mask: ecan::StandardId::ZERO, }) .unwrap_or_else(|_| panic!("Ground Station filter")); - + + // info!("Can Device 0 initialized"); let can = can.finalize().unwrap(); + // info!("Can Device 0 finalized"); ( CanDevice0 { can, @@ -193,7 +196,7 @@ impl CanDevice0 { data_manager.handle_data(data); } Err(_) => { - herror!(Error, ErrorContext::UnkownCanMessage); + // error!("Error, ErrorContext::UnkownCanMessage"); } } } @@ -205,7 +208,7 @@ impl CanDevice0 { data_manager.handle_data(data); } Err(_) => { - herror!(Error, ErrorContext::UnkownCanMessage); + // error!("Error, ErrorContext::UnkownCanMessage"); } } } @@ -222,37 +225,16 @@ pub struct RadioDevice { } impl RadioDevice { - pub fn new( - radio_token: PclkToken, - mclk: &MCLK, - sercom: SERCOM2, - rx_pin: Pin>, - tx_pin: Pin>, - gclk0: S, - ) -> (Self, S::Inc) - where - S: Source + Increment, - { - let (pclk_radio, gclk0) = Pclk::enable(radio_token, gclk0); - let pads = uart::Pads::::default() - .rx(rx_pin) - .tx(tx_pin); - let uart = GroundStationUartConfig::new(mclk, sercom, pads, pclk_radio.freq()) - .baud( - 57600.Hz(), - uart::BaudMode::Fractional(uart::Oversampling::Bits16), - ) - .enable(); + pub fn new(mut uart: atsamd_hal::sercom::uart::Uart) -> Self { let (mut rx, tx) = uart.split(); + // setup interrupts rx.enable_interrupts(uart::Flags::RXC); - ( - RadioDevice { - transmitter: tx, - receiver: PeekReader::new(rx), - }, - gclk0, - ) + + RadioDevice { + transmitter: tx, + receiver: PeekReader::new(rx), + } } } @@ -271,15 +253,20 @@ impl RadioManager { mav_sequence: 0, } } - pub fn send_message(&mut self, payload: [u8; 255]) -> Result<(), HydraError> { + 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: payload }, + mavlink::uorocketry::POSTCARD_MESSAGE_DATA { message: fixed_payload }, ); mavlink::write_versioned_msg( &mut self.radio.transmitter, @@ -306,7 +293,7 @@ impl RadioManager { // weird Ok syntax to coerce to hydra error type. } _ => { - herror!(Error, ErrorContext::UnkownPostcardMessage); + // error!("Error, ErrorContext::UnkownPostcardMessage"); return Err(mavlink::error::MessageReadError::Io.into()); } } diff --git a/boards/communication/src/main.rs b/boards/communication/src/main.rs index 7adf8b99..0ee69934 100644 --- a/boards/communication/src/main.rs +++ b/boards/communication/src/main.rs @@ -5,22 +5,24 @@ mod communication; mod data_manager; mod types; +use atsamd_hal::rtc::Count32Mode; use atsamd_hal as hal; use common_arm::HealthManager; use common_arm::HealthMonitor; use common_arm::SdManager; use common_arm::*; use common_arm_atsame::mcan; +// use panic_halt as _; + use communication::Capacities; use communication::{RadioDevice, RadioManager}; use data_manager::DataManager; -use defmt::info; use hal::adc::Adc; use hal::clock::v2::pclk::Pclk; use hal::clock::v2::Source; use hal::gpio::Pins; use hal::gpio::{ - Alternate, Output, Pin, PushPull, PushPullOutput, C, D, PA02, PA04, PA05, PA06, PA07, PB12, + Alternate, Output, Pin, PushPull, PushPullOutput, C, D, PA02, PA04, PA05, PA06, PA03, PA07, PB12, PA08, PA09, PB13, PB14, PB15, }; use hal::prelude::*; @@ -34,18 +36,53 @@ use messages::health::Health; use messages::state::State; use messages::*; use systick_monotonic::*; +use core::cell::RefCell; use types::*; +use atsamd_hal::{ + clock::v2::{ + clock_system_at_reset, + xosc::{CrystalCurrent, SafeClockDiv, StartUpDelay, Xosc}, + }, + pac::Peripherals, +}; +use cortex_m::interrupt::Mutex; +use atsamd_hal::time::*; +use fugit::ExtU64; +use cortex_m_rt::exception; +use cortex_m_rt::ExceptionFrame; +use defmt::info; +use defmt_rtt as _; // global logger +use panic_probe as _; +use fugit::ExtU32; +use fugit::RateExtU32; + +#[inline(never)] +#[defmt::panic_handler] +fn panic() -> ! { + cortex_m::asm::udf() +} -/// Custom panic handler. -/// Reset the system if a panic occurs. -#[panic_handler] -fn panic(_info: &core::panic::PanicInfo) -> ! { - atsamd_hal::pac::SCB::sys_reset(); +/// Hardfault handler. +/// +/// Terminates the application and makes a semihosting-capable debug tool exit +/// with an error. This seems better than the default, which is to spin in a +/// loop. +#[cortex_m_rt::exception] +unsafe fn HardFault(_frame: &cortex_m_rt::ExceptionFrame) -> ! { + loop { + + } } +static RTC: Mutex>>> = Mutex::new(RefCell::new(None)); + #[rtic::app(device = hal::pac, peripherals = true, dispatchers = [EVSYS_0, EVSYS_1, EVSYS_2])] mod app { + use atsamd_hal::clock::v2::gclk::{self, Gclk}; + use fugit::RateExtU64; + use state::StateData; + use super::*; #[shared] @@ -58,27 +95,28 @@ mod app { #[local] struct Local { - led: Pin, - gps_uart: GpsUart, - //sd_manager: SdManager< - // Spi< - // Config< - // Pads< - // hal::pac::SERCOM0, - // IoSet3, - // Pin>, - // Pin>, - // Pin>, + led: Pin, + // gps_uart: GpsUart, + // sd_manager: SdManager< + // Spi< + // Config< + // Pads< + // hal::pac::SERCOM0, + // IoSet3, + // Pin>, + // Pin>, + // Pin>, + // >, // >, + // Duplex, // >, - // Duplex, + // Pin>, // >, - // Pin>, - //>, } #[monotonic(binds = SysTick, default = true)] - type SysMono = Systick<100>; // 100 Hz / 10 ms granularity + type MyMono = Systick<100>; // 100 Hz / 10 ms granularity + #[init(local = [ #[link_section = ".can"] @@ -86,14 +124,14 @@ mod app { ])] fn init(cx: init::Context) -> (Shared, Local, init::Monotonics) { let mut peripherals = cx.device; - let core = cx.core; + let mut core = cx.core; let pins = Pins::new(peripherals.PORT); // let mut dmac = DmaController::init(peripherals.DMAC, &mut peripherals.PM); // let dmaChannels = dmac.split(); /* Logging Setup */ - HydraLogging::set_ground_station_callback(queue_gs_message); + // HydraLogging::set_ground_station_callback(queue_gs_message); /* Clock setup */ let (_, clocks, tokens) = atsamd_hal::clock::v2::clock_system_at_reset( @@ -103,6 +141,7 @@ mod app { peripherals.MCLK, &mut peripherals.NVMCTRL, ); + let gclk0 = clocks.gclk0; // SAFETY: Misusing the PAC API can break the system. @@ -110,6 +149,7 @@ mod app { let (_, _, _, mut mclk) = unsafe { clocks.pac.steal() }; /* CAN config */ + let (pclk_can, gclk0) = Pclk::enable(tokens.pclks.can0, gclk0); let (can0, gclk0) = communication::CanDevice0::new( pins.pa23.into_mode(), @@ -119,61 +159,100 @@ mod app { peripherals.CAN0, gclk0, cx.local.can_memory, - false, + true, ); + // setup external osc + // let xosc0 = atsamd_hal::clock::v2::xosc::Xosc::from_crystal( + // tokens.xosc0, + // pins.pa14, + // pins.pa15, + // 48_000_000.Hz(), + // ).current(CrystalCurrent::Medium) + // .loop_control(true) + // .low_buf_gain(true) + // .start_up_delay(StartUpDelay::Delay488us).enable(); + // while !xosc0.is_ready() { + // info!("Waiting for XOSC0 to stabilize"); + // } + + // let (mut gclk0, dfll) = + // hal::clock::v2::gclk::Gclk::from_source(tokens.gclks.gclk2, xosc0); + // let gclk2 = gclk2.div(gclk::GclkDiv8::Div(1)).enable(); + let (pclk_radio, gclk0) = Pclk::enable(tokens.pclks.sercom2, gclk0); /* Radio config */ - let (radio, gclk0) = RadioDevice::new( - tokens.pclks.sercom2, - &mclk, - peripherals.SERCOM2, - pins.pa08, - pins.pa09, - gclk0, - ); + + let rx: Pin> = pins.pa08.into_alternate(); + let tx: Pin> = pins.pa09.into_alternate(); + let pads = uart::Pads::::default() + .rx(rx) + .tx(tx); + let mut uart = GroundStationUartConfig::new(&mclk, peripherals.SERCOM2, pads, pclk_radio.freq()) + .baud( + RateExtU32::Hz(57600), + uart::BaudMode::Fractional(uart::Oversampling::Bits16), + ) + .enable(); + + let (radio) = RadioDevice::new(uart); let radio_manager = RadioManager::new(radio); - /* SD config */ - //let (pclk_sd, gclk0) = Pclk::enable(tokens.pclks.sercom0, gclk0); - //let pads_spi = spi::Pads::::default() - // .sclk(pins.pa05) - // .data_in(pins.pa07) - // .data_out(pins.pa04); - //let sdmmc_spi = spi::Config::new(&mclk, peripherals.SERCOM0, pads_spi, pclk_sd.freq()) - // .length::() - // .bit_order(spi::BitOrder::MsbFirst) - // .spi_mode(spi::MODE_0) - // .enable(); - //let sd_manager = SdManager::new(sdmmc_spi, pins.pa06.into_push_pull_output()); + // /* SD config */ + // let (mut gclk1, dfll) = + // hal::clock::v2::gclk::Gclk::from_source(tokens.gclks.gclk1, clocks.dfll); + // let gclk1 = gclk1.div(gclk::GclkDiv16::Div(3)).enable(); // 48 / 3 = 16 MHzs + // let (pclk_sd, gclk1) = Pclk::enable(tokens.pclks.sercom0, gclk1); + + // let pads_spi = spi::Pads::::default() + // .sclk(pins.pa05) + // .data_in(pins.pa07) + // .data_out(pins.pa04); + // let sdmmc_spi = spi::Config::new(&mclk, peripherals.SERCOM0, pads_spi, pclk_sd.freq()) + // .length::() + // .bit_order(spi::BitOrder::MsbFirst) + // .spi_mode(spi::MODE_0) + // .enable(); + // let sd_manager = SdManager::new(sdmmc_spi, pins.pa06.into_push_pull_output()); let (pclk_gps, gclk0) = Pclk::enable(tokens.pclks.sercom4, gclk0); //info!("here"); let pads = hal::sercom::uart::Pads::::default() - .rx(pins.pa13) - .tx(pins.pa12); + .rx(pins.pa12) + .tx(pins.pa13); let mut gps_uart = GpsUartConfig::new(&mclk, peripherals.SERCOM4, pads, pclk_gps.freq()) .baud( - 9600.Hz(), + RateExtU32::Hz(9600), uart::BaudMode::Fractional(uart::Oversampling::Bits16), ) .enable(); - gps_uart.enable_interrupts(hal::sercom::uart::Flags::RXC); + // gps_uart.enable_interrupts(hal::sercom::uart::Flags::RXC); /* Status LED */ - let led = pins.pa02.into_push_pull_output(); - let mut gps_enable = pins.pb09.into_push_pull_output(); - //gps_enable.set_high().ok(); - gps_enable.set_low().ok(); + // info!("Setting up LED"); + // let led = pins.pa02.into_push_pull_output(); + // let mut gps_enable = pins.pb09.into_push_pull_output(); + // //gps_enable.set_high().ok(); + // gps_enable.set_low().ok(); /* Spawn tasks */ - sensor_send::spawn().ok(); - state_send::spawn().ok(); + // sensor_send::spawn().ok(); + // state_send::spawn().ok(); + info!("RTC"); + let mut rtc = hal::rtc::Rtc::count32_mode(peripherals.RTC, RateExtU32::Hz(1024), &mut mclk); + // let mut rtc = rtc_temp.into_count32_mode(); + rtc.set_count32(0); + + cortex_m::interrupt::free(|cs| { + RTC.borrow(cs).replace(Some(rtc)); + }); + info!("RTC done"); + let led = pins.pa03.into_push_pull_output(); blink::spawn().ok(); - - /* Monotonic clock */ + generate_random_messages::spawn().ok(); let mono = Systick::new(core.SYST, gclk0.freq().to_Hz()); + // info!("Init done"); ( Shared { em: ErrorManager::new(), @@ -181,7 +260,10 @@ mod app { radio_manager, can0, }, - Local { led, gps_uart }, + Local { + led, + // sd_manager, + }, init::Monotonics(mono), ) } @@ -192,14 +274,14 @@ mod app { loop {} } - #[task(binds = SERCOM2_2, local = [gps_uart], shared = [&em])] - fn gps_rx(mut cx: gps_rx::Context) { - cx.shared.em.run(|| { - let byte = cx.local.gps_uart.read().unwrap(); - info!("GPS: {}", byte); - Ok(()) - }); - } + // #[task(binds = SERCOM2_2, local = [gps_uart], shared = [&em])] + // fn gps_rx(mut cx: gps_rx::Context) { + // cx.shared.em.run(|| { + // let byte = cx.local.gps_uart.read().unwrap(); + // info!("GPS: {}", byte); + // Ok(()) + // }); + // } /// Handles the CAN0 interrupt. #[task(priority = 3, binds = CAN0, shared = [can0, data_manager])] @@ -219,11 +301,14 @@ mod app { cx.shared.can0.lock(|can| can.send_message(m))?; 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) { - let message = Message::new(&monotonics::now(), COM_ID, d.into()); + let message = Message::new(cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + rtc.count32()}), COM_ID, d.into()); send_gs::spawn(message).ok(); } @@ -235,14 +320,16 @@ mod app { fn send_gs(mut cx: send_gs::Context, m: Message) { cx.shared.radio_manager.lock(|radio_manager| { cx.shared.em.run(|| { + let mut buf = [0; 255]; + let data = postcard::to_slice(&m, &mut buf)?; radio_manager - .send_message(postcard::to_slice(&m, &mut [0; 255])?.try_into().unwrap())?; + .send_message(data)?; Ok(()) }) }); } - // #[task(capacity = 10, local = [sd_manager], shared = [&em])] + // #[task(capacity = 10, local = [sd_manager], shared = [&em])] // fn sd_dump(cx: sd_dump::Context, m: Message) { // let manager = cx.local.sd_manager; // cx.shared.em.run(|| { @@ -257,7 +344,7 @@ mod app { // } // Ok(()) // }); - //} + // } /** * Sends information about the sensors. @@ -293,32 +380,54 @@ mod app { } } - #[task(shared = [data_manager, &em])] - fn state_send(mut cx: state_send::Context) { - let state_data = cx - .shared - .data_manager - .lock(|data_manager| data_manager.state.clone()); + #[task(shared = [&em])] + fn generate_random_messages(cx: generate_random_messages::Context) { cx.shared.em.run(|| { - if let Some(x) = state_data { - let message = Message::new(&monotonics::now(), COM_ID, State::new(x)); - spawn!(send_gs, message)?; - } // if there is none we still return since we simply don't have data yet. + let message = Message::new( + cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + rtc.count32()}), + COM_ID, + State::new(StateData::Initializing), + ); + spawn!(send_gs, message)?; + // spawn!(send_internal, message)?; Ok(()) }); - spawn_after!(state_send, ExtU64::secs(5)).ok(); + spawn!(generate_random_messages).ok(); } - #[task(binds = SERCOM5_2, shared = [&em, radio_manager])] - fn radio_rx(mut cx: radio_rx::Context) { - cx.shared.radio_manager.lock(|radio_manager| { - cx.shared.em.run(|| { - let msg = radio_manager.receive_message()?; - spawn!(send_internal, msg)?; // just broadcast the message throughout the system for now. - Ok(()) - }); - }); - } + // #[task(shared = [data_manager, &em])] + // 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(cortex_m::interrupt::free(|cs| { + // let mut rc = RTC.borrow(cs).borrow_mut(); + // let rtc = rc.as_mut().unwrap(); + // rtc.count32()}), COM_ID, State::new(x)); + // // spawn!(send_gs, message)?; + // } // if there is none we still return since we simply don't have data yet. + // Ok(()) + // }); + // spawn_after!(state_send, ExtU64::secs(5)).ok(); + // } + + // #[task(binds = SERCOM5_2, shared = [&em, radio_manager])] + // fn radio_rx(mut cx: radio_rx::Context) { + // cx.shared.radio_manager.lock(|radio_manager| { + // cx.shared.em.run(|| { + // info!("Interrupt on Sercom5"); + // let msg = radio_manager.receive_message()?; + // // spawn!(send_internal, msg)?; // just broadcast the message throughout the system for now. + // Ok(()) + // }); + // }); + // } /** * Simple blink task to test the system. diff --git a/boards/communication/src/types.rs b/boards/communication/src/types.rs index 3813ee9a..bc688857 100644 --- a/boards/communication/src/types.rs +++ b/boards/communication/src/types.rs @@ -18,6 +18,6 @@ pub type GroundStationUartConfig = uart::Config; // ------- // GPS UART // ------- -pub type GpsPads = uart::PadsFromIds; +pub type GpsPads = uart::PadsFromIds; pub type GpsUartConfig = uart::Config; pub type GpsUart = uart::Uart; diff --git a/boards/nav/Cargo.toml b/boards/nav/Cargo.toml index 76363015..c3c31786 100644 --- a/boards/nav/Cargo.toml +++ b/boards/nav/Cargo.toml @@ -17,5 +17,9 @@ messages = { path = "../../libraries/messages" } systick-monotonic = "1.0.1" defmt = "0.3.2" fdcan = "0.2" - +sbg-rs = {path = "../../libraries/sbg-rs"} +embedded-alloc = "0.5.0" heapless = "0.7.16" + +#panic-halt = "0.2.0" +chrono = {version = "0.4.0", default-features = false} \ No newline at end of file diff --git a/boards/nav/src/communication.rs b/boards/nav/src/communication.rs index e69de29b..07fbfe03 100644 --- a/boards/nav/src/communication.rs +++ b/boards/nav/src/communication.rs @@ -0,0 +1,53 @@ +use stm32h7xx_hal as hal; +use stm32h7xx_hal::prelude::*; +use fdcan::{ + Transmit, Receive, + config::NominalBitTiming, filter::{StandardFilter, StandardFilterSlot}, frame::{FrameFormat, TxFrameHeader}, id::StandardId, FdCan, Instance +}; +use crate::types::COM_ID; +use messages::Message; +use postcard::from_bytes; +use common_arm::HydraError; +use defmt::info; +use crate::data_manager::DataManager; + +/// Clock configuration is out of scope for this builder +/// easiest way to avoid alloc is to use no generics +pub struct CanDevice { + can: fdcan::FdCan, fdcan::NormalOperationMode> , +} + +impl CanDevice { + pub fn new(can:fdcan::FdCan, 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]; + for message in self.can.receive0(&mut buf) { + match from_bytes::(&buf) { + Ok(data) => { + data_manager.handle_data(data)?; + } + Err(e) => { + info!("Error: {:?}", e) + } + } + } + Ok(()) + } +} \ No newline at end of file diff --git a/boards/nav/src/main.rs b/boards/nav/src/main.rs index 639c0331..3d6c2022 100644 --- a/boards/nav/src/main.rs +++ b/boards/nav/src/main.rs @@ -2,7 +2,9 @@ #![no_main] mod data_manager; +// mod sbg_manager; mod types; +mod communication; use common_arm::*; use core::num::{NonZeroU16, NonZeroU8}; @@ -14,8 +16,12 @@ use fdcan::{ frame::{FrameFormat, TxFrameHeader}, id::StandardId, }; +use chrono::NaiveDate; +use stm32h7xx_hal::prelude::*; +use sbg_rs::sbg::CallbackData; +use sbg_rs::sbg::SBG_BUFFER_SIZE; use fugit::RateExtU32; -use stm32h7xx_hal::gpio::gpioa::{PA1, PA2, PA3}; +use stm32h7xx_hal::gpio::gpioa::{PA1, PA2, PA3, PA4}; use stm32h7xx_hal::gpio::gpioc::{PC13, PC3}; use stm32h7xx_hal::gpio::Input; use stm32h7xx_hal::gpio::Speed; @@ -26,9 +32,10 @@ use stm32h7xx_hal::spi; use stm32h7xx_hal::{rcc, rcc::rec}; use systick_monotonic::*; use types::SBGSerial; - -/// Custom panic handler. -/// Reset the system if a panic occurs. +use heapless::Vec; +use stm32h7xx_hal::rtc::Rtc; +use stm32h7xx_hal::dma::dma::StreamsTuple; +// use panic_halt as _; #[panic_handler] fn panic(info: &core::panic::PanicInfo) -> ! { stm32h7xx_hal::pac::SCB::sys_reset(); @@ -36,18 +43,23 @@ fn panic(info: &core::panic::PanicInfo) -> ! { #[rtic::app(device = stm32h7xx_hal::stm32, dispatchers = [EXTI0, EXTI1])] mod app { + use stm32h7xx_hal::{gpio::{Alternate, Pin}, pac::SPI1}; + use super::*; #[shared] struct SharedResources { data_manager: DataManager, em: ErrorManager, - // rtc: Rtc, + sd_manager: + SdManager, PA4>>, + rtc: Rtc, } #[local] struct LocalResources { led_red: PA2>, led_green: PA3>, + // sbg_manager: sbg_manager::SBGManager, } #[monotonic(binds = SysTick, default = true)] @@ -60,24 +72,29 @@ mod app { 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 pwrcfg = pwr.freeze(); + let mut pwrcfg = pwr.freeze(); + info!("Power enabled"); + let backup = pwrcfg.backup().unwrap(); + info!("Backup domain enabled"); // RCC let rcc = ctx.device.RCC.constrain(); + info!("RCC enabled"); let ccdr = rcc .use_hse(48.MHz()) - .sys_ck(300.MHz()) + .sys_ck(200.MHz()) .pll1_strategy(rcc::PllConfigStrategy::Iterative) .pll1_q_ck(24.MHz()) .freeze(pwrcfg, &ctx.device.SYSCFG); + info!("RCC configured"); let btr = NominalBitTiming { - prescaler: NonZeroU16::new(12).unwrap(), + prescaler: NonZeroU16::new(4).unwrap(), seg1: NonZeroU8::new(13).unwrap(), seg2: NonZeroU8::new(2).unwrap(), sync_jump_width: NonZeroU8::new(1).unwrap(), }; - + info!("CAN enabled"); // GPIO let gpioc = ctx.device.GPIOC.split(ccdr.peripheral.GPIOC); let gpioa = ctx.device.GPIOA.split(ccdr.peripheral.GPIOA); @@ -85,23 +102,24 @@ mod app { let gpiob = ctx.device.GPIOB.split(ccdr.peripheral.GPIOB); let pins = gpiob.pb14.into_alternate(); - let mut c0 = ctx .device .TIM12 - .pwm(pins, 1.kHz(), ccdr.peripheral.TIM12, &ccdr.clocks); + .pwm(pins, 4.kHz(), ccdr.peripheral.TIM12, &ccdr.clocks); c0.set_duty(c0.get_max_duty() / 2); // PWM outputs are disabled by default c0.enable(); - assert_eq!(ccdr.clocks.pll1_q_ck().unwrap().raw(), 24_000_000); + info!("PWM enabled"); + // assert_eq!(ccdr.clocks.pll1_q_ck().unwrap().raw(), 24_000_000); waaat + info!("PLL1Q:"); let fdcan_prec = ccdr .peripheral .FDCAN .kernel_clk_mux(rec::FdcanClkSel::Pll1Q); - let can1 = { + let can1: fdcan::FdCan, 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) @@ -117,7 +135,7 @@ mod app { StandardFilter::accept_all_into_fifo0(), ); - let spi: stm32h7xx_hal::spi::Spi< + let spi_sd: stm32h7xx_hal::spi::Spi< stm32h7xx_hal::stm32::SPI1, stm32h7xx_hal::spi::Enabled, u8, @@ -128,24 +146,60 @@ mod app { gpioa.pa7.into_alternate(), ), spi::Config::new(spi::MODE_0), - 1.MHz(), + 16.MHz(), ccdr.peripheral.SPI1, &ccdr.clocks, ); + let cs_sd = gpioa.pa4.into_push_pull_output(); + + let mut sd = SdManager::new(spi_sd, cs_sd); + // leds let mut led_red = gpioa.pa2.into_push_pull_output(); let mut led_green = gpioa.pa3.into_push_pull_output(); // UART for sbg - let tx = gpiod.pd1.into_alternate(); - let rx = gpiod.pd0.into_alternate(); - - let serial = ctx - .device - .UART4 - .serial((tx, rx), 9_800.bps(), ccdr.peripheral.UART4, &ccdr.clocks) + 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_sbg = ctx + .device + .UART4 + .serial((tx, rx), 9_800.bps(), ccdr.peripheral.UART4, &ccdr.clocks) + .unwrap(); + // let sbg_manager = sbg_manager::SBGManager::new(uart_sbg, stream_tuple); + // let serial = ctx + // .device + // .UART4 + // .serial((tx, rx), 9_800.bps(), ccdr.peripheral.UART4, &ccdr.clocks) + // .unwrap(); + // let (sbg_rx, sbg_tx) = serial.split(); + + 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); + rtc.enable_wakeup(10); + rtc.listen(&mut ctx.device.EXTI, stm32h7xx_hal::rtc::Event::Wakeup); + + unsafe { + pac::NVIC::unmask(pac::interrupt::RTC_WKUP); + } + + info!("Time: {}", rtc.date_time().unwrap().and_utc().timestamp_subsec_millis()); + blink::spawn().ok(); /* Monotonic clock */ @@ -157,8 +211,14 @@ mod app { SharedResources { data_manager: DataManager::new(), em: ErrorManager::new(), + sd_manager: sd, + rtc, + }, + LocalResources { + led_red, + led_green, + // sbg_manager: sbg_manager, }, - LocalResources { led_red, led_green }, init::Monotonics(mono), ) } @@ -179,6 +239,9 @@ mod app { } else { cx.local.led_green.toggle(); info!("Blinking"); + cx.shared.rtc.lock(|rtc| { + info!("Time: {}", rtc.date_time().unwrap().and_utc().timestamp_subsec_millis()); + }); spawn_after!(blink, ExtU64::secs(1))?; } Ok(()) @@ -190,17 +253,23 @@ mod app { // Turn off the SBG and CAN } - //extern "Rust" { - // #[task(capacity = 3, shared = [&em, sd_manager])] - // fn sbg_sd_task(context: sbg_sd_task::Context, data: [u8; SBG_BUFFER_SIZE]); + // extern "Rust" { + // #[task(capacity = 3, shared = [&em, sd_manager])] + // fn sbg_sd_task(context: sbg_sd_task::Context, data: [u8; SBG_BUFFER_SIZE]); + + // #[task(binds = DMA1_STR0, shared = [&em], local = [sbg_manager])] + // fn sbg_dma(context: sbg_dma::Context); + + // #[task(capacity = 20, shared = [data_manager])] + // fn sbg_handle_data(context: sbg_handle_data::Context, data: CallbackData); - //#[task(binds = DMAC_0, shared = [&em], local = [sbg_manager])] - //fn sbg_dma(context: sbg_dma::Context); + // #[task(shared = [rtc, &em])] + // fn sbg_get_time(context: sbg_get_time::Context); - //#[task(capacity = 20, shared = [data_manager])] - //fn sbg_handle_data(context: sbg_handle_data::Context, data: CallbackData); + // #[task()] + // fn sbg_flush(context: sbg_flush::Context); - //#[task(shared = [rtc, &em])] - //fn sbg_get_time(context: sbg_get_time::Context); - //} + // #[task(shared = [&em])] + // fn sbg_write_data(context: sbg_write_data::Context, data: Vec); + // } } diff --git a/boards/nav/src/sbg_manager.rs b/boards/nav/src/sbg_manager.rs index f4456fcd..ae94f827 100644 --- a/boards/nav/src/sbg_manager.rs +++ b/boards/nav/src/sbg_manager.rs @@ -1,21 +1,14 @@ -use crate::types::{ConfigSBG, SBGBuffer, SBGTransfer}; -use atsamd_hal::clock::v2::gclk::Gclk0Id; -use atsamd_hal::clock::v2::pclk::Pclk; -use atsamd_hal::dmac; -use atsamd_hal::dmac::Transfer; -use atsamd_hal::sercom::IoSet6; -// use atsamd_hal::prelude::_atsamd21_hal_time_U32Ext; -use atsamd_hal::rtc::Rtc; +use crate::types::SBGBuffer; use core::alloc::{GlobalAlloc, Layout}; use core::ffi::c_void; use core::mem::size_of; use core::ptr; // use atsamd_hal::time::*; +use crate::app::sbg_flush; use crate::app::sbg_get_time; use crate::app::sbg_handle_data; use crate::app::sbg_sd_task as sbg_sd; -use atsamd_hal::prelude::*; -use atsamd_hal::sercom::{uart, Sercom, Sercom5}; +use crate::app::sbg_write_data; use common_arm::spawn; use core::{mem, mem::MaybeUninit}; use defmt::info; @@ -23,6 +16,7 @@ use embedded_alloc::Heap; use rtic::Mutex; use sbg_rs::sbg; use sbg_rs::sbg::{CallbackData, SBG, SBG_BUFFER_SIZE}; +use stm32h7xx_hal::dma::dma::StreamX; use stm32h7xx_hal::dma::{ dma::{DmaConfig, StreamsTuple}, PeripheralToMemory, Transfer, @@ -30,26 +24,33 @@ use stm32h7xx_hal::dma::{ use stm32h7xx_hal::gpio::Alternate; use stm32h7xx_hal::gpio::Pin; use stm32h7xx_hal::rtc::Rtc; +use stm32h7xx_hal::serial::Rx; //#[link_section = ".axisram.buffers"] //static mut SBG_BUFFER: MayberUninit<[u8; SBG_BUFFER_SIZE]> = MaybeUninit::uninit(); #[link_section = ".axisram.buffers"] -pub static mut SBG_BUFFER: SBGBuffer = &mut [0; SBG_BUFFER_SIZE]; +pub static mut SBG_BUFFER: MaybeUninit<[u8; SBG_BUFFER_SIZE]> = MaybeUninit::uninit(); // Simple heap required by the SBG library static HEAP: Heap = Heap::empty(); pub struct SBGManager { sbg_device: SBG, - xfer: Option, + xfer: Option< + Transfer< + StreamX, + Rx, + stm32h7xx_hal::dma::PeripheralToMemory, + MaybeUninit<[u8;SBG_BUFFER_SIZE]>, + stm32h7xx_hal::dma::DBTransfer, + >, + >, } impl SBGManager { pub fn new( - rx: Pin<'D', 0, Alternate<8>>, - tx: Pin<'D', 1, Alternate<8>>, - rtc: Rtc, + serial: stm32h7xx_hal::serial::Serial, mut stream_tuple: StreamsTuple, //mut dma_channel: dmac::Channel, ) -> Self { @@ -62,53 +63,68 @@ impl SBGManager { unsafe { HEAP.init(HEAP_MEM.as_ptr() as usize, HEAP_SIZE) } } - let uart_sbg = ctx - .device - .UART4 - .serial((tx, rx), 9_800.bps(), ccdr.peripheral.UART4, &ccdr.clocks) - .unwrap(); - let (sbg_rx, sbg_tx) = uart_sbg.split(); + let (sbg_tx, sbg_rx) = serial.split(); // TODO: This could be wrong. It's a bit of a guess. - //let sbg_buffer: &'static mut [u8; SBG_BUFFER_SIZE] = { - // let buf: &mut [MaybeUninit; SBG_BUFFER_SIZE] = - // unsafe { &mut *(core::ptr::addr_of_mut!(SBG_BUFFER) as *mut _) }; - // for (i, value) in buf.iter_mut().enumerate() { - // unsafe { value.as_mut_ptr().write(i as u8) }; - // } - // unsafe { SBG_BUFFER.assume_init_mut() } - // }; + let sbg_buffer: &'static mut [u8; SBG_BUFFER_SIZE] = { + let buf: &mut [MaybeUninit; SBG_BUFFER_SIZE] = + unsafe { &mut *(core::ptr::addr_of_mut!(SBG_BUFFER) as *mut _) }; + for (i, value) in buf.iter_mut().enumerate() { + unsafe { value.as_mut_ptr().write(i as u8) }; + } + unsafe { SBG_BUFFER.assume_init_mut() } + }; let config = DmaConfig::default().memory_increment(true); - let transfer: Transfer<_, _, _, PeripheralToMemory> = Transfer::init( - stream_tuple.0, - sbg_rx, - unsafe { &mut *SBG_BUFFER }, - None, - config, - ); + let transfer: Transfer< + StreamX, + Rx, + stm32h7xx_hal::dma::PeripheralToMemory, + &mut [u8], + stm32h7xx_hal::dma::DBTransfer, + > = Transfer::init(stream_tuple.0, sbg_rx, &mut sbg_buffer[..], None, config); transfer.start(|serial| { - serial.enable_rx_dma(); + serial.enable_dma_rx(); }); let sbg: sbg::SBG = sbg::SBG::new( - sbg_tx, - |_| sbg_get_time::spawn().ok(), |data| { sbg_handle_data::spawn(data).ok(); }, + |data| { + sbg_write_data::spawn(data).ok(); + }, + || sbg_get_time::spawn(), + || { + sbg_flush::spawn().ok(); + }, ); SBGManager { sbg_device: sbg, - xfer: Some(xfer), + xfer: Some(transfer), } } } +pub fn sbg_flush(mut cx: sbg_flush::Context) { + cx.shared.sbg_manager.lock(|sbg| { + sbg.sbg_device.flush(); + }); +} +pub fn sbg_write_data(mut cx: sbg_write_data::Context, data: &[u8]) { + cx.shared.sbg_manager.lock(|sbg| { + for byte in data { + sbg.sbg_device.write(*byte); + } + }); +} + pub fn sbg_get_time(mut cx: sbg_get_time::Context) -> u32 { - cx.shared.rtc.lock(|rtc| rtc.date_time().unwrap()) + cx.shared + .rtc + .lock(|rtc| rtc.date_time().unwrap().and_utc().timestamp_subsec_millis()) } pub fn sbg_handle_data(mut cx: sbg_handle_data::Context, data: CallbackData) { @@ -148,23 +164,7 @@ pub fn sbg_dma(cx: crate::app::sbg_dma::Context) { let sbg = cx.local.sbg_manager; match &mut sbg.xfer { - Some(xfer) => { - if xfer.complete() { - let (chan0, source, buf) = sbg.xfer.take().unwrap().stop(); - let mut xfer = dmac::Transfer::new(chan0, source, unsafe { &mut *BUF_DST }, false) - .unwrap() - .begin(Sercom5::DMA_RX_TRIGGER, dmac::TriggerAction::BURST); - let buf_clone = buf.clone(); - sbg.sbg_device.read_data(buf); - unsafe { BUF_DST.copy_from_slice(&[0; SBG_BUFFER_SIZE]) }; - xfer.block_transfer_interrupt(); - sbg.xfer = Some(xfer); - cx.shared.em.run(|| { - spawn!(sbg_sd, buf_clone)?; // this warning isn't right but it's fine - Ok(()) - }); - } - } + Some(xfer) => if xfer.complete() {}, None => { // it should be impossible to reach here. info!("None"); diff --git a/boards/nav/src/types.rs b/boards/nav/src/types.rs index 59f32710..0771aefd 100644 --- a/boards/nav/src/types.rs +++ b/boards/nav/src/types.rs @@ -1,5 +1,11 @@ use hal::pac; use hal::serial::Serial; use stm32h7xx_hal as hal; +use sbg_rs::sbg::SBG_BUFFER_SIZE; +use messages::sender::{Sender, Sender::SensorBoard}; + +pub static COM_ID: Sender = SensorBoard; + pub type SBGSerial = Serial; +pub type SBGBuffer = &'static mut [u8; SBG_BUFFER_SIZE]; diff --git a/boards/recovery/src/main.rs b/boards/recovery/src/main.rs index b6cbe7fe..a428370e 100644 --- a/boards/recovery/src/main.rs +++ b/boards/recovery/src/main.rs @@ -32,16 +32,23 @@ use ms5611_01ba::{calibration::OversamplingRatio, MS5611_01BA}; use state_machine::{StateMachine, StateMachineContext}; use systick_monotonic::*; use types::COM_ID; +use cortex_m::interrupt::Mutex; +use hal::rtc::Count32Mode; +use core::cell::RefCell; +pub static RTC: Mutex>>> = Mutex::new(RefCell::new(None)); /// Custom panic handler. /// Reset the system if a panic occurs. #[panic_handler] fn panic(_info: &core::panic::PanicInfo) -> ! { + info!("Panic"); atsamd_hal::pac::SCB::sys_reset(); } #[rtic::app(device = hal::pac, peripherals = true, dispatchers = [EVSYS_0, EVSYS_1, EVSYS_2])] mod app { + use cortex_m::asm; + use super::*; #[shared] @@ -100,7 +107,7 @@ mod app { // SAFETY: Misusing the PAC API can break the system. // This is safe because we only steal the MCLK. - let (_, _, _, mclk) = unsafe { clocks.pac.steal() }; + let (_, _, _, mut mclk) = unsafe { clocks.pac.steal() }; /* CAN config */ let (pclk_can, gclk0) = Pclk::enable(tokens.pclks.can0, gclk0); @@ -143,8 +150,18 @@ mod app { let barometer = MS5611_01BA::new(baro_spi, OversamplingRatio::OSR2048); + info!("RTC"); + let mut rtc = hal::rtc::Rtc::count32_mode(peripherals.RTC, 1024.Hz(), &mut mclk); + // let mut rtc = rtc_temp.into_count32_mode(); + rtc.set_count32(0); + cortex_m::interrupt::free(|cs| { + RTC.borrow(cs).replace(Some(rtc)); + }); + // info!("RTC done"); + /* Spawn tasks */ run_sm::spawn().ok(); + read_barometer::spawn().ok(); state_send::spawn().ok(); blink::spawn().ok(); // fire_main::spawn_after(ExtU64::secs(15)).ok(); @@ -173,7 +190,7 @@ mod app { /// Idle task for when no other tasks are running. #[idle] fn idle(_: idle::Context) -> ! { - loop {} + loop {asm::nop()} } #[task(local = [barometer], shared = [&em])] @@ -184,6 +201,7 @@ mod app { info!("pressure {} temperature {}", p, t); Ok(()) }); + spawn_after!(read_barometer, ExtU64::secs(1)).ok(); } #[task(priority = 3, local = [fired: bool = false], shared=[gpio, &em])] @@ -273,7 +291,10 @@ mod app { return Ok(()); }; let board_state = messages::state::State { data: state.into() }; - let message = Message::new(&monotonics::now(), COM_ID, board_state); + let message = Message::new(cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + rtc.count32()}), COM_ID, board_state); spawn!(send_internal, message)?; spawn_after!(state_send, ExtU64::secs(2))?; // I think this is fine here. Ok(()) diff --git a/boards/recovery/src/state_machine/states/ascent.rs b/boards/recovery/src/state_machine/states/ascent.rs index 5caf9ec1..c2a8b98e 100644 --- a/boards/recovery/src/state_machine/states/ascent.rs +++ b/boards/recovery/src/state_machine/states/ascent.rs @@ -8,6 +8,7 @@ use defmt::{write, Format, Formatter}; use messages::command::{Command, RadioRate, RadioRateChange}; use messages::Message; use rtic::mutex::Mutex; +use crate::RTC; #[derive(Debug, Clone)] pub struct Ascent {} @@ -19,7 +20,10 @@ impl State for Ascent { rate: RadioRate::Fast, }; let message_com = - Message::new(&monotonics::now(), COM_ID, Command::new(radio_rate_change)); + Message::new(cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + rtc.count32()}), COM_ID, Command::new(radio_rate_change)); context.shared_resources.can0.lock(|can| { context.shared_resources.em.run(|| { can.send_message(message_com)?; diff --git a/boards/recovery/src/state_machine/states/wait_for_recovery.rs b/boards/recovery/src/state_machine/states/wait_for_recovery.rs index cd404c88..01af80ad 100644 --- a/boards/recovery/src/state_machine/states/wait_for_recovery.rs +++ b/boards/recovery/src/state_machine/states/wait_for_recovery.rs @@ -8,6 +8,7 @@ use messages::command::{Command, PowerDown, RadioRate, RadioRateChange}; use messages::sender::Sender::SensorBoard; use messages::Message; use rtic::mutex::Mutex; +use crate::RTC; #[derive(Debug, Clone)] pub struct WaitForRecovery {} @@ -21,9 +22,15 @@ impl State for WaitForRecovery { let radio_rate_change = RadioRateChange { rate: RadioRate::Slow, }; - let message = Message::new(&monotonics::now(), COM_ID, Command::new(sensor_power_down)); + let message = Message::new(cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + rtc.count32()}), COM_ID, Command::new(sensor_power_down)); let message_com = - Message::new(&monotonics::now(), COM_ID, Command::new(radio_rate_change)); + Message::new(cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + rtc.count32()}), COM_ID, Command::new(radio_rate_change)); context.shared_resources.can0.lock(|can| { context.shared_resources.em.run(|| { can.send_message(message)?; diff --git a/boards/sensor/src/sbg_manager.rs b/boards/sensor/src/sbg_manager.rs index 25b8e259..e5c0e0f5 100644 --- a/boards/sensor/src/sbg_manager.rs +++ b/boards/sensor/src/sbg_manager.rs @@ -79,7 +79,7 @@ impl SBGManager { let sbg: sbg::SBG = sbg::SBG::new(sbg_tx, rtc, |data| { sbg_handle_data::spawn(data).ok(); - }); + }, ); SBGManager { sbg_device: sbg, diff --git a/examples/rtic/src/main.rs b/examples/rtic/src/main.rs index 3419586f..5fb6ece8 100644 --- a/examples/rtic/src/main.rs +++ b/examples/rtic/src/main.rs @@ -46,9 +46,11 @@ mod app { let led = pins.pa14.into_push_pull_output(); // Tell the MCU to sleep deeply for maximum power savings - core.SCB.set_sleepdeep(); + // core.SCB.set_sleepdeep(); + core.SCB.clear_sleepdeep(); // Spawn the LED blink task right after init + info!("Spawned blink task"); blink::spawn().ok(); // Use the system's Systick for RTIC to keep track of the time diff --git a/examples/simple/src/main.rs b/examples/simple/src/main.rs index 1ef5e4c4..db6fa267 100644 --- a/examples/simple/src/main.rs +++ b/examples/simple/src/main.rs @@ -4,12 +4,12 @@ use atsamd_hal as hal; use atsamd_hal::prelude::*; use cortex_m_rt::entry; -use defmt::info; use defmt_rtt as _; use hal::gpio::Pins; use hal::pac; use pac::Peripherals; use panic_halt as _; +use defmt::println; #[entry] fn main() -> ! { @@ -20,7 +20,7 @@ fn main() -> ! { let mut led = pins.pa14.into_push_pull_output(); // External 32KHz clock for stability - let mut clock = hal::clock::GenericClockController::with_external_32kosc( + let mut clock = hal::clock::GenericClockController::with_internal_32kosc( peripherals.GCLK, &mut peripherals.MCLK, &mut peripherals.OSC32KCTRL, @@ -34,13 +34,21 @@ fn main() -> ! { pac::gclk::genctrl::SRCSELECT_A::DFLL, false, ); + println!("Clock configured"); + let mut tx_pin = pins.pa09.into_push_pull_output(); + let mut rx_pin = pins.pa08.into_push_pull_output(); + match rx_pin.set_high() { + Ok(_) => println!("RX pin set high"), + Err(_) => println!("RX pin set low"), + }; + tx_pin.set_high().unwrap(); let mut delay = hal::delay::Delay::new(p2.SYST, &mut clock); loop { led.set_high().unwrap(); delay.delay_ms(1000_u16); - info!("Test"); + println!("Test"); led.set_low().unwrap(); delay.delay_ms(1000_u16); } diff --git a/libraries/common-arm/Cargo.toml b/libraries/common-arm/Cargo.toml index adac4d99..f56a825a 100644 --- a/libraries/common-arm/Cargo.toml +++ b/libraries/common-arm/Cargo.toml @@ -20,5 +20,4 @@ embedded-hal = "0.2.7" nb = "1.1.0" mcan = "0.3.0" messages = { path = "../messages" } - ms5611-01ba = { git = "https://github.com/NoahSprenger/ms5611-01ba", branch = "embedded-hal-02" } diff --git a/libraries/common-arm/src/error/error_manager.rs b/libraries/common-arm/src/error/error_manager.rs index e5a03931..059e20ab 100644 --- a/libraries/common-arm/src/error/error_manager.rs +++ b/libraries/common-arm/src/error/error_manager.rs @@ -42,11 +42,11 @@ impl ErrorManager { if let Err(e) = result { self.has_error.store(true, Relaxed); - error!("{}", e); + // error!("{}", e); if let Some(c) = e.get_context() { error!("{}", e); - herror!(Error, c); + // herror!(Error, c); } interrupt::free(|cs| { diff --git a/libraries/common-arm/src/error/hydra_error.rs b/libraries/common-arm/src/error/hydra_error.rs index 5aa90833..4fe2cf01 100644 --- a/libraries/common-arm/src/error/hydra_error.rs +++ b/libraries/common-arm/src/error/hydra_error.rs @@ -5,7 +5,7 @@ use derive_more::From; use embedded_sdmmc as sd; use messages::ErrorContext; use ms5611_01ba::error::DeviceError; - +use nb::Error as NbError; /// Open up atsamd hal errors without including the whole crate. /// Contains all the various error types that can be encountered in the Hydra codebase. Extra errors @@ -31,6 +31,7 @@ pub enum HydraErrorType { CanMessageError(mcan::message::TooMuchData), /// Error from the MS5611 barometer library. BarometerError(DeviceError), + NbError(NbError), } impl defmt::Format for HydraErrorType { @@ -66,6 +67,9 @@ impl defmt::Format for HydraErrorType { HydraErrorType::CanMessageError(_) => { write!(f, "CAN message error!"); } + HydraErrorType::NbError(_) => { + write!(f, "Nb error!"); + } } } } diff --git a/libraries/common-arm/src/sd_manager.rs b/libraries/common-arm/src/sd_manager.rs index d4abe100..f11c9728 100644 --- a/libraries/common-arm/src/sd_manager.rs +++ b/libraries/common-arm/src/sd_manager.rs @@ -1,5 +1,6 @@ use core::{fmt::Debug, marker::PhantomData}; use defmt::info; +use defmt::panic; use embedded_hal as hal; use embedded_sdmmc as sd; use hal::spi::FullDuplex; @@ -52,6 +53,7 @@ where { pub fn new(spi: SPI, cs: CS) -> Self { let time_sink: TimeSink = TimeSink::new(); // Need to give this a DateTime object for actual timing. + info!("Initializing SD card"); let mut sd_cont = sd::Controller::new(sd::SdMmcSpi::new(spi, cs), time_sink); match sd_cont.device().init() { Ok(_) => match sd_cont.device().card_size_bytes() { diff --git a/libraries/messages/src/lib.rs b/libraries/messages/src/lib.rs index c89fb673..6c8017fb 100644 --- a/libraries/messages/src/lib.rs +++ b/libraries/messages/src/lib.rs @@ -34,7 +34,7 @@ pub use logging::{ErrorContext, Event, Log, LogLevel}; pub struct Message { /// Time in milliseconds since epoch. Note that the epoch here can be arbitrary and is not the /// Unix epoch. - pub timestamp: u64, + pub timestamp: u32, /// The original sender of this message. pub sender: Sender, @@ -55,13 +55,13 @@ pub enum Data { } impl Message { - pub fn new( - timestamp: &Instant, + pub fn new( + timestamp: u32, sender: Sender, data: impl Into, ) -> Self { Message { - timestamp: timestamp.duration_since_epoch().to_millis(), + timestamp: timestamp, sender, data: data.into(), } From d88316d7bd41d86fc3dd3f59f7d8b43533ea0907 Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Mon, 5 Aug 2024 13:45:44 -0400 Subject: [PATCH 09/47] sync --- boards/nav/src/main.rs | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/boards/nav/src/main.rs b/boards/nav/src/main.rs index 3d6c2022..6aedbf23 100644 --- a/boards/nav/src/main.rs +++ b/boards/nav/src/main.rs @@ -109,7 +109,7 @@ mod app { c0.set_duty(c0.get_max_duty() / 2); // PWM outputs are disabled by default - c0.enable(); + // c0.enable(); info!("PWM enabled"); // assert_eq!(ccdr.clocks.pll1_q_ck().unwrap().raw(), 24_000_000); waaat @@ -159,6 +159,10 @@ mod app { let mut led_red = gpioa.pa2.into_push_pull_output(); let mut 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(); @@ -230,7 +234,7 @@ mod app { } } - #[task(local = [led_red, led_green], shared = [&em])] + #[task(local = [led_red, led_green], shared = [&em,rtc])] fn blink(mut cx: blink::Context) { cx.shared.em.run(|| { if cx.shared.em.has_error() { From 818526551bb61c4e5cf928db7d7f14cf00055d50 Mon Sep 17 00:00:00 2001 From: NoahSprenger Date: Tue, 6 Aug 2024 14:44:51 +0000 Subject: [PATCH 10/47] Add command CAN to link board, auto crlf. --- .devcontainer/Dockerfile | 2 +- .devcontainer/devcontainer.json | 1 - .gitconfig | 2 + Cargo.lock | 149 +++++++++++++----- boards/communication/Cargo.toml | 2 +- boards/communication/src/communication.rs | 179 +++++++++++++++++++++- boards/communication/src/data_manager.rs | 29 +++- boards/communication/src/main.rs | 136 ++++++++++------ boards/nav/src/data_manager.rs | 13 +- boards/nav/src/main.rs | 38 ++--- boards/nav/src/sbg_manager.rs | 32 ++-- libraries/common-arm-atsame/memory.x | 3 +- 12 files changed, 444 insertions(+), 142 deletions(-) create mode 100644 .gitconfig diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 8c950e5a..755ffba4 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -7,5 +7,5 @@ RUN apt-get update && apt-get upgrade -y && apt-get install -y cmake pkg-config RUN mkdir -p toolchain && \ curl -L "https://developer.arm.com/-/media/Files/downloads/gnu/13.2.rel1/binrel/arm-gnu-toolchain-13.2.rel1-x86_64-arm-none-eabi.tar.xz" \ | tar --strip-components=1 -xJ -C toolchain && \ - cargo install --locked probe-rs --features cli + cargo install --locked probe-rs@0.23.0 --features cli ENV PATH="${PATH}:/toolchain/bin" \ No newline at end of file diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 6ada23bc..748d56fc 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -6,5 +6,4 @@ }, "remoteUser": "root", "runArgs": ["--privileged"] - } \ No newline at end of file diff --git a/.gitconfig b/.gitconfig new file mode 100644 index 00000000..281c8ea6 --- /dev/null +++ b/.gitconfig @@ -0,0 +1,2 @@ +[core] + autocrlf = true \ No newline at end of file diff --git a/Cargo.lock b/Cargo.lock index f2a269ee..85466b1b 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -50,7 +50,7 @@ dependencies = [ "cipher", "cortex-m", "defmt", - "embedded-hal", + "embedded-hal 0.2.7", "fugit", "mcan-core", "modular-bitfield", @@ -172,7 +172,7 @@ dependencies = [ "cortex-m-rtic", "defmt", "enum_dispatch", - "heapless", + "heapless 0.7.17", "messages", "postcard", "systick-monotonic", @@ -187,9 +187,9 @@ checksum = "37b2a672a2cb129a2e41c10b1224bb368f9f37a2b16b612598138befd7b37eb5" [[package]] name = "cc" -version = "1.1.6" +version = "1.1.7" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2aba8f4e9906c7ce3c73463f62a7f0c65183ada1a2d47e397cc8810827f9694f" +checksum = "26a5c3fd7bfa1ce3897a3a3501d362b2d87b7f2583ebcb4a949ec25911025cbc" [[package]] name = "cfg-if" @@ -239,9 +239,9 @@ dependencies = [ "defmt", "defmt-rtt", "derive_more", - "embedded-hal", - "embedded-sdmmc", - "heapless", + "embedded-hal 0.2.7", + "embedded-sdmmc 0.3.0", + "heapless 0.7.17", "mcan", "messages", "ms5611-01ba", @@ -255,7 +255,7 @@ version = "0.1.0" dependencies = [ "atsamd-hal", "common-arm", - "embedded-hal", + "embedded-hal 0.2.7", "mcan", ] @@ -299,8 +299,8 @@ dependencies = [ "cortex-m-rtic", "defmt", "defmt-rtt", - "embedded-sdmmc", - "heapless", + "embedded-sdmmc 0.8.0", + "heapless 0.7.17", "messages", "panic-probe", "postcard", @@ -323,7 +323,7 @@ dependencies = [ "bare-metal 0.2.5", "bitfield 0.13.2", "critical-section", - "embedded-hal", + "embedded-hal 0.2.7", "volatile-register", ] @@ -356,7 +356,7 @@ dependencies = [ "bare-metal 1.0.0", "cortex-m", "cortex-m-rtic-macros", - "heapless", + "heapless 0.7.17", "rtic-core", "rtic-monotonic", "version_check", @@ -521,6 +521,12 @@ dependencies = [ "void", ] +[[package]] +name = "embedded-hal" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "361a90feb7004eca4019fb28352a9465666b24f840f5c3cddf0ff13920590b89" + [[package]] name = "embedded-io" version = "0.4.0" @@ -534,11 +540,23 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "6d3bf0a2b5becb87e9a329d9290f131e4d10fec39b56d129926826a7cbea1e7a" dependencies = [ "byteorder", - "embedded-hal", + "embedded-hal 0.2.7", "log", "nb 0.1.3", ] +[[package]] +name = "embedded-sdmmc" +version = "0.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "150f320125310e179b9e73b081173b349e63c5c7d4ca44db4e5b9121b10387ec" +dependencies = [ + "byteorder", + "embedded-hal 1.0.0", + "heapless 0.8.0", + "log", +] + [[package]] name = "embedded-storage" version = "0.3.1" @@ -573,7 +591,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "534c5cf6194dfab3db3242765c03bbe257cf92f22b38f6bc0c58d59108a820ba" dependencies = [ "libc", - "windows-sys", + "windows-sys 0.52.0", ] [[package]] @@ -647,6 +665,15 @@ dependencies = [ "byteorder", ] +[[package]] +name = "hash32" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "47d60b12902ba28e2730cd37e95b8c9223af2808df9e902d4df49588d1470606" +dependencies = [ + "byteorder", +] + [[package]] name = "hashbrown" version = "0.12.3" @@ -660,13 +687,23 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "cdc6457c0eb62c71aac4bc17216026d8410337c4126773b9c5daba343f17964f" dependencies = [ "atomic-polyfill", - "hash32", + "hash32 0.2.1", "rustc_version 0.4.0", "serde", "spin", "stable_deref_trait", ] +[[package]] +name = "heapless" +version = "0.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0bfb9eb618601c89945a70e254898da93b13be0388091d42117462b265bb3fad" +dependencies = [ + "hash32 0.3.1", + "stable_deref_trait", +] + [[package]] name = "indexmap" version = "1.9.3" @@ -766,7 +803,7 @@ source = "git+https://github.com/uorocketry/rust-mavlink.git#e9f4057deedce85cae5 dependencies = [ "byteorder", "crc-any", - "embedded-hal", + "embedded-hal 0.2.7", "nb 1.1.0", "serde", "serde_arrays", @@ -811,7 +848,7 @@ dependencies = [ "defmt", "derive_more", "fugit", - "heapless", + "heapless 0.7.17", "mavlink", "messages-proc-macros-lib", "postcard", @@ -855,7 +892,7 @@ name = "ms5611-01ba" version = "0.1.0" source = "git+https://github.com/NoahSprenger/ms5611-01ba?branch=embedded-hal-02#fdb9b51f953d854c7b99079ae3583f82c6378bbc" dependencies = [ - "embedded-hal", + "embedded-hal 0.2.7", ] [[package]] @@ -871,7 +908,7 @@ dependencies = [ "defmt", "embedded-alloc", "fdcan", - "heapless", + "heapless 0.7.17", "messages", "postcard", "sbg-rs", @@ -1011,7 +1048,7 @@ dependencies = [ "cobs", "defmt", "embedded-io", - "heapless", + "heapless 0.7.17", "serde", ] @@ -1027,7 +1064,7 @@ dependencies = [ "cortex-m-rtic", "defmt", "enum_dispatch", - "heapless", + "heapless 0.7.17", "messages", "postcard", "systick-monotonic", @@ -1036,9 +1073,12 @@ dependencies = [ [[package]] name = "ppv-lite86" -version = "0.2.17" +version = "0.2.20" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5b40af805b3121feab8a3c29f04d8ad262fa8e0561883e7653e024ae4479e6de" +checksum = "77957b295656769bb8ad2b6a6b09d897d94f05c41b069aede1fcdaa675eaea04" +dependencies = [ + "zerocopy", +] [[package]] name = "proc-macro-error" @@ -1196,9 +1236,9 @@ dependencies = [ "cortex-m-rt", "cortex-m-rtic", "defmt", - "embedded-hal", + "embedded-hal 0.2.7", "enum_dispatch", - "heapless", + "heapless 0.7.17", "messages", "ms5611-01ba", "postcard", @@ -1257,7 +1297,7 @@ dependencies = [ "defmt", "defmt-rtt", "fugit", - "heapless", + "heapless 0.7.17", "messages", "panic-halt", "postcard", @@ -1293,7 +1333,7 @@ dependencies = [ "errno", "libc", "linux-raw-sys", - "windows-sys", + "windows-sys 0.52.0", ] [[package]] @@ -1319,8 +1359,8 @@ dependencies = [ "cortex-m", "cortex-m-rt", "defmt", - "embedded-hal", - "heapless", + "embedded-hal 0.2.7", + "heapless 0.7.17", "messages", "nb 1.1.0", "serde", @@ -1365,8 +1405,8 @@ dependencies = [ "cortex-m-rtic", "defmt", "embedded-alloc", - "embedded-sdmmc", - "heapless", + "embedded-sdmmc 0.3.0", + "heapless 0.7.17", "messages", "postcard", "sbg-rs", @@ -1508,7 +1548,7 @@ dependencies = [ "cortex-m", "defmt", "embedded-dma", - "embedded-hal", + "embedded-hal 0.2.7", "embedded-storage", "fdcan", "fugit", @@ -1539,7 +1579,7 @@ dependencies = [ "cast", "cortex-m", "cortex-m-rt", - "embedded-hal", + "embedded-hal 0.2.7", "embedded-time", "nb 1.1.0", "rtcc", @@ -1593,14 +1633,15 @@ dependencies = [ [[package]] name = "tempfile" -version = "3.10.1" +version = "3.11.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "85b77fafb263dd9d05cbeac119526425676db3784113aa9295c88498cbf8bff1" +checksum = "b8fcd239983515c23a32fb82099f97d0b11b8c72f654ed659363a95c3dad7a53" dependencies = [ "cfg-if", "fastrand", + "once_cell", "rustix", - "windows-sys", + "windows-sys 0.52.0", ] [[package]] @@ -1732,11 +1773,11 @@ checksum = "9c8d87e72b64a3b4db28d11ce29237c246188f4f51057d65a7eab63b7987e423" [[package]] name = "winapi-util" -version = "0.1.8" +version = "0.1.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4d4cc384e1e73b93bafa6fb4f1df8c41695c8a91cf9c4c64358067d15a7b6c6b" +checksum = "cf221c93e13a30d793f7645a0e7762c55d169dbb0a49671918a2319d289b10bb" dependencies = [ - "windows-sys", + "windows-sys 0.59.0", ] [[package]] @@ -1748,6 +1789,15 @@ dependencies = [ "windows-targets", ] +[[package]] +name = "windows-sys" +version = "0.59.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1e38bc4d79ed67fd075bcc251a1c39b32a1776bbe92e5bef1f0bf1f8c531853b" +dependencies = [ + "windows-targets", +] + [[package]] name = "windows-targets" version = "0.52.6" @@ -1811,3 +1861,24 @@ name = "windows_x86_64_msvc" version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "589f6da84c646204747d1270a2a5661ea66ed1cced2631d546fdfb155959f9ec" + +[[package]] +name = "zerocopy" +version = "0.7.35" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1b9b4fd18abc82b8136838da5d50bae7bdea537c574d8dc1a34ed098d6c166f0" +dependencies = [ + "byteorder", + "zerocopy-derive", +] + +[[package]] +name = "zerocopy-derive" +version = "0.7.35" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fa4f8080344d4671fb4e831a13ad1e68092748387dfc4f55e356242fae12ce3e" +dependencies = [ + "proc-macro2 1.0.86", + "quote 1.0.36", + "syn 2.0.72", +] diff --git a/boards/communication/Cargo.toml b/boards/communication/Cargo.toml index cbb5dada..60811447 100644 --- a/boards/communication/Cargo.toml +++ b/boards/communication/Cargo.toml @@ -17,7 +17,7 @@ common-arm = { path = "../../libraries/common-arm" } atsamd-hal = { workspace = true } messages = { path = "../../libraries/messages" } typenum = "1.16.0" -embedded-sdmmc = "0.3.0" +embedded-sdmmc = "0.8.0" #panic-halt = "0.2.0" defmt = "0.3" defmt-rtt = "0.4" diff --git a/boards/communication/src/communication.rs b/boards/communication/src/communication.rs index 4825ed05..54985edf 100644 --- a/boards/communication/src/communication.rs +++ b/boards/communication/src/communication.rs @@ -5,14 +5,14 @@ use atsamd_hal::clock::v2::ahb::AhbClk; use atsamd_hal::clock::v2::gclk::{Gclk0Id, Gclk1Id}; use atsamd_hal::clock::v2::pclk::Pclk; use atsamd_hal::clock::v2::pclk::PclkToken; -use atsamd_hal::clock::v2::types::Can0; +use atsamd_hal::clock::v2::types::{Can0, Can1}; use atsamd_hal::clock::v2::Source; -use atsamd_hal::gpio::PA08; +use atsamd_hal::gpio::{AlternateH, H, PA08, PB14, PB15}; use atsamd_hal::gpio::PA09; use atsamd_hal::gpio::{ Alternate, AlternateI, Disabled, Floating, Pin, I, PA12, PA13, PA22, PA23, PB16, PB17, }; -use atsamd_hal::pac::CAN0; +use atsamd_hal::pac::{CAN0, CAN1}; use atsamd_hal::pac::MCLK; use atsamd_hal::pac::SERCOM0; use atsamd_hal::pac::SERCOM2; @@ -64,6 +64,18 @@ impl mcan::messageram::Capacities for Capacities { type TxEventFifo = U32; } +// macro_rules! create_filter { +// ($can:expr, $action:expr, $sender:expr) => { +// $can.filters_standard() +// .push(Filter::Classic { +// action: $action, +// filter: ecan::StandardId::new($sender.into()).unwrap(), +// mask: ecan::StandardId::ZERO, +// }) +// .unwrap_or_else(|_| panic!("Filter Error")); +// }; +// } + pub struct CanDevice0 { pub can: Can< 'static, @@ -158,9 +170,7 @@ impl CanDevice0 { }) .unwrap_or_else(|_| panic!("Ground Station filter")); - // info!("Can Device 0 initialized"); let can = can.finalize().unwrap(); - // info!("Can Device 0 finalized"); ( CanDevice0 { can, @@ -219,6 +229,165 @@ impl CanDevice0 { } } +// So I really am not a fan of this can device 0 and can device 1, I think it would be better to have a single generic can manager +// that can also take a list of filters and apply them. + +pub struct CanCommandManager { + pub can: Can< + 'static, + Can1, + Dependencies>, Pin>, CAN1>, + Capacities, + >, + line_interrupts: OwnedInterruptSet, +} + +impl CanCommandManager { + pub fn new( + can_rx: Pin, + can_tx: Pin, + pclk_can: Pclk, + ahb_clock: AhbClk, + peripheral: CAN1, + gclk0: S, + can_memory: &'static mut SharedMemory, + loopback: bool, + ) -> (Self, S::Inc) + where + S: Source + Increment, + { + let (can_dependencies, gclk0) = + Dependencies::new(gclk0, pclk_can, ahb_clock, can_rx, can_tx, peripheral); + + let mut can = + mcan::bus::CanConfigurable::new(fugit::RateExtU32::kHz(200), can_dependencies, can_memory).unwrap(); + can.config().mode = Mode::Fd { + allow_bit_rate_switching: false, + data_phase_timing: BitTiming::new(fugit::RateExtU32::kHz(500)), + }; + + if loopback { + can.config().loopback = true; + } + + let interrupts_to_be_enabled = can + .interrupts() + .split( + [ + Interrupt::RxFifo0NewMessage, + Interrupt::RxFifo0Full, + // Interrupt::RxFifo0MessageLost, + Interrupt::RxFifo1NewMessage, + Interrupt::RxFifo1Full, + // Interrupt::RxFifo1MessageLost, + ] + .into_iter() + .collect(), + ) + .unwrap(); + + // Line 0 and 1 are connected to the same interrupt line + let line_interrupts = can + .interrupt_configuration() + .enable_line_0(interrupts_to_be_enabled); + + can.filters_standard() + .push(Filter::Classic { + action: Action::StoreFifo0, + filter: ecan::StandardId::new(messages::sender::Sender::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::sender::Sender::SensorBoard.into()) + .unwrap(), + mask: ecan::StandardId::ZERO, + }) + .unwrap_or_else(|_| panic!("Sensor filter")); + + can.filters_standard() + .push(Filter::Classic { + action: Action::StoreFifo0, + filter: ecan::StandardId::new(messages::sender::Sender::PowerBoard.into()).unwrap(), + mask: ecan::StandardId::ZERO, + }) + .unwrap_or_else(|_| panic!("Power filter")); + + can.filters_standard() + .push(Filter::Classic { + action: Action::StoreFifo0, + filter: ecan::StandardId::new(messages::sender::Sender::GroundStation.into()) + .unwrap(), + mask: ecan::StandardId::ZERO, + }) + .unwrap_or_else(|_| panic!("Ground Station filter")); + + let can = can.finalize().unwrap(); + ( + CanCommandManager { + can, + line_interrupts, + }, + gclk0, + ) + } + pub fn send_message(&mut self, m: Message) -> Result<(), HydraError> { + let payload: Vec = postcard::to_vec(&m)?; + self.can.tx.transmit_queued( + tx::MessageBuilder { + id: ecan::Id::Standard(ecan::StandardId::new(m.sender.into()).unwrap()), + frame_type: tx::FrameType::FlexibleDatarate { + payload: &payload[..], + bit_rate_switching: false, + force_error_state_indicator: false, + }, + store_tx_event: None, + } + .build()?, + )?; + Ok(()) + } + pub fn process_data(&mut self, data_manager: &mut DataManager) { + let line_interrupts = &self.line_interrupts; + for interrupt in line_interrupts.iter_flagged() { + match interrupt { + Interrupt::RxFifo0NewMessage => { + for message in &mut self.can.rx_fifo_0 { + match from_bytes::(message.data()) { + Ok(data) => { + data_manager.handle_command(data); + } + Err(_) => { + // error!("Error, ErrorContext::UnkownCanMessage"); + } + } + } + } + Interrupt::RxFifo1NewMessage => { + for message in &mut self.can.rx_fifo_1 { + match from_bytes::(message.data()) { + Ok(data) => { + data_manager.handle_command(data); + } + Err(_) => { + // error!("Error, ErrorContext::UnkownCanMessage"); + } + } + } + } + _ => (), + } + } + } +} + + + + pub struct RadioDevice { transmitter: Uart, receiver: PeekReader>, diff --git a/boards/communication/src/data_manager.rs b/boards/communication/src/data_manager.rs index 6d600691..a374aee3 100644 --- a/boards/communication/src/data_manager.rs +++ b/boards/communication/src/data_manager.rs @@ -74,6 +74,19 @@ impl DataManager { pub fn clone_states(&self) -> [Option; 1] { [self.state.clone()] } + pub fn handle_command(&mut self, command: Message) { + match command.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(_) => {} + }, + _ => {} + } + } pub fn handle_data(&mut self, data: Message) { match data.data { messages::Data::Sensor(ref sensor) => match sensor.data { @@ -120,14 +133,14 @@ impl DataManager { 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(_) => {} - }, + // 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(_) => {} + // }, _ => {} } } diff --git a/boards/communication/src/main.rs b/boards/communication/src/main.rs index 0ee69934..dfd413a5 100644 --- a/boards/communication/src/main.rs +++ b/boards/communication/src/main.rs @@ -5,8 +5,8 @@ mod communication; mod data_manager; mod types; -use atsamd_hal::rtc::Count32Mode; use atsamd_hal as hal; +use atsamd_hal::rtc::Count32Mode; use common_arm::HealthManager; use common_arm::HealthMonitor; use common_arm::SdManager; @@ -14,16 +14,33 @@ use common_arm::*; use common_arm_atsame::mcan; // use panic_halt as _; +use atsamd_hal::time::*; +use atsamd_hal::{ + clock::v2::{ + clock_system_at_reset, + xosc::{CrystalCurrent, SafeClockDiv, StartUpDelay, Xosc}, + }, + pac::Peripherals, +}; use communication::Capacities; -use communication::{RadioDevice, RadioManager}; +use communication::{CanCommandManager, RadioDevice, RadioManager}; +use core::cell::RefCell; +use cortex_m::interrupt::Mutex; +use cortex_m_rt::exception; +use cortex_m_rt::ExceptionFrame; use data_manager::DataManager; +use defmt::info; +use defmt_rtt as _; // global logger +use fugit::ExtU32; +use fugit::ExtU64; +use fugit::RateExtU32; use hal::adc::Adc; use hal::clock::v2::pclk::Pclk; use hal::clock::v2::Source; use hal::gpio::Pins; use hal::gpio::{ - Alternate, Output, Pin, PushPull, PushPullOutput, C, D, PA02, PA04, PA05, PA06, PA03, PA07, PB12, PA08, PA09, - PB13, PB14, PB15, + Alternate, Output, Pin, PushPull, PushPullOutput, C, D, PA02, PA03, PA04, PA05, PA06, PA07, + PA08, PA09, PB12, PB13, PB14, PB15, }; use hal::prelude::*; use hal::sercom::uart; @@ -35,26 +52,9 @@ use messages::command::RadioRate; use messages::health::Health; use messages::state::State; use messages::*; +use panic_probe as _; use systick_monotonic::*; -use core::cell::RefCell; use types::*; -use atsamd_hal::{ - clock::v2::{ - clock_system_at_reset, - xosc::{CrystalCurrent, SafeClockDiv, StartUpDelay, Xosc}, - }, - pac::Peripherals, -}; -use cortex_m::interrupt::Mutex; -use atsamd_hal::time::*; -use fugit::ExtU64; -use cortex_m_rt::exception; -use cortex_m_rt::ExceptionFrame; -use defmt::info; -use defmt_rtt as _; // global logger -use panic_probe as _; -use fugit::ExtU32; -use fugit::RateExtU32; #[inline(never)] #[defmt::panic_handler] @@ -69,9 +69,7 @@ fn panic() -> ! { /// loop. #[cortex_m_rt::exception] unsafe fn HardFault(_frame: &cortex_m_rt::ExceptionFrame) -> ! { - loop { - - } + loop {} } static RTC: Mutex>>> = Mutex::new(RefCell::new(None)); @@ -91,6 +89,7 @@ mod app { data_manager: DataManager, radio_manager: RadioManager, can0: communication::CanDevice0, + can_command_manager: CanCommandManager, } #[local] @@ -117,10 +116,11 @@ mod app { #[monotonic(binds = SysTick, default = true)] type MyMono = Systick<100>; // 100 Hz / 10 ms granularity - #[init(local = [ #[link_section = ".can"] - can_memory: SharedMemory = SharedMemory::new() + can_memory: SharedMemory = SharedMemory::new(), + #[link_section = ".can_command"] + can_command_memory: SharedMemory = SharedMemory::new() ])] fn init(cx: init::Context) -> (Shared, Local, init::Monotonics) { let mut peripherals = cx.device; @@ -162,7 +162,19 @@ mod app { true, ); - // setup external osc + let (pclk_can_command, gclk0) = Pclk::enable(tokens.pclks.can1, gclk0); + let (can_command_manager, gclk0) = CanCommandManager::new( + pins.pb15.into_mode(), + pins.pb14.into_mode(), + pclk_can_command, + clocks.ahbs.can1, + peripherals.CAN1, + gclk0, + cx.local.can_command_memory, + true, + ); + + // setup external osc // let xosc0 = atsamd_hal::clock::v2::xosc::Xosc::from_crystal( // tokens.xosc0, // pins.pa14, @@ -177,8 +189,8 @@ mod app { // } // let (mut gclk0, dfll) = - // hal::clock::v2::gclk::Gclk::from_source(tokens.gclks.gclk2, xosc0); - // let gclk2 = gclk2.div(gclk::GclkDiv8::Div(1)).enable(); + // hal::clock::v2::gclk::Gclk::from_source(tokens.gclks.gclk2, xosc0); + // let gclk2 = gclk2.div(gclk::GclkDiv8::Div(1)).enable(); let (pclk_radio, gclk0) = Pclk::enable(tokens.pclks.sercom2, gclk0); /* Radio config */ @@ -187,14 +199,15 @@ mod app { let pads = uart::Pads::::default() .rx(rx) .tx(tx); - let mut uart = GroundStationUartConfig::new(&mclk, peripherals.SERCOM2, pads, pclk_radio.freq()) - .baud( - RateExtU32::Hz(57600), - uart::BaudMode::Fractional(uart::Oversampling::Bits16), - ) - .enable(); + let mut uart = + GroundStationUartConfig::new(&mclk, peripherals.SERCOM2, pads, pclk_radio.freq()) + .baud( + RateExtU32::Hz(57600), + uart::BaudMode::Fractional(uart::Oversampling::Bits16), + ) + .enable(); - let (radio) = RadioDevice::new(uart); + let radio = RadioDevice::new(uart); let radio_manager = RadioManager::new(radio); @@ -259,6 +272,7 @@ mod app { data_manager: DataManager::new(), radio_manager, can0, + can_command_manager, }, Local { led, @@ -283,6 +297,16 @@ mod app { // }); // } + /// Handles the CAN0 interrupt. + #[task(priority = 3, binds = CAN1, shared = [can_command_manager, data_manager])] + fn can_command(mut cx: can_command::Context) { + cx.shared.can_command_manager.lock(|can| { + cx.shared + .data_manager + .lock(|data_manager| can.process_data(data_manager)); + }); + } + /// Handles the CAN0 interrupt. #[task(priority = 3, binds = CAN0, shared = [can0, data_manager])] fn can0(mut cx: can0::Context) { @@ -295,20 +319,36 @@ mod app { /** * Sends a message over CAN. */ - #[task(capacity = 10, local = [counter: u16 = 0], shared = [can0, &em])] + #[task(capacity = 10, shared = [can0, &em])] fn send_internal(mut cx: send_internal::Context, m: Message) { cx.shared.em.run(|| { cx.shared.can0.lock(|can| can.send_message(m))?; Ok(()) }); - } + } + + /** + * Sends a message over CAN. + */ + #[task(capacity = 5, shared = [can_command_manager, &em])] + fn send_command(mut cx: send_command::Context, m: Message) { + cx.shared.em.run(|| { + cx.shared.can_command_manager.lock(|can| can.send_message(m))?; + 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) { - let message = Message::new(cortex_m::interrupt::free(|cs| { - let mut rc = RTC.borrow(cs).borrow_mut(); - let rtc = rc.as_mut().unwrap(); - rtc.count32()}), COM_ID, d.into()); + let message = Message::new( + cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + rtc.count32() + }), + COM_ID, + d.into(), + ); send_gs::spawn(message).ok(); } @@ -322,8 +362,7 @@ mod app { cx.shared.em.run(|| { let mut buf = [0; 255]; let data = postcard::to_slice(&m, &mut buf)?; - radio_manager - .send_message(data)?; + radio_manager.send_message(data)?; Ok(()) }) }); @@ -387,12 +426,13 @@ mod app { cortex_m::interrupt::free(|cs| { let mut rc = RTC.borrow(cs).borrow_mut(); let rtc = rc.as_mut().unwrap(); - rtc.count32()}), + rtc.count32() + }), COM_ID, State::new(StateData::Initializing), ); - spawn!(send_gs, message)?; - // spawn!(send_internal, message)?; + spawn!(send_gs, message.clone())?; + spawn!(send_internal, message)?; Ok(()) }); spawn!(generate_random_messages).ok(); diff --git a/boards/nav/src/data_manager.rs b/boards/nav/src/data_manager.rs index efc3936f..58352615 100644 --- a/boards/nav/src/data_manager.rs +++ b/boards/nav/src/data_manager.rs @@ -1,18 +1,18 @@ use common_arm::{spawn, HydraError}; use messages::sensor::{ - Air, EkfNav1, EkfNav2, EkfQuat, GpsPos1, GpsPos2, GpsVel, Imu1, Imu2, SensorData, UtcTime, + Air, EkfNav1, EkfNav2, EkfNavAcc, EkfQuat, GpsPos1, GpsPos2, GpsPosAcc, GpsVel, GpsVelAcc, Imu1, Imu2, SensorData, UtcTime }; use messages::Message; #[derive(Clone)] pub struct DataManager { pub air: Option, - pub ekf_nav: Option<(EkfNav1, EkfNav2)>, + pub ekf_nav: Option<(EkfNav1, EkfNav2, EkfNavAcc)>, pub ekf_quat: Option, pub imu: Option<(Imu1, Imu2)>, pub utc_time: Option, - pub gps_vel: Option, - pub gps_pos: Option<(GpsPos1, GpsPos2)>, + pub gps_vel: Option<(GpsVel, GpsVelAcc)>, + pub gps_pos: Option<(GpsPos1, GpsPos2, GpsPosAcc)>, } impl DataManager { @@ -28,7 +28,7 @@ impl DataManager { } } - pub fn clone_sensors(&self) -> [Option; 10] { + pub fn clone_sensors(&self) -> [Option; 11] { [ self.air.clone().map(|x| x.into()), self.ekf_nav.clone().map(|x| x.0.into()), @@ -37,7 +37,8 @@ impl DataManager { self.imu.clone().map(|x| x.0.into()), self.imu.clone().map(|x| x.1.into()), self.utc_time.clone().map(|x| x.into()), - self.gps_vel.clone().map(|x| x.into()), + self.gps_vel.clone().map(|x| x.0.into()), + self.gps_vel_acc.clone().map(|x| x.1.into()), self.gps_pos.clone().map(|x| x.0.into()), self.gps_pos.clone().map(|x| x.1.into()), ] diff --git a/boards/nav/src/main.rs b/boards/nav/src/main.rs index 6aedbf23..5e0f8d20 100644 --- a/boards/nav/src/main.rs +++ b/boards/nav/src/main.rs @@ -2,7 +2,7 @@ #![no_main] mod data_manager; -// mod sbg_manager; +mod sbg_manager; mod types; mod communication; @@ -38,7 +38,7 @@ use stm32h7xx_hal::dma::dma::StreamsTuple; // use panic_halt as _; #[panic_handler] fn panic(info: &core::panic::PanicInfo) -> ! { - stm32h7xx_hal::pac::SCB::sys_reset(); + stm32h7xx_hal::pac::SCB::sys_reset() } #[rtic::app(device = stm32h7xx_hal::stm32, dispatchers = [EXTI0, EXTI1])] @@ -59,7 +59,7 @@ mod app { struct LocalResources { led_red: PA2>, led_green: PA3>, - // sbg_manager: sbg_manager::SBGManager, + sbg_manager: sbg_manager::SBGManager, } #[monotonic(binds = SysTick, default = true)] @@ -173,7 +173,7 @@ mod app { .UART4 .serial((tx, rx), 9_800.bps(), ccdr.peripheral.UART4, &ccdr.clocks) .unwrap(); - // let sbg_manager = sbg_manager::SBGManager::new(uart_sbg, stream_tuple); + let sbg_manager = sbg_manager::SBGManager::new(uart_sbg, stream_tuple); // let serial = ctx // .device // .UART4 @@ -221,7 +221,7 @@ mod app { LocalResources { led_red, led_green, - // sbg_manager: sbg_manager, + sbg_manager: sbg_manager, }, init::Monotonics(mono), ) @@ -257,23 +257,23 @@ mod app { // Turn off the SBG and CAN } - // extern "Rust" { - // #[task(capacity = 3, shared = [&em, sd_manager])] - // fn sbg_sd_task(context: sbg_sd_task::Context, data: [u8; SBG_BUFFER_SIZE]); + extern "Rust" { + #[task(capacity = 3, shared = [&em, sd_manager])] + fn sbg_sd_task(context: sbg_sd_task::Context, data: [u8; SBG_BUFFER_SIZE]); - // #[task(binds = DMA1_STR0, shared = [&em], local = [sbg_manager])] - // fn sbg_dma(context: sbg_dma::Context); + #[task(binds = DMA1_STR0, shared = [&em], local = [sbg_manager])] + fn sbg_dma(context: sbg_dma::Context); - // #[task(capacity = 20, shared = [data_manager])] - // fn sbg_handle_data(context: sbg_handle_data::Context, data: CallbackData); + #[task(capacity = 20, shared = [data_manager])] + fn sbg_handle_data(context: sbg_handle_data::Context, data: CallbackData); - // #[task(shared = [rtc, &em])] - // fn sbg_get_time(context: sbg_get_time::Context); + #[task(shared = [rtc, &em])] + fn sbg_get_time(context: sbg_get_time::Context); - // #[task()] - // fn sbg_flush(context: sbg_flush::Context); + #[task()] + fn sbg_flush(context: sbg_flush::Context); - // #[task(shared = [&em])] - // fn sbg_write_data(context: sbg_write_data::Context, data: Vec); - // } + #[task(shared = [&em])] + fn sbg_write_data(context: sbg_write_data::Context, data: Vec); + } } diff --git a/boards/nav/src/sbg_manager.rs b/boards/nav/src/sbg_manager.rs index ae94f827..686706e7 100644 --- a/boards/nav/src/sbg_manager.rs +++ b/boards/nav/src/sbg_manager.rs @@ -42,7 +42,7 @@ pub struct SBGManager { StreamX, Rx, stm32h7xx_hal::dma::PeripheralToMemory, - MaybeUninit<[u8;SBG_BUFFER_SIZE]>, + MaybeUninit<[u8; SBG_BUFFER_SIZE]>, stm32h7xx_hal::dma::DBTransfer, >, >, @@ -66,23 +66,29 @@ impl SBGManager { let (sbg_tx, sbg_rx) = serial.split(); // TODO: This could be wrong. It's a bit of a guess. - let sbg_buffer: &'static mut [u8; SBG_BUFFER_SIZE] = { - let buf: &mut [MaybeUninit; SBG_BUFFER_SIZE] = - unsafe { &mut *(core::ptr::addr_of_mut!(SBG_BUFFER) as *mut _) }; - for (i, value) in buf.iter_mut().enumerate() { - unsafe { value.as_mut_ptr().write(i as u8) }; - } - unsafe { SBG_BUFFER.assume_init_mut() } - }; + // let sbg_buffer: &'static mut [u8; SBG_BUFFER_SIZE] = { + // let buf: &mut [MaybeUninit; SBG_BUFFER_SIZE] = + // unsafe { &mut *(core::ptr::addr_of_mut!(SBG_BUFFER) as *mut _) }; + // for (i, value) in buf.iter_mut().enumerate() { + // unsafe { value.as_mut_ptr().write(i as u8) }; + // } + // unsafe { SBG_BUFFER.assume_init_mut() } + // }; let config = DmaConfig::default().memory_increment(true); let transfer: Transfer< StreamX, Rx, - stm32h7xx_hal::dma::PeripheralToMemory, - &mut [u8], + PeripheralToMemory, + MaybeUninit<[u8; SBG_BUFFER_SIZE]>, stm32h7xx_hal::dma::DBTransfer, - > = Transfer::init(stream_tuple.0, sbg_rx, &mut sbg_buffer[..], None, config); + > = Transfer::init( + stream_tuple.0, + sbg_rx, + unsafe { (*core::ptr::addr_of_mut!(TARGET_BUFFER)).assume_init_mut() }, // Uninitialised memory + None, + config, + ); transfer.start(|serial| { serial.enable_dma_rx(); @@ -164,7 +170,7 @@ pub fn sbg_dma(cx: crate::app::sbg_dma::Context) { let sbg = cx.local.sbg_manager; match &mut sbg.xfer { - Some(xfer) => if xfer.complete() {}, + Some(xfer) => if xfer.get_transfer_complete_flag(){}, None => { // it should be impossible to reach here. info!("None"); diff --git a/libraries/common-arm-atsame/memory.x b/libraries/common-arm-atsame/memory.x index 690b7767..615b4ebe 100644 --- a/libraries/common-arm-atsame/memory.x +++ b/libraries/common-arm-atsame/memory.x @@ -2,7 +2,8 @@ MEMORY { /* NOTE K = KiBi = 1024 bytes */ FLASH : ORIGIN = 0x00000000, LENGTH = 256K - CAN : ORIGIN = 0x20000000, LENGTH = 64K + CAN : ORIGIN = 0x20000000, LENGTH = 32K + CAN_COMMAND : ORIGIN = 0x20008000, LENGTH = 32K RAM : ORIGIN = 0x20010000, LENGTH = 64K } SECTIONS From 65896180b4b952c5234e5f0cbcf68109ff62ea9a Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Tue, 6 Aug 2024 16:09:06 -0400 Subject: [PATCH 11/47] WIP: SBG. --- .vscode/launch.json | 18 +- Cargo.lock | 4 + Cargo.toml | 2 +- boards/beacon/src/communication.rs | 2 +- boards/beacon/src/main.rs | 29 ++- boards/beacon/src/types.rs | 2 +- boards/camera/src/data_manager.rs | 6 +- boards/communication/src/communication.rs | 54 +++-- boards/communication/src/data_manager.rs | 2 + boards/communication/src/main.rs | 60 ++++-- boards/nav/Cargo.toml | 3 +- boards/nav/src/communication.rs | 39 ++-- boards/nav/src/data_manager.rs | 6 +- boards/nav/src/main.rs | 189 +++++++++++------- boards/nav/src/sbg_manager.rs | 130 +++++++----- boards/nav/src/types.rs | 5 +- boards/recovery/Cargo.toml | 3 +- boards/recovery/src/communication.rs | 161 ++++++++++++++- boards/recovery/src/data_manager.rs | 21 +- boards/recovery/src/gpio_manager.rs | 2 +- boards/recovery/src/main.rs | 125 +++++++++--- .../src/state_machine/states/ascent.rs | 13 +- .../state_machine/states/wait_for_recovery.rs | 47 +++-- boards/sensor/src/data_manager.rs | 3 +- boards/sensor/src/sbg_manager.rs | 2 +- examples/simple/src/main.rs | 2 +- libraries/common-arm-atsame/memory.x | 4 + .../common-arm/src/error/error_manager.rs | 4 +- libraries/common-arm/src/error/hydra_error.rs | 2 +- libraries/messages/src/lib.rs | 7 +- libraries/sbg-rs/src/data_conversion.rs | 79 +++++--- libraries/sbg-rs/src/sbg.rs | 38 ++-- 32 files changed, 748 insertions(+), 316 deletions(-) diff --git a/.vscode/launch.json b/.vscode/launch.json index 88fc5d3d..7de0df59 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -11,7 +11,18 @@ "chip": "ATSAME51J18A", "coreConfigs": [ { - "programBinary": "${workspaceFolder}/target/thumbv7em-none-eabihf/debug/recovery", + "programBinary": "${workspaceFolder}/target/thumbv7em-none-eabihf/release/recovery", + } + ] + }, + { + "type": "probe-rs-debug", + "request": "attach", + "name": "Probe-rs attach nav (Debug)", + "chip": "STM32H733VGTx", + "coreConfigs": [ + { + "programBinary": "${workspaceFolder}/target/thumbv7em-none-eabihf/debug/nav", } ] }, @@ -39,6 +50,11 @@ "request": "launch", "name": "Probe-rs Debug nav (Debug)", "chip": "STM32H733VGTx", + "flashingConfig": { + "haltAfterReset": true, + // "flashingEnabled": true, + + }, "coreConfigs": [ { "programBinary": "${workspaceFolder}/target/thumbv7em-none-eabihf/debug/nav", diff --git a/Cargo.lock b/Cargo.lock index 85466b1b..e3363864 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -906,10 +906,12 @@ dependencies = [ "cortex-m-rt", "cortex-m-rtic", "defmt", + "defmt-rtt", "embedded-alloc", "fdcan", "heapless 0.7.17", "messages", + "panic-probe", "postcard", "sbg-rs", "stm32h7xx-hal", @@ -1236,11 +1238,13 @@ dependencies = [ "cortex-m-rt", "cortex-m-rtic", "defmt", + "defmt-rtt", "embedded-hal 0.2.7", "enum_dispatch", "heapless 0.7.17", "messages", "ms5611-01ba", + "panic-probe", "postcard", "systick-monotonic", "typenum", diff --git a/Cargo.toml b/Cargo.toml index a93172d4..76987246 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -58,7 +58,7 @@ codegen-units = 1 debug = 2 debug-assertions = false # <- incremental = false -lto = 'fat' +lto = 'thin' opt-level = 3 # <- overflow-checks = false # <- diff --git a/boards/beacon/src/communication.rs b/boards/beacon/src/communication.rs index 52a04d58..765597b7 100644 --- a/boards/beacon/src/communication.rs +++ b/boards/beacon/src/communication.rs @@ -1 +1 @@ -//! Interface the SPI-based CAN device here. \ No newline at end of file +//! Interface the SPI-based CAN device here. diff --git a/boards/beacon/src/main.rs b/boards/beacon/src/main.rs index 5fb6b46a..fbf26102 100644 --- a/boards/beacon/src/main.rs +++ b/boards/beacon/src/main.rs @@ -9,19 +9,22 @@ use common_arm::SdManager; use common_arm::*; use data_manager::DataManager; // use defmt::info; -use messages::sensor::Sensor; -use messages::*; -use stm32l0xx_hal as hal; use hal::{ gpio::*, + gpio::{ + gpioa::{PA10, PA9}, + Output, PushPull, + }, prelude::*, rcc::Config, - gpio::{gpioa::{PA9, PA10}, Output, PushPull}, }; +use messages::sensor::Sensor; +use messages::*; +use stm32l0xx_hal as hal; use systick_monotonic::*; -// use https://github.com/lora-rs/lora-rs.git +// use https://github.com/lora-rs/lora-rs.git /// Custom panic handler. /// Reset the system if a panic occurs. @@ -30,7 +33,7 @@ fn panic(_info: &core::panic::PanicInfo) -> ! { stm32l0xx_hal::pac::SCB::sys_reset(); } -// Add dispatchers +// Add dispatchers #[rtic::app(device = hal::pac, peripherals = true, dispatchers = [EXTI0_1, EXTI2_3, EXTI4_15])] mod app { @@ -56,11 +59,11 @@ mod app { fn init(cx: init::Context) -> (Shared, Local, init::Monotonics) { let device = cx.device; let core = cx.core; - - // configure the clock + + // configure the clock let mut rcc = device.RCC.freeze(Config::hse(64_000_000u32.Hz())); - // configure leds + // configure leds let gpioa = device.GPIOA.split(&mut rcc); let mut green_led = gpioa.pa9.into_push_pull_output(); let mut red_led = gpioa.pa10.into_push_pull_output(); @@ -73,12 +76,8 @@ mod app { Shared { em: ErrorManager::new(), data_manager: DataManager::new(), - - }, - Local { - green_led, - red_led, }, + Local { green_led, red_led }, init::Monotonics(mono), ) } @@ -97,7 +96,7 @@ mod app { todo!("Send messages over CAN"); } - #[task(capacity = 5)] + #[task(capacity = 5)] fn send_external(cx: send_external::Context, m: Message) { todo!("Send messages over LORA"); } diff --git a/boards/beacon/src/types.rs b/boards/beacon/src/types.rs index d2fccd10..c83ce98d 100644 --- a/boards/beacon/src/types.rs +++ b/boards/beacon/src/types.rs @@ -4,4 +4,4 @@ use messages::sender::Sender::BeaconBoard; // ------- // Sender ID // ------- -pub static COM_ID: Sender = BeaconBoard; \ No newline at end of file +pub static COM_ID: Sender = BeaconBoard; diff --git a/boards/camera/src/data_manager.rs b/boards/camera/src/data_manager.rs index 5e62aca9..bcafdcb1 100644 --- a/boards/camera/src/data_manager.rs +++ b/boards/camera/src/data_manager.rs @@ -64,7 +64,7 @@ impl DataManager { Some(air) => match air.altitude { Some(altitude) => altitude > HEIGHT_MIN, None => false, - } + }, None => false, } } @@ -105,8 +105,8 @@ impl DataManager { match self.air.as_ref() { Some(air) => match air.altitude { Some(altitude) => altitude < MAIN_HEIGHT, - None => false - } + None => false, + }, None => false, } } diff --git a/boards/communication/src/communication.rs b/boards/communication/src/communication.rs index 54985edf..347628f5 100644 --- a/boards/communication/src/communication.rs +++ b/boards/communication/src/communication.rs @@ -7,26 +7,27 @@ use atsamd_hal::clock::v2::pclk::Pclk; use atsamd_hal::clock::v2::pclk::PclkToken; use atsamd_hal::clock::v2::types::{Can0, Can1}; use atsamd_hal::clock::v2::Source; -use atsamd_hal::gpio::{AlternateH, H, PA08, PB14, PB15}; use atsamd_hal::gpio::PA09; use atsamd_hal::gpio::{ Alternate, AlternateI, Disabled, Floating, Pin, I, PA12, PA13, PA22, PA23, PB16, PB17, }; -use atsamd_hal::pac::{CAN0, CAN1}; +use atsamd_hal::gpio::{AlternateH, H, PA08, PB14, PB15}; use atsamd_hal::pac::MCLK; use atsamd_hal::pac::SERCOM0; use atsamd_hal::pac::SERCOM2; use atsamd_hal::pac::SERCOM4; use atsamd_hal::pac::SERCOM5; +use atsamd_hal::pac::{CAN0, CAN1}; use atsamd_hal::sercom; use atsamd_hal::sercom::uart; use atsamd_hal::sercom::uart::Uart; use atsamd_hal::sercom::uart::{RxDuplex, TxDuplex}; use atsamd_hal::typelevel::Increment; -use common_arm:: HydraError; +use common_arm::HydraError; use common_arm_atsame::mcan; use common_arm_atsame::mcan::message::{rx, Raw}; use common_arm_atsame::mcan::tx_buffers::DynTx; +use defmt::error; use heapless::HistoryBuffer; use heapless::Vec; use mavlink::embedded::Read; @@ -103,8 +104,12 @@ impl CanDevice0 { let (can_dependencies, gclk0) = Dependencies::new(gclk0, pclk_can, ahb_clock, can_rx, can_tx, peripheral); - let mut can = - mcan::bus::CanConfigurable::new(fugit::RateExtU32::kHz(200), can_dependencies, can_memory).unwrap(); + let mut can = mcan::bus::CanConfigurable::new( + fugit::RateExtU32::kHz(200), + can_dependencies, + can_memory, + ) + .unwrap(); can.config().mode = Mode::Fd { allow_bit_rate_switching: false, data_phase_timing: BitTiming::new(fugit::RateExtU32::kHz(500)), @@ -169,7 +174,7 @@ impl CanDevice0 { mask: ecan::StandardId::ZERO, }) .unwrap_or_else(|_| panic!("Ground Station filter")); - + let can = can.finalize().unwrap(); ( CanDevice0 { @@ -229,8 +234,8 @@ impl CanDevice0 { } } -// So I really am not a fan of this can device 0 and can device 1, I think it would be better to have a single generic can manager -// that can also take a list of filters and apply them. +// So I really am not a fan of this can device 0 and can device 1, I think it would be better to have a single generic can manager +// that can also take a list of filters and apply them. pub struct CanCommandManager { pub can: Can< @@ -259,8 +264,12 @@ impl CanCommandManager { let (can_dependencies, gclk0) = Dependencies::new(gclk0, pclk_can, ahb_clock, can_rx, can_tx, peripheral); - let mut can = - mcan::bus::CanConfigurable::new(fugit::RateExtU32::kHz(200), can_dependencies, can_memory).unwrap(); + let mut can = mcan::bus::CanConfigurable::new( + fugit::RateExtU32::kHz(200), + can_dependencies, + can_memory, + ) + .unwrap(); can.config().mode = Mode::Fd { allow_bit_rate_switching: false, data_phase_timing: BitTiming::new(fugit::RateExtU32::kHz(500)), @@ -279,7 +288,7 @@ impl CanCommandManager { // Interrupt::RxFifo0MessageLost, Interrupt::RxFifo1NewMessage, Interrupt::RxFifo1Full, - // Interrupt::RxFifo1MessageLost, + // Interrupt::RxFifo1MessageLost, ] .into_iter() .collect(), @@ -325,7 +334,7 @@ impl CanCommandManager { mask: ecan::StandardId::ZERO, }) .unwrap_or_else(|_| panic!("Ground Station filter")); - + let can = can.finalize().unwrap(); ( CanCommandManager { @@ -362,7 +371,7 @@ impl CanCommandManager { data_manager.handle_command(data); } Err(_) => { - // error!("Error, ErrorContext::UnkownCanMessage"); + error!("Error, ErrorContext::UnkownCanMessage"); } } } @@ -374,7 +383,7 @@ impl CanCommandManager { data_manager.handle_command(data); } Err(_) => { - // error!("Error, ErrorContext::UnkownCanMessage"); + error!("Error, ErrorContext::UnkownCanMessage"); } } } @@ -385,16 +394,18 @@ impl CanCommandManager { } } - - - pub struct RadioDevice { transmitter: Uart, receiver: PeekReader>, } impl RadioDevice { - pub fn new(mut uart: atsamd_hal::sercom::uart::Uart) -> Self { + pub fn new( + mut uart: atsamd_hal::sercom::uart::Uart< + GroundStationUartConfig, + atsamd_hal::sercom::uart::Duplex, + >, + ) -> Self { let (mut rx, tx) = uart.split(); // setup interrupts @@ -428,14 +439,15 @@ impl RadioManager { component_id: 1, sequence: self.increment_mav_sequence(), }; - // Create a fixed-size array and copy the payload into it + // 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::uorocketry::POSTCARD_MESSAGE_DATA { + message: fixed_payload, + }, ); mavlink::write_versioned_msg( &mut self.radio.transmitter, diff --git a/boards/communication/src/data_manager.rs b/boards/communication/src/data_manager.rs index a374aee3..5e48b58a 100644 --- a/boards/communication/src/data_manager.rs +++ b/boards/communication/src/data_manager.rs @@ -1,3 +1,4 @@ +use defmt::info; use messages::command::RadioRate; use messages::state::StateData; use messages::Message; @@ -75,6 +76,7 @@ impl DataManager { [self.state.clone()] } pub fn handle_command(&mut self, command: Message) { + info!("Handling command"); match command.data { messages::Data::Command(command) => match command.data { messages::command::CommandData::RadioRateChange(command_data) => { diff --git a/boards/communication/src/main.rs b/boards/communication/src/main.rs index dfd413a5..ba266515 100644 --- a/boards/communication/src/main.rs +++ b/boards/communication/src/main.rs @@ -78,7 +78,9 @@ static RTC: Mutex>>> = Mutex::new(RefC mod app { use atsamd_hal::clock::v2::gclk::{self, Gclk}; + use command::{Command, CommandData, RadioRateChange}; use fugit::RateExtU64; + use sender::Sender; use state::StateData; use super::*; @@ -94,7 +96,8 @@ mod app { #[local] struct Local { - led: Pin, + led_green: Pin, + led_red: Pin, // gps_uart: GpsUart, // sd_manager: SdManager< // Spi< @@ -131,7 +134,7 @@ mod app { // let dmaChannels = dmac.split(); /* Logging Setup */ - // HydraLogging::set_ground_station_callback(queue_gs_message); + HydraLogging::set_ground_station_callback(queue_gs_message); /* Clock setup */ let (_, clocks, tokens) = atsamd_hal::clock::v2::clock_system_at_reset( @@ -159,7 +162,7 @@ mod app { peripherals.CAN0, gclk0, cx.local.can_memory, - true, + false, ); let (pclk_can_command, gclk0) = Pclk::enable(tokens.pclks.can1, gclk0); @@ -171,7 +174,7 @@ mod app { peripherals.CAN1, gclk0, cx.local.can_command_memory, - true, + false, ); // setup external osc @@ -251,18 +254,23 @@ mod app { /* Spawn tasks */ // sensor_send::spawn().ok(); // state_send::spawn().ok(); - info!("RTC"); - let mut rtc = hal::rtc::Rtc::count32_mode(peripherals.RTC, RateExtU32::Hz(1024), &mut mclk); - // let mut rtc = rtc_temp.into_count32_mode(); + let mut rtc = hal::rtc::Rtc::clock_mode( + peripherals.RTC, + atsamd_hal::fugit::RateExtU32::Hz(1024), + &mut mclk, + ); + let mut rtc = rtc.into_count32_mode(); // hal bug this must be done rtc.set_count32(0); - cortex_m::interrupt::free(|cs| { RTC.borrow(cs).replace(Some(rtc)); }); - info!("RTC done"); - let led = pins.pa03.into_push_pull_output(); + let mut led_green = pins.pa03.into_push_pull_output(); + let mut led_red = pins.pa02.into_push_pull_output(); + led_green.set_low(); + led_red.set_low(); blink::spawn().ok(); generate_random_messages::spawn().ok(); + generate_random_command::spawn().ok(); let mono = Systick::new(core.SYST, gclk0.freq().to_Hz()); // info!("Init done"); @@ -275,7 +283,8 @@ mod app { can_command_manager, }, Local { - led, + led_green, + led_red, // sd_manager, }, init::Monotonics(mono), @@ -333,7 +342,9 @@ mod app { #[task(capacity = 5, shared = [can_command_manager, &em])] fn send_command(mut cx: send_command::Context, m: Message) { cx.shared.em.run(|| { - cx.shared.can_command_manager.lock(|can| can.send_message(m))?; + cx.shared + .can_command_manager + .lock(|can| can.send_message(m))?; Ok(()) }); } @@ -438,6 +449,26 @@ mod app { spawn!(generate_random_messages).ok(); } + #[task(shared = [&em])] + fn generate_random_command(cx: generate_random_command::Context) { + cx.shared.em.run(|| { + let message = Message::new( + cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + rtc.count32() + }), + COM_ID, + Command::new(CommandData::PowerDown(command::PowerDown { + board: Sender::BeaconBoard, + })), + ); + spawn!(send_command, message)?; + Ok(()) + }); + spawn!(generate_random_command).ok(); + } + // #[task(shared = [data_manager, &em])] // fn state_send(mut cx: state_send::Context) { // let state_data = cx @@ -473,13 +504,14 @@ mod app { * Simple blink task to test the system. * Acts as a heartbeat for the system. */ - #[task(local = [led], shared = [&em])] + #[task(local = [led_green, led_red], shared = [&em])] fn blink(cx: blink::Context) { cx.shared.em.run(|| { - cx.local.led.toggle()?; if cx.shared.em.has_error() { + cx.local.led_red.toggle()?; spawn_after!(blink, ExtU64::millis(200))?; } else { + cx.local.led_green.toggle()?; spawn_after!(blink, ExtU64::secs(1))?; } Ok(()) diff --git a/boards/nav/Cargo.toml b/boards/nav/Cargo.toml index c3c31786..af91c318 100644 --- a/boards/nav/Cargo.toml +++ b/boards/nav/Cargo.toml @@ -21,5 +21,6 @@ sbg-rs = {path = "../../libraries/sbg-rs"} embedded-alloc = "0.5.0" heapless = "0.7.16" -#panic-halt = "0.2.0" +defmt-rtt = "0.4" +panic-probe = { version = "0.3", features = ["print-defmt"] } chrono = {version = "0.4.0", default-features = false} \ No newline at end of file diff --git a/boards/nav/src/communication.rs b/boards/nav/src/communication.rs index 07fbfe03..3ff22e21 100644 --- a/boards/nav/src/communication.rs +++ b/boards/nav/src/communication.rs @@ -1,27 +1,36 @@ -use stm32h7xx_hal as hal; -use stm32h7xx_hal::prelude::*; +use crate::data_manager::DataManager; +use crate::types::COM_ID; +use common_arm::HydraError; +use defmt::info; use fdcan::{ - Transmit, Receive, - config::NominalBitTiming, filter::{StandardFilter, StandardFilterSlot}, frame::{FrameFormat, TxFrameHeader}, id::StandardId, FdCan, Instance + config::NominalBitTiming, + filter::{StandardFilter, StandardFilterSlot}, + frame::{FrameFormat, TxFrameHeader}, + id::StandardId, + FdCan, Instance, Receive, Transmit, }; -use crate::types::COM_ID; use messages::Message; use postcard::from_bytes; -use common_arm::HydraError; -use defmt::info; -use crate::data_manager::DataManager; +use stm32h7xx_hal as hal; +use stm32h7xx_hal::prelude::*; /// Clock configuration is out of scope for this builder -/// easiest way to avoid alloc is to use no generics +/// easiest way to avoid alloc is to use no generics pub struct CanDevice { - can: fdcan::FdCan, fdcan::NormalOperationMode> , + can: fdcan::FdCan< + stm32h7xx_hal::can::Can, + fdcan::NormalOperationMode, + >, } impl CanDevice { - pub fn new(can:fdcan::FdCan, fdcan::NormalOperationMode> ) -> Self { - Self { - can, - } + 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]; @@ -50,4 +59,4 @@ impl CanDevice { } Ok(()) } -} \ No newline at end of file +} diff --git a/boards/nav/src/data_manager.rs b/boards/nav/src/data_manager.rs index 58352615..fe164ae2 100644 --- a/boards/nav/src/data_manager.rs +++ b/boards/nav/src/data_manager.rs @@ -1,6 +1,7 @@ use common_arm::{spawn, HydraError}; use messages::sensor::{ - Air, EkfNav1, EkfNav2, EkfNavAcc, EkfQuat, GpsPos1, GpsPos2, GpsPosAcc, GpsVel, GpsVelAcc, Imu1, Imu2, SensorData, UtcTime + Air, EkfNav1, EkfNav2, EkfNavAcc, EkfQuat, GpsPos1, GpsPos2, GpsPosAcc, GpsVel, GpsVelAcc, + Imu1, Imu2, SensorData, UtcTime, }; use messages::Message; @@ -28,6 +29,7 @@ impl DataManager { } } + // TODO: stop cloning so much this is a waste of resources. pub fn clone_sensors(&self) -> [Option; 11] { [ self.air.clone().map(|x| x.into()), @@ -38,7 +40,7 @@ impl DataManager { self.imu.clone().map(|x| x.1.into()), self.utc_time.clone().map(|x| x.into()), self.gps_vel.clone().map(|x| x.0.into()), - self.gps_vel_acc.clone().map(|x| x.1.into()), + self.gps_vel.clone().map(|x| x.1.into()), self.gps_pos.clone().map(|x| x.0.into()), self.gps_pos.clone().map(|x| x.1.into()), ] diff --git a/boards/nav/src/main.rs b/boards/nav/src/main.rs index 5e0f8d20..151afecc 100644 --- a/boards/nav/src/main.rs +++ b/boards/nav/src/main.rs @@ -1,11 +1,13 @@ #![no_std] #![no_main] +mod communication; mod data_manager; -mod sbg_manager; +// mod sbg_manager; mod types; -mod communication; +use types::COM_ID; +use chrono::{NaiveDate, NaiveDateTime, NaiveTime}; use common_arm::*; use core::num::{NonZeroU16, NonZeroU8}; use data_manager::DataManager; @@ -16,11 +18,13 @@ use fdcan::{ frame::{FrameFormat, TxFrameHeader}, id::StandardId, }; -use chrono::NaiveDate; -use stm32h7xx_hal::prelude::*; +use messages::Data; +use fugit::RateExtU32; +use heapless::Vec; +// use sbg_manager::{sbg_dma, sbg_flush, sbg_get_time, sbg_handle_data, sbg_write_data}; use sbg_rs::sbg::CallbackData; use sbg_rs::sbg::SBG_BUFFER_SIZE; -use fugit::RateExtU32; +use stm32h7xx_hal::dma::dma::StreamsTuple; use stm32h7xx_hal::gpio::gpioa::{PA1, PA2, PA3, PA4}; use stm32h7xx_hal::gpio::gpioc::{PC13, PC3}; use stm32h7xx_hal::gpio::Input; @@ -28,22 +32,40 @@ use stm32h7xx_hal::gpio::Speed; use stm32h7xx_hal::gpio::{Output, PushPull}; use stm32h7xx_hal::pac; use stm32h7xx_hal::prelude::*; +use stm32h7xx_hal::prelude::*; +use stm32h7xx_hal::rtc; use stm32h7xx_hal::spi; use stm32h7xx_hal::{rcc, rcc::rec}; use systick_monotonic::*; +use cortex_m::{asm, interrupt::Mutex}; +use core::cell::RefCell; use types::SBGSerial; -use heapless::Vec; -use stm32h7xx_hal::rtc::Rtc; -use stm32h7xx_hal::dma::dma::StreamsTuple; -// use panic_halt as _; -#[panic_handler] -fn panic(info: &core::panic::PanicInfo) -> ! { - stm32h7xx_hal::pac::SCB::sys_reset() +use panic_probe as _; +use defmt_rtt as _; // global logger + +#[inline(never)] +#[defmt::panic_handler] +fn panic() -> ! { + cortex_m::asm::udf() +} + +/// Hardfault handler. +/// +/// Terminates the application and makes a semihosting-capable debug tool exit +/// with an error. This seems better than the default, which is to spin in a +/// loop. +#[cortex_m_rt::exception] +unsafe fn HardFault(_frame: &cortex_m_rt::ExceptionFrame) -> ! { + loop {} } +static RTC: Mutex>> = Mutex::new(RefCell::new(None)); -#[rtic::app(device = stm32h7xx_hal::stm32, dispatchers = [EXTI0, EXTI1])] +#[rtic::app(device = stm32h7xx_hal::stm32, dispatchers = [SPI1, SPI2, SPI3])] mod app { - use stm32h7xx_hal::{gpio::{Alternate, Pin}, pac::SPI1}; + use stm32h7xx_hal::{ + gpio::{Alternate, Pin}, + pac::SPI1, + }; use super::*; @@ -51,15 +73,17 @@ mod app { struct SharedResources { data_manager: DataManager, em: ErrorManager, - sd_manager: - SdManager, PA4>>, - rtc: Rtc, + // sd_manager: SdManager< + // stm32h7xx_hal::spi::Spi, + // PA4>, + // >, + // rtc: Rtc, + // sbg_manager: sbg_manager::SBGManager, } #[local] struct LocalResources { led_red: PA2>, led_green: PA3>, - sbg_manager: sbg_manager::SBGManager, } #[monotonic(binds = SysTick, default = true)] @@ -69,6 +93,9 @@ mod app { fn init(mut ctx: init::Context) -> (SharedResources, LocalResources, init::Monotonics) { 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(); @@ -112,14 +139,17 @@ mod app { // c0.enable(); info!("PWM enabled"); - // assert_eq!(ccdr.clocks.pll1_q_ck().unwrap().raw(), 24_000_000); waaat + // assert_eq!(ccdr.clocks.pll1_q_ck().unwrap().raw(), 24_000_000); waaat info!("PLL1Q:"); let fdcan_prec = ccdr .peripheral .FDCAN .kernel_clk_mux(rec::FdcanClkSel::Pll1Q); - let can1: fdcan::FdCan, fdcan::ConfigMode> = { + let can1: 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) @@ -153,27 +183,28 @@ mod app { let cs_sd = gpioa.pa4.into_push_pull_output(); - let mut sd = SdManager::new(spi_sd, cs_sd); + // let mut sd = SdManager::new(spi_sd, cs_sd); // leds let mut led_red = gpioa.pa2.into_push_pull_output(); let mut led_green = gpioa.pa3.into_push_pull_output(); - // sbg power pin + // 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_sbg = ctx - .device - .UART4 - .serial((tx, rx), 9_800.bps(), ccdr.peripheral.UART4, &ccdr.clocks) - .unwrap(); - let sbg_manager = sbg_manager::SBGManager::new(uart_sbg, stream_tuple); + let rx: Pin<'D', 0, Alternate<8>> = gpiod.pd0.into_alternate(); + + // let stream_tuple = StreamsTuple::new(ctx.device.DMA1, ccdr.peripheral.DMA1); + // let mut uart_sbg = ctx + // .device + // .UART4 + // .serial((tx, rx), 115_200.bps(), ccdr.peripheral.UART4, &ccdr.clocks) + // .unwrap(); + + // let sbg_manager = sbg_manager::SBGManager::new(uart_sbg, stream_tuple); // let serial = ctx // .device // .UART4 @@ -187,27 +218,24 @@ mod app { 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); - rtc.enable_wakeup(10); - rtc.listen(&mut ctx.device.EXTI, stm32h7xx_hal::rtc::Event::Wakeup); - - unsafe { - pac::NVIC::unmask(pac::interrupt::RTC_WKUP); - } - - info!("Time: {}", rtc.date_time().unwrap().and_utc().timestamp_subsec_millis()); - blink::spawn().ok(); + cortex_m::interrupt::free(|cs| { + RTC.borrow(cs).replace(Some(rtc)); + }); + /* Monotonic clock */ - let mono = Systick::new(core.SYST, ccdr.clocks.hclk().raw()); + let mono = Systick::new(core.SYST, ccdr.clocks.sysclk().to_Hz()); + blink::spawn().ok(); + display_data::spawn().ok(); info!("Online"); @@ -215,14 +243,11 @@ mod app { SharedResources { data_manager: DataManager::new(), em: ErrorManager::new(), - sd_manager: sd, - rtc, - }, - LocalResources { - led_red, - led_green, - sbg_manager: sbg_manager, + // sd_manager: sd, + // sbg_manager, + // rtc, }, + LocalResources { led_red, led_green }, init::Monotonics(mono), ) } @@ -230,22 +255,48 @@ mod app { #[idle] fn idle(mut ctx: idle::Context) -> ! { loop { - cortex_m::asm::nop(); // do nothing. just keep the MCU running } } - #[task(local = [led_red, led_green], shared = [&em,rtc])] + #[task(shared = [&em, data_manager])] + fn display_data(mut cx: display_data::Context) { + let data = cx.shared.data_manager.lock(|manager| manager.clone_sensors()); + info!("{:?}", data); + spawn_after!(display_data, ExtU64::secs(1)).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) { + let message = messages::Message::new( + cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + rtc.date_time() + .unwrap_or(NaiveDateTime::new( + NaiveDate::from_ymd_opt(2024, 1, 1).unwrap(), + NaiveTime::from_hms_milli_opt(0, 0, 0, 0).unwrap(), + )) + .and_utc() + .timestamp_subsec_millis() + }), + COM_ID, + d.into(), + ); + info!("{:?}", message); + // send_in::spawn(message).ok(); + } + + #[task(local = [led_red, led_green], shared = [&em])] fn blink(mut cx: blink::Context) { + info!("Blinking"); cx.shared.em.run(|| { if cx.shared.em.has_error() { cx.local.led_red.toggle(); + info!("Error"); spawn_after!(blink, ExtU64::millis(200))?; } else { cx.local.led_green.toggle(); info!("Blinking"); - cx.shared.rtc.lock(|rtc| { - info!("Time: {}", rtc.date_time().unwrap().and_utc().timestamp_subsec_millis()); - }); spawn_after!(blink, ExtU64::secs(1))?; } Ok(()) @@ -257,23 +308,23 @@ mod app { // Turn off the SBG and CAN } - extern "Rust" { - #[task(capacity = 3, shared = [&em, sd_manager])] - fn sbg_sd_task(context: sbg_sd_task::Context, data: [u8; SBG_BUFFER_SIZE]); + // extern "Rust" { + // // #[task(capacity = 3, shared = [&em, sd_manager])] + // // fn sbg_sd_task(context: sbg_sd_task::Context, data: [u8; SBG_BUFFER_SIZE]); - #[task(binds = DMA1_STR0, shared = [&em], local = [sbg_manager])] - fn sbg_dma(context: sbg_dma::Context); + // #[task(binds = DMA1_STR1, shared = [&em, sbg_manager])] + // fn sbg_dma(mut context: sbg_dma::Context); - #[task(capacity = 20, shared = [data_manager])] - fn sbg_handle_data(context: sbg_handle_data::Context, data: CallbackData); + // #[task(capacity = 10, shared = [data_manager])] + // fn sbg_handle_data(context: sbg_handle_data::Context, data: CallbackData); - #[task(shared = [rtc, &em])] - fn sbg_get_time(context: sbg_get_time::Context); + // // #[task(shared = [ &em])] + // // fn sbg_get_time(context: sbg_get_time::Context); - #[task()] - fn sbg_flush(context: sbg_flush::Context); + // #[task(shared = [&em, sbg_manager])] + // fn sbg_flush(context: sbg_flush::Context); - #[task(shared = [&em])] - fn sbg_write_data(context: sbg_write_data::Context, data: Vec); - } + // #[task(shared = [&em, sbg_manager])] + // fn sbg_write_data(context: sbg_write_data::Context, data: Vec); + // } } diff --git a/boards/nav/src/sbg_manager.rs b/boards/nav/src/sbg_manager.rs index 686706e7..b295dcd4 100644 --- a/boards/nav/src/sbg_manager.rs +++ b/boards/nav/src/sbg_manager.rs @@ -4,16 +4,19 @@ use core::ffi::c_void; use core::mem::size_of; use core::ptr; // use atsamd_hal::time::*; +use crate::app::sbg_dma; use crate::app::sbg_flush; -use crate::app::sbg_get_time; use crate::app::sbg_handle_data; -use crate::app::sbg_sd_task as sbg_sd; +// use crate::app::sbg_sd_task as sbg_sd; use crate::app::sbg_write_data; +use crate::RTC; +use chrono::{NaiveDate, NaiveDateTime, NaiveTime}; use common_arm::spawn; use core::{mem, mem::MaybeUninit}; use defmt::info; use embedded_alloc::Heap; -use rtic::Mutex; +use heapless::Vec; +use messages::mavlink::embedded::Write; use sbg_rs::sbg; use sbg_rs::sbg::{CallbackData, SBG, SBG_BUFFER_SIZE}; use stm32h7xx_hal::dma::dma::StreamX; @@ -23,8 +26,11 @@ use stm32h7xx_hal::dma::{ }; use stm32h7xx_hal::gpio::Alternate; use stm32h7xx_hal::gpio::Pin; +use stm32h7xx_hal::pac::UART4; use stm32h7xx_hal::rtc::Rtc; -use stm32h7xx_hal::serial::Rx; +use stm32h7xx_hal::serial::{Rx, Tx}; +// use cortex_m::{asm}; +use rtic::Mutex; //#[link_section = ".axisram.buffers"] //static mut SBG_BUFFER: MayberUninit<[u8; SBG_BUFFER_SIZE]> = MaybeUninit::uninit(); @@ -39,13 +45,14 @@ pub struct SBGManager { sbg_device: SBG, xfer: Option< Transfer< - StreamX, + StreamX, Rx, stm32h7xx_hal::dma::PeripheralToMemory, - MaybeUninit<[u8; SBG_BUFFER_SIZE]>, + &'static mut [u8; SBG_BUFFER_SIZE], stm32h7xx_hal::dma::DBTransfer, >, >, + sbg_tx: Tx, } impl SBGManager { @@ -75,24 +82,24 @@ impl SBGManager { // unsafe { SBG_BUFFER.assume_init_mut() } // }; - let config = DmaConfig::default().memory_increment(true); - let transfer: Transfer< - StreamX, + let config = DmaConfig::default().memory_increment(true).transfer_complete_interrupt(true); + let mut transfer: Transfer< + StreamX, Rx, PeripheralToMemory, - MaybeUninit<[u8; SBG_BUFFER_SIZE]>, + &mut [u8; SBG_BUFFER_SIZE], stm32h7xx_hal::dma::DBTransfer, > = Transfer::init( - stream_tuple.0, + stream_tuple.1, sbg_rx, - unsafe { (*core::ptr::addr_of_mut!(TARGET_BUFFER)).assume_init_mut() }, // Uninitialised memory + unsafe { SBG_BUFFER.assume_init_mut() }, // Uninitialised memory None, config, ); - transfer.start(|serial| { - serial.enable_dma_rx(); - }); + // transfer.start(|serial| { + // serial.enable_dma_rx(); + // }); let sbg: sbg::SBG = sbg::SBG::new( |data| { @@ -101,7 +108,7 @@ impl SBGManager { |data| { sbg_write_data::spawn(data).ok(); }, - || sbg_get_time::spawn(), + || sbg_get_time(), || { sbg_flush::spawn().ok(); }, @@ -110,27 +117,34 @@ impl SBGManager { SBGManager { sbg_device: sbg, xfer: Some(transfer), + sbg_tx, } } } pub fn sbg_flush(mut cx: sbg_flush::Context) { - cx.shared.sbg_manager.lock(|sbg| { - sbg.sbg_device.flush(); - }); + // cx.shared.sbg_manager.lock(|sbg| { + // sbg.sbg_tx + // }); } -pub fn sbg_write_data(mut cx: sbg_write_data::Context, data: &[u8]) { +pub fn sbg_write_data(mut cx: sbg_write_data::Context, data: Vec) { cx.shared.sbg_manager.lock(|sbg| { - for byte in data { - sbg.sbg_device.write(*byte); - } + sbg.sbg_tx.write_all(data.as_slice()); }); } -pub fn sbg_get_time(mut cx: sbg_get_time::Context) -> u32 { - cx.shared - .rtc - .lock(|rtc| rtc.date_time().unwrap().and_utc().timestamp_subsec_millis()) +pub fn sbg_get_time() -> u32 { + cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + rtc.date_time() + .unwrap_or(NaiveDateTime::new( + NaiveDate::from_ymd_opt(2024, 1, 1).unwrap(), + NaiveTime::from_hms_milli_opt(0, 0, 0, 0).unwrap(), + )) + .and_utc() + .timestamp_subsec_millis() + }) } pub fn sbg_handle_data(mut cx: sbg_handle_data::Context, data: CallbackData) { @@ -145,37 +159,47 @@ pub fn sbg_handle_data(mut cx: sbg_handle_data::Context, data: CallbackData) { }); } -pub fn sbg_sd_task(mut cx: crate::app::sbg_sd_task::Context, data: [u8; SBG_BUFFER_SIZE]) { - cx.shared.sd_manager.lock(|manager| { - if let Some(mut file) = manager.file.take() { - cx.shared.em.run(|| { - manager.write(&mut file, &data)?; - Ok(()) - }); - manager.file = Some(file); // give the file back after use - } else if let Ok(mut file) = manager.open_file("sbg.txt") { - cx.shared.em.run(|| { - manager.write(&mut file, &data)?; - Ok(()) - }); - manager.file = Some(file); - } - }); -} +// pub fn sbg_sd_task(mut cx: crate::app::sbg_sd_task::Context, data: [u8; SBG_BUFFER_SIZE]) { +// cx.shared.sd_manager.lock(|manager| { +// if let Some(mut file) = manager.file.take() { +// cx.shared.em.run(|| { +// manager.write(&mut file, &data)?; +// Ok(()) +// }); +// manager.file = Some(file); // give the file back after use +// } else if let Ok(mut file) = manager.open_file("sbg.txt") { +// cx.shared.em.run(|| { +// manager.write(&mut file, &data)?; +// Ok(()) +// }); +// manager.file = Some(file); +// } +// }); +// } /** * Handles the DMA interrupt. * Handles the SBG data. */ -pub fn sbg_dma(cx: crate::app::sbg_dma::Context) { - let sbg = cx.local.sbg_manager; - - match &mut sbg.xfer { - Some(xfer) => if xfer.get_transfer_complete_flag(){}, - None => { - // it should be impossible to reach here. - info!("None"); +pub fn sbg_dma(mut cx: crate::app::sbg_dma::Context) { + info!("DMA"); + cx.shared.sbg_manager.lock(|sbg| { + match &mut sbg.xfer { + Some(xfer) => { + if xfer.get_transfer_complete_flag() { + let data = unsafe { SBG_BUFFER.assume_init_read() }; + xfer.clear_transfer_complete_interrupt(); + xfer.next_transfer( + unsafe { (*core::ptr::addr_of_mut!(SBG_BUFFER)).assume_init_mut() }, // Uninitialised memory + ); + sbg.sbg_device.read_data(&data); + } + } + None => { + // it should be impossible to reach here. + info!("None"); + } } - } + }); } /// Stored right before an allocation. Stores information that is needed to deallocate memory. diff --git a/boards/nav/src/types.rs b/boards/nav/src/types.rs index 0771aefd..48e3e9a1 100644 --- a/boards/nav/src/types.rs +++ b/boards/nav/src/types.rs @@ -1,11 +1,10 @@ use hal::pac; use hal::serial::Serial; -use stm32h7xx_hal as hal; -use sbg_rs::sbg::SBG_BUFFER_SIZE; use messages::sender::{Sender, Sender::SensorBoard}; +use sbg_rs::sbg::SBG_BUFFER_SIZE; +use stm32h7xx_hal as hal; pub static COM_ID: Sender = SensorBoard; - pub type SBGSerial = Serial; pub type SBGBuffer = &'static mut [u8; SBG_BUFFER_SIZE]; diff --git a/boards/recovery/Cargo.toml b/boards/recovery/Cargo.toml index ce71e27e..64888ce9 100644 --- a/boards/recovery/Cargo.toml +++ b/boards/recovery/Cargo.toml @@ -20,5 +20,6 @@ messages = { path = "../../libraries/messages" } typenum = "1.16.0" enum_dispatch = "0.3.11" ms5611-01ba = { git = "https://github.com/NoahSprenger/ms5611-01ba", branch = "embedded-hal-02" } - +defmt-rtt = "0.4" +panic-probe = { version = "0.3", features = ["print-defmt"] } embedded-hal = "0.2.7" diff --git a/boards/recovery/src/communication.rs b/boards/recovery/src/communication.rs index 50d2e2a2..bf2667be 100644 --- a/boards/recovery/src/communication.rs +++ b/boards/recovery/src/communication.rs @@ -3,13 +3,14 @@ use atsamd_hal::can::Dependencies; use atsamd_hal::clock::v2::ahb::AhbClk; use atsamd_hal::clock::v2::gclk::Gclk0Id; use atsamd_hal::clock::v2::pclk::Pclk; -use atsamd_hal::clock::v2::types::Can0; +use atsamd_hal::clock::v2::types::{Can0, Can1}; use atsamd_hal::clock::v2::Source; -use atsamd_hal::gpio::{Alternate, AlternateI, Pin, I, PA22, PA23}; -use atsamd_hal::pac::CAN0; +use atsamd_hal::gpio::{Alternate, AlternateH, AlternateI, Pin, H, I, PA22, PA23, PB14, PB15}; +use atsamd_hal::pac::{CAN0, CAN1}; use atsamd_hal::typelevel::Increment; use common_arm::HydraError; use common_arm_atsame::mcan; +use defmt::error; use defmt::info; use heapless::Vec; use mcan::bus::Can; @@ -160,6 +161,7 @@ impl CanDevice0 { for message in &mut self.can.rx_fifo_0 { match from_bytes::(message.data()) { Ok(data) => { + info!("Sender: {:?}", data.sender); data_manager.handle_data(data)?; } Err(e) => { @@ -186,3 +188,156 @@ impl CanDevice0 { Ok(()) } } + +pub struct CanCommandManager { + pub can: Can< + 'static, + Can1, + Dependencies>, Pin>, CAN1>, + Capacities, + >, + line_interrupts: OwnedInterruptSet, +} + +impl CanCommandManager { + pub fn new( + can_rx: Pin, + can_tx: Pin, + pclk_can: Pclk, + ahb_clock: AhbClk, + peripheral: CAN1, + gclk0: S, + can_memory: &'static mut SharedMemory, + loopback: bool, + ) -> (Self, S::Inc) + where + S: Source + Increment, + { + let (can_dependencies, gclk0) = + Dependencies::new(gclk0, pclk_can, ahb_clock, can_rx, can_tx, peripheral); + + let mut can = + mcan::bus::CanConfigurable::new(200.kHz(), can_dependencies, can_memory).unwrap(); + can.config().mode = Mode::Fd { + allow_bit_rate_switching: false, + data_phase_timing: BitTiming::new(500.kHz()), + }; + + if loopback { + can.config().loopback = true; + } + + let interrupts_to_be_enabled = can + .interrupts() + .split( + [ + Interrupt::RxFifo0NewMessage, + Interrupt::RxFifo0Full, + // Interrupt::RxFifo0MessageLost, + Interrupt::RxFifo1NewMessage, + Interrupt::RxFifo1Full, + // Interrupt::RxFifo1MessageLost, + ] + .into_iter() + .collect(), + ) + .unwrap(); + + // Line 0 and 1 are connected to the same interrupt line + let line_interrupts = can + .interrupt_configuration() + .enable_line_0(interrupts_to_be_enabled); + + can.filters_standard() + .push(Filter::Classic { + action: Action::StoreFifo0, + filter: ecan::StandardId::new(messages::sender::Sender::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::sender::Sender::SensorBoard.into()) + .unwrap(), + mask: ecan::StandardId::ZERO, + }) + .unwrap_or_else(|_| panic!("Sensor filter")); + + can.filters_standard() + .push(Filter::Classic { + action: Action::StoreFifo0, + filter: ecan::StandardId::new(messages::sender::Sender::PowerBoard.into()).unwrap(), + mask: ecan::StandardId::ZERO, + }) + .unwrap_or_else(|_| panic!("Power filter")); + + can.filters_standard() + .push(Filter::Classic { + action: Action::StoreFifo0, + filter: ecan::StandardId::new(messages::sender::Sender::GroundStation.into()) + .unwrap(), + mask: ecan::StandardId::ZERO, + }) + .unwrap_or_else(|_| panic!("Ground Station filter")); + + let can = can.finalize().unwrap(); + ( + CanCommandManager { + can, + line_interrupts, + }, + gclk0, + ) + } + pub fn send_message(&mut self, m: Message) -> Result<(), HydraError> { + let payload: Vec = postcard::to_vec(&m)?; + self.can.tx.transmit_queued( + tx::MessageBuilder { + id: ecan::Id::Standard(ecan::StandardId::new(m.sender.into()).unwrap()), + frame_type: tx::FrameType::FlexibleDatarate { + payload: &payload[..], + bit_rate_switching: false, + force_error_state_indicator: false, + }, + store_tx_event: None, + } + .build()?, + )?; + Ok(()) + } + pub fn process_data(&mut self, data_manager: &mut DataManager) { + let line_interrupts = &self.line_interrupts; + for interrupt in line_interrupts.iter_flagged() { + match interrupt { + Interrupt::RxFifo0NewMessage => { + for message in &mut self.can.rx_fifo_0 { + match from_bytes::(message.data()) { + Ok(data) => { + data_manager.handle_command(data); + } + Err(_) => { + error!("Error, ErrorContext::UnkownCanMessage"); + } + } + } + } + Interrupt::RxFifo1NewMessage => { + for message in &mut self.can.rx_fifo_1 { + match from_bytes::(message.data()) { + Ok(data) => { + data_manager.handle_command(data); + } + Err(_) => { + error!("Error, ErrorContext::UnkownCanMessage"); + } + } + } + } + _ => (), + } + } + } +} diff --git a/boards/recovery/src/data_manager.rs b/boards/recovery/src/data_manager.rs index 605d6285..1659d01f 100644 --- a/boards/recovery/src/data_manager.rs +++ b/boards/recovery/src/data_manager.rs @@ -78,7 +78,7 @@ impl DataManager { Some(air) => match air.altitude { Some(altitude) => altitude > HEIGHT_MIN, None => false, - } + }, None => false, } } @@ -120,7 +120,7 @@ impl DataManager { Some(air) => match air.altitude { Some(altitude) => altitude < MAIN_HEIGHT, None => false, - } + }, None => false, } } @@ -132,11 +132,11 @@ impl DataManager { messages::Data::Sensor(sensor) => match sensor.data { messages::sensor::SensorData::Air(air_data) => { /* - NOTE!!! - There should be added a counter to check how many times - the alt is dropped, if the number is high switch to - the on board barometer. - */ + NOTE!!! + There should be added a counter to check how many times + the alt is dropped, if the number is high switch to + the on board barometer. + */ if let Some(alt) = air_data.altitude { let tup_data: (f32, u32) = (alt, air_data.time_stamp); @@ -173,6 +173,13 @@ impl DataManager { } _ => {} }, + + _ => {} + } + Ok(()) + } + pub fn handle_command(&mut self, command: Message) -> Result<(), HydraError> { + match command.data { messages::Data::Command(command) => match command.data { messages::command::CommandData::DeployDrogue(_) => { spawn!(fire_drogue)?; // need someway to capture this error. diff --git a/boards/recovery/src/gpio_manager.rs b/boards/recovery/src/gpio_manager.rs index 54078f30..01add0b6 100644 --- a/boards/recovery/src/gpio_manager.rs +++ b/boards/recovery/src/gpio_manager.rs @@ -1,4 +1,4 @@ -use atsamd_hal::gpio::{Pin, PushPullOutput, PA06, PA09, PB11, PB12}; +use atsamd_hal::gpio::{Pin, PushPullOutput, PB11, PB12}; use atsamd_hal::prelude::*; pub struct GPIOManager { diff --git a/boards/recovery/src/main.rs b/boards/recovery/src/main.rs index a428370e..c6ecac2d 100644 --- a/boards/recovery/src/main.rs +++ b/boards/recovery/src/main.rs @@ -14,17 +14,20 @@ use atsamd_hal::dmac::DmaController; use common_arm::hinfo; use common_arm::*; use common_arm_atsame::mcan; +use communication::CanCommandManager; use communication::Capacities; -use core::fmt::Debug; use data_manager::DataManager; use defmt::info; +use defmt_rtt as _; use embedded_hal::spi::FullDuplex; use gpio_manager::GPIOManager; -use hal::gpio::{ - Alternate, Output, Pin, Pins, PushPull, PushPullOutput, C, D, PA04, PA05, PA06, PA07, PA09, - PB16, PB17, -}; +use hal::gpio::{Alternate, Pin, Pins, PushPullOutput, D, PA04, PA05, PA07}; +use panic_probe as _; // global logger + +use core::cell::RefCell; +use cortex_m::interrupt::Mutex; use hal::prelude::*; +use hal::rtc::Count32Mode; use hal::sercom::{spi, spi::Config, spi::Duplex, spi::Pads, spi::Spi, IoSet3, Sercom0}; use mcan::messageram::SharedMemory; use messages::*; @@ -32,22 +35,26 @@ use ms5611_01ba::{calibration::OversamplingRatio, MS5611_01BA}; use state_machine::{StateMachine, StateMachineContext}; use systick_monotonic::*; use types::COM_ID; -use cortex_m::interrupt::Mutex; -use hal::rtc::Count32Mode; -use core::cell::RefCell; pub static RTC: Mutex>>> = Mutex::new(RefCell::new(None)); +#[inline(never)] +#[defmt::panic_handler] +fn panic() -> ! { + cortex_m::asm::udf() +} -/// Custom panic handler. -/// Reset the system if a panic occurs. -#[panic_handler] -fn panic(_info: &core::panic::PanicInfo) -> ! { - info!("Panic"); - atsamd_hal::pac::SCB::sys_reset(); +/// Hardfault handler. +/// +/// Terminates the application and makes a semihosting-capable debug tool exit +/// with an error. This seems better than the default, which is to spin in a +/// loop. +#[cortex_m_rt::exception] +unsafe fn HardFault(_frame: &cortex_m_rt::ExceptionFrame) -> ! { + loop {} } #[rtic::app(device = hal::pac, peripherals = true, dispatchers = [EVSYS_0, EVSYS_1, EVSYS_2])] mod app { - use cortex_m::asm; + use atsamd_hal::gpio::{PA03, PB04}; use super::*; @@ -57,12 +64,13 @@ mod app { data_manager: DataManager, can0: communication::CanDevice0, gpio: GPIOManager, + can_command_manager: CanCommandManager, } #[local] struct Local { - led_green: Pin, - led_red: Pin, + led_green: Pin, + led_red: Pin, state_machine: StateMachine, barometer: MS5611_01BA< Spi< @@ -85,13 +93,17 @@ mod app { #[init(local = [ #[link_section = ".can"] - can_memory: SharedMemory = SharedMemory::new() + can_memory: SharedMemory = SharedMemory::new(), + #[link_section = ".can_command"] + can_command_memory: SharedMemory = SharedMemory::new() ])] fn init(cx: init::Context) -> (Shared, Local, init::Monotonics) { let mut peripherals = cx.device; let core = cx.core; let pins = Pins::new(peripherals.PORT); + HydraLogging::set_ground_station_callback(queue_gs_message); + let mut dmac = DmaController::init(peripherals.DMAC, &mut peripherals.PM); let _dmaChannels = dmac.split(); @@ -122,9 +134,21 @@ mod app { false, ); + let (pclk_can_command, gclk0) = Pclk::enable(tokens.pclks.can1, gclk0); + let (can_command_manager, gclk0) = CanCommandManager::new( + pins.pb15.into_mode(), + pins.pb14.into_mode(), + pclk_can_command, + clocks.ahbs.can1, + peripherals.CAN1, + gclk0, + cx.local.can_command_memory, + false, + ); + /* GPIO config */ - let led_green = pins.pb16.into_push_pull_output(); - let led_red = pins.pb17.into_push_pull_output(); + let led_green = pins.pa03.into_push_pull_output(); + let led_red = pins.pb04.into_push_pull_output(); let gpio = GPIOManager::new( // pins switched from schematic pins.pb12.into_push_pull_output(), @@ -151,8 +175,8 @@ mod app { let barometer = MS5611_01BA::new(baro_spi, OversamplingRatio::OSR2048); info!("RTC"); - let mut rtc = hal::rtc::Rtc::count32_mode(peripherals.RTC, 1024.Hz(), &mut mclk); - // let mut rtc = rtc_temp.into_count32_mode(); + let rtc = hal::rtc::Rtc::clock_mode(peripherals.RTC, 1024.Hz(), &mut mclk); + let mut rtc = rtc.into_count32_mode(); // hal bug this must be done rtc.set_count32(0); cortex_m::interrupt::free(|cs| { RTC.borrow(cs).replace(Some(rtc)); @@ -175,6 +199,7 @@ mod app { em: ErrorManager::new(), data_manager: DataManager::new(), can0, + can_command_manager, gpio, }, Local { @@ -190,13 +215,51 @@ mod app { /// Idle task for when no other tasks are running. #[idle] fn idle(_: idle::Context) -> ! { - loop {asm::nop()} + loop {} + } + + /// Handles the CAN0 interrupt. + #[task(priority = 3, binds = CAN1, shared = [can_command_manager, data_manager])] + fn can_command(mut cx: can_command::Context) { + cx.shared.can_command_manager.lock(|can| { + cx.shared + .data_manager + .lock(|data_manager| can.process_data(data_manager)); + }); + } + + /// Receives a log message from the custom logger so that it can be sent over the radio. + pub fn queue_gs_message(d: impl Into) { + let message = Message::new( + cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + rtc.count32() + }), + COM_ID, + d.into(), + ); + + send_internal::spawn(message).ok(); + } + + /** + * Sends a message over CAN. + */ + #[task(capacity = 5, shared = [can_command_manager, &em])] + fn send_command(mut cx: send_command::Context, m: Message) { + cx.shared.em.run(|| { + cx.shared + .can_command_manager + .lock(|can| can.send_message(m))?; + Ok(()) + }); } #[task(local = [barometer], shared = [&em])] fn read_barometer(cx: read_barometer::Context) { cx.shared.em.run(|| { - let mut barometer = cx.local.barometer; + let barometer = cx.local.barometer; let (p, t) = barometer.get_data()?; info!("pressure {} temperature {}", p, t); Ok(()) @@ -291,10 +354,16 @@ mod app { return Ok(()); }; let board_state = messages::state::State { data: state.into() }; - let message = Message::new(cortex_m::interrupt::free(|cs| { - let mut rc = RTC.borrow(cs).borrow_mut(); - let rtc = rc.as_mut().unwrap(); - rtc.count32()}), COM_ID, board_state); + let message = Message::new( + cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + // info!("RTC: {:?}", rtc.count32()); + rtc.count32() + }), + COM_ID, + board_state, + ); spawn!(send_internal, message)?; spawn_after!(state_send, ExtU64::secs(2))?; // I think this is fine here. Ok(()) diff --git a/boards/recovery/src/state_machine/states/ascent.rs b/boards/recovery/src/state_machine/states/ascent.rs index c2a8b98e..11a5a615 100644 --- a/boards/recovery/src/state_machine/states/ascent.rs +++ b/boards/recovery/src/state_machine/states/ascent.rs @@ -1,14 +1,13 @@ -use crate::app::monotonics; use crate::state_machine::states::descent::Descent; use crate::state_machine::states::wait_for_takeoff::WaitForTakeoff; use crate::state_machine::{RocketStates, State, StateMachineContext, TransitionInto}; use crate::types::COM_ID; +use crate::RTC; use crate::{no_transition, transition}; use defmt::{write, Format, Formatter}; use messages::command::{Command, RadioRate, RadioRateChange}; use messages::Message; use rtic::mutex::Mutex; -use crate::RTC; #[derive(Debug, Clone)] pub struct Ascent {} @@ -19,11 +18,15 @@ impl State for Ascent { let radio_rate_change = RadioRateChange { rate: RadioRate::Fast, }; - let message_com = - Message::new(cortex_m::interrupt::free(|cs| { + let message_com = Message::new( + cortex_m::interrupt::free(|cs| { let mut rc = RTC.borrow(cs).borrow_mut(); let rtc = rc.as_mut().unwrap(); - rtc.count32()}), COM_ID, Command::new(radio_rate_change)); + rtc.count32() + }), + COM_ID, + Command::new(radio_rate_change), + ); context.shared_resources.can0.lock(|can| { context.shared_resources.em.run(|| { can.send_message(message_com)?; diff --git a/boards/recovery/src/state_machine/states/wait_for_recovery.rs b/boards/recovery/src/state_machine/states/wait_for_recovery.rs index 01af80ad..6f689b4a 100644 --- a/boards/recovery/src/state_machine/states/wait_for_recovery.rs +++ b/boards/recovery/src/state_machine/states/wait_for_recovery.rs @@ -1,14 +1,14 @@ use super::TerminalDescent; -use crate::app::monotonics; +use crate::app::send_command; use crate::no_transition; use crate::state_machine::{RocketStates, State, StateMachineContext, TransitionInto}; use crate::types::COM_ID; +use crate::RTC; +use common_arm::spawn; use defmt::{write, Format, Formatter}; use messages::command::{Command, PowerDown, RadioRate, RadioRateChange}; use messages::sender::Sender::SensorBoard; use messages::Message; -use rtic::mutex::Mutex; -use crate::RTC; #[derive(Debug, Clone)] pub struct WaitForRecovery {} @@ -22,22 +22,37 @@ impl State for WaitForRecovery { let radio_rate_change = RadioRateChange { rate: RadioRate::Slow, }; - let message = Message::new(cortex_m::interrupt::free(|cs| { - let mut rc = RTC.borrow(cs).borrow_mut(); - let rtc = rc.as_mut().unwrap(); - rtc.count32()}), COM_ID, Command::new(sensor_power_down)); - let message_com = - Message::new(cortex_m::interrupt::free(|cs| { + let message = Message::new( + cortex_m::interrupt::free(|cs| { let mut rc = RTC.borrow(cs).borrow_mut(); let rtc = rc.as_mut().unwrap(); - rtc.count32()}), COM_ID, Command::new(radio_rate_change)); - context.shared_resources.can0.lock(|can| { - context.shared_resources.em.run(|| { - can.send_message(message)?; - can.send_message(message_com)?; - Ok(()) - }) + rtc.count32() + }), + COM_ID, + Command::new(sensor_power_down), + ); + let message_com = Message::new( + cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + rtc.count32() + }), + COM_ID, + Command::new(radio_rate_change), + ); + context.shared_resources.em.run(|| { + spawn!(send_command, message)?; + spawn!(send_command, message_com)?; + Ok(()) }); + + // context.shared_resources.can0.lock(|can| { + // context.shared_resources.em.run(|| { + // can.send_message(message)?; + // can.send_message(message_com)?; + // Ok(()) + // }) + // }); } } fn step(&mut self, _context: &mut StateMachineContext) -> Option { diff --git a/boards/sensor/src/data_manager.rs b/boards/sensor/src/data_manager.rs index 4c895db0..5691b5cd 100644 --- a/boards/sensor/src/data_manager.rs +++ b/boards/sensor/src/data_manager.rs @@ -1,7 +1,8 @@ use crate::app::sleep_system; use common_arm::{spawn, HydraError}; use messages::sensor::{ - Air, EkfNav1, EkfNav2, EkfQuat, GpsPos1, GpsPos2, GpsVel, Imu1, Imu2, SensorData, UtcTime, EkfNavAcc, GpsPosAcc, GpsVelAcc, + Air, EkfNav1, EkfNav2, EkfNavAcc, EkfQuat, GpsPos1, GpsPos2, GpsPosAcc, GpsVel, GpsVelAcc, + Imu1, Imu2, SensorData, UtcTime, }; use messages::Message; diff --git a/boards/sensor/src/sbg_manager.rs b/boards/sensor/src/sbg_manager.rs index e5c0e0f5..25b8e259 100644 --- a/boards/sensor/src/sbg_manager.rs +++ b/boards/sensor/src/sbg_manager.rs @@ -79,7 +79,7 @@ impl SBGManager { let sbg: sbg::SBG = sbg::SBG::new(sbg_tx, rtc, |data| { sbg_handle_data::spawn(data).ok(); - }, ); + }); SBGManager { sbg_device: sbg, diff --git a/examples/simple/src/main.rs b/examples/simple/src/main.rs index db6fa267..b2fa5a97 100644 --- a/examples/simple/src/main.rs +++ b/examples/simple/src/main.rs @@ -4,12 +4,12 @@ use atsamd_hal as hal; use atsamd_hal::prelude::*; use cortex_m_rt::entry; +use defmt::println; use defmt_rtt as _; use hal::gpio::Pins; use hal::pac; use pac::Peripherals; use panic_halt as _; -use defmt::println; #[entry] fn main() -> ! { diff --git a/libraries/common-arm-atsame/memory.x b/libraries/common-arm-atsame/memory.x index 615b4ebe..fd693582 100644 --- a/libraries/common-arm-atsame/memory.x +++ b/libraries/common-arm-atsame/memory.x @@ -12,5 +12,9 @@ SECTIONS { *(.can .can.*); } > CAN + .can_command (NOLOAD) : + { + *(.can_command .can_command.*); + } > CAN_COMMAND } /* _stack_start is optional and we can define this later */ \ No newline at end of file diff --git a/libraries/common-arm/src/error/error_manager.rs b/libraries/common-arm/src/error/error_manager.rs index 059e20ab..a462c028 100644 --- a/libraries/common-arm/src/error/error_manager.rs +++ b/libraries/common-arm/src/error/error_manager.rs @@ -42,11 +42,9 @@ impl ErrorManager { if let Err(e) = result { self.has_error.store(true, Relaxed); - // error!("{}", e); - if let Some(c) = e.get_context() { error!("{}", e); - // herror!(Error, c); + herror!(Error, c); } interrupt::free(|cs| { diff --git a/libraries/common-arm/src/error/hydra_error.rs b/libraries/common-arm/src/error/hydra_error.rs index 4fe2cf01..d085cc22 100644 --- a/libraries/common-arm/src/error/hydra_error.rs +++ b/libraries/common-arm/src/error/hydra_error.rs @@ -31,7 +31,7 @@ pub enum HydraErrorType { CanMessageError(mcan::message::TooMuchData), /// Error from the MS5611 barometer library. BarometerError(DeviceError), - NbError(NbError), + NbError(NbError), } impl defmt::Format for HydraErrorType { diff --git a/libraries/messages/src/lib.rs b/libraries/messages/src/lib.rs index 6c8017fb..1be9e154 100644 --- a/libraries/messages/src/lib.rs +++ b/libraries/messages/src/lib.rs @@ -11,7 +11,6 @@ use crate::sender::Sender; use crate::sensor::Sensor; use crate::state::State; use derive_more::From; -use fugit::Instant; /// This is to help control versions. pub use mavlink; use messages_proc_macros_lib::common_derives; @@ -55,11 +54,7 @@ pub enum Data { } impl Message { - pub fn new( - timestamp: u32, - sender: Sender, - data: impl Into, - ) -> Self { + pub fn new(timestamp: u32, sender: Sender, data: impl Into) -> Self { Message { timestamp: timestamp, sender, diff --git a/libraries/sbg-rs/src/data_conversion.rs b/libraries/sbg-rs/src/data_conversion.rs index 2ff7a675..f54c8c6d 100644 --- a/libraries/sbg-rs/src/data_conversion.rs +++ b/libraries/sbg-rs/src/data_conversion.rs @@ -1,11 +1,15 @@ use crate::bindings::{ - SbgLogAirData, SbgLogEkfNavData, SbgLogEkfQuatData, SbgLogGpsVel, SbgLogImuData, SbgLogUtcData, SbgLogGpsPos, + SbgLogAirData, SbgLogEkfNavData, SbgLogEkfQuatData, SbgLogGpsPos, SbgLogGpsVel, SbgLogImuData, + SbgLogUtcData, }; use bitflags::Flags; -use messages::sensor::{Air, EkfNav1, EkfNav2, EkfNavAcc, EkfQuat, GpsVel, Imu1, Imu2, UtcTime, GpsPos1, GpsPos2, GpsPosAcc, GpsVelAcc}; +use messages::sensor::{ + Air, EkfNav1, EkfNav2, EkfNavAcc, EkfQuat, GpsPos1, GpsPos2, GpsPosAcc, GpsVel, GpsVelAcc, + Imu1, Imu2, UtcTime, +}; use messages::sensor_status::{ - AirFlags, AirStatus, EkfFlags, EkfStatus, GpsVelStatus, GpsVelStatusE, ImuFlags, ImuStatus, - UtcStatus, UtcTimeStatus, GpsPositionStatus, GpsPositionStatusE + AirFlags, AirStatus, EkfFlags, EkfStatus, GpsPositionStatus, GpsPositionStatusE, GpsVelStatus, + GpsVelStatusE, ImuFlags, ImuStatus, UtcStatus, UtcTimeStatus, }; /// Simple helper function to work with the flags structure and set the fields as needed. @@ -23,29 +27,49 @@ where impl From for (GpsPos1, GpsPos2, GpsPosAcc) { fn from(value: SbgLogGpsPos) -> Self { let status = GpsPositionStatus::new(value.status); - + let valid = matches!(status.get_status(), Some(GpsPositionStatusE::SolComputed)); - + ( GpsPos1 { - latitude: if valid {Some(value.latitude)} else {None}, - longitude: if valid {Some(value.longitude)} else {None}, + latitude: if valid { Some(value.latitude) } else { None }, + longitude: if valid { Some(value.longitude) } else { None }, }, GpsPos2 { - altitude: if valid {Some(value.altitude)} else {None}, - undulation: if valid {Some(value.undulation)} else {None}, - time_of_week: if valid {Some(value.timeOfWeek)} else {None}, + altitude: if valid { Some(value.altitude) } else { None }, + undulation: if valid { Some(value.undulation) } else { None }, + time_of_week: if valid { Some(value.timeOfWeek) } else { None }, }, GpsPosAcc { status, time_stamp: value.timeStamp, - latitude_accuracy: if valid {Some(value.latitudeAccuracy)} else {None}, - longitude_accuracy: if valid {Some(value.longitudeAccuracy)} else {None}, - altitude_accuracy: if valid {Some(value.altitudeAccuracy)} else {None}, - num_sv_used: if valid {Some(value.numSvUsed)} else {None}, - base_station_id: if valid {Some(value.baseStationId)} else {None}, - differential_age: if valid {Some(value.differentialAge)} else {None}, - } + latitude_accuracy: if valid { + Some(value.latitudeAccuracy) + } else { + None + }, + longitude_accuracy: if valid { + Some(value.longitudeAccuracy) + } else { + None + }, + altitude_accuracy: if valid { + Some(value.altitudeAccuracy) + } else { + None + }, + num_sv_used: if valid { Some(value.numSvUsed) } else { None }, + base_station_id: if valid { + Some(value.baseStationId) + } else { + None + }, + differential_age: if valid { + Some(value.differentialAge) + } else { + None + }, + }, ) } } @@ -53,10 +77,13 @@ impl From for (GpsPos1, GpsPos2, GpsPosAcc) { impl From for UtcTime { fn from(value: SbgLogUtcData) -> Self { let status = UtcTimeStatus::new(value.status); - let valid = matches!(status.get_utc_status(), Some(UtcStatus::Valid | UtcStatus::NoLeapSec)); + let valid = matches!( + status.get_utc_status(), + Some(UtcStatus::Valid | UtcStatus::NoLeapSec) + ); Self { - time_stamp: value.timeStamp, // not convinced this is matched valid to the Utc Status bitmask. + time_stamp: value.timeStamp, // not convinced this is matched valid to the Utc Status bitmask. status, year: if valid { Some(value.year) } else { None }, month: if valid { Some(value.month) } else { None }, @@ -114,7 +141,6 @@ impl From for (EkfNav1, EkfNav2, EkfNavAcc) { EkfNav1 { time_stamp: value.timeStamp, velocity: check(&flags, EkfFlags::VelocityValid, value.velocity), - }, EkfNav2 { undulation: check(&flags, EkfFlags::AttitudeValid, value.undulation), @@ -124,7 +150,7 @@ impl From for (EkfNav1, EkfNav2, EkfNavAcc) { velocity_std_dev: check(&flags, EkfFlags::VelocityValid, value.velocityStdDev), position_std_dev: check(&flags, EkfFlags::PositionValid, value.positionStdDev), status, - } + }, ) } } @@ -142,7 +168,7 @@ impl From for (Imu1, Imu2) { accelerometers: check(&flags, ImuFlags::AccelsInRange, value.accelerometers), }, Imu2 { - temperature: Some(value.temperature), // we cannot check since no flag exists. Keep in option for uniformity. + temperature: Some(value.temperature), // we cannot check since no flag exists. Keep in option for uniformity. delta_velocity: check(&flags, ImuFlags::AccelsInRange, value.deltaVelocity), delta_angle: check(&flags, ImuFlags::GyrosInRange, value.deltaAngle), }, @@ -159,16 +185,15 @@ impl From for (GpsVel, GpsVelAcc) { ( GpsVel { time_stamp: value.timeStamp, - time_of_week: if valid {Some(value.timeOfWeek) } else {None}, + time_of_week: if valid { Some(value.timeOfWeek) } else { None }, status, velocity: if valid { Some(value.velocity) } else { None }, course: if valid { Some(value.course) } else { None }, - }, + }, GpsVelAcc { velocity_acc: if valid { Some(value.velocityAcc) } else { None }, course_acc: if valid { Some(value.courseAcc) } else { None }, - } - + }, ) } } diff --git a/libraries/sbg-rs/src/sbg.rs b/libraries/sbg-rs/src/sbg.rs index ee8bcc50..c4072174 100644 --- a/libraries/sbg-rs/src/sbg.rs +++ b/libraries/sbg-rs/src/sbg.rs @@ -23,6 +23,7 @@ use hal::sercom::uart::Duplex; use hal::sercom::uart::{self, EightBit, Uart}; use hal::sercom::{IoSet1, IoSet6, Sercom5}; use heapless::Deque; +use heapless::Vec; use messages::sensor::*; /** @@ -43,7 +44,7 @@ static mut DEQ: Deque = Deque::new(); static mut DATA_CALLBACK: Option = None; -static mut SERIAL_WRITE_CALLBACK: Option = None; +static mut SERIAL_WRITE_CALLBACK: Option)> = None; static mut RTC_GET_TIME: Option u32> = None; @@ -76,7 +77,7 @@ impl SBG { */ pub fn new( callback: fn(CallbackData), - serial_write_callback: fn(&[u8]), + serial_write_callback: fn(Vec), rtc_get_time: fn() -> u32, serial_flush_callback: fn(), ) -> Self { @@ -144,7 +145,7 @@ impl SBG { /** * Reads SBG data frames for a buffer and returns the most recent data. */ - pub fn read_data(&mut self, buffer: &'static [u8; SBG_BUFFER_SIZE]) { + pub fn read_data(&mut self, buffer: &[u8; SBG_BUFFER_SIZE]) { // SAFETY: We are assigning a static mut variable. // Buf can only be accessed from functions called by sbgEComHandle after this assignment. // unsafe { BUF = buffer }; @@ -322,18 +323,23 @@ impl SBG { // This is safe because we ensure pBuffer is valid, pBuffer is not accessed during the lifetime of this function, // and the SBGECom library ensures the buffer given is of the correct size. let array: &[u8] = unsafe { from_raw_parts(pBuffer as *const u8, bytesToWrite) }; - let mut counter: usize = 0; - loop { - if bytesToWrite == counter { - break; - } - // SAFETY: We are accessing a Uart Peripheral pointer. - // This is safe because we ensure that the pointer is not accessed during the lifetime of this function. - match unsafe { SERIAL_WRITE_CALLBACK } { - Some(callback) => callback(&array[counter..counter + 1]), - None => return _SbgErrorCode_SBG_WRITE_ERROR, - } + let vec = array.iter().copied().collect::>(); + match unsafe { SERIAL_WRITE_CALLBACK } { + Some(callback) => callback(vec), + None => return _SbgErrorCode_SBG_WRITE_ERROR, } + // let mut counter: usize = 0; + // loop { + // if bytesToWrite == counter { + // break; + // } + // // SAFETY: We are accessing a Uart Peripheral pointer. + // // This is safe because we ensure that the pointer is not accessed during the lifetime of this function. + // match unsafe { SERIAL_WRITE_CALLBACK } { + // Some(callback) => callback(&array[counter..counter + 1]), + // None => return _SbgErrorCode_SBG_WRITE_ERROR, + // } + // } _SbgErrorCode_SBG_NO_ERROR } @@ -482,7 +488,9 @@ pub extern "C" fn sbgGetTime() -> u32 { // SAFETY: We are accessing a static mut variable. // This is safe because this is the only place where we access the RTC. match unsafe { RTC_GET_TIME } { - Some(get_time) => get_time(), + Some(get_time) => { + get_time() + } None => 0, } } From 8624b359877706d5a76ceff1f6f7df155392326a Mon Sep 17 00:00:00 2001 From: NoahSprenger Date: Wed, 7 Aug 2024 03:10:40 +0000 Subject: [PATCH 12/47] Modify compiler ops, and add launch config. --- .vscode/launch.json | 17 ++++++ Cargo.toml | 22 +++---- boards/nav/src/communication.rs | 4 -- boards/nav/src/data_manager.rs | 2 +- boards/nav/src/main.rs | 104 +++++++++++++------------------- boards/nav/src/sbg_manager.rs | 48 +++++++-------- 6 files changed, 90 insertions(+), 107 deletions(-) diff --git a/.vscode/launch.json b/.vscode/launch.json index 7de0df59..ee4c5098 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -61,6 +61,23 @@ } ] }, + { + "type": "probe-rs-debug", + "request": "launch", + "name": "Probe-rs Release nav", + "chip": "STM32H733VGTx", + "flashingConfig": { + "haltAfterReset": true, + "flashingEnabled": true, + }, + "coreConfigs": [ + { + "programBinary": "${workspaceFolder}/target/thumbv7em-none-eabihf/release/nav", + "rttEnabled": true, + "coreIndex": 0, + } + ] + }, { "type": "cortex-debug", "request": "launch", diff --git a/Cargo.toml b/Cargo.toml index 76987246..9575830c 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -40,28 +40,24 @@ features = ["critical-section-single-core"] lto = false panic = 'abort' -# Only optimize dependencies for size in debug, keeping the top crate debug friendly -[profile.dev.package."*"] -opt-level = "s" - -[profile.dev.package.sbg-rs] -opt-level = 0 -debug = true - -# Try to remove this -[profile.dev.build-override] -#opt-level = 0 Commenting this may cause issues. -debug = true +# # Try to remove this +# [profile.dev.build-override] +# #opt-level = 0 Commenting this may cause issues. +# debug = true [profile.release] codegen-units = 1 debug = 2 debug-assertions = false # <- incremental = false -lto = 'thin' +# lto = 'thin' opt-level = 3 # <- overflow-checks = false # <- +[profile.dev.package.sbg-rs] +opt-level = 0 +debug = true + [profile.release.package.sbg-rs] debug = true opt-level = 0 diff --git a/boards/nav/src/communication.rs b/boards/nav/src/communication.rs index 3ff22e21..0f073bb2 100644 --- a/boards/nav/src/communication.rs +++ b/boards/nav/src/communication.rs @@ -3,15 +3,11 @@ use crate::types::COM_ID; use common_arm::HydraError; use defmt::info; use fdcan::{ - config::NominalBitTiming, - filter::{StandardFilter, StandardFilterSlot}, frame::{FrameFormat, TxFrameHeader}, id::StandardId, - FdCan, Instance, Receive, Transmit, }; use messages::Message; use postcard::from_bytes; -use stm32h7xx_hal as hal; use stm32h7xx_hal::prelude::*; /// Clock configuration is out of scope for this builder diff --git a/boards/nav/src/data_manager.rs b/boards/nav/src/data_manager.rs index fe164ae2..29868be6 100644 --- a/boards/nav/src/data_manager.rs +++ b/boards/nav/src/data_manager.rs @@ -1,4 +1,4 @@ -use common_arm::{spawn, HydraError}; +use common_arm::HydraError; use messages::sensor::{ Air, EkfNav1, EkfNav2, EkfNavAcc, EkfQuat, GpsPos1, GpsPos2, GpsPosAcc, GpsVel, GpsVelAcc, Imu1, Imu2, SensorData, UtcTime, diff --git a/boards/nav/src/main.rs b/boards/nav/src/main.rs index 151afecc..610c6e16 100644 --- a/boards/nav/src/main.rs +++ b/boards/nav/src/main.rs @@ -3,7 +3,7 @@ mod communication; mod data_manager; -// mod sbg_manager; +mod sbg_manager; mod types; use types::COM_ID; @@ -15,31 +15,24 @@ use defmt::info; use fdcan::{ config::NominalBitTiming, filter::{StandardFilter, StandardFilterSlot}, - frame::{FrameFormat, TxFrameHeader}, - id::StandardId, }; use messages::Data; use fugit::RateExtU32; use heapless::Vec; -// use sbg_manager::{sbg_dma, sbg_flush, sbg_get_time, sbg_handle_data, sbg_write_data}; +use sbg_manager::{sbg_dma, sbg_flush, sbg_sd_task, sbg_handle_data, sbg_write_data}; use sbg_rs::sbg::CallbackData; use sbg_rs::sbg::SBG_BUFFER_SIZE; use stm32h7xx_hal::dma::dma::StreamsTuple; -use stm32h7xx_hal::gpio::gpioa::{PA1, PA2, PA3, PA4}; -use stm32h7xx_hal::gpio::gpioc::{PC13, PC3}; -use stm32h7xx_hal::gpio::Input; +use stm32h7xx_hal::gpio::gpioa::{PA2, PA3, PA4}; use stm32h7xx_hal::gpio::Speed; use stm32h7xx_hal::gpio::{Output, PushPull}; -use stm32h7xx_hal::pac; -use stm32h7xx_hal::prelude::*; use stm32h7xx_hal::prelude::*; use stm32h7xx_hal::rtc; use stm32h7xx_hal::spi; use stm32h7xx_hal::{rcc, rcc::rec}; use systick_monotonic::*; -use cortex_m::{asm, interrupt::Mutex}; +use cortex_m::interrupt::Mutex; use core::cell::RefCell; -use types::SBGSerial; use panic_probe as _; use defmt_rtt as _; // global logger @@ -58,14 +51,12 @@ fn panic() -> ! { unsafe fn HardFault(_frame: &cortex_m_rt::ExceptionFrame) -> ! { loop {} } + static RTC: Mutex>> = Mutex::new(RefCell::new(None)); #[rtic::app(device = stm32h7xx_hal::stm32, dispatchers = [SPI1, SPI2, SPI3])] mod app { - use stm32h7xx_hal::{ - gpio::{Alternate, Pin}, - pac::SPI1, - }; + use stm32h7xx_hal::gpio::{Alternate, Pin}; use super::*; @@ -73,12 +64,11 @@ mod app { struct SharedResources { data_manager: DataManager, em: ErrorManager, - // sd_manager: SdManager< - // stm32h7xx_hal::spi::Spi, - // PA4>, - // >, - // rtc: Rtc, - // sbg_manager: sbg_manager::SBGManager, + sd_manager: SdManager< + stm32h7xx_hal::spi::Spi, + PA4>, + >, + sbg_manager: sbg_manager::SBGManager, } #[local] struct LocalResources { @@ -87,10 +77,10 @@ mod app { } #[monotonic(binds = SysTick, default = true)] - type SysMono = Systick<100>; // 100 Hz / 10 ms granularity + type SysMono = Systick<500>; // 100 Hz / 10 ms granularity #[init] - fn init(mut ctx: init::Context) -> (SharedResources, LocalResources, init::Monotonics) { + fn init(ctx: init::Context) -> (SharedResources, LocalResources, init::Monotonics) { let core = ctx.core; /* Logging Setup */ @@ -108,7 +98,7 @@ mod app { let rcc = ctx.device.RCC.constrain(); info!("RCC enabled"); let ccdr = rcc - .use_hse(48.MHz()) + .use_hse(48.MHz()) // check the clock hardware .sys_ck(200.MHz()) .pll1_strategy(rcc::PllConfigStrategy::Iterative) .pll1_q_ck(24.MHz()) @@ -183,11 +173,11 @@ mod app { let cs_sd = gpioa.pa4.into_push_pull_output(); - // let mut sd = SdManager::new(spi_sd, cs_sd); + let sd = SdManager::new(spi_sd, cs_sd); // leds - let mut led_red = gpioa.pa2.into_push_pull_output(); - let mut led_green = gpioa.pa3.into_push_pull_output(); + 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(); @@ -197,20 +187,14 @@ mod app { 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 mut uart_sbg = ctx - // .device - // .UART4 - // .serial((tx, rx), 115_200.bps(), ccdr.peripheral.UART4, &ccdr.clocks) - // .unwrap(); - - // let sbg_manager = sbg_manager::SBGManager::new(uart_sbg, stream_tuple); - // let serial = ctx - // .device - // .UART4 - // .serial((tx, rx), 9_800.bps(), ccdr.peripheral.UART4, &ccdr.clocks) - // .unwrap(); - // let (sbg_rx, sbg_tx) = serial.split(); + let stream_tuple = StreamsTuple::new(ctx.device.DMA1, ccdr.peripheral.DMA1); + let uart_sbg = ctx + .device + .UART4 + .serial((tx, rx), 115_200.bps(), ccdr.peripheral.UART4, &ccdr.clocks) + .unwrap(); + + let sbg_manager = sbg_manager::SBGManager::new(uart_sbg, stream_tuple); let mut rtc = stm32h7xx_hal::rtc::Rtc::open_or_init( ctx.device.RTC, @@ -243,9 +227,8 @@ mod app { SharedResources { data_manager: DataManager::new(), em: ErrorManager::new(), - // sd_manager: sd, - // sbg_manager, - // rtc, + sd_manager: sd, + sbg_manager, }, LocalResources { led_red, led_green }, init::Monotonics(mono), @@ -253,7 +236,7 @@ mod app { } #[idle] - fn idle(mut ctx: idle::Context) -> ! { + fn idle(ctx: idle::Context) -> ! { loop { } } @@ -287,7 +270,7 @@ mod app { } #[task(local = [led_red, led_green], shared = [&em])] - fn blink(mut cx: blink::Context) { + fn blink(cx: blink::Context) { info!("Blinking"); cx.shared.em.run(|| { if cx.shared.em.has_error() { @@ -304,27 +287,24 @@ mod app { } #[task(shared = [&em])] - fn sleep_system(mut cx: sleep_system::Context) { + fn sleep_system(cx: sleep_system::Context) { // Turn off the SBG and CAN } - // extern "Rust" { - // // #[task(capacity = 3, shared = [&em, sd_manager])] - // // fn sbg_sd_task(context: sbg_sd_task::Context, data: [u8; SBG_BUFFER_SIZE]); - - // #[task(binds = DMA1_STR1, shared = [&em, sbg_manager])] - // fn sbg_dma(mut context: sbg_dma::Context); + extern "Rust" { + #[task(capacity = 3, shared = [&em, sd_manager])] + fn sbg_sd_task(context: sbg_sd_task::Context, data: [u8; SBG_BUFFER_SIZE]); - // #[task(capacity = 10, shared = [data_manager])] - // fn sbg_handle_data(context: sbg_handle_data::Context, data: CallbackData); + #[task(priority = 3, binds = DMA1_STR1, shared = [&em, sbg_manager])] + fn sbg_dma(mut context: sbg_dma::Context); - // // #[task(shared = [ &em])] - // // fn sbg_get_time(context: sbg_get_time::Context); + #[task(capacity = 10, shared = [data_manager])] + fn sbg_handle_data(context: sbg_handle_data::Context, data: CallbackData); - // #[task(shared = [&em, sbg_manager])] - // fn sbg_flush(context: sbg_flush::Context); + #[task(shared = [&em, sbg_manager])] + fn sbg_flush(context: sbg_flush::Context); - // #[task(shared = [&em, sbg_manager])] - // fn sbg_write_data(context: sbg_write_data::Context, data: Vec); - // } + #[task(shared = [&em, sbg_manager])] + fn sbg_write_data(context: sbg_write_data::Context, data: Vec); + } } diff --git a/boards/nav/src/sbg_manager.rs b/boards/nav/src/sbg_manager.rs index b295dcd4..5c6c2669 100644 --- a/boards/nav/src/sbg_manager.rs +++ b/boards/nav/src/sbg_manager.rs @@ -1,18 +1,15 @@ -use crate::types::SBGBuffer; use core::alloc::{GlobalAlloc, Layout}; use core::ffi::c_void; use core::mem::size_of; use core::ptr; // use atsamd_hal::time::*; -use crate::app::sbg_dma; use crate::app::sbg_flush; use crate::app::sbg_handle_data; // use crate::app::sbg_sd_task as sbg_sd; use crate::app::sbg_write_data; use crate::RTC; use chrono::{NaiveDate, NaiveDateTime, NaiveTime}; -use common_arm::spawn; -use core::{mem, mem::MaybeUninit}; +use core::mem::MaybeUninit; use defmt::info; use embedded_alloc::Heap; use heapless::Vec; @@ -24,10 +21,7 @@ use stm32h7xx_hal::dma::{ dma::{DmaConfig, StreamsTuple}, PeripheralToMemory, Transfer, }; -use stm32h7xx_hal::gpio::Alternate; -use stm32h7xx_hal::gpio::Pin; use stm32h7xx_hal::pac::UART4; -use stm32h7xx_hal::rtc::Rtc; use stm32h7xx_hal::serial::{Rx, Tx}; // use cortex_m::{asm}; use rtic::Mutex; @@ -58,7 +52,7 @@ pub struct SBGManager { impl SBGManager { pub fn new( serial: stm32h7xx_hal::serial::Serial, - mut stream_tuple: StreamsTuple, + stream_tuple: StreamsTuple, //mut dma_channel: dmac::Channel, ) -> Self { /* Initialize the Heap */ @@ -83,7 +77,7 @@ impl SBGManager { // }; let config = DmaConfig::default().memory_increment(true).transfer_complete_interrupt(true); - let mut transfer: Transfer< + let transfer: Transfer< StreamX, Rx, PeripheralToMemory, @@ -122,7 +116,7 @@ impl SBGManager { } } -pub fn sbg_flush(mut cx: sbg_flush::Context) { +pub fn sbg_flush(cx: sbg_flush::Context) { // cx.shared.sbg_manager.lock(|sbg| { // sbg.sbg_tx // }); @@ -159,23 +153,23 @@ pub fn sbg_handle_data(mut cx: sbg_handle_data::Context, data: CallbackData) { }); } -// pub fn sbg_sd_task(mut cx: crate::app::sbg_sd_task::Context, data: [u8; SBG_BUFFER_SIZE]) { -// cx.shared.sd_manager.lock(|manager| { -// if let Some(mut file) = manager.file.take() { -// cx.shared.em.run(|| { -// manager.write(&mut file, &data)?; -// Ok(()) -// }); -// manager.file = Some(file); // give the file back after use -// } else if let Ok(mut file) = manager.open_file("sbg.txt") { -// cx.shared.em.run(|| { -// manager.write(&mut file, &data)?; -// Ok(()) -// }); -// manager.file = Some(file); -// } -// }); -// } +pub fn sbg_sd_task(mut cx: crate::app::sbg_sd_task::Context, data: [u8; SBG_BUFFER_SIZE]) { + cx.shared.sd_manager.lock(|manager| { + if let Some(mut file) = manager.file.take() { + cx.shared.em.run(|| { + manager.write(&mut file, &data)?; + Ok(()) + }); + manager.file = Some(file); // give the file back after use + } else if let Ok(mut file) = manager.open_file("sbg.txt") { + cx.shared.em.run(|| { + manager.write(&mut file, &data)?; + Ok(()) + }); + manager.file = Some(file); + } + }); +} /** * Handles the DMA interrupt. * Handles the SBG data. From 7678ebb6b198089a3e2710d356d866edea82860e Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Wed, 7 Aug 2024 16:02:18 -0400 Subject: [PATCH 13/47] WIP: Works, but needs cleaning. --- .vscode/launch.json | 31 ++ Cargo.lock | 184 +++++++++++- Cargo.toml | 6 +- boards/nav/Cargo.toml | 8 +- boards/nav/src/main.rs | 131 ++++----- boards/nav/src/sbg_manager.rs | 28 +- dump.txt | 400 ++++++++++++++++++++++++++ examples/simple/Cargo.toml | 14 +- examples/simple/src/data_manager.rs | 69 +++++ examples/simple/src/main.rs | 189 ++++++++---- examples/simple/src/sbg_manager.rs | 252 ++++++++++++++++ libraries/common-arm-stm32h7/memory.x | 17 +- libraries/sbg-rs/Cargo.toml | 1 - libraries/sbg-rs/src/sbg.rs | 58 ++-- 14 files changed, 1203 insertions(+), 185 deletions(-) create mode 100644 dump.txt create mode 100644 examples/simple/src/data_manager.rs create mode 100644 examples/simple/src/sbg_manager.rs diff --git a/.vscode/launch.json b/.vscode/launch.json index ee4c5098..739e02b6 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -50,7 +50,9 @@ "request": "launch", "name": "Probe-rs Debug nav (Debug)", "chip": "STM32H733VGTx", + "wireProtocol": "Swd", "flashingConfig": { + "haltAfterReset": true, // "flashingEnabled": true, @@ -61,11 +63,40 @@ } ] }, + { + "type": "probe-rs-debug", + "request": "attach", + "name": "Probe-rs attach Release nav", + "chip": "STM32H733VGTx", + "wireProtocol": "Swd", + "coreConfigs": [ + { + "programBinary": "${workspaceFolder}/target/thumbv7em-none-eabihf/release/nav", + "rttEnabled": true, + "coreIndex": 0, + } + ] + }, + { + "type": "probe-rs-debug", + "request": "attach", + "name": "Probe-rs attach Release simple example", + "chip": "STM32H733VGTx", + "wireProtocol": "Swd", + "coreConfigs": [ + { + "programBinary": "${workspaceFolder}/target/thumbv7em-none-eabihf/release/simple_example", + "rttEnabled": true, + "coreIndex": 0, + } + ] + }, { "type": "probe-rs-debug", "request": "launch", "name": "Probe-rs Release nav", "chip": "STM32H733VGTx", + "wireProtocol": "Swd", "flashingConfig": { "haltAfterReset": true, "flashingEnabled": true, diff --git a/Cargo.lock b/Cargo.lock index e3363864..891a04e2 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -187,9 +187,9 @@ checksum = "37b2a672a2cb129a2e41c10b1224bb368f9f37a2b16b612598138befd7b37eb5" [[package]] name = "cc" -version = "1.1.7" +version = "1.1.8" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "26a5c3fd7bfa1ce3897a3a3501d362b2d87b7f2583ebcb4a949ec25911025cbc" +checksum = "504bdec147f2cc13c8b57ed9401fd8a147cc66b67ad5cb241394244f2c947549" [[package]] name = "cfg-if" @@ -527,6 +527,15 @@ version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "361a90feb7004eca4019fb28352a9465666b24f840f5c3cddf0ff13920590b89" +[[package]] +name = "embedded-hal-async" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0c4c685bbef7fe13c3c6dd4da26841ed3980ef33e841cddfa15ce8a8fb3f1884" +dependencies = [ + "embedded-hal 1.0.0", +] + [[package]] name = "embedded-io" version = "0.4.0" @@ -584,6 +593,12 @@ dependencies = [ "syn 2.0.72", ] +[[package]] +name = "equivalent" +version = "1.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5443807d6dff69373d433ab9ef5378ad8df50ca6298caf15de6e52e24aaf54d5" + [[package]] name = "errno" version = "0.3.9" @@ -629,6 +644,30 @@ dependencies = [ "gcd", ] +[[package]] +name = "futures-core" +version = "0.3.30" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dfc6580bb841c5a68e9ef15c77ccc837b40a7504914d52e47b8b0e9bbda25a1d" + +[[package]] +name = "futures-task" +version = "0.3.30" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "38d84fa142264698cdce1a9f9172cf383a0c82de1bddcf3092901442c4097004" + +[[package]] +name = "futures-util" +version = "0.3.30" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3d6401deb83407ab3da39eba7e33987a73c3df0c82b4bb5813ee871c19c41d48" +dependencies = [ + "futures-core", + "futures-task", + "pin-project-lite", + "pin-utils", +] + [[package]] name = "gcd" version = "2.3.0" @@ -680,6 +719,12 @@ version = "0.12.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8a9ee70c43aaf417c914396645a0fa852624801b24ebb7ae78fe8272889ac888" +[[package]] +name = "hashbrown" +version = "0.14.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e5274423e17b7c9fc20b6e7e208532f9b19825d82dfd615708b70edd83df41f1" + [[package]] name = "heapless" version = "0.7.17" @@ -711,7 +756,17 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bd070e393353796e801d209ad339e89596eb4c8d430d18ede6a1cced8fafbd99" dependencies = [ "autocfg", - "hashbrown", + "hashbrown 0.12.3", +] + +[[package]] +name = "indexmap" +version = "2.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "de3fc2e30ba82dd1b3911c8de1ffc143c74a914a14e99514d7637e3099df5ea0" +dependencies = [ + "equivalent", + "hashbrown 0.14.5", ] [[package]] @@ -904,7 +959,6 @@ dependencies = [ "common-arm-stm32h7", "cortex-m", "cortex-m-rt", - "cortex-m-rtic", "defmt", "defmt-rtt", "embedded-alloc", @@ -913,6 +967,8 @@ dependencies = [ "messages", "panic-probe", "postcard", + "rtic", + "rtic-monotonics", "sbg-rs", "stm32h7xx-hal", "systick-monotonic", @@ -1041,6 +1097,24 @@ version = "1.0.15" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a" +[[package]] +name = "pin-project-lite" +version = "0.2.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bda66fc9667c18cb2758a2ac84d1167245054bcf85d5d1aaa6923f45801bdd02" + +[[package]] +name = "pin-utils" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184" + +[[package]] +name = "portable-atomic" +version = "1.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "da544ee218f0d287a911e9c99a39a8c9bc8fcad3cb8db5959940044ecfc67265" + [[package]] name = "postcard" version = "1.0.8" @@ -1265,30 +1339,96 @@ dependencies = [ "chrono", ] +[[package]] +name = "rtic" +version = "2.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c443db16326376bdd64377da268f6616d5f804aba8ce799bac7d1f7f244e9d51" +dependencies = [ + "atomic-polyfill", + "bare-metal 1.0.0", + "cortex-m", + "critical-section", + "rtic-core", + "rtic-macros", +] + +[[package]] +name = "rtic-common" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0786b50b81ef9d2a944a000f60405bb28bf30cd45da2d182f3fe636b2321f35c" +dependencies = [ + "critical-section", +] + [[package]] name = "rtic-core" version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "d9369355b04d06a3780ec0f51ea2d225624db777acbc60abd8ca4832da5c1a42" +[[package]] +name = "rtic-macros" +version = "2.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "54053598ea24b1b74937724e366558412a1777eb2680b91ef646db540982789a" +dependencies = [ + "indexmap 2.3.0", + "proc-macro-error", + "proc-macro2 1.0.86", + "quote 1.0.36", + "syn 2.0.72", +] + [[package]] name = "rtic-monotonic" version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "fb8b0b822d1a366470b9cea83a1d4e788392db763539dc4ba022bcc787fece82" +[[package]] +name = "rtic-monotonics" +version = "2.0.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4e17f88319061d17d3b99997263397b176e3260177e2b4ff4ffed0078d97894c" +dependencies = [ + "cfg-if", + "cortex-m", + "fugit", + "portable-atomic", + "proc-macro2 1.0.86", + "quote 1.0.36", + "rtic-time", + "stm32-metapac", +] + [[package]] name = "rtic-syntax" version = "1.0.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "5f5e215601dc467752c2bddc6284a622c6f3d2bab569d992adcd5ab7e4cb9478" dependencies = [ - "indexmap", + "indexmap 1.9.3", "proc-macro2 1.0.86", "quote 1.0.36", "syn 1.0.109", ] +[[package]] +name = "rtic-time" +version = "2.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a7b1d853fa50dc125695414ce4510567a0d420221e455b1568cfa8c9aece9614" +dependencies = [ + "critical-section", + "embedded-hal 1.0.0", + "embedded-hal-async", + "fugit", + "futures-util", + "rtic-common", +] + [[package]] name = "rtic_example" version = "0.1.0" @@ -1356,7 +1496,6 @@ dependencies = [ name = "sbg-rs" version = "0.1.0" dependencies = [ - "atsamd-hal", "bitflags 2.6.0", "cmake", "common-arm", @@ -1499,13 +1638,20 @@ dependencies = [ name = "simple_example" version = "0.1.0" dependencies = [ - "atsamd-hal", - "common-arm-atsame", + "chrono", + "common-arm", + "common-arm-stm32h7", "cortex-m", "cortex-m-rt", + "cortex-m-rtic", "defmt", "defmt-rtt", - "panic-halt", + "embedded-alloc", + "heapless 0.7.17", + "messages", + "panic-probe", + "sbg-rs", + "stm32h7xx-hal", ] [[package]] @@ -1529,6 +1675,15 @@ version = "1.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a2eb9349b6444b326872e140eb1cf5e7c522154d69e7a0ffb0fb81c06b37543f" +[[package]] +name = "stm32-metapac" +version = "15.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "deabea56a8821dcea05d0109f3ab3135f31eb572444e5da203d06149c594c8c6" +dependencies = [ + "cortex-m", +] + [[package]] name = "stm32h7" version = "0.15.1" @@ -1543,8 +1698,9 @@ dependencies = [ [[package]] name = "stm32h7xx-hal" -version = "0.16.0" -source = "git+https://github.com/stm32-rs/stm32h7xx-hal#5166da8a5485d51e60a42d3a564d1edae0c8e164" +version = "0.15.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e08bcfbdbe4458133f2fd55994a5c4f1b4bf28084f0218e93cdbc19d7c70219f" dependencies = [ "bare-metal 1.0.0", "cast", @@ -1637,15 +1793,15 @@ dependencies = [ [[package]] name = "tempfile" -version = "3.11.0" +version = "3.12.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b8fcd239983515c23a32fb82099f97d0b11b8c72f654ed659363a95c3dad7a53" +checksum = "04cbcdd0c794ebb0d4cf35e88edd2f7d2c4c3e9a5a6dab322839b321c6a87a64" dependencies = [ "cfg-if", "fastrand", "once_cell", "rustix", - "windows-sys 0.52.0", + "windows-sys 0.59.0", ] [[package]] diff --git a/Cargo.toml b/Cargo.toml index 9575830c..676a558f 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -11,7 +11,8 @@ default-members = ["boards/*", "examples/*"] version = "0.2.7" [workspace.dependencies.stm32h7xx-hal] -git = "https://github.com/stm32-rs/stm32h7xx-hal" +#git = "https://github.com/stm32-rs/stm32h7xx-hal" +version = "0.15.1" # We use 35 even though we have the 33. features = ["defmt", "rt", "stm32h735", "can", "rtc"] @@ -38,7 +39,6 @@ features = ["critical-section-single-core"] [profile.dev] # Using LTO causes issues with GDB. lto = false -panic = 'abort' # # Try to remove this # [profile.dev.build-override] @@ -64,4 +64,4 @@ opt-level = 0 #Try to remove this [profile.release.build-override] -#opt-level = 0 +opt-level = 0 diff --git a/boards/nav/Cargo.toml b/boards/nav/Cargo.toml index af91c318..1a03a8db 100644 --- a/boards/nav/Cargo.toml +++ b/boards/nav/Cargo.toml @@ -6,9 +6,11 @@ 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 = "^0.7.0" -cortex-m-rtic = "1.1.3" +#cortex-m = { workspace = true } +cortex-m = "0.7.4" +cortex-m-rt = "0.7.1" +rtic = {version = "2.0.0", features = ["thumbv7-backend"]} +rtic-monotonics = {version = "2.0.2", features = ["cortex-m-systick", "stm32h733vg"]} common-arm-stm32h7 = { path = "../../libraries/common-arm-stm32h7" } common-arm = { path = "../../libraries/common-arm" } stm32h7xx-hal = { workspace = true } diff --git a/boards/nav/src/main.rs b/boards/nav/src/main.rs index 610c6e16..17cfed7d 100644 --- a/boards/nav/src/main.rs +++ b/boards/nav/src/main.rs @@ -6,20 +6,22 @@ mod data_manager; mod sbg_manager; mod types; -use types::COM_ID; use chrono::{NaiveDate, NaiveDateTime, NaiveTime}; use common_arm::*; +use core::cell::RefCell; use core::num::{NonZeroU16, NonZeroU8}; +use cortex_m::interrupt::Mutex; use data_manager::DataManager; use defmt::info; +use defmt_rtt as _; use fdcan::{ config::NominalBitTiming, filter::{StandardFilter, StandardFilterSlot}, }; -use messages::Data; -use fugit::RateExtU32; use heapless::Vec; -use sbg_manager::{sbg_dma, sbg_flush, sbg_sd_task, sbg_handle_data, sbg_write_data}; +use messages::Data; +use panic_probe as _; +use sbg_manager::{sbg_dma, sbg_flush, sbg_handle_data, sbg_sd_task, sbg_write_data}; use sbg_rs::sbg::CallbackData; use sbg_rs::sbg::SBG_BUFFER_SIZE; use stm32h7xx_hal::dma::dma::StreamsTuple; @@ -30,11 +32,10 @@ use stm32h7xx_hal::prelude::*; use stm32h7xx_hal::rtc; use stm32h7xx_hal::spi; use stm32h7xx_hal::{rcc, rcc::rec}; -use systick_monotonic::*; -use cortex_m::interrupt::Mutex; -use core::cell::RefCell; -use panic_probe as _; -use defmt_rtt as _; // global logger +use types::COM_ID; // global logger +use rtic_monotonics::systick::prelude::*; + +systick_monotonic!(Mono, 100); #[inline(never)] #[defmt::panic_handler] @@ -42,19 +43,9 @@ fn panic() -> ! { cortex_m::asm::udf() } -/// Hardfault handler. -/// -/// Terminates the application and makes a semihosting-capable debug tool exit -/// with an error. This seems better than the default, which is to spin in a -/// loop. -#[cortex_m_rt::exception] -unsafe fn HardFault(_frame: &cortex_m_rt::ExceptionFrame) -> ! { - loop {} -} - static RTC: Mutex>> = Mutex::new(RefCell::new(None)); -#[rtic::app(device = stm32h7xx_hal::stm32, dispatchers = [SPI1, SPI2, SPI3])] +#[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, dispatchers = [EXTI0, EXTI1, EXTI2])] mod app { use stm32h7xx_hal::gpio::{Alternate, Pin}; @@ -76,11 +67,8 @@ mod app { led_green: PA3>, } - #[monotonic(binds = SysTick, default = true)] - type SysMono = Systick<500>; // 100 Hz / 10 ms granularity - #[init] - fn init(ctx: init::Context) -> (SharedResources, LocalResources, init::Monotonics) { + fn init(ctx: init::Context) -> (SharedResources, LocalResources) { let core = ctx.core; /* Logging Setup */ @@ -98,8 +86,8 @@ mod app { let rcc = ctx.device.RCC.constrain(); info!("RCC enabled"); let ccdr = rcc - .use_hse(48.MHz()) // check the clock hardware - .sys_ck(200.MHz()) + .use_hse(48.MHz()) // check the clock hardware + .sys_ck(100.MHz()) .pll1_strategy(rcc::PllConfigStrategy::Iterative) .pll1_q_ck(24.MHz()) .freeze(pwrcfg, &ctx.device.SYSCFG); @@ -173,7 +161,7 @@ mod app { let cs_sd = gpioa.pa4.into_push_pull_output(); - let sd = SdManager::new(spi_sd, cs_sd); + let sd_manager = SdManager::new(spi_sd, cs_sd); // leds let led_red = gpioa.pa2.into_push_pull_output(); @@ -186,6 +174,7 @@ mod app { // 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_sbg = ctx @@ -193,7 +182,6 @@ mod app { .UART4 .serial((tx, rx), 115_200.bps(), ccdr.peripheral.UART4, &ccdr.clocks) .unwrap(); - let sbg_manager = sbg_manager::SBGManager::new(uart_sbg, stream_tuple); let mut rtc = stm32h7xx_hal::rtc::Rtc::open_or_init( @@ -215,41 +203,53 @@ mod app { RTC.borrow(cs).replace(Some(rtc)); }); - /* Monotonic clock */ - let mono = Systick::new(core.SYST, ccdr.clocks.sysclk().to_Hz()); - blink::spawn().ok(); - display_data::spawn().ok(); + // let mono = Systick::new(core.SYST, ccdr.clocks.sysclk().to_Hz()); + // blink::spawn().ok(); + // display_data::spawn().ok(); + Mono::start(core.SYST, 100_000_000); info!("Online"); + let data_manager = DataManager::new(); + let em = ErrorManager::new(); + // let mono_token = rtic_monotonics::create_systick_token!(); + // let mono = Systick::start(ctx.core.SYST, 36_000_000, mono_token); + blink::spawn().ok(); ( SharedResources { - data_manager: DataManager::new(), - em: ErrorManager::new(), - sd_manager: sd, + data_manager, + em, + sd_manager, sbg_manager, }, LocalResources { led_red, led_green }, - init::Monotonics(mono), ) } #[idle] fn idle(ctx: idle::Context) -> ! { loop { + // info!("Idle"); + cortex_m::asm::wfi(); } } - #[task(shared = [&em, data_manager])] - fn display_data(mut cx: display_data::Context) { - let data = cx.shared.data_manager.lock(|manager| manager.clone_sensors()); + #[task(priority = 2, shared = [&em, data_manager])] + async fn display_data(mut cx: display_data::Context) { + info!("Displaying data"); + let data = cx + .shared + .data_manager + .lock(|manager| manager.clone_sensors()); info!("{:?}", data); - spawn_after!(display_data, ExtU64::secs(1)).ok(); + Mono::delay(1000.millis()).await; + // spawn_after!(display_data, ExtU64::secs(1)).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"); let message = messages::Message::new( cortex_m::interrupt::free(|cs| { let mut rc = RTC.borrow(cs).borrow_mut(); @@ -269,42 +269,43 @@ mod app { // send_in::spawn(message).ok(); } - #[task(local = [led_red, led_green], shared = [&em])] - fn blink(cx: blink::Context) { - info!("Blinking"); - cx.shared.em.run(|| { - if cx.shared.em.has_error() { - cx.local.led_red.toggle(); - info!("Error"); - spawn_after!(blink, ExtU64::millis(200))?; - } else { - cx.local.led_green.toggle(); - info!("Blinking"); - spawn_after!(blink, ExtU64::secs(1))?; - } - Ok(()) - }); + #[task(priority = 1, local = [led_red, led_green], shared = [&em])] + async fn blink(cx: blink::Context) { + loop { + info!("Blinking"); + cx.shared.em.run(|| { + if cx.shared.em.has_error() { + cx.local.led_red.toggle(); + info!("Error"); + } else { + cx.local.led_green.toggle(); + info!("Blinking"); + } + Ok(()) + }); + Mono::delay(1000.millis()).await; + } } - #[task(shared = [&em])] - fn sleep_system(cx: sleep_system::Context) { + #[task(priority = 1, shared = [&em])] + async fn sleep_system(cx: sleep_system::Context) { // Turn off the SBG and CAN } extern "Rust" { - #[task(capacity = 3, shared = [&em, sd_manager])] - fn sbg_sd_task(context: sbg_sd_task::Context, data: [u8; SBG_BUFFER_SIZE]); + #[task(priority = 1, shared = [&em, sd_manager])] + async fn sbg_sd_task(context: sbg_sd_task::Context, data: [u8; SBG_BUFFER_SIZE]); #[task(priority = 3, binds = DMA1_STR1, shared = [&em, sbg_manager])] fn sbg_dma(mut context: sbg_dma::Context); - #[task(capacity = 10, shared = [data_manager])] - fn sbg_handle_data(context: sbg_handle_data::Context, data: CallbackData); + #[task(priority = 1, shared = [data_manager])] + async fn sbg_handle_data(context: sbg_handle_data::Context, data: CallbackData); - #[task(shared = [&em, sbg_manager])] - fn sbg_flush(context: sbg_flush::Context); + #[task(priority = 1, shared = [&em, sbg_manager])] + async fn sbg_flush(context: sbg_flush::Context); - #[task(shared = [&em, sbg_manager])] - fn sbg_write_data(context: sbg_write_data::Context, data: Vec); + #[task(priority = 1, shared = [&em, sbg_manager])] + async fn sbg_write_data(context: sbg_write_data::Context, data: Vec); } } diff --git a/boards/nav/src/sbg_manager.rs b/boards/nav/src/sbg_manager.rs index 5c6c2669..c97bb000 100644 --- a/boards/nav/src/sbg_manager.rs +++ b/boards/nav/src/sbg_manager.rs @@ -29,7 +29,7 @@ use rtic::Mutex; //#[link_section = ".axisram.buffers"] //static mut SBG_BUFFER: MayberUninit<[u8; SBG_BUFFER_SIZE]> = MaybeUninit::uninit(); -#[link_section = ".axisram.buffers"] +// #[link_section = ".axisram.buffers"] pub static mut SBG_BUFFER: MaybeUninit<[u8; SBG_BUFFER_SIZE]> = MaybeUninit::uninit(); // Simple heap required by the SBG library @@ -77,7 +77,7 @@ impl SBGManager { // }; let config = DmaConfig::default().memory_increment(true).transfer_complete_interrupt(true); - let transfer: Transfer< + let mut transfer: Transfer< StreamX, Rx, PeripheralToMemory, @@ -90,10 +90,18 @@ impl SBGManager { None, config, ); + info!("Starting transfer"); + transfer.start(|serial| { + serial.enable_dma_rx(); + + }); + info!("Transfer started"); + + // while !transfer.get_transfer_complete_flag() { - // transfer.start(|serial| { - // serial.enable_dma_rx(); - // }); + // } + // info!("Transfer complete"); + // info!("{}", unsafe { SBG_BUFFER.assume_init_read() }); let sbg: sbg::SBG = sbg::SBG::new( |data| { @@ -107,7 +115,7 @@ impl SBGManager { sbg_flush::spawn().ok(); }, ); - + SBGManager { sbg_device: sbg, xfer: Some(transfer), @@ -116,12 +124,12 @@ impl SBGManager { } } -pub fn sbg_flush(cx: sbg_flush::Context) { +pub async fn sbg_flush(cx: sbg_flush::Context<'_>) { // cx.shared.sbg_manager.lock(|sbg| { // sbg.sbg_tx // }); } -pub fn sbg_write_data(mut cx: sbg_write_data::Context, data: Vec) { +pub async fn sbg_write_data(mut cx: sbg_write_data::Context<'_>, data: Vec) { cx.shared.sbg_manager.lock(|sbg| { sbg.sbg_tx.write_all(data.as_slice()); }); @@ -141,7 +149,7 @@ pub fn sbg_get_time() -> u32 { }) } -pub fn sbg_handle_data(mut cx: sbg_handle_data::Context, data: CallbackData) { +pub async fn sbg_handle_data(mut cx: sbg_handle_data::Context<'_>, data: CallbackData) { cx.shared.data_manager.lock(|manager| match data { CallbackData::UtcTime(x) => manager.utc_time = Some(x), CallbackData::Air(x) => manager.air = Some(x), @@ -153,7 +161,7 @@ pub fn sbg_handle_data(mut cx: sbg_handle_data::Context, data: CallbackData) { }); } -pub fn sbg_sd_task(mut cx: crate::app::sbg_sd_task::Context, data: [u8; SBG_BUFFER_SIZE]) { +pub async fn sbg_sd_task(mut cx: crate::app::sbg_sd_task::Context<'_>, data: [u8; SBG_BUFFER_SIZE]) { cx.shared.sd_manager.lock(|manager| { if let Some(mut file) = manager.file.take() { cx.shared.em.run(|| { diff --git a/dump.txt b/dump.txt new file mode 100644 index 00000000..9931b1b1 --- /dev/null +++ b/dump.txt @@ -0,0 +1,400 @@ +us: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 7950000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 7950000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8225000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8225000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8225000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8225000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO DMA +└─ simple_example::sbg_manager::sbg_dma @ examples/simple/src/sbg_manager.rs:176 +INFO Reading SBG Data +└─ sbg_rs::sbg::{impl#0}::read_data @ libraries/sbg-rs/src/sbg.rs:149 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO DMA +└─ simple_example::sbg_manager::sbg_dma @ examples/simple/src/sbg_manager.rs:176 +INFO Reading SBG Data +└─ sbg_rs::sbg::{impl#0}::read_data @ libraries/sbg-rs/src/sbg.rs:149 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO DMA +└─ simple_example::sbg_manager::sbg_dma @ examples/simple/src/sbg_manager.rs:176 +INFO Reading SBG Data +└─ sbg_rs::sbg::{impl#0}::read_data @ libraries/sbg-rs/src/sbg.rs:149 +INFO /home/ns/Rocketry/hydra/libraries/sbg-rs/sbgECom/src/protocol/sbgEComProtocol.c:sbgEComProtocolParseFrame:300:None:4:invalid end-of-frame: byte:%#hhx +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:470 +ERROR SBG Error +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:474 +INFO /home/ns/Rocketry/hydra/libraries/sbg-rs/sbgECom/src/protocol/sbgEComProtocol.c:sbgEComProtocolParseFrame:300:None:4:invalid end-of-frame: byte:%#hhx +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:470 +ERROR SBG Error +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:474 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO DMA +└─ simple_example::sbg_manager::sbg_dma @ examples/simple/src/sbg_manager.rs:176 +INFO Reading SBG Data +└─ sbg_rs::sbg::{impl#0}::read_data @ libraries/sbg-rs/src/sbg.rs:149 +INFO /home/ns/Rocketry/hydra/libraries/sbg-rs/sbgECom/src/protocol/sbgEComProtocol.c:sbgEComProtocolParseFrame:300:None:4:invalid end-of-frame: byte:%#hhx +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:470 +ERROR SBG Error +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:474 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO DMA +└─ simple_example::sbg_manager::sbg_dma @ examples/simple/src/sbg_manager.rs:176 +INFO Reading SBG Data +└─ sbg_rs::sbg::{impl#0}::read_data @ libraries/sbg-rs/src/sbg.rs:149 +INFO /home/ns/Rocketry/hydra/libraries/sbg-rs/sbgECom/src/protocol/sbgEComProtocol.c:sbgEComProtocolParseFrame:300:None:4:invalid end-of-frame: byte:%#hhx +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:470 +ERROR SBG Error +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:474 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO DMA +└─ simple_example::sbg_manager::sbg_dma @ examples/simple/src/sbg_manager.rs:176 +INFO Reading SBG Data +└─ sbg_rs::sbg::{impl#0}::read_data @ libraries/sbg-rs/src/sbg.rs:149 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO DMA +└─ simple_example::sbg_manager::sbg_dma @ examples/simple/src/sbg_manager.rs:176 +INFO Reading SBG Data +└─ sbg_rs::sbg::{impl#0}::read_data @ libraries/sbg-rs/src/sbg.rs:149 +INFO /home/ns/Rocketry/hydra/libraries/sbg-rs/sbgECom/src/protocol/sbgEComProtocol.c:sbgEComProtocolParseFrame:300:None:4:invalid end-of-frame: byte:%#hhx +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:470 +ERROR SBG Error +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:474 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO DMA +└─ simple_example::sbg_manager::sbg_dma @ examples/simple/src/sbg_manager.rs:176 +INFO Reading SBG Data +└─ sbg_rs::sbg::{impl#0}::read_data @ libraries/sbg-rs/src/sbg.rs:149 +INFO /home/ns/Rocketry/hydra/libraries/sbg-rs/sbgECom/src/protocol/sbgEComProtocol.c:sbgEComProtocolParseFrame:300:None:4:invalid end-of-frame: byte:%#hhx +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:470 +ERROR SBG Error +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:474 +INFO /home/ns/Rocketry/hydra/libraries/sbg-rs/sbgECom/src/protocol/sbgEComProtocol.c:sbgEComProtocolParseFrame:249:None:4:invalid page information : %hu/%hu +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:470 +ERROR SBG Error +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:474 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9150000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9150000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9150000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9150000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9150000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9150000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9150000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO DMA +└─ simple_example::sbg_manager::sbg_dma @ examples/simple/src/sbg_manager.rs:176 +INFO Reading SBG Data +└─ sbg_rs::sbg::{impl#0}::read_data @ libraries/sbg-rs/src/sbg.rs:149 +INFO /home/ns/Rocketry/hydra/libraries/sbg-rs/sbgECom/src/protocol/sbgEComProtocol.c:sbgEComProtocolParseFrame:300:None:4:invalid end-of-frame: byte:%#hhx +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:470 +ERROR SBG Error +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:474 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9350000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7754529, 8.46911, -4.8705688]), gyroscopes: Some([0.05026379, -0.04602995, 0.047635462]) })), Some(Imu2(Imu2 { temperature: Some(35.50069), delta_velocity: Some([-0.7782066, 8.469136, -4.8717275]), delta_angle: Some([0.1043067, -0.09052475, 0.09423562]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9350000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7754529, 8.46911, -4.8705688]), gyroscopes: Some([0.05026379, -0.04602995, 0.047635462]) })), Some(Imu2(Imu2 { temperature: Some(35.50069), delta_velocity: Some([-0.7782066, 8.469136, -4.8717275]), delta_angle: Some([0.1043067, -0.09052475, 0.09423562]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9350000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7754529, 8.46911, -4.8705688]), gyroscopes: Some([0.05026379, -0.04602995, 0.047635462]) })), Some(Imu2(Imu2 { temperature: Some(35.50069), delta_velocity: Some([-0.7782066, 8.469136, -4.8717275]), delta_angle: Some([0.1043067, -0.09052475, 0.09423562]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9350000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7754529, 8.46911, -4.8705688]), gyroscopes: Some([0.05026379, -0.04602995, 0.047635462]) })), Some(Imu2(Imu2 { temperature: Some(35.50069), delta_velocity: Some([-0.7782066, 8.469136, -4.8717275]), delta_angle: Some([0.1043067, -0.09052475, 0.09423562]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9350000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7754529, 8.46911, -4.8705688]), gyroscopes: Some([0.05026379, -0.04602995, 0.047635462]) })), Some(Imu2(Imu2 { temperature: Some(35.50069), delta_velocity: Some([-0.7782066, 8.469136, -4.8717275]), delta_angle: Some([0.1043067, -0.09052475, 0.09423562]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9350000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7754529, 8.46911, -4.8705688]), gyroscopes: Some([0.05026379, -0.04602995, 0.047635462]) })), Some(Imu2(Imu2 { temperature: Some(35.50069), delta_velocity: Some([-0.7782066, 8.469136, -4.8717275]), delta_angle: Some([0.1043067, -0.09052475, 0.09423562]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9350000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7754529, 8.46911, -4.8705688]), gyroscopes: Some([0.05026379, -0.04602995, 0.047635462]) })), Some(Imu2(Imu2 { temperature: Some(35.50069), delta_velocity: Some([-0.7782066, 8.469136, -4.8717275]), delta_angle: Some([0.1043067, -0.09052475, 0.09423562]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9350000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7754529, 8.46911, -4.8705688]), gyroscopes: Some([0.05026379, -0.04602995, 0.047635462]) })), Some(Imu2(Imu2 { temperature: Some(35.50069), delta_velocity: Some([-0.7782066, 8.469136, -4.8717275]), delta_angle: Some([0.1043067, -0.09052475, 0.09423562]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9350000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7754529, 8.46911, -4.8705688]), gyroscopes: Some([0.05026379, -0.04602995, 0.047635462]) })), Some(Imu2(Imu2 { temperature: Some(35.50069), delta_velocity: Some([-0.7782066, 8.469136, -4.8717275]), delta_angle: Some([0.1043067, -0.09052475, 0.09423562]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9350000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7754529, 8.46911, -4.8705688]), gyroscopes: Some([0.05026379, -0.04602995, 0.047635462]) })), Some(Imu2(Imu2 { temperature: Some(35.50069), delta_velocity: Some([-0.7782066, 8.469136, -4.8717275]), delta_angle: Some([0.1043067, -0.09052475, 0.09423562]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9350000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7754529, 8.46911, -4.8705688]), gyroscopes: Some([0.05026379, -0.04602995, 0.047635462]) })), Some(Imu2(Imu2 { temperature: Some(35.50069), delta_velocity: Some([-0.7782066, 8.469136, -4.8717275]), delta_angle: Some([0.1043067, -0.09052475, 0.09423562]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9350000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7754529, 8.46911, -4.8705688]), gyroscopes: Some([0.05026379, -0.04602995, 0.047635462]) })), Some(Imu2(Imu2 { temperature: Some(35.50069), delta_velocity: Some([-0.7782066, 8.469136, -4.8717275]), delta_angle: Some([0.1043067, -0.09052475, 0.09423562]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO DMA +└─ simple_example::sbg_manager::sbg_dma @ examples/simple/src/sbg_manager.rs:176 +INFO Reading SBG Data +└─ sbg_rs::sbg::{impl#0}::read_data @ libraries/sbg-rs/src/sbg.rs:149 +INFO /home/ns/Rocketry/hydra/libraries/sbg-rs/sbgECom/src/protocol/sbgEComProtocol.c:sbgEComProtocolParseFrame:300:None:4:invalid end-of-frame: byte:%#hhx +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:470 +ERROR SBG Error +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:474 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO DMA +└─ simple_example::sbg_manager::sbg_dma @ examples/simple/src/sbg_manager.rs:176 +INFO Reading SBG Data +└─ sbg_rs::sbg::{impl#0}::read_data @ libraries/sbg-rs/src/sbg.rs:149 +INFO /home/ns/Rocketry/hydra/libraries/sbg-rs/sbgECom/src/protocol/sbgEComProtocol.c:sbgEComProtocolParseFrame:300:None:4:invalid end-of-frame: byte:%#hhx +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:470 +ERROR SBG Error +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:474 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO DMA +└─ simple_example::sbg_manager::sbg_dma @ examples/simple/src/sbg_manager.rs:176 +INFO Reading SBG Data +└─ sbg_rs::sbg::{impl#0}::read_data @ libraries/sbg-rs/src/sbg.rs:149 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9625000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7815678, 8.472983, -4.879673]), gyroscopes: Some([0.00068770646, 0.00042975583, 0.00083899405]) })), Some(Imu2(Imu2 { temperature: Some(35.12805), delta_velocity: Some([-0.7833217, 8.470738, -4.8809614]), delta_angle: Some([0.0005976452, 0.00045764886, 0.0010904415]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9625000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7815678, 8.472983, -4.879673]), gyroscopes: Some([0.00068770646, 0.00042975583, 0.00083899405]) })), Some(Imu2(Imu2 { temperature: Some(35.12805), delta_velocity: Some([-0.7833217, 8.470738, -4.8809614]), delta_angle: Some([0.0005976452, 0.00045764886, 0.0010904415]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9625000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7815678, 8.472983, -4.879673]), gyroscopes: Some([0.00068770646, 0.00042975583, 0.00083899405]) })), Some(Imu2(Imu2 { temperature: Some(35.12805), delta_velocity: Some([-0.7833217, 8.470738, -4.8809614]), delta_angle: Some([0.0005976452, 0.00045764886, 0.0010904415]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9625000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7815678, 8.472983, -4.879673]), gyroscopes: Some([0.00068770646, 0.00042975583, 0.00083899405]) })), Some(Imu2(Imu2 { temperature: Some(35.12805), delta_velocity: Some([-0.7833217, 8.470738, -4.8809614]), delta_angle: Some([0.0005976452, 0.00045764886, 0.0010904415]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9625000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7815678, 8.472983, -4.879673]), gyroscopes: Some([0.00068770646, 0.00042975583, 0.00083899405]) })), Some(Imu2(Imu2 { temperature: Some(35.12805), delta_velocity: Some([-0.7833217, 8.470738, -4.8809614]), delta_angle: Some([0.0005976452, 0.00045764886, 0.0010904415]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9625000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7815678, 8.472983, -4.879673]), gyroscopes: Some([0.00068770646, 0.00042975583, 0.00083899405]) })), Some(Imu2(Imu2 { temperature: Some(35.12805), delta_velocity: Some([-0.7833217, 8.470738, -4.8809614]), delta_angle: Some([0.0005976452, 0.00045764886, 0.0010904415]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9625000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7815678, 8.472983, -4.879673]), gyroscopes: Some([0.00068770646, 0.00042975583, 0.00083899405]) })), Some(Imu2(Imu2 { temperature: Some(35.12805), delta_velocity: Some([-0.7833217, 8.470738, -4.8809614]), delta_angle: Some([0.0005976452, 0.00045764886, 0.0010904415]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9625000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7815678, 8.472983, -4.879673]), gyroscopes: Some([0.00068770646, 0.00042975583, 0.00083899405]) })), Some(Imu2(Imu2 { temperature: Some(35.12805), delta_velocity: Some([-0.7833217, 8.470738, -4.8809614]), delta_angle: Some([0.0005976452, 0.00045764886, 0.0010904415]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO DMA +└─ simple_example::sbg_manager::sbg_dma @ examples/simple/src/sbg_manager.rs:176 +INFO Reading SBG Data +└─ sbg_rs::sbg::{impl#0}::read_data @ libraries/sbg-rs/src/sbg.rs:149 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9625000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7815678, 8.472983, -4.879673]), gyroscopes: Some([0.00068770646, 0.00042975583, 0.00083899405]) })), Some(Imu2(Imu2 { temperature: Some(35.12805), delta_velocity: Some([-0.7833217, 8.470738, -4.8809614]), delta_angle: Some([0.0005976452, 0.00045764886, 0.0010904415]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9625000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7815678, 8.472983, -4.879673]), gyroscopes: Some([0.00068770646, 0.00042975583, 0.00083899405]) })), Some(Imu2(Imu2 { temperature: Some(35.12805), delta_velocity: Some([-0.7833217, 8.470738, -4.8809614]), delta_angle: Some([0.0005976452, 0.00045764886, 0.0010904415]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9625000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7815678, 8.472983, -4.879673]), gyroscopes: Some([0.00068770646, 0.00042975583, 0.00083899405]) })), Some(Imu2(Imu2 { temperature: Some(35.12805), delta_velocity: Some([-0.7833217, 8.470738, -4.8809614]), delta_angle: Some([0.0005976452, 0.00045764886, 0.0010904415]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9625000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7815678, 8.472983, -4.879673]), gyroscopes: Some([0.00068770646, 0.00042975583, 0.00083899405]) })), Some(Imu2(Imu2 { temperature: Some(35.12805), delta_velocity: Some([-0.7833217, 8.470738, -4.8809614]), delta_angle: Some([0.0005976452, 0.00045764886, 0.0010904415]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9625000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7815678, 8.472983, -4.879673]), gyroscopes: Some([0.00068770646, 0.00042975583, 0.00083899405]) })), Some(Imu2(Imu2 { temperature: Some(35.12805), delta_velocity: Some([-0.7833217, 8.470738, -4.8809614]), delta_angle: Some([0.0005976452, 0.00045764886, 0.0010904415]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO DMA +└─ simple_example::sbg_manager::sbg_dma @ examples/simple/src/sbg_manager.rs:176 +INFO Reading SBG Data +└─ sbg_rs::sbg::{impl#0}::read_data @ libraries/sbg-rs/src/sbg.rs:149 +INFO /home/ns/Rocketry/hydra/libraries/sbg-rs/sbgECom/src/protocol/sbgEComProtocol.c:sbgEComProtocolParseFrame:300:None:4:invalid end-of-frame: byte:%#hhx +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:470 +ERROR SBG Error +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:474 +INFO /home/ns/Rocketry/hydra/libraries/sbg-rs/sbgECom/src/protocol/sbgEComProtocol.c:sbgEComProtocolParseFrame:300:None:4:invalid end-of-frame: byte:%#hhx +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:470 +ERROR SBG Error +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:474 +INFO Data [Some(Air(Air { time_stamp: 9710143, status: AirStatus { status: 6 }, pressure_abs: Some(100994.0), altitude: Some(27.57832), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9625000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7815678, 8.472983, -4.879673]), gyroscopes: Some([0.00068770646, 0.00042975583, 0.00083899405]) })), Some(Imu2(Imu2 { temperature: Some(35.12805), delta_velocity: Some([-0.7833217, 8.470738, -4.8809614]), delta_angle: Some([0.0005976452, 0.00045764886, 0.0010904415]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 9710143, status: AirStatus { status: 6 }, pressure_abs: Some(100994.0), altitude: Some(27.57832), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9625000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7815678, 8.472983, -4.879673]), gyroscopes: Some([0.00068770646, 0.00042975583, 0.00083899405]) })), Some(Imu2(Imu2 { temperature: Some(35.12805), delta_velocity: Some([-0.7833217, 8.470738, -4.8809614]), delta_angle: Some([0.0005976452, 0.00045764886, 0.0010904415]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 9710143, status: AirStatus { status: 6 }, pressure_abs: Some(100994.0), altitude: Some(27.57832), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9625000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7815678, 8.472983, -4.879673]), gyroscopes: Some([0.00068770646, 0.00042975583, 0.00083899405]) })), Some(Imu2(Imu2 { temperature: Some(35.12805), delta_velocity: Some([-0.7833217, 8.470738, -4.8809614]), delta_angle: Some([0.0005976452, 0.00045764886, 0.0010904415]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 9710143, status: AirStatus { status: 6 }, pressure_abs: Some(100994.0), altitude: Some(27.57832), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9625000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7815678, 8.472983, -4.879673]), gyroscopes: Some([0.00068770646, 0.00042975583, 0.00083899405]) })), Some(Imu2(Imu2 { temperature: Some(35.12805), delta_velocity: Some([-0.7833217, 8.470738, -4.8809614]), delta_angle: Some([0.0005976452, 0.00045764886, 0.0010904415]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 \ No newline at end of file diff --git a/examples/simple/Cargo.toml b/examples/simple/Cargo.toml index 7d3b1ca4..5c413866 100644 --- a/examples/simple/Cargo.toml +++ b/examples/simple/Cargo.toml @@ -8,8 +8,16 @@ edition = "2021" [dependencies] cortex-m = { version = "^0.7.0", features = ["critical-section-single-core"] } cortex-m-rt = "^0.7.0" -panic-halt = "0.2.0" +cortex-m-rtic = "1.1.3" + defmt-rtt = "0.4.0" defmt = "0.3.2" -atsamd-hal = { workspace = true } -common-arm-atsame = { path = "../../libraries/common-arm-atsame" } +stm32h7xx-hal = { workspace = true } +common-arm-stm32h7 = { path = "../../libraries/common-arm-stm32h7" } +panic-probe = { version = "0.3", features = ["print-defmt"] } +chrono = {version = "0.4.0", default-features = false} +sbg-rs = {path = "../../libraries/sbg-rs"} +messages = { path = "../../libraries/messages" } +heapless = "0.7.16" +embedded-alloc = "0.5.0" +common-arm = { path = "../../libraries/common-arm" } diff --git a/examples/simple/src/data_manager.rs b/examples/simple/src/data_manager.rs new file mode 100644 index 00000000..29868be6 --- /dev/null +++ b/examples/simple/src/data_manager.rs @@ -0,0 +1,69 @@ +use common_arm::HydraError; +use messages::sensor::{ + Air, EkfNav1, EkfNav2, EkfNavAcc, EkfQuat, GpsPos1, GpsPos2, GpsPosAcc, GpsVel, GpsVelAcc, + Imu1, Imu2, SensorData, UtcTime, +}; +use messages::Message; + +#[derive(Clone)] +pub struct DataManager { + pub air: Option, + pub ekf_nav: Option<(EkfNav1, EkfNav2, EkfNavAcc)>, + pub ekf_quat: Option, + pub imu: Option<(Imu1, Imu2)>, + pub utc_time: Option, + pub gps_vel: Option<(GpsVel, GpsVelAcc)>, + pub gps_pos: Option<(GpsPos1, GpsPos2, GpsPosAcc)>, +} + +impl DataManager { + pub fn new() -> Self { + Self { + air: None, + ekf_nav: None, + ekf_quat: None, + imu: None, + utc_time: None, + gps_vel: None, + gps_pos: None, + } + } + + // TODO: stop cloning so much this is a waste of resources. + pub fn clone_sensors(&self) -> [Option; 11] { + [ + self.air.clone().map(|x| x.into()), + self.ekf_nav.clone().map(|x| x.0.into()), + self.ekf_nav.clone().map(|x| x.1.into()), + self.ekf_quat.clone().map(|x| x.into()), + self.imu.clone().map(|x| x.0.into()), + self.imu.clone().map(|x| x.1.into()), + self.utc_time.clone().map(|x| x.into()), + self.gps_vel.clone().map(|x| x.0.into()), + self.gps_vel.clone().map(|x| x.1.into()), + self.gps_pos.clone().map(|x| x.0.into()), + self.gps_pos.clone().map(|x| x.1.into()), + ] + } + + pub fn handle_data(&mut self, data: Message) -> Result<(), HydraError> { + match data.data { + messages::Data::Command(command) => match command.data { + messages::command::CommandData::PowerDown(_) => {} + _ => { + // We don't care atm about these other commands. + } + }, + _ => { + // we can disregard all other messages for now. + } + } + Ok(()) + } +} + +impl Default for DataManager { + fn default() -> Self { + Self::new() + } +} diff --git a/examples/simple/src/main.rs b/examples/simple/src/main.rs index b2fa5a97..bdabab4c 100644 --- a/examples/simple/src/main.rs +++ b/examples/simple/src/main.rs @@ -1,55 +1,144 @@ -#![no_std] #![no_main] +#![no_std] + +use panic_probe as _; -use atsamd_hal as hal; -use atsamd_hal::prelude::*; +use defmt::Format; +use defmt_rtt as _; // global logger + +#[inline(never)] +#[defmt::panic_handler] +fn panic() -> ! { + cortex_m::asm::udf() +} + +use core::cell::RefCell; +mod data_manager; +use stm32h7xx_hal::gpio::{Pin, Alternate, PA3}; +use heapless::Vec; + +use data_manager::DataManager; + +use chrono::prelude::*; +use cortex_m::{asm, interrupt::Mutex}; use cortex_m_rt::entry; -use defmt::println; -use defmt_rtt as _; -use hal::gpio::Pins; -use hal::pac; -use pac::Peripherals; -use panic_halt as _; - -#[entry] -fn main() -> ! { - let mut peripherals = Peripherals::take().unwrap(); - let p2 = cortex_m::peripheral::Peripherals::take().unwrap(); - - let pins = Pins::new(peripherals.PORT); - let mut led = pins.pa14.into_push_pull_output(); - - // External 32KHz clock for stability - let mut clock = hal::clock::GenericClockController::with_internal_32kosc( - peripherals.GCLK, - &mut peripherals.MCLK, - &mut peripherals.OSC32KCTRL, - &mut peripherals.OSCCTRL, - &mut peripherals.NVMCTRL, - ); - - clock.configure_gclk_divider_and_source( - pac::gclk::pchctrl::GENSELECT_A::GCLK2, - 1, - pac::gclk::genctrl::SRCSELECT_A::DFLL, - false, - ); - println!("Clock configured"); - let mut tx_pin = pins.pa09.into_push_pull_output(); - let mut rx_pin = pins.pa08.into_push_pull_output(); - match rx_pin.set_high() { - Ok(_) => println!("RX pin set high"), - Err(_) => println!("RX pin set low"), - }; - tx_pin.set_high().unwrap(); - - let mut delay = hal::delay::Delay::new(p2.SYST, &mut clock); - - loop { - led.set_high().unwrap(); - delay.delay_ms(1000_u16); - println!("Test"); - led.set_low().unwrap(); - delay.delay_ms(1000_u16); +use defmt::info; + +use pac::interrupt; +use stm32h7xx_hal::{pac, prelude::*, rtc}; +mod sbg_manager; +use sbg_manager::SBGManager; +use sbg_manager::{sbg_dma, sbg_flush, sbg_handle_data, sbg_write_data}; +use sbg_rs::sbg::CallbackData; +use sbg_rs::sbg::SBG_BUFFER_SIZE; +use stm32h7xx_hal::dma::dma::StreamsTuple; +use common_arm::ErrorManager; +static RTC: Mutex>> = Mutex::new(RefCell::new(None)); + +#[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, dispatchers = [EXTI2])] +mod app { + use stm32h7xx_hal::gpio::gpioc::{PC13, PC3}; + use stm32h7xx_hal::gpio::{Edge, ExtiPin, Input, PA2}; + use stm32h7xx_hal::gpio::{Output, PushPull}; + use stm32h7xx_hal::prelude::*; + + use super::*; + + #[shared] + struct SharedResources { + sbg_manager: SBGManager, + em: ErrorManager, + data_manager: DataManager, + } + #[local] + struct LocalResources { + button: PC13, + led: PC3>, + green_led: PA3>, + } + + #[init] + fn init(mut ctx: init::Context) -> (SharedResources, LocalResources, init::Monotonics) { + let pwr = ctx.device.PWR.constrain(); + let pwrcfg = pwr.freeze(); + + // RCC + let rcc = ctx.device.RCC.constrain(); + let ccdr = rcc.sys_ck(100.MHz()).freeze(pwrcfg, &ctx.device.SYSCFG); + + // GPIO + let gpioc = ctx.device.GPIOC.split(ccdr.peripheral.GPIOC); + + // Button + let mut button = gpioc.pc13.into_floating_input(); + button.make_interrupt_source(&mut ctx.device.SYSCFG); + button.trigger_on_edge(&mut ctx.device.EXTI, Edge::Rising); + button.enable_interrupt(&mut ctx.device.EXTI); + + // 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); + // leds + let led_red = gpioa.pa2.into_push_pull_output(); + let green_led = 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_sbg = ctx + .device + .UART4 + .serial((tx, rx), 115_200.bps(), ccdr.peripheral.UART4, &ccdr.clocks) + .unwrap(); + let sbg_manager = sbg_manager::SBGManager::new(uart_sbg, stream_tuple); + + ( + SharedResources { + sbg_manager, + em: ErrorManager::new(), + data_manager: DataManager::new(), + }, + LocalResources { + button, + led: gpioc.pc3.into_push_pull_output(), + green_led, + }, + init::Monotonics(), + ) + } + + #[idle(shared = [data_manager])] + fn idle(mut cx: idle::Context) -> ! { + loop { + info!("Data {}", cx.shared.data_manager.lock(|data| data.clone_sensors())) + } + } + + #[task(binds = EXTI15_10, local = [button, led])] + fn button_click(ctx: button_click::Context) { + ctx.local.button.clear_interrupt_pending_bit(); + ctx.local.led.toggle(); + } + + extern "Rust" { + + #[task(priority = 3, binds = DMA1_STR1, shared = [&em, sbg_manager])] + fn sbg_dma(mut context: sbg_dma::Context); + + #[task(priority = 1, shared = [data_manager])] + fn sbg_handle_data(context: sbg_handle_data::Context, data: CallbackData); + + #[task(priority = 1, shared = [&em, sbg_manager])] + fn sbg_flush(context: sbg_flush::Context); + + #[task(priority = 1, shared = [&em, sbg_manager])] + fn sbg_write_data(context: sbg_write_data::Context, data: Vec); } } diff --git a/examples/simple/src/sbg_manager.rs b/examples/simple/src/sbg_manager.rs new file mode 100644 index 00000000..bdfeecad --- /dev/null +++ b/examples/simple/src/sbg_manager.rs @@ -0,0 +1,252 @@ +use core::alloc::{GlobalAlloc, Layout}; +use core::ffi::c_void; +use core::mem::size_of; +use core::ptr; +// use atsamd_hal::time::*; +use crate::app::sbg_flush; +use crate::app::sbg_handle_data; +// use crate::app::sbg_sd_task as sbg_sd; +use crate::app::sbg_write_data; +use crate::RTC; +use chrono::{NaiveDate, NaiveDateTime, NaiveTime}; +use core::mem::MaybeUninit; +use defmt::{info, panic}; +use embedded_alloc::Heap; +use heapless::Vec; +use messages::mavlink::embedded::{Read, Write}; +use sbg_rs::sbg; +use sbg_rs::sbg::{CallbackData, SBG, SBG_BUFFER_SIZE}; +use stm32h7xx_hal::dma::dma::StreamX; +use stm32h7xx_hal::dma::{ + dma::{DmaConfig, StreamsTuple}, + PeripheralToMemory, Transfer, +}; +use stm32h7xx_hal::pac::UART4; +use stm32h7xx_hal::serial::{Rx, Tx}; +// use cortex_m::{asm}; +use rtic::Mutex; + +//#[link_section = ".axisram.buffers"] +//static mut SBG_BUFFER: MayberUninit<[u8; SBG_BUFFER_SIZE]> = MaybeUninit::uninit(); + +#[link_section = ".axisram.buffers"] +pub static mut SBG_BUFFER: MaybeUninit<[u8; SBG_BUFFER_SIZE]> = MaybeUninit::uninit(); + +// Simple heap required by the SBG library +static HEAP: Heap = Heap::empty(); + +pub struct SBGManager { + sbg_device: SBG, + xfer: Option< + Transfer< + StreamX, + Rx, + stm32h7xx_hal::dma::PeripheralToMemory, + &'static mut [u8; SBG_BUFFER_SIZE], + stm32h7xx_hal::dma::DBTransfer, + >, + >, + sbg_tx: Tx, +} + +impl SBGManager { + pub fn new( + mut serial: stm32h7xx_hal::serial::Serial, + stream_tuple: StreamsTuple, + //mut dma_channel: dmac::Channel, + ) -> Self { + /* Initialize the Heap */ + { + use core::mem::MaybeUninit; + const HEAP_SIZE: usize = 1024; + // TODO: Could add a link section here to memory. + static mut HEAP_MEM: [MaybeUninit; HEAP_SIZE] = [MaybeUninit::uninit(); HEAP_SIZE]; + unsafe { HEAP.init(HEAP_MEM.as_ptr() as usize, HEAP_SIZE) } + } + + let (sbg_tx, mut sbg_rx) = serial.split(); + + // TODO: This could be wrong. It's a bit of a guess. + // let sbg_buffer: &'static mut [u8; SBG_BUFFER_SIZE] = { + // let buf: &mut [MaybeUninit; SBG_BUFFER_SIZE] = + // unsafe { &mut *(core::ptr::addr_of_mut!(SBG_BUFFER) as *mut _) }; + // for (i, value) in buf.iter_mut().enumerate() { + // unsafe { value.as_mut_ptr().write(i as u8) }; + // } + // unsafe { SBG_BUFFER.assume_init_mut() } + // }; + unsafe { + // Convert an uninitialised array into an array of uninitialised + let buf: &mut [core::mem::MaybeUninit; SBG_BUFFER_SIZE] = + &mut *(core::ptr::addr_of_mut!(SBG_BUFFER) as *mut _); + buf.iter_mut().for_each(|x| x.as_mut_ptr().write(0)); + } + + let config = DmaConfig::default().memory_increment(true).transfer_complete_interrupt(true); + let mut transfer: Transfer< + StreamX, + Rx, + PeripheralToMemory, + &mut [u8; SBG_BUFFER_SIZE], + stm32h7xx_hal::dma::DBTransfer, + > = Transfer::init( + stream_tuple.1, + sbg_rx, + unsafe { SBG_BUFFER.assume_init_mut() }, // Uninitialised memory + None, + config, + ); + + + + info!("Starting transfer"); + transfer.start(|serial| { + serial.enable_dma_rx(); + + }); + info!("Transfer started"); + + while !transfer.get_transfer_complete_flag() { + // info!("Transfer not complete"); + } + + let mut sbg: sbg::SBG = sbg::SBG::new( + |data| { + sbg_handle_data::spawn(data).ok(); + }, + |data| { + sbg_write_data::spawn(data).ok(); + }, + || sbg_get_time(), + || { + sbg_flush::spawn().ok(); + }, + ); + sbg.read_data(&unsafe { SBG_BUFFER.assume_init_read() }); + SBGManager { + sbg_device: sbg, + xfer: Some(transfer), + sbg_tx, + } + } +} + +pub fn sbg_flush(cx: sbg_flush::Context<'_>) { + // cx.shared.sbg_manager.lock(|sbg| { + // sbg.sbg_tx + // }); +} +pub fn sbg_write_data(mut cx: sbg_write_data::Context<'_>, data: Vec) { + cx.shared.sbg_manager.lock(|sbg| { + sbg.sbg_tx.write_all(data.as_slice()); + }); +} + +pub fn sbg_get_time() -> u32 { + cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + rtc.date_time() + .unwrap_or(NaiveDateTime::new( + NaiveDate::from_ymd_opt(2024, 1, 1).unwrap(), + NaiveTime::from_hms_milli_opt(0, 0, 0, 0).unwrap(), + )) + .and_utc() + .timestamp_subsec_millis() + }) +} + +pub fn sbg_handle_data(mut cx: sbg_handle_data::Context<'_>, data: CallbackData) { + cx.shared.data_manager.lock(|manager| match data { + CallbackData::UtcTime(x) => manager.utc_time = Some(x), + CallbackData::Air(x) => manager.air = Some(x), + CallbackData::EkfQuat(x) => manager.ekf_quat = Some(x), + CallbackData::EkfNav(x) => manager.ekf_nav = Some(x), + CallbackData::Imu(x) => manager.imu = Some(x), + CallbackData::GpsVel(x) => manager.gps_vel = Some(x), + CallbackData::GpsPos(x) => manager.gps_pos = Some(x), + }); +} + +/** + * Handles the DMA interrupt. + * Handles the SBG data. + */ +pub fn sbg_dma(mut cx: crate::app::sbg_dma::Context) { + info!("DMA"); + cx.shared.sbg_manager.lock(|sbg| { + match &mut sbg.xfer { + Some(xfer) => { + if xfer.get_transfer_complete_flag() { + let data = unsafe { SBG_BUFFER.assume_init_read() }; + xfer.clear_transfer_complete_interrupt(); + xfer.next_transfer( + unsafe { (*core::ptr::addr_of_mut!(SBG_BUFFER)).assume_init_mut() }, // Uninitialised memory + ); + sbg.sbg_device.read_data(&data); + } + } + None => { + // it should be impossible to reach here. + info!("None"); + } + } + }); +} + +/// Stored right before an allocation. Stores information that is needed to deallocate memory. +#[derive(Copy, Clone)] +struct AllocInfo { + layout: Layout, + ptr: *mut u8, +} + +/// Custom malloc for the SBG library. This uses the HEAP object initialized at the start of the +/// [`SBGManager`]. The [`Layout`] of the allocation is stored right before the returned pointed, +/// which makes it possible to implement [`free`] without any other data structures. +#[no_mangle] +pub extern "C" fn malloc(size: usize) -> *mut c_void { + if size == 0 { + return ptr::null_mut(); + } + + // Get a layout for both the requested size + let header_layout = Layout::new::(); + let requested_layout = Layout::from_size_align(size, 8).unwrap(); + let (layout, offset) = header_layout.extend(requested_layout).unwrap(); + + // Ask the allocator for memory + let orig_ptr = unsafe { HEAP.alloc(layout) }; + if orig_ptr.is_null() { + return orig_ptr as *mut c_void; + } + + // Compute the pointer that we will return + let result_ptr = unsafe { orig_ptr.add(offset) }; + + // Store the allocation information right before the returned pointer + let info_ptr = unsafe { result_ptr.sub(size_of::()) as *mut AllocInfo }; + unsafe { + info_ptr.write_unaligned(AllocInfo { + layout, + ptr: orig_ptr, + }); + } + + result_ptr as *mut c_void +} + +/// Custom free implementation for the SBG library. This uses the stored allocation information +/// right before the pointer to free up the resources. +/// +/// SAFETY: The value passed to ptr must have been obtained from a previous call to [`malloc`]. +#[no_mangle] +pub unsafe extern "C" fn free(ptr: *mut c_void) { + assert!(!ptr.is_null()); + + let info_ptr = unsafe { ptr.sub(size_of::()) as *const AllocInfo }; + let info = unsafe { info_ptr.read_unaligned() }; + unsafe { + HEAP.dealloc(info.ptr, info.layout); + } +} diff --git a/libraries/common-arm-stm32h7/memory.x b/libraries/common-arm-stm32h7/memory.x index eb063383..428106a9 100644 --- a/libraries/common-arm-stm32h7/memory.x +++ b/libraries/common-arm-stm32h7/memory.x @@ -22,12 +22,11 @@ MEMORY RAM : ORIGIN = 0x20000000, LENGTH = 128K /* AXISRAM */ - AXISRAM : ORIGIN = 0x24000000, LENGTH = 512K + AXISRAM : ORIGIN = 0x24000000, LENGTH = 128K /* SRAM */ - SRAM1 : ORIGIN = 0x30000000, LENGTH = 128K - SRAM2 : ORIGIN = 0x30020000, LENGTH = 128K - SRAM3 : ORIGIN = 0x30040000, LENGTH = 32K + SRAM1 : ORIGIN = 0x30000000, LENGTH = 64K + SRAM2 : ORIGIN = 0x30004000, LENGTH = 64K SRAM4 : ORIGIN = 0x38000000, LENGTH = 64K /* Backup SRAM */ @@ -53,10 +52,14 @@ SECTIONS { } > AXISRAM /* The SRAM1 and SRAM2 section are commonly used as the stack and heap for the CM4 core in dualcore versions and should thus not be used in examples*/ - .sram3 (NOLOAD) : ALIGN(4) { - *(.sram3 .sram3.*); + .sram1 (NOLOAD) : ALIGN(4) { + *(.sram1 .sram1.*); . = ALIGN(4); - } > SRAM3 + } > SRAM1 + .sram2 (NOLOAD) : ALIGN(4) { + *(.sram2 .sram2.*); + . = ALIGN(4); + } > SRAM2 .sram4 (NOLOAD) : ALIGN(4) { *(.sram4 .sram4.*); . = ALIGN(4); diff --git a/libraries/sbg-rs/Cargo.toml b/libraries/sbg-rs/Cargo.toml index cb2bd91e..669cc3e0 100644 --- a/libraries/sbg-rs/Cargo.toml +++ b/libraries/sbg-rs/Cargo.toml @@ -9,7 +9,6 @@ edition = "2021" embedded-hal = "0.2.7" nb = "1.0.0" defmt = "0.3.2" -atsamd-hal = { workspace = true } cortex-m = { workspace = true } serde = { workspace = true } diff --git a/libraries/sbg-rs/src/sbg.rs b/libraries/sbg-rs/src/sbg.rs index c4072174..6716ad04 100644 --- a/libraries/sbg-rs/src/sbg.rs +++ b/libraries/sbg-rs/src/sbg.rs @@ -11,19 +11,15 @@ use crate::bindings::{ _SbgEComLog_SBG_ECOM_LOG_EKF_QUAT, _SbgEComLog_SBG_ECOM_LOG_IMU_DATA, _SbgEComOutputPort_SBG_ECOM_OUTPUT_PORT_A, _SbgEComProtocol, _SbgErrorCode, _SbgInterface, }; -use atsamd_hal as hal; use core::ffi::c_void; use core::ptr::null_mut; use core::slice::{from_raw_parts, from_raw_parts_mut}; use core::sync::atomic::AtomicUsize; -use defmt::{flush, warn}; +use defmt::{flush, debug, info, warn, error}; use embedded_hal::serial::Write; -use hal::gpio::{PB02, PB03, PB16, PB17}; -use hal::sercom::uart::Duplex; -use hal::sercom::uart::{self, EightBit, Uart}; -use hal::sercom::{IoSet1, IoSet6, Sercom5}; use heapless::Deque; use heapless::Vec; +use core::ffi::CStr; use messages::sensor::*; /** @@ -81,11 +77,17 @@ impl SBG { rtc_get_time: fn() -> u32, serial_flush_callback: fn(), ) -> Self { + unsafe { + DATA_CALLBACK = Some(callback); + SERIAL_WRITE_CALLBACK = Some(serial_write_callback); + RTC_GET_TIME = Some(rtc_get_time); + SERIAL_FLUSH_CALLBACK = Some(serial_flush_callback); + } // SAFETY: We are assigning the RTC instance to a static variable. // This is safe because we are the only ones who have access to it. let interface = UARTSBGInterface { interface: &mut _SbgInterface { - handle: null_mut() as *mut c_void, // SAFEY: No idea what I just did. + handle: null_mut(), // SAFEY: No idea what I just did. type_: 0, name: [0; 48], pDestroyFunc: Some(SBG::SbgDestroyFunc), @@ -120,12 +122,7 @@ impl SBG { cmdDefaultTimeOut: 500, }; - unsafe { - DATA_CALLBACK = Some(callback); - SERIAL_WRITE_CALLBACK = Some(serial_write_callback); - RTC_GET_TIME = Some(rtc_get_time); - SERIAL_FLUSH_CALLBACK = Some(serial_flush_callback); - } + let isInitialized = false; @@ -149,6 +146,7 @@ impl SBG { // SAFETY: We are assigning a static mut variable. // Buf can only be accessed from functions called by sbgEComHandle after this assignment. // unsafe { BUF = buffer }; + info!("Reading SBG Data"); for i in buffer { unsafe { match DEQ.push_back(*i) { @@ -450,31 +448,33 @@ unsafe impl Send for SBG {} */ #[no_mangle] pub unsafe extern "C" fn sbgPlatformDebugLogMsg( - _pFileName: *const ::core::ffi::c_char, - _pFunctionName: *const ::core::ffi::c_char, - _line: u32, - _pCategory: *const ::core::ffi::c_char, + pFileName: *const ::core::ffi::c_char, + pFunctionName: *const ::core::ffi::c_char, + line: u32, + pCategory: *const ::core::ffi::c_char, logType: _SbgDebugLogType, - _errorCode: _SbgErrorCode, - _pFormat: *const ::core::ffi::c_char, + errorCode: _SbgErrorCode, + pFormat: *const ::core::ffi::c_char, ) { - // if pFileName.is_null() || pFunctionName.is_null() || pCategory.is_null() || pFormat.is_null() { - // return; - // } + if pFileName.is_null() || pFunctionName.is_null() || pCategory.is_null() || pFormat.is_null() { + return; + } // // SAFETY: We are converting a raw pointer to a CStr and then to a str. // // This is safe because we check if the pointers are null and // // the pointers can only be accessed during the lifetime of this function. - // let file = unsafe { CStr::from_ptr(pFileName).to_str().unwrap() }; - // let function = unsafe { CStr::from_ptr(pFunctionName).to_str().unwrap() }; - // let _category = unsafe { CStr::from_ptr(pCategory).to_str().unwrap() }; - // let _format = unsafe { CStr::from_ptr(pFormat).to_str().unwrap() }; + let file = unsafe { CStr::from_ptr(pFileName).to_str().unwrap() }; + let function = unsafe { CStr::from_ptr(pFunctionName).to_str().unwrap() }; + let category = unsafe { CStr::from_ptr(pCategory).to_str().unwrap() }; + let format = unsafe { CStr::from_ptr(pFormat).to_str().unwrap() }; + + info!("{}:{}:{}:{}:{}:{}", file, function, line, category, errorCode, format); match logType { // silently handle errors - // _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_ERROR => error!("SBG Error"), + _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_ERROR => error!("SBG Error"), _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_WARNING => warn!("SBG Warning"), - // _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_INFO => info!("SBG Info {} {}", file, function), - // _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_DEBUG => debug!("SBG Debug {} {}", file, function), + _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_INFO => info!("SBG Info "), + _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_DEBUG => debug!("SBG Debug "), _ => (), }; flush(); From 64b0e8bc983239c62f0e53051b35d43a3a5bd272 Mon Sep 17 00:00:00 2001 From: NoahSprenger Date: Thu, 8 Aug 2024 17:15:07 +0000 Subject: [PATCH 14/47] Add NAV CAN-FD netowrks and implement channels for message passing. --- Cargo.lock | 27 +++++ boards/communication/src/main.rs | 2 +- boards/nav/Cargo.toml | 2 +- boards/nav/src/communication.rs | 52 +++++++++- boards/nav/src/data_manager.rs | 17 ++- boards/nav/src/main.rs | 171 ++++++++++++++++++++++++++----- boards/nav/src/sbg_manager.rs | 3 + 7 files changed, 241 insertions(+), 33 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 891a04e2..f044be1c 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -536,6 +536,17 @@ dependencies = [ "embedded-hal 1.0.0", ] +[[package]] +name = "embedded-hal-bus" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "57b4e6ede84339ebdb418cd986e6320a34b017cdf99b5cc3efceec6450b06886" +dependencies = [ + "critical-section", + "embedded-hal 1.0.0", + "embedded-hal-async", +] + [[package]] name = "embedded-io" version = "0.4.0" @@ -969,6 +980,7 @@ dependencies = [ "postcard", "rtic", "rtic-monotonics", + "rtic-sync", "sbg-rs", "stm32h7xx-hal", "systick-monotonic", @@ -1403,6 +1415,21 @@ dependencies = [ "stm32-metapac", ] +[[package]] +name = "rtic-sync" +version = "1.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "49b1200137ccb2bf272a1801fa6e27264535facd356cb2c1d5bc8e12aa211bad" +dependencies = [ + "critical-section", + "embedded-hal 1.0.0", + "embedded-hal-async", + "embedded-hal-bus", + "heapless 0.8.0", + "portable-atomic", + "rtic-common", +] + [[package]] name = "rtic-syntax" version = "1.0.3" diff --git a/boards/communication/src/main.rs b/boards/communication/src/main.rs index ba266515..5c28ea6f 100644 --- a/boards/communication/src/main.rs +++ b/boards/communication/src/main.rs @@ -306,7 +306,7 @@ mod app { // }); // } - /// Handles the CAN0 interrupt. + /// Handles the CAN1 interrupt. #[task(priority = 3, binds = CAN1, shared = [can_command_manager, data_manager])] fn can_command(mut cx: can_command::Context) { cx.shared.can_command_manager.lock(|can| { diff --git a/boards/nav/Cargo.toml b/boards/nav/Cargo.toml index 1a03a8db..6cf4812a 100644 --- a/boards/nav/Cargo.toml +++ b/boards/nav/Cargo.toml @@ -22,7 +22,7 @@ fdcan = "0.2" sbg-rs = {path = "../../libraries/sbg-rs"} embedded-alloc = "0.5.0" heapless = "0.7.16" - +rtic-sync = "1.3.0" defmt-rtt = "0.4" panic-probe = { version = "0.3", features = ["print-defmt"] } chrono = {version = "0.4.0", default-features = false} \ No newline at end of file diff --git a/boards/nav/src/communication.rs b/boards/nav/src/communication.rs index 0f073bb2..f8689965 100644 --- a/boards/nav/src/communication.rs +++ b/boards/nav/src/communication.rs @@ -12,14 +12,14 @@ use stm32h7xx_hal::prelude::*; /// Clock configuration is out of scope for this builder /// easiest way to avoid alloc is to use no generics -pub struct CanDevice { +pub struct CanCommandManager { can: fdcan::FdCan< stm32h7xx_hal::can::Can, fdcan::NormalOperationMode, >, } -impl CanDevice { +impl CanCommandManager { pub fn new( can: fdcan::FdCan< stm32h7xx_hal::can::Can, @@ -41,6 +41,54 @@ impl CanDevice { self.can.transmit(header, &payload)?; Ok(()) } + pub fn process_data(&mut self, data_manager: &mut DataManager) -> Result<(), HydraError> { + let mut buf = [0u8; 64]; + for message in self.can.receive0(&mut buf) { + match from_bytes::(&buf) { + Ok(data) => { + data_manager.handle_command(data)?; + } + Err(e) => { + info!("Error: {:?}", e) + } + } + } + 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::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]; for message in self.can.receive0(&mut buf) { diff --git a/boards/nav/src/data_manager.rs b/boards/nav/src/data_manager.rs index 29868be6..09a0ef2a 100644 --- a/boards/nav/src/data_manager.rs +++ b/boards/nav/src/data_manager.rs @@ -47,9 +47,24 @@ impl DataManager { } pub fn handle_data(&mut self, data: Message) -> Result<(), HydraError> { + match data.data { + messages::Data::Sensor(sensor) => match sensor.data { + _ => { + } + }, + _ => { + // we can disregard all other messages for now. + } + } + Ok(()) + } + + pub fn handle_command(&mut self, data: Message) -> Result<(), HydraError> { match data.data { messages::Data::Command(command) => match command.data { - messages::command::CommandData::PowerDown(_) => {} + messages::command::CommandData::PowerDown(_) => { + crate::app::sleep_system::spawn().ok(); + } _ => { // We don't care atm about these other commands. } diff --git a/boards/nav/src/main.rs b/boards/nav/src/main.rs index 17cfed7d..ed1d0eab 100644 --- a/boards/nav/src/main.rs +++ b/boards/nav/src/main.rs @@ -8,6 +8,7 @@ mod types; use chrono::{NaiveDate, NaiveDateTime, NaiveTime}; use common_arm::*; +use communication::{CanCommandManager, CanDataManager}; use core::cell::RefCell; use core::num::{NonZeroU16, NonZeroU8}; use cortex_m::interrupt::Mutex; @@ -21,19 +22,24 @@ use fdcan::{ use heapless::Vec; use messages::Data; use panic_probe as _; +use rtic_monotonics::systick::prelude::*; +use rtic_sync::{channel::*, make_channel}; use sbg_manager::{sbg_dma, sbg_flush, sbg_handle_data, sbg_sd_task, sbg_write_data}; use sbg_rs::sbg::CallbackData; use sbg_rs::sbg::SBG_BUFFER_SIZE; use stm32h7xx_hal::dma::dma::StreamsTuple; use stm32h7xx_hal::gpio::gpioa::{PA2, PA3, PA4}; +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::spi; +use messages::sensor::Sensor; use stm32h7xx_hal::{rcc, rcc::rec}; use types::COM_ID; // global logger -use rtic_monotonics::systick::prelude::*; + +const DATA_CHANNEL_CAPACITY: usize = 15; systick_monotonic!(Mono, 100); @@ -47,6 +53,7 @@ static RTC: Mutex>> = Mutex::new(RefCell::new(None)); #[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, dispatchers = [EXTI0, EXTI1, EXTI2])] mod app { + use messages::Message; use stm32h7xx_hal::gpio::{Alternate, Pin}; use super::*; @@ -60,15 +67,21 @@ mod app { PA4>, >, sbg_manager: sbg_manager::SBGManager, + can_command_manager: CanCommandManager, + can_data_manager: CanDataManager, } #[local] struct LocalResources { led_red: PA2>, led_green: PA3>, + sbg_power: PB4>, } #[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 */ @@ -84,6 +97,10 @@ mod app { info!("Backup domain enabled"); // RCC let rcc = ctx.device.RCC.constrain(); + let fdcan_prec_unsafe = unsafe { rcc.steal_peripheral_rec() } + .FDCAN + .kernel_clk_mux(rec::FdcanClkSel::Pll1Q); + info!("RCC enabled"); let ccdr = rcc .use_hse(48.MHz()) // check the clock hardware @@ -92,6 +109,10 @@ mod app { .pll1_q_ck(24.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(4).unwrap(), @@ -119,12 +140,9 @@ mod app { info!("PWM enabled"); // assert_eq!(ccdr.clocks.pll1_q_ck().unwrap().raw(), 24_000_000); waaat info!("PLL1Q:"); - let fdcan_prec = ccdr - .peripheral - .FDCAN - .kernel_clk_mux(rec::FdcanClkSel::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 can1: fdcan::FdCan< + let can2: fdcan::FdCan< stm32h7xx_hal::can::Can, fdcan::ConfigMode, > = { @@ -133,16 +151,50 @@ mod app { ctx.device.FDCAN2.fdcan(tx, rx, fdcan_prec) }; - let mut can = can1; - can.set_protocol_exception_handling(false); + let mut can_command = can2; + 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(), + ); + + let config = can_command + .get_config() + .set_frame_transmit(fdcan::config::FrameTransmissionConfig::AllowFdCanAndBRS); + can_command.apply_config(config); + + let can_command_manager = CanCommandManager::new(can_command.into_normal()); + + /// SAFETY: This is done as a result of a single memory mapped bit in hardware. Safe in this context. + 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_data = can1; + can_data.set_protocol_exception_handling(false); - can.set_nominal_bit_timing(btr); + can_data.set_nominal_bit_timing(btr); - can.set_standard_filter( + can_data.set_standard_filter( StandardFilterSlot::_0, StandardFilter::accept_all_into_fifo0(), ); + let config = can_data + .get_config() + .set_frame_transmit(fdcan::config::FrameTransmissionConfig::AllowFdCanAndBRS); // check this maybe don't bit switch allow. + can_data.apply_config(config); + + let can_data_manager = CanDataManager::new(can_data.into_normal()); + let spi_sd: stm32h7xx_hal::spi::Spi< stm32h7xx_hal::stm32::SPI1, stm32h7xx_hal::spi::Enabled, @@ -174,7 +226,6 @@ mod app { // 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_sbg = ctx @@ -207,7 +258,7 @@ mod app { // let mono = Systick::new(core.SYST, ccdr.clocks.sysclk().to_Hz()); // blink::spawn().ok(); // display_data::spawn().ok(); - Mono::start(core.SYST, 100_000_000); + Mono::start(core.SYST, 100_000_000); info!("Online"); let data_manager = DataManager::new(); @@ -215,6 +266,7 @@ mod app { // let mono_token = rtic_monotonics::create_systick_token!(); // let mono = Systick::start(ctx.core.SYST, 36_000_000, mono_token); blink::spawn().ok(); + display_data::spawn(s).ok(); ( SharedResources { @@ -222,8 +274,14 @@ mod app { em, sd_manager, sbg_manager, + can_command_manager, + can_data_manager, + }, + LocalResources { + led_red, + led_green, + sbg_power, }, - LocalResources { led_red, led_green }, ) } @@ -231,20 +289,44 @@ mod app { fn idle(ctx: idle::Context) -> ! { loop { // info!("Idle"); - cortex_m::asm::wfi(); + cortex_m::asm::wfi(); // could case issue with CAN Bus. Was an issue with ATSAME51. } } #[task(priority = 2, shared = [&em, data_manager])] - async fn display_data(mut cx: display_data::Context) { - info!("Displaying data"); - let data = cx - .shared - .data_manager - .lock(|manager| manager.clone_sensors()); - info!("{:?}", data); - Mono::delay(1000.millis()).await; - // spawn_after!(display_data, ExtU64::secs(1)).ok(); + async fn display_data( + mut cx: display_data::Context, + mut sender: Sender<'static, Message, DATA_CHANNEL_CAPACITY>, + ) { + loop { + info!("Displaying data"); + let data = cx + .shared + .data_manager + .lock(|manager| manager.clone_sensors()); + info!("{:?}", data.clone()); + let messages = data.into_iter().flatten().map(|x| { + Message::new( + cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + rtc.date_time() + .unwrap_or(NaiveDateTime::new( + NaiveDate::from_ymd_opt(2024, 1, 1).unwrap(), + NaiveTime::from_hms_milli_opt(0, 0, 0, 0).unwrap(), + )) + .and_utc() + .timestamp_subsec_millis() + }), + COM_ID, + Sensor::new(x), + ) + }); + for msg in messages { + sender.send(msg).await.ok(); + } + Mono::delay(1000.millis()).await; // what if there was no delay and we used chanenls to control the rate of messages. + } } /// Receives a log message from the custom logger so that it can be sent over the radio. @@ -269,17 +351,49 @@ mod app { // send_in::spawn(message).ok(); } + #[task(priority = 3, binds = FDCAN1_IT0, shared = [can_command_manager, data_manager])] + fn can_command(mut cx: can_command::Context) { + cx.shared.can_command_manager.lock(|can| { + cx.shared + .data_manager + .lock(|data_manager| can.process_data(data_manager)); + }); + } + + // Might be the wrong interrupt + #[task(priority = 3, binds = FDCAN2_IT0, shared = [can_data_manager, data_manager])] + fn can_data(mut cx: can_data::Context) { + cx.shared.can_data_manager.lock(|can| { + cx.shared + .data_manager + .lock(|data_manager| can.process_data(data_manager)); + }); + } + + #[task(priority = 2, shared = [can_data_manager, data_manager])] + async fn send_data_internal(mut cx: send_data_internal::Context, m: Message) { + cx.shared.can_data_manager.lock(|can| { + can.send_message(m); + }); + } + + #[task(priority = 3, shared = [can_command_manager, data_manager])] + async fn send_command_internal(mut cx: send_command_internal::Context, mut receiver: Receiver<'static, Message, DATA_CHANNEL_CAPACITY>) { + while let Ok(m) = receiver.recv().await { + cx.shared.can_command_manager.lock(|can| { + can.send_message(m); + }); + } + } + #[task(priority = 1, local = [led_red, led_green], shared = [&em])] async fn blink(cx: blink::Context) { loop { - info!("Blinking"); cx.shared.em.run(|| { if cx.shared.em.has_error() { cx.local.led_red.toggle(); - info!("Error"); } else { cx.local.led_green.toggle(); - info!("Blinking"); } Ok(()) }); @@ -287,9 +401,10 @@ mod app { } } - #[task(priority = 1, shared = [&em])] + #[task(priority = 1, local = [sbg_power], shared = [&em])] async fn sleep_system(cx: sleep_system::Context) { - // Turn off the SBG and CAN + // Turn off the SBG and CAN, also start a timer to wake up the system. Put the chip in sleep mode. + cx.local.sbg_power.set_low(); } extern "Rust" { diff --git a/boards/nav/src/sbg_manager.rs b/boards/nav/src/sbg_manager.rs index c97bb000..e83ffb23 100644 --- a/boards/nav/src/sbg_manager.rs +++ b/boards/nav/src/sbg_manager.rs @@ -91,6 +91,9 @@ impl SBGManager { config, ); info!("Starting transfer"); + + // Could this be unsafe because what happens if the interrupt fires before the object is created which is used in the interrupt handler. + transfer.start(|serial| { serial.enable_dma_rx(); From 9aa725620da70ec72c82b441fac38ba32ee3492a Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Thu, 8 Aug 2024 20:03:28 -0400 Subject: [PATCH 15/47] WIP: Abort mailbox to prevent blocking forever on CAN. --- .cargo/config.toml | 4 +- boards/communication/src/communication.rs | 7 ++ boards/communication/src/main.rs | 12 ++- boards/nav/src/communication.rs | 26 +++-- boards/nav/src/main.rs | 112 ++++++++++++++-------- boards/nav/src/sbg_manager.rs | 22 +++-- boards/recovery/src/communication.rs | 5 +- boards/recovery/src/main.rs | 2 + libraries/sbg-rs/src/sbg.rs | 1 - 9 files changed, 131 insertions(+), 60 deletions(-) diff --git a/.cargo/config.toml b/.cargo/config.toml index 4951091d..e3b9e473 100644 --- a/.cargo/config.toml +++ b/.cargo/config.toml @@ -1,7 +1,7 @@ [target.'cfg(all(target_arch = "arm", target_os = "none"))'] # This runner needs to be parametric so we can pass in the chip name -runner = "probe-rs run --chip STM32H733VGTx --protocol swd" -#runner = "probe-rs run --chip ATSAME51J18A --protocol swd" +#runner = "probe-rs run --chip STM32H733VGTx --protocol swd" +runner = "probe-rs run --chip ATSAME51J18A --protocol swd" rustflags = [ "-C", "link-arg=-Tlink.x", "-C", "link-arg=-Tdefmt.x", diff --git a/boards/communication/src/communication.rs b/boards/communication/src/communication.rs index 347628f5..58e3c232 100644 --- a/boards/communication/src/communication.rs +++ b/boards/communication/src/communication.rs @@ -8,6 +8,7 @@ use atsamd_hal::clock::v2::pclk::PclkToken; use atsamd_hal::clock::v2::types::{Can0, Can1}; use atsamd_hal::clock::v2::Source; use atsamd_hal::gpio::PA09; +use defmt::info; use atsamd_hal::gpio::{ Alternate, AlternateI, Disabled, Floating, Pin, I, PA12, PA13, PA22, PA23, PB16, PB17, }; @@ -208,6 +209,8 @@ impl CanDevice0 { for message in &mut self.can.rx_fifo_0 { match from_bytes::(message.data()) { Ok(data) => { + info!("Received message {}", data.clone()); + data_manager.handle_data(data); } Err(_) => { @@ -220,6 +223,8 @@ impl CanDevice0 { for message in &mut self.can.rx_fifo_1 { match from_bytes::(message.data()) { Ok(data) => { + info!("Received message {}", data.clone()); + data_manager.handle_data(data); } Err(_) => { @@ -368,6 +373,7 @@ impl CanCommandManager { for message in &mut self.can.rx_fifo_0 { match from_bytes::(message.data()) { Ok(data) => { + info!("Received message {}", data.clone()); data_manager.handle_command(data); } Err(_) => { @@ -380,6 +386,7 @@ impl CanCommandManager { for message in &mut self.can.rx_fifo_1 { match from_bytes::(message.data()) { Ok(data) => { + info!("Received message {}", data.clone()); data_manager.handle_command(data); } Err(_) => { diff --git a/boards/communication/src/main.rs b/boards/communication/src/main.rs index 5c28ea6f..508ed88a 100644 --- a/boards/communication/src/main.rs +++ b/boards/communication/src/main.rs @@ -269,8 +269,8 @@ mod app { led_green.set_low(); led_red.set_low(); blink::spawn().ok(); - generate_random_messages::spawn().ok(); - generate_random_command::spawn().ok(); + // generate_random_messages::spawn().ok(); + // generate_random_command::spawn().ok(); let mono = Systick::new(core.SYST, gclk0.freq().to_Hz()); // info!("Init done"); @@ -309,6 +309,7 @@ mod app { /// Handles the CAN1 interrupt. #[task(priority = 3, binds = CAN1, shared = [can_command_manager, data_manager])] fn can_command(mut cx: can_command::Context) { + info!("CAN1 interrupt"); cx.shared.can_command_manager.lock(|can| { cx.shared .data_manager @@ -319,6 +320,7 @@ mod app { /// Handles the CAN0 interrupt. #[task(priority = 3, binds = CAN0, shared = [can0, data_manager])] fn can0(mut cx: can0::Context) { + info!("CAN0 interrupt"); cx.shared.can0.lock(|can| { cx.shared .data_manager @@ -330,6 +332,7 @@ mod app { */ #[task(capacity = 10, shared = [can0, &em])] fn send_internal(mut cx: send_internal::Context, m: Message) { + info!("{}", m.clone()); cx.shared.em.run(|| { cx.shared.can0.lock(|can| can.send_message(m))?; Ok(()) @@ -442,11 +445,12 @@ mod app { COM_ID, State::new(StateData::Initializing), ); - spawn!(send_gs, message.clone())?; + // spawn!(send_gs, message.clone())?; spawn!(send_internal, message)?; Ok(()) }); - spawn!(generate_random_messages).ok(); + spawn_after!(generate_random_messages, ExtU64::millis(200)).ok(); + } #[task(shared = [&em])] diff --git a/boards/nav/src/communication.rs b/boards/nav/src/communication.rs index f8689965..0a9b2549 100644 --- a/boards/nav/src/communication.rs +++ b/boards/nav/src/communication.rs @@ -14,7 +14,7 @@ use stm32h7xx_hal::prelude::*; /// easiest way to avoid alloc is to use no generics pub struct CanCommandManager { can: fdcan::FdCan< - stm32h7xx_hal::can::Can, + stm32h7xx_hal::can::Can, fdcan::NormalOperationMode, >, } @@ -22,7 +22,7 @@ pub struct CanCommandManager { impl CanCommandManager { pub fn new( can: fdcan::FdCan< - stm32h7xx_hal::can::Can, + stm32h7xx_hal::can::Can, fdcan::NormalOperationMode, >, ) -> Self { @@ -46,6 +46,7 @@ impl CanCommandManager { for message in self.can.receive0(&mut buf) { match from_bytes::(&buf) { Ok(data) => { + info!("Received message {}", data.clone()); data_manager.handle_command(data)?; } Err(e) => { @@ -62,7 +63,7 @@ impl CanCommandManager { /// easiest way to avoid alloc is to use no generics pub struct CanDataManager { can: fdcan::FdCan< - stm32h7xx_hal::can::Can, + stm32h7xx_hal::can::Can, fdcan::NormalOperationMode, >, } @@ -70,7 +71,7 @@ pub struct CanDataManager { impl CanDataManager { pub fn new( can: fdcan::FdCan< - stm32h7xx_hal::can::Can, + stm32h7xx_hal::can::Can, fdcan::NormalOperationMode, >, ) -> Self { @@ -82,11 +83,22 @@ impl CanDataManager { 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, + frame_format: FrameFormat::Fdcan, bit_rate_switching: false, marker: None, }; - self.can.transmit(header, &payload)?; + self.can.abort(fdcan::Mailbox::_2); + // info!("{}", self.can.); + match stm32h7xx_hal::nb::block!(self.can.transmit(header, &payload)) { // must block or messages can fail to send. + Ok(_) => { + // info!("Message sent"); + + } + Err(e) => { + info!("Error {}", m); + } + } + Ok(()) } pub fn process_data(&mut self, data_manager: &mut DataManager) -> Result<(), HydraError> { @@ -94,6 +106,7 @@ impl CanDataManager { for message in self.can.receive0(&mut buf) { match from_bytes::(&buf) { Ok(data) => { + info!("Received message {}", data.clone()); data_manager.handle_data(data)?; } Err(e) => { @@ -101,6 +114,7 @@ impl CanDataManager { } } } + self.can.clear_interrupt(fdcan::interrupt::Interrupt::RxFifo0NewMsg); Ok(()) } } diff --git a/boards/nav/src/main.rs b/boards/nav/src/main.rs index ed1d0eab..66a2e0cb 100644 --- a/boards/nav/src/main.rs +++ b/boards/nav/src/main.rs @@ -16,10 +16,11 @@ use data_manager::DataManager; use defmt::info; use defmt_rtt as _; use fdcan::{ - config::NominalBitTiming, + config::{NominalBitTiming, DataBitTiming}, filter::{StandardFilter, StandardFilterSlot}, }; use heapless::Vec; +use messages::sensor::Sensor; use messages::Data; use panic_probe as _; use rtic_monotonics::systick::prelude::*; @@ -35,13 +36,12 @@ use stm32h7xx_hal::gpio::{Output, PushPull}; use stm32h7xx_hal::prelude::*; use stm32h7xx_hal::rtc; use stm32h7xx_hal::spi; -use messages::sensor::Sensor; use stm32h7xx_hal::{rcc, rcc::rec}; use types::COM_ID; // global logger -const DATA_CHANNEL_CAPACITY: usize = 15; +const DATA_CHANNEL_CAPACITY: usize = 10; -systick_monotonic!(Mono, 100); +systick_monotonic!(Mono, 500); #[inline(never)] #[defmt::panic_handler] @@ -51,7 +51,7 @@ fn panic() -> ! { static RTC: Mutex>> = Mutex::new(RefCell::new(None)); -#[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, dispatchers = [EXTI0, EXTI1, EXTI2])] +#[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}; @@ -115,11 +115,21 @@ mod app { .kernel_clk_mux(rec::FdcanClkSel::Pll1Q); let btr = NominalBitTiming { - prescaler: NonZeroU16::new(4).unwrap(), + prescaler: NonZeroU16::new(3).unwrap(), seg1: NonZeroU8::new(13).unwrap(), seg2: NonZeroU8::new(2).unwrap(), - sync_jump_width: NonZeroU8::new(1).unwrap(), + sync_jump_width: NonZeroU8::new(0x4).unwrap(), }; + + // let data_bit_timing = DataBitTiming { + // prescaler: NonZeroU8::new(48).unwrap(), + // seg1: NonZeroU8::new(0xB).unwrap(), + // seg2: NonZeroU8::new(0x4).unwrap(), + // sync_jump_width: NonZeroU8::new(0x4).unwrap(), + // transceiver_delay_compensation: true, + // }; + + info!("CAN enabled"); // GPIO let gpioc = ctx.device.GPIOC.split(ccdr.peripheral.GPIOC); @@ -151,22 +161,27 @@ mod app { ctx.device.FDCAN2.fdcan(tx, rx, fdcan_prec) }; - let mut can_command = can2; - can_command.set_protocol_exception_handling(false); + let mut can_data = can2; + can_data.set_protocol_exception_handling(false); - can_command.set_nominal_bit_timing(btr); + can_data.set_nominal_bit_timing(btr); - can_command.set_standard_filter( + // can_command.set_data_bit_timing(data_bit_timing); + + can_data.set_standard_filter( StandardFilterSlot::_0, StandardFilter::accept_all_into_fifo0(), ); - let config = can_command + let config = can_data .get_config() - .set_frame_transmit(fdcan::config::FrameTransmissionConfig::AllowFdCanAndBRS); - can_command.apply_config(config); + .set_frame_transmit(fdcan::config::FrameTransmissionConfig::AllowFdCan); + can_data.apply_config(config); - let can_command_manager = CanCommandManager::new(can_command.into_normal()); + can_data.enable_interrupt(fdcan::interrupt::Interrupt::RxFifo0NewMsg); + + can_data.enable_interrupt_line(fdcan::interrupt::InterruptLine::_0, true); + let can_data_manager = CanDataManager::new(can_data.into_normal()); /// SAFETY: This is done as a result of a single memory mapped bit in hardware. Safe in this context. let can1: fdcan::FdCan< @@ -178,22 +193,28 @@ mod app { ctx.device.FDCAN1.fdcan(tx, rx, fdcan_prec_unsafe) }; - let mut can_data = can1; - can_data.set_protocol_exception_handling(false); + let mut can_command = can1; + can_command.set_protocol_exception_handling(true); - can_data.set_nominal_bit_timing(btr); + can_command.set_nominal_bit_timing(btr); - can_data.set_standard_filter( + can_command.set_standard_filter( StandardFilterSlot::_0, StandardFilter::accept_all_into_fifo0(), ); - - let config = can_data + // can_data.set_data_bit_timing(data_bit_timing); + + let config = can_command .get_config() .set_frame_transmit(fdcan::config::FrameTransmissionConfig::AllowFdCanAndBRS); // check this maybe don't bit switch allow. - can_data.apply_config(config); + can_command.apply_config(config); + can_command.enable_interrupt(fdcan::interrupt::Interrupt::RxFifo0NewMsg); + + can_command.enable_interrupt_line(fdcan::interrupt::InterruptLine::_0, true); + + let can_command_manager = CanCommandManager::new(can_command.into_normal()); + - let can_data_manager = CanDataManager::new(can_data.into_normal()); let spi_sd: stm32h7xx_hal::spi::Spi< stm32h7xx_hal::stm32::SPI1, @@ -266,8 +287,8 @@ mod app { // let mono_token = rtic_monotonics::create_systick_token!(); // let mono = Systick::start(ctx.core.SYST, 36_000_000, mono_token); blink::spawn().ok(); + send_data_internal::spawn(r).ok(); display_data::spawn(s).ok(); - ( SharedResources { data_manager, @@ -289,7 +310,7 @@ mod app { fn idle(ctx: idle::Context) -> ! { loop { // info!("Idle"); - cortex_m::asm::wfi(); // could case issue with CAN Bus. Was an issue with ATSAME51. + // cortex_m::asm::wfi(); // could case issue with CAN Bus. Was an issue with ATSAME51. } } @@ -299,12 +320,11 @@ mod app { mut sender: Sender<'static, Message, DATA_CHANNEL_CAPACITY>, ) { loop { - info!("Displaying data"); let data = cx .shared .data_manager .lock(|manager| manager.clone_sensors()); - info!("{:?}", data.clone()); + // info!("{:?}", data.clone()); let messages = data.into_iter().flatten().map(|x| { Message::new( cortex_m::interrupt::free(|cs| { @@ -323,9 +343,9 @@ mod app { ) }); for msg in messages { - sender.send(msg).await.ok(); + sender.send(msg).await; } - Mono::delay(1000.millis()).await; // what if there was no delay and we used chanenls to control the rate of messages. + Mono::delay(100.millis()).await; // what if there was no delay and we used chanenls to control the rate of messages. } } @@ -353,6 +373,8 @@ mod app { #[task(priority = 3, binds = FDCAN1_IT0, shared = [can_command_manager, data_manager])] fn can_command(mut cx: can_command::Context) { + info!("CAN Command"); + panic!("CAN Command"); cx.shared.can_command_manager.lock(|can| { cx.shared .data_manager @@ -363,6 +385,8 @@ mod app { // Might be the wrong interrupt #[task(priority = 3, binds = FDCAN2_IT0, shared = [can_data_manager, data_manager])] fn can_data(mut cx: can_data::Context) { + info!("CAN Data"); + panic!("CAN Data"); cx.shared.can_data_manager.lock(|can| { cx.shared .data_manager @@ -371,15 +395,25 @@ mod app { } #[task(priority = 2, shared = [can_data_manager, data_manager])] - async fn send_data_internal(mut cx: send_data_internal::Context, m: Message) { - cx.shared.can_data_manager.lock(|can| { - can.send_message(m); - }); + 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| { + can.send_message(m); + }); + } + } } - #[task(priority = 3, shared = [can_command_manager, data_manager])] - async fn send_command_internal(mut cx: send_command_internal::Context, mut receiver: Receiver<'static, Message, DATA_CHANNEL_CAPACITY>) { - while let Ok(m) = receiver.recv().await { + #[task(priority = 2, shared = [can_command_manager, data_manager])] + async fn send_command_internal( + mut cx: send_command_internal::Context, + mut receiver: Receiver<'static, Message, DATA_CHANNEL_CAPACITY>, + ) { + while let Ok(m) = receiver.recv().await { cx.shared.can_command_manager.lock(|can| { can.send_message(m); }); @@ -401,7 +435,7 @@ mod app { } } - #[task(priority = 1, local = [sbg_power], shared = [&em])] + #[task(priority = 3, local = [sbg_power], shared = [&em])] async fn sleep_system(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.local.sbg_power.set_low(); @@ -411,10 +445,10 @@ mod app { #[task(priority = 1, shared = [&em, sd_manager])] async fn sbg_sd_task(context: sbg_sd_task::Context, data: [u8; SBG_BUFFER_SIZE]); - #[task(priority = 3, binds = DMA1_STR1, shared = [&em, sbg_manager])] + #[task(priority = 1, binds = DMA1_STR1, shared = [&em, sbg_manager])] fn sbg_dma(mut context: sbg_dma::Context); - #[task(priority = 1, shared = [data_manager])] + #[task(priority = 3, shared = [data_manager])] async fn sbg_handle_data(context: sbg_handle_data::Context, data: CallbackData); #[task(priority = 1, shared = [&em, sbg_manager])] diff --git a/boards/nav/src/sbg_manager.rs b/boards/nav/src/sbg_manager.rs index e83ffb23..0092db7f 100644 --- a/boards/nav/src/sbg_manager.rs +++ b/boards/nav/src/sbg_manager.rs @@ -28,8 +28,8 @@ use rtic::Mutex; //#[link_section = ".axisram.buffers"] //static mut SBG_BUFFER: MayberUninit<[u8; SBG_BUFFER_SIZE]> = MaybeUninit::uninit(); - -// #[link_section = ".axisram.buffers"] +// must have this link section. +#[link_section = ".axisram.buffers"] pub static mut SBG_BUFFER: MaybeUninit<[u8; SBG_BUFFER_SIZE]> = MaybeUninit::uninit(); // Simple heap required by the SBG library @@ -75,6 +75,12 @@ impl SBGManager { // } // unsafe { SBG_BUFFER.assume_init_mut() } // }; + unsafe { + // Convert an uninitialised array into an array of uninitialised + let buf: &mut [core::mem::MaybeUninit; SBG_BUFFER_SIZE] = + &mut *(core::ptr::addr_of_mut!(SBG_BUFFER) as *mut _); + buf.iter_mut().for_each(|x| x.as_mut_ptr().write(0)); + } let config = DmaConfig::default().memory_increment(true).transfer_complete_interrupt(true); let mut transfer: Transfer< @@ -90,6 +96,8 @@ impl SBGManager { None, config, ); + + info!("Starting transfer"); // Could this be unsafe because what happens if the interrupt fires before the object is created which is used in the interrupt handler. @@ -100,13 +108,13 @@ impl SBGManager { }); info!("Transfer started"); - // while !transfer.get_transfer_complete_flag() { + while !transfer.get_transfer_complete_flag() { - // } + } // info!("Transfer complete"); // info!("{}", unsafe { SBG_BUFFER.assume_init_read() }); - let sbg: sbg::SBG = sbg::SBG::new( + let mut sbg: sbg::SBG = sbg::SBG::new( |data| { sbg_handle_data::spawn(data).ok(); }, @@ -118,7 +126,8 @@ impl SBGManager { sbg_flush::spawn().ok(); }, ); - + sbg.read_data(&unsafe { SBG_BUFFER.assume_init_read() }); + SBGManager { sbg_device: sbg, xfer: Some(transfer), @@ -186,7 +195,6 @@ pub async fn sbg_sd_task(mut cx: crate::app::sbg_sd_task::Context<'_>, data: [u8 * Handles the SBG data. */ pub fn sbg_dma(mut cx: crate::app::sbg_dma::Context) { - info!("DMA"); cx.shared.sbg_manager.lock(|sbg| { match &mut sbg.xfer { Some(xfer) => { diff --git a/boards/recovery/src/communication.rs b/boards/recovery/src/communication.rs index bf2667be..54fecc9e 100644 --- a/boards/recovery/src/communication.rs +++ b/boards/recovery/src/communication.rs @@ -161,7 +161,7 @@ impl CanDevice0 { for message in &mut self.can.rx_fifo_0 { match from_bytes::(message.data()) { Ok(data) => { - info!("Sender: {:?}", data.sender); + info!("Sender: {:?}", data.clone()); data_manager.handle_data(data)?; } Err(e) => { @@ -174,6 +174,7 @@ impl CanDevice0 { for message in &mut self.can.rx_fifo_1 { match from_bytes::(message.data()) { Ok(data) => { + info!("CAN Command: {:?}", data.clone()); data_manager.handle_data(data)?; } Err(e) => { @@ -316,6 +317,7 @@ impl CanCommandManager { for message in &mut self.can.rx_fifo_0 { match from_bytes::(message.data()) { Ok(data) => { + info!("CAN Command: {:?}", data.clone()); data_manager.handle_command(data); } Err(_) => { @@ -328,6 +330,7 @@ impl CanCommandManager { for message in &mut self.can.rx_fifo_1 { match from_bytes::(message.data()) { Ok(data) => { + info!("CAN Command: {:?}", data.clone()); data_manager.handle_command(data); } Err(_) => { diff --git a/boards/recovery/src/main.rs b/boards/recovery/src/main.rs index c6ecac2d..f4e248de 100644 --- a/boards/recovery/src/main.rs +++ b/boards/recovery/src/main.rs @@ -221,6 +221,7 @@ mod app { /// Handles the CAN0 interrupt. #[task(priority = 3, binds = CAN1, shared = [can_command_manager, data_manager])] fn can_command(mut cx: can_command::Context) { + info!("CAN1 interrupt"); cx.shared.can_command_manager.lock(|can| { cx.shared .data_manager @@ -320,6 +321,7 @@ mod app { /// Handles the CAN0 interrupt. #[task(binds = CAN0, shared = [can0, data_manager, &em])] fn can0(mut cx: can0::Context) { + info!("CAN0 interrupt"); cx.shared.can0.lock(|can| { cx.shared.data_manager.lock(|data_manager| { cx.shared.em.run(|| { diff --git a/libraries/sbg-rs/src/sbg.rs b/libraries/sbg-rs/src/sbg.rs index 6716ad04..6b229a74 100644 --- a/libraries/sbg-rs/src/sbg.rs +++ b/libraries/sbg-rs/src/sbg.rs @@ -146,7 +146,6 @@ impl SBG { // SAFETY: We are assigning a static mut variable. // Buf can only be accessed from functions called by sbgEComHandle after this assignment. // unsafe { BUF = buffer }; - info!("Reading SBG Data"); for i in buffer { unsafe { match DEQ.push_back(*i) { From 60ef78f17873409db3e33c6218399e41fc029e5e Mon Sep 17 00:00:00 2001 From: NoahSprenger Date: Fri, 9 Aug 2024 17:31:58 +0000 Subject: [PATCH 16/47] WIP: Fix nominal bit timing. --- boards/nav/src/main.rs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/boards/nav/src/main.rs b/boards/nav/src/main.rs index 66a2e0cb..a0dbeaf6 100644 --- a/boards/nav/src/main.rs +++ b/boards/nav/src/main.rs @@ -106,7 +106,7 @@ mod app { .use_hse(48.MHz()) // check the clock hardware .sys_ck(100.MHz()) .pll1_strategy(rcc::PllConfigStrategy::Iterative) - .pll1_q_ck(24.MHz()) + .pll1_q_ck(32.MHz()) .freeze(pwrcfg, &ctx.device.SYSCFG); info!("RCC configured"); let fdcan_prec = ccdr @@ -115,10 +115,10 @@ mod app { .kernel_clk_mux(rec::FdcanClkSel::Pll1Q); let btr = NominalBitTiming { - prescaler: NonZeroU16::new(3).unwrap(), + prescaler: NonZeroU16::new(10).unwrap(), seg1: NonZeroU8::new(13).unwrap(), seg2: NonZeroU8::new(2).unwrap(), - sync_jump_width: NonZeroU8::new(0x4).unwrap(), + sync_jump_width: NonZeroU8::new(4).unwrap(), }; // let data_bit_timing = DataBitTiming { @@ -148,7 +148,7 @@ mod app { // c0.enable(); info!("PWM enabled"); - // assert_eq!(ccdr.clocks.pll1_q_ck().unwrap().raw(), 24_000_000); waaat + 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 From 3bc62e362a26ae343e9dc489068be6739c0e2a10 Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Fri, 9 Aug 2024 16:21:16 -0400 Subject: [PATCH 17/47] Add CAN-FD to NAV board. --- .cargo/config.toml | 4 ++-- boards/nav/src/communication.rs | 14 +++----------- boards/nav/src/main.rs | 8 ++++---- libraries/sbg-rs/src/sbg.rs | 2 +- 4 files changed, 10 insertions(+), 18 deletions(-) diff --git a/.cargo/config.toml b/.cargo/config.toml index e3b9e473..4951091d 100644 --- a/.cargo/config.toml +++ b/.cargo/config.toml @@ -1,7 +1,7 @@ [target.'cfg(all(target_arch = "arm", target_os = "none"))'] # This runner needs to be parametric so we can pass in the chip name -#runner = "probe-rs run --chip STM32H733VGTx --protocol swd" -runner = "probe-rs run --chip ATSAME51J18A --protocol swd" +runner = "probe-rs run --chip STM32H733VGTx --protocol swd" +#runner = "probe-rs run --chip ATSAME51J18A --protocol swd" rustflags = [ "-C", "link-arg=-Tlink.x", "-C", "link-arg=-Tdefmt.x", diff --git a/boards/nav/src/communication.rs b/boards/nav/src/communication.rs index 0a9b2549..f29cc275 100644 --- a/boards/nav/src/communication.rs +++ b/boards/nav/src/communication.rs @@ -87,17 +87,9 @@ impl CanDataManager { bit_rate_switching: false, marker: None, }; - self.can.abort(fdcan::Mailbox::_2); - // info!("{}", self.can.); - match stm32h7xx_hal::nb::block!(self.can.transmit(header, &payload)) { // must block or messages can fail to send. - Ok(_) => { - // info!("Message sent"); - - } - Err(e) => { - info!("Error {}", m); - } - } + // 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(()) } diff --git a/boards/nav/src/main.rs b/boards/nav/src/main.rs index a0dbeaf6..15ec8ee0 100644 --- a/boards/nav/src/main.rs +++ b/boards/nav/src/main.rs @@ -166,6 +166,8 @@ mod app { 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( @@ -324,7 +326,7 @@ mod app { .shared .data_manager .lock(|manager| manager.clone_sensors()); - // info!("{:?}", data.clone()); + // info!("{:?}", data[4].clone()); let messages = data.into_iter().flatten().map(|x| { Message::new( cortex_m::interrupt::free(|cs| { @@ -385,8 +387,6 @@ mod app { // Might be the wrong interrupt #[task(priority = 3, binds = FDCAN2_IT0, shared = [can_data_manager, data_manager])] fn can_data(mut cx: can_data::Context) { - info!("CAN Data"); - panic!("CAN Data"); cx.shared.can_data_manager.lock(|can| { cx.shared .data_manager @@ -445,7 +445,7 @@ mod app { #[task(priority = 1, shared = [&em, sd_manager])] async fn sbg_sd_task(context: sbg_sd_task::Context, data: [u8; SBG_BUFFER_SIZE]); - #[task(priority = 1, binds = DMA1_STR1, shared = [&em, sbg_manager])] + #[task(priority = 3, binds = DMA1_STR1, shared = [&em, sbg_manager])] fn sbg_dma(mut context: sbg_dma::Context); #[task(priority = 3, shared = [data_manager])] diff --git a/libraries/sbg-rs/src/sbg.rs b/libraries/sbg-rs/src/sbg.rs index 6b229a74..76199e3a 100644 --- a/libraries/sbg-rs/src/sbg.rs +++ b/libraries/sbg-rs/src/sbg.rs @@ -470,7 +470,7 @@ pub unsafe extern "C" fn sbgPlatformDebugLogMsg( match logType { // silently handle errors - _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_ERROR => error!("SBG Error"), + // _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_ERROR => error!("SBG Error"), _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_WARNING => warn!("SBG Warning"), _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_INFO => info!("SBG Info "), _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_DEBUG => debug!("SBG Debug "), From 634a981223f20ffd148693c5d4a69cf96c2ea466 Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Sat, 10 Aug 2024 13:26:13 -0400 Subject: [PATCH 18/47] Update messages crate feature flags. --- .vscode/launch.json | 11 +++++++++++ boards/communication/src/main.rs | 1 + boards/nav/src/main.rs | 16 ++++++++-------- boards/recovery/src/communication.rs | 2 +- libraries/messages/Cargo.toml | 2 +- 5 files changed, 22 insertions(+), 10 deletions(-) diff --git a/.vscode/launch.json b/.vscode/launch.json index 739e02b6..b9e7f7b8 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -26,6 +26,17 @@ } ] }, + { + "type": "probe-rs-debug", + "request": "attach", + "name": "Probe-rs attach recovery (Release)", + "chip": "ATSAME51J18A", + "coreConfigs": [ + { + "programBinary": "${workspaceFolder}/target/thumbv7em-none-eabihf/release/recovery", + } + ] + }, { "type": "probe-rs-debug", "request": "launch", diff --git a/boards/communication/src/main.rs b/boards/communication/src/main.rs index 508ed88a..54f0478a 100644 --- a/boards/communication/src/main.rs +++ b/boards/communication/src/main.rs @@ -269,6 +269,7 @@ mod app { led_green.set_low(); led_red.set_low(); blink::spawn().ok(); + sensor_send::spawn().ok(); // generate_random_messages::spawn().ok(); // generate_random_command::spawn().ok(); let mono = Systick::new(core.SYST, gclk0.freq().to_Hz()); diff --git a/boards/nav/src/main.rs b/boards/nav/src/main.rs index 15ec8ee0..819e6a24 100644 --- a/boards/nav/src/main.rs +++ b/boards/nav/src/main.rs @@ -118,14 +118,14 @@ mod app { prescaler: NonZeroU16::new(10).unwrap(), seg1: NonZeroU8::new(13).unwrap(), seg2: NonZeroU8::new(2).unwrap(), - sync_jump_width: NonZeroU8::new(4).unwrap(), + sync_jump_width: NonZeroU8::new(1).unwrap(), }; // let data_bit_timing = DataBitTiming { - // prescaler: NonZeroU8::new(48).unwrap(), - // seg1: NonZeroU8::new(0xB).unwrap(), - // seg2: NonZeroU8::new(0x4).unwrap(), - // sync_jump_width: NonZeroU8::new(0x4).unwrap(), + // 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, // }; @@ -166,7 +166,7 @@ mod app { can_data.set_nominal_bit_timing(btr); - can_data.set_automatic_retransmit(false); // data can be dropped due to its volume. + // can_data.set_automatic_retransmit(false); // data can be dropped due to its volume. // can_command.set_data_bit_timing(data_bit_timing); @@ -196,7 +196,7 @@ mod app { }; let mut can_command = can1; - can_command.set_protocol_exception_handling(true); + can_command.set_protocol_exception_handling(false); can_command.set_nominal_bit_timing(btr); @@ -376,7 +376,6 @@ mod app { #[task(priority = 3, binds = FDCAN1_IT0, shared = [can_command_manager, data_manager])] fn can_command(mut cx: can_command::Context) { info!("CAN Command"); - panic!("CAN Command"); cx.shared.can_command_manager.lock(|can| { cx.shared .data_manager @@ -387,6 +386,7 @@ mod app { // Might be the wrong interrupt #[task(priority = 3, binds = FDCAN2_IT0, shared = [can_data_manager, data_manager])] fn can_data(mut cx: can_data::Context) { + info!("CAN Data"); cx.shared.can_data_manager.lock(|can| { cx.shared .data_manager diff --git a/boards/recovery/src/communication.rs b/boards/recovery/src/communication.rs index 54fecc9e..392ddf79 100644 --- a/boards/recovery/src/communication.rs +++ b/boards/recovery/src/communication.rs @@ -144,7 +144,7 @@ impl CanDevice0 { id: ecan::Id::Standard(ecan::StandardId::new(m.sender.into()).unwrap()), frame_type: tx::FrameType::FlexibleDatarate { payload: &payload[..], - bit_rate_switching: false, + bit_rate_switching: true, force_error_state_indicator: false, }, store_tx_event: None, diff --git a/libraries/messages/Cargo.toml b/libraries/messages/Cargo.toml index 1c66efb5..8a70b22d 100644 --- a/libraries/messages/Cargo.toml +++ b/libraries/messages/Cargo.toml @@ -28,5 +28,5 @@ postcard = { version = "1.0.4", features = ["alloc"] } [features] default = ["mavlink/embedded-hal-02", "mavlink/uorocketry"] -std = ["mavlink/default", "dep:proptest", "dep:proptest-derive"] +std = ["mavlink/default", "mavlink/std", "mavlink/tcp", "mavlink/udp", "mavlink/direct-serial", "mavlink/serde", "dep:proptest", "dep:proptest-derive"] ts = ["std", "dep:ts-rs"] From c22dbe265135141f4cc2b4fb43f02ec56ec40ac2 Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Sat, 10 Aug 2024 13:39:22 -0400 Subject: [PATCH 19/47] Remove redundant feature flag. --- libraries/messages/Cargo.toml | 1 - 1 file changed, 1 deletion(-) diff --git a/libraries/messages/Cargo.toml b/libraries/messages/Cargo.toml index 8a70b22d..df42e310 100644 --- a/libraries/messages/Cargo.toml +++ b/libraries/messages/Cargo.toml @@ -13,7 +13,6 @@ fugit = "0.3.6" heapless = "0.7.16" ts-rs = { version = "6.2.1", optional = true } mavlink = { git = "https://github.com/uorocketry/rust-mavlink.git", features = [ - "embedded-hal-02", "uorocketry", ], default-features = false } bitflags = { version = "2.3.1", features = ["serde"] } From e875aae7216fb0a26df620b18d125230aadf6152 Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Sat, 10 Aug 2024 13:40:58 -0400 Subject: [PATCH 20/47] Remove failing mavlink code. --- libraries/messages/Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/messages/Cargo.toml b/libraries/messages/Cargo.toml index df42e310..8a8391fe 100644 --- a/libraries/messages/Cargo.toml +++ b/libraries/messages/Cargo.toml @@ -27,5 +27,5 @@ postcard = { version = "1.0.4", features = ["alloc"] } [features] default = ["mavlink/embedded-hal-02", "mavlink/uorocketry"] -std = ["mavlink/default", "mavlink/std", "mavlink/tcp", "mavlink/udp", "mavlink/direct-serial", "mavlink/serde", "dep:proptest", "dep:proptest-derive"] +std = ["mavlink/std", "mavlink/tcp", "mavlink/udp", "mavlink/direct-serial", "mavlink/serde", "dep:proptest", "dep:proptest-derive"] ts = ["std", "dep:ts-rs"] From 958b9638dcd9bde059c06eb345dc5809c35cc1ed Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Sat, 10 Aug 2024 20:24:29 -0400 Subject: [PATCH 21/47] WIP --- .cargo/config.toml | 4 +- .vscode/launch.json | 11 ++++ boards/communication/src/communication.rs | 29 +++++---- boards/communication/src/main.rs | 75 +++++++++++++---------- boards/recovery/src/main.rs | 2 +- 5 files changed, 74 insertions(+), 47 deletions(-) diff --git a/.cargo/config.toml b/.cargo/config.toml index 4951091d..e3b9e473 100644 --- a/.cargo/config.toml +++ b/.cargo/config.toml @@ -1,7 +1,7 @@ [target.'cfg(all(target_arch = "arm", target_os = "none"))'] # This runner needs to be parametric so we can pass in the chip name -runner = "probe-rs run --chip STM32H733VGTx --protocol swd" -#runner = "probe-rs run --chip ATSAME51J18A --protocol swd" +#runner = "probe-rs run --chip STM32H733VGTx --protocol swd" +runner = "probe-rs run --chip ATSAME51J18A --protocol swd" rustflags = [ "-C", "link-arg=-Tlink.x", "-C", "link-arg=-Tdefmt.x", diff --git a/.vscode/launch.json b/.vscode/launch.json index b9e7f7b8..c98b2496 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -37,6 +37,17 @@ } ] }, + { + "type": "probe-rs-debug", + "request": "attach", + "name": "Probe-rs attach com (Release)", + "chip": "ATSAME51J18A", + "coreConfigs": [ + { + "programBinary": "${workspaceFolder}/target/thumbv7em-none-eabihf/release/communication", + } + ] + }, { "type": "probe-rs-debug", "request": "launch", diff --git a/boards/communication/src/communication.rs b/boards/communication/src/communication.rs index 58e3c232..cad073bc 100644 --- a/boards/communication/src/communication.rs +++ b/boards/communication/src/communication.rs @@ -9,6 +9,7 @@ use atsamd_hal::clock::v2::types::{Can0, Can1}; use atsamd_hal::clock::v2::Source; use atsamd_hal::gpio::PA09; use defmt::info; +use atsamd_hal::prelude::*; use atsamd_hal::gpio::{ Alternate, AlternateI, Disabled, Floating, Pin, I, PA12, PA13, PA22, PA23, PB16, PB17, }; @@ -469,14 +470,18 @@ impl RadioManager { self.mav_sequence } pub fn receive_message(&mut self) -> Result { - if let Ok(data) = self.radio.receiver.read_u8() { + // if let Ok(data) = self.radio.receiver.read_u8() { // lets add this data to the buffer and see if we can parse it - self.buf.write(data); + // info!("Received data {}", data); + // self.buf.write(data); + // self.buf.write(nb::block!(self.radio.receiver.read()?)?); + let (_header, msg) = mavlink::read_versioned_msg(&mut self.radio.receiver, mavlink::MavlinkVersion::V2)?; // Do we need the header? match msg { mavlink::uorocketry::MavMessage::POSTCARD_MESSAGE(msg) => { + return Ok(postcard::from_bytes::(&msg.message)?); // weird Ok syntax to coerce to hydra error type. } @@ -485,16 +490,16 @@ impl RadioManager { return Err(mavlink::error::MessageReadError::Io.into()); } } - } else { - return Err(mavlink::error::MessageReadError::Io.into()); - } + // } else { + // return Err(mavlink::error::MessageReadError::Io.into()); + // } } } -impl Read for RadioManager { - fn read_exact(&mut self, buf: &mut [u8]) -> Result<(), mavlink::error::MessageReadError> { - let len = buf.len().min(self.buf.len()); - buf[..len].copy_from_slice(&self.buf[..len]); - Ok(()) - } -} +// impl Read for RadioManager { +// fn read_exact(&mut self, buf: &mut [u8]) -> Result<(), mavlink::error::MessageReadError> { +// let len = buf.len().min(self.buf.len()); +// buf[..len].copy_from_slice(&self.buf[..len]); +// Ok(()) +// } +// } diff --git a/boards/communication/src/main.rs b/boards/communication/src/main.rs index 54f0478a..4de85135 100644 --- a/boards/communication/src/main.rs +++ b/boards/communication/src/main.rs @@ -80,6 +80,7 @@ mod app { use atsamd_hal::clock::v2::gclk::{self, Gclk}; use command::{Command, CommandData, RadioRateChange}; use fugit::RateExtU64; + use mavlink::embedded::Read; use sender::Sender; use state::StateData; @@ -269,7 +270,8 @@ mod app { led_green.set_low(); led_red.set_low(); blink::spawn().ok(); - sensor_send::spawn().ok(); + // sensor_send::spawn().ok(); + // radio_rx::spawn().ok(); // generate_random_messages::spawn().ok(); // generate_random_command::spawn().ok(); let mono = Systick::new(core.SYST, gclk0.freq().to_Hz()); @@ -426,33 +428,33 @@ mod app { }); match logging_rate { RadioRate::Fast => { - spawn_after!(sensor_send, ExtU64::millis(250)).ok(); + spawn_after!(sensor_send, ExtU64::millis(100)).ok(); } RadioRate::Slow => { - spawn_after!(sensor_send, ExtU64::millis(2000)).ok(); + spawn_after!(sensor_send, ExtU64::millis(250)).ok(); } } } - #[task(shared = [&em])] - fn generate_random_messages(cx: generate_random_messages::Context) { - cx.shared.em.run(|| { - let message = Message::new( - cortex_m::interrupt::free(|cs| { - let mut rc = RTC.borrow(cs).borrow_mut(); - let rtc = rc.as_mut().unwrap(); - rtc.count32() - }), - COM_ID, - State::new(StateData::Initializing), - ); - // spawn!(send_gs, message.clone())?; - spawn!(send_internal, message)?; - Ok(()) - }); - spawn_after!(generate_random_messages, ExtU64::millis(200)).ok(); + // #[task(shared = [&em])] + // fn generate_random_messages(cx: generate_random_messages::Context) { + // cx.shared.em.run(|| { + // let message = Message::new( + // cortex_m::interrupt::free(|cs| { + // let mut rc = RTC.borrow(cs).borrow_mut(); + // let rtc = rc.as_mut().unwrap(); + // rtc.count32() + // }), + // COM_ID, + // State::new(StateData::Initializing), + // ); + // // spawn!(send_gs, message.clone())?; + // spawn!(send_internal, message)?; + // Ok(()) + // }); + // spawn_after!(generate_random_messages, ExtU64::millis(200)).ok(); - } + // } #[task(shared = [&em])] fn generate_random_command(cx: generate_random_command::Context) { @@ -493,17 +495,26 @@ mod app { // spawn_after!(state_send, ExtU64::secs(5)).ok(); // } - // #[task(binds = SERCOM5_2, shared = [&em, radio_manager])] - // fn radio_rx(mut cx: radio_rx::Context) { - // cx.shared.radio_manager.lock(|radio_manager| { - // cx.shared.em.run(|| { - // info!("Interrupt on Sercom5"); - // let msg = radio_manager.receive_message()?; - // // spawn!(send_internal, msg)?; // just broadcast the message throughout the system for now. - // Ok(()) - // }); - // }); - // } + #[task(priority = 3, binds = SERCOM2_2, shared = [&em, radio_manager])] + fn radio_rx(mut cx: radio_rx::Context) { + info!("Interrupt on Sercom2"); + cx.shared.radio_manager.lock(|radio_manager| { + cx.shared.em.run(|| { + // info!("Interrupt on Sercom5"); + // let mut buf = [0; 255]; + // radio_manager.read_exact(&mut buf)?; + // info!("Read data: {:?}", buf); + info!("Reading message {}", radio_manager.receive_message()?); + + + // let msg = radio_manager.receive_message(); + // info!("Received message: {:?}", msg); + // spawn!(send_internal, msg)?; // just broadcast the message throughout the system for now. + // spawn_after!(radio_rx, ExtU64::millis(200)).ok(); + Ok(()) + }); + }); + } /** * Simple blink task to test the system. diff --git a/boards/recovery/src/main.rs b/boards/recovery/src/main.rs index f4e248de..234722ef 100644 --- a/boards/recovery/src/main.rs +++ b/boards/recovery/src/main.rs @@ -188,7 +188,7 @@ mod app { read_barometer::spawn().ok(); state_send::spawn().ok(); blink::spawn().ok(); - // fire_main::spawn_after(ExtU64::secs(15)).ok(); + // fire_main::spawn_after(ExtU64::secs(1000)).ok(); // fire_drogue::spawn_after(ExtU64::secs(15)).ok(); /* Monotonic clock */ From 62a21a62452d4b2b30d41213ef44fd5e0eed094b Mon Sep 17 00:00:00 2001 From: NoahSprenger Date: Sun, 11 Aug 2024 05:03:09 +0000 Subject: [PATCH 22/47] Increase CPU and CAN speed. --- Cargo.lock | 12 ++--- boards/communication/src/communication.rs | 45 +++++++----------- boards/communication/src/main.rs | 58 +++++++++++------------ boards/nav/src/main.rs | 7 +-- boards/nav/src/sbg_manager.rs | 20 ++++---- boards/recovery/src/communication.rs | 4 +- 6 files changed, 68 insertions(+), 78 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index f044be1c..80818464 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -187,9 +187,9 @@ checksum = "37b2a672a2cb129a2e41c10b1224bb368f9f37a2b16b612598138befd7b37eb5" [[package]] name = "cc" -version = "1.1.8" +version = "1.1.10" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "504bdec147f2cc13c8b57ed9401fd8a147cc66b67ad5cb241394244f2c947549" +checksum = "e9e8aabfac534be767c909e0690571677d49f41bd8465ae876fe043d52ba5292" [[package]] name = "cfg-if" @@ -1592,9 +1592,9 @@ checksum = "a3f0bf26fd526d2a95683cd0f87bf103b8539e2ca1ef48ce002d67aad59aa0b4" [[package]] name = "serde" -version = "1.0.204" +version = "1.0.205" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bc76f558e0cbb2a839d37354c575f1dc3fdc6546b5be373ba43d95f231bf7c12" +checksum = "e33aedb1a7135da52b7c21791455563facbbcc43d0f0f66165b42c21b3dfb150" dependencies = [ "serde_derive", ] @@ -1610,9 +1610,9 @@ dependencies = [ [[package]] name = "serde_derive" -version = "1.0.204" +version = "1.0.205" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e0cd7e117be63d3c3678776753929474f3b04a43a080c744d6b0ae2a8c28e222" +checksum = "692d6f5ac90220161d6774db30c662202721e64aed9058d2c394f451261420c1" dependencies = [ "proc-macro2 1.0.86", "quote 1.0.36", diff --git a/boards/communication/src/communication.rs b/boards/communication/src/communication.rs index cad073bc..5f53cfef 100644 --- a/boards/communication/src/communication.rs +++ b/boards/communication/src/communication.rs @@ -8,8 +8,6 @@ use atsamd_hal::clock::v2::pclk::PclkToken; use atsamd_hal::clock::v2::types::{Can0, Can1}; use atsamd_hal::clock::v2::Source; use atsamd_hal::gpio::PA09; -use defmt::info; -use atsamd_hal::prelude::*; use atsamd_hal::gpio::{ Alternate, AlternateI, Disabled, Floating, Pin, I, PA12, PA13, PA22, PA23, PB16, PB17, }; @@ -20,6 +18,7 @@ use atsamd_hal::pac::SERCOM2; use atsamd_hal::pac::SERCOM4; use atsamd_hal::pac::SERCOM5; use atsamd_hal::pac::{CAN0, CAN1}; +use atsamd_hal::prelude::*; use atsamd_hal::sercom; use atsamd_hal::sercom::uart; use atsamd_hal::sercom::uart::Uart; @@ -30,6 +29,7 @@ use common_arm_atsame::mcan; use common_arm_atsame::mcan::message::{rx, Raw}; use common_arm_atsame::mcan::tx_buffers::DynTx; use defmt::error; +use defmt::info; use heapless::HistoryBuffer; use heapless::Vec; use mavlink::embedded::Read; @@ -44,7 +44,8 @@ use mcan::{ config::{BitTiming, Mode}, filter::{Action, Filter}, }; -use messages::mavlink; +use messages::mavlink::uorocketry::MavMessage; +use messages::mavlink::{self, MessageData}; use messages::ErrorContext; use messages::Message; use postcard::from_bytes; @@ -107,7 +108,7 @@ impl CanDevice0 { Dependencies::new(gclk0, pclk_can, ahb_clock, can_rx, can_tx, peripheral); let mut can = mcan::bus::CanConfigurable::new( - fugit::RateExtU32::kHz(200), + BitTiming {sjw: 1, phase_seg_1: 13, phase_seg_2: 2, bitrate: 500.kHz()}, can_dependencies, can_memory, ) @@ -271,7 +272,7 @@ impl CanCommandManager { Dependencies::new(gclk0, pclk_can, ahb_clock, can_rx, can_tx, peripheral); let mut can = mcan::bus::CanConfigurable::new( - fugit::RateExtU32::kHz(200), + BitTiming {sjw: 1, phase_seg_1: 13, phase_seg_2: 2, bitrate: 500.kHz()}, // needs a prescaler of 6 to be changed in the mcan source because mcan is meh. can_dependencies, can_memory, ) @@ -470,29 +471,19 @@ impl RadioManager { self.mav_sequence } pub fn receive_message(&mut self) -> Result { - // if let Ok(data) = self.radio.receiver.read_u8() { - // lets add this data to the buffer and see if we can parse it - // info!("Received data {}", data); - // self.buf.write(data); - // self.buf.write(nb::block!(self.radio.receiver.read()?)?); - - let (_header, msg) = - mavlink::read_versioned_msg(&mut self.radio.receiver, mavlink::MavlinkVersion::V2)?; - // Do we need the header? - match msg { - mavlink::uorocketry::MavMessage::POSTCARD_MESSAGE(msg) => { - - return Ok(postcard::from_bytes::(&msg.message)?); - // weird Ok syntax to coerce to hydra error type. - } - _ => { - // error!("Error, ErrorContext::UnkownPostcardMessage"); - return Err(mavlink::error::MessageReadError::Io.into()); - } + let (_header, msg) = + mavlink::read_versioned_msg(&mut self.radio.receiver, mavlink::MavlinkVersion::V2)?; + // Do we need the header? + match msg { + mavlink::uorocketry::MavMessage::POSTCARD_MESSAGE(msg) => { + return Ok(postcard::from_bytes::(&msg.message)?); + // weird Ok syntax to coerce to hydra error type. } - // } else { - // return Err(mavlink::error::MessageReadError::Io.into()); - // } + _ => { + // error!("Error, ErrorContext::UnkownPostcardMessage"); + return Err(mavlink::error::MessageReadError::Io.into()); + } + } } } diff --git a/boards/communication/src/main.rs b/boards/communication/src/main.rs index 4de85135..54f64fb3 100644 --- a/boards/communication/src/main.rs +++ b/boards/communication/src/main.rs @@ -270,8 +270,7 @@ mod app { led_green.set_low(); led_red.set_low(); blink::spawn().ok(); - // sensor_send::spawn().ok(); - // radio_rx::spawn().ok(); + sensor_send::spawn().ok(); // generate_random_messages::spawn().ok(); // generate_random_command::spawn().ok(); let mono = Systick::new(core.SYST, gclk0.freq().to_Hz()); @@ -476,41 +475,38 @@ mod app { spawn!(generate_random_command).ok(); } - // #[task(shared = [data_manager, &em])] - // 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(cortex_m::interrupt::free(|cs| { - // let mut rc = RTC.borrow(cs).borrow_mut(); - // let rtc = rc.as_mut().unwrap(); - // rtc.count32()}), COM_ID, State::new(x)); - // // spawn!(send_gs, message)?; - // } // if there is none we still return since we simply don't have data yet. - // Ok(()) - // }); - // spawn_after!(state_send, ExtU64::secs(5)).ok(); - // } + #[task(shared = [data_manager, &em])] + 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(cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + rtc.count32()}), COM_ID, State::new(x)); + // spawn!(send_gs, message)?; + } // if there is none we still return since we simply don't have data yet. + Ok(()) + }); + spawn_after!(state_send, ExtU64::secs(5)).ok(); + } + /// Handles the radio interrupt. + /// This only needs to be called when the radio has data to read, this is why an interrupt handler is used above polling which would waste resources. + /// We use a critical section to ensure that we are not interrupted while servicing the mavlink message. #[task(priority = 3, binds = SERCOM2_2, shared = [&em, radio_manager])] fn radio_rx(mut cx: radio_rx::Context) { info!("Interrupt on Sercom2"); cx.shared.radio_manager.lock(|radio_manager| { cx.shared.em.run(|| { - // info!("Interrupt on Sercom5"); - // let mut buf = [0; 255]; - // radio_manager.read_exact(&mut buf)?; - // info!("Read data: {:?}", buf); - info!("Reading message {}", radio_manager.receive_message()?); - - - // let msg = radio_manager.receive_message(); - // info!("Received message: {:?}", msg); - // spawn!(send_internal, msg)?; // just broadcast the message throughout the system for now. - // spawn_after!(radio_rx, ExtU64::millis(200)).ok(); + let msg = cortex_m::interrupt::free(|cs| { + radio_manager.receive_message() + })?; + spawn!(send_command, msg.clone())?; + info!("Reading message {}", msg); Ok(()) }); }); diff --git a/boards/nav/src/main.rs b/boards/nav/src/main.rs index 819e6a24..230fbc10 100644 --- a/boards/nav/src/main.rs +++ b/boards/nav/src/main.rs @@ -104,7 +104,7 @@ mod app { info!("RCC enabled"); let ccdr = rcc .use_hse(48.MHz()) // check the clock hardware - .sys_ck(100.MHz()) + .sys_ck(300.MHz()) .pll1_strategy(rcc::PllConfigStrategy::Iterative) .pll1_q_ck(32.MHz()) .freeze(pwrcfg, &ctx.device.SYSCFG); @@ -115,7 +115,7 @@ mod app { .kernel_clk_mux(rec::FdcanClkSel::Pll1Q); let btr = NominalBitTiming { - prescaler: NonZeroU16::new(10).unwrap(), + prescaler: NonZeroU16::new(4).unwrap(), seg1: NonZeroU8::new(13).unwrap(), seg2: NonZeroU8::new(2).unwrap(), sync_jump_width: NonZeroU8::new(1).unwrap(), @@ -281,7 +281,7 @@ mod app { // let mono = Systick::new(core.SYST, ccdr.clocks.sysclk().to_Hz()); // blink::spawn().ok(); // display_data::spawn().ok(); - Mono::start(core.SYST, 100_000_000); + Mono::start(core.SYST, 300_000_000); info!("Online"); let data_manager = DataManager::new(); @@ -291,6 +291,7 @@ mod app { blink::spawn().ok(); send_data_internal::spawn(r).ok(); display_data::spawn(s).ok(); + ( SharedResources { data_manager, diff --git a/boards/nav/src/sbg_manager.rs b/boards/nav/src/sbg_manager.rs index 0092db7f..1195e709 100644 --- a/boards/nav/src/sbg_manager.rs +++ b/boards/nav/src/sbg_manager.rs @@ -82,7 +82,9 @@ impl SBGManager { buf.iter_mut().for_each(|x| x.as_mut_ptr().write(0)); } - let config = DmaConfig::default().memory_increment(true).transfer_complete_interrupt(true); + let config = DmaConfig::default() + .memory_increment(true) + .transfer_complete_interrupt(true); let mut transfer: Transfer< StreamX, Rx, @@ -97,20 +99,16 @@ impl SBGManager { config, ); - info!("Starting transfer"); - // Could this be unsafe because what happens if the interrupt fires before the object is created which is used in the interrupt handler. - + // Could this be unsafe because what happens if the interrupt fires before the object is created which is used in the interrupt handler. + transfer.start(|serial| { serial.enable_dma_rx(); - }); info!("Transfer started"); - while !transfer.get_transfer_complete_flag() { - - } + while !transfer.get_transfer_complete_flag() {} // info!("Transfer complete"); // info!("{}", unsafe { SBG_BUFFER.assume_init_read() }); @@ -173,7 +171,10 @@ pub async fn sbg_handle_data(mut cx: sbg_handle_data::Context<'_>, data: Callbac }); } -pub async fn sbg_sd_task(mut cx: crate::app::sbg_sd_task::Context<'_>, data: [u8; SBG_BUFFER_SIZE]) { +pub async fn sbg_sd_task( + mut cx: crate::app::sbg_sd_task::Context<'_>, + data: [u8; SBG_BUFFER_SIZE], +) { cx.shared.sd_manager.lock(|manager| { if let Some(mut file) = manager.file.take() { cx.shared.em.run(|| { @@ -205,6 +206,7 @@ pub fn sbg_dma(mut cx: crate::app::sbg_dma::Context) { unsafe { (*core::ptr::addr_of_mut!(SBG_BUFFER)).assume_init_mut() }, // Uninitialised memory ); sbg.sbg_device.read_data(&data); + crate::app::sbg_sd_task::spawn(data).ok(); } } None => { diff --git a/boards/recovery/src/communication.rs b/boards/recovery/src/communication.rs index 392ddf79..d07e37e2 100644 --- a/boards/recovery/src/communication.rs +++ b/boards/recovery/src/communication.rs @@ -77,7 +77,7 @@ impl CanDevice0 { Dependencies::new(gclk0, pclk_can, ahb_clock, can_rx, can_tx, peripheral); let mut can = - mcan::bus::CanConfigurable::new(200.kHz(), can_dependencies, can_memory).unwrap(); + mcan::bus::CanConfigurable::new(BitTiming {sjw: 1, phase_seg_1: 13, phase_seg_2: 2, bitrate: 500.kHz()}, can_dependencies, can_memory).unwrap(); can.config().mode = Mode::Fd { allow_bit_rate_switching: false, data_phase_timing: BitTiming::new(500.kHz()), @@ -218,7 +218,7 @@ impl CanCommandManager { Dependencies::new(gclk0, pclk_can, ahb_clock, can_rx, can_tx, peripheral); let mut can = - mcan::bus::CanConfigurable::new(200.kHz(), can_dependencies, can_memory).unwrap(); + mcan::bus::CanConfigurable::new(BitTiming {sjw: 1, phase_seg_1: 13, phase_seg_2: 2, bitrate: 500.kHz()}, can_dependencies, can_memory).unwrap(); can.config().mode = Mode::Fd { allow_bit_rate_switching: false, data_phase_timing: BitTiming::new(500.kHz()), From 78c063da51175d85973502c6b3cc1deac0dd464b Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Sun, 11 Aug 2024 13:50:09 -0400 Subject: [PATCH 23/47] Add command reading and sending. --- Cargo.lock | 28 +++++++++++------------ boards/communication/src/communication.rs | 17 ++++++++++---- boards/communication/src/main.rs | 13 ++++++----- boards/nav/src/main.rs | 6 ++--- boards/nav/src/sbg_manager.rs | 2 +- boards/recovery/src/communication.rs | 4 ++-- boards/recovery/src/data_manager.rs | 2 +- boards/recovery/src/main.rs | 2 ++ 8 files changed, 43 insertions(+), 31 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 80818464..35f99efe 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -425,7 +425,7 @@ dependencies = [ "proc-macro-error", "proc-macro2 1.0.86", "quote 1.0.36", - "syn 2.0.72", + "syn 2.0.74", ] [[package]] @@ -467,7 +467,7 @@ checksum = "984bc6eca246389726ac2826acc2488ca0fe5fcd6b8d9b48797021951d76a125" dependencies = [ "proc-macro2 1.0.86", "quote 1.0.36", - "syn 2.0.72", + "syn 2.0.74", ] [[package]] @@ -480,7 +480,7 @@ dependencies = [ "proc-macro2 1.0.86", "quote 1.0.36", "rustc_version 0.4.0", - "syn 2.0.72", + "syn 2.0.74", ] [[package]] @@ -601,7 +601,7 @@ dependencies = [ "once_cell", "proc-macro2 1.0.86", "quote 1.0.36", - "syn 2.0.72", + "syn 2.0.74", ] [[package]] @@ -1390,7 +1390,7 @@ dependencies = [ "proc-macro-error", "proc-macro2 1.0.86", "quote 1.0.36", - "syn 2.0.72", + "syn 2.0.74", ] [[package]] @@ -1592,9 +1592,9 @@ checksum = "a3f0bf26fd526d2a95683cd0f87bf103b8539e2ca1ef48ce002d67aad59aa0b4" [[package]] name = "serde" -version = "1.0.205" +version = "1.0.206" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e33aedb1a7135da52b7c21791455563facbbcc43d0f0f66165b42c21b3dfb150" +checksum = "5b3e4cd94123dd520a128bcd11e34d9e9e423e7e3e50425cb1b4b1e3549d0284" dependencies = [ "serde_derive", ] @@ -1610,13 +1610,13 @@ dependencies = [ [[package]] name = "serde_derive" -version = "1.0.205" +version = "1.0.206" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "692d6f5ac90220161d6774db30c662202721e64aed9058d2c394f451261420c1" +checksum = "fabfb6138d2383ea8208cf98ccf69cdfb1aff4088460681d84189aa259762f97" dependencies = [ "proc-macro2 1.0.86", "quote 1.0.36", - "syn 2.0.72", + "syn 2.0.74", ] [[package]] @@ -1798,9 +1798,9 @@ dependencies = [ [[package]] name = "syn" -version = "2.0.72" +version = "2.0.74" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dc4b9b9bf2add8093d3f2c0204471e951b2285580335de42f9d2534f3ae7a8af" +checksum = "1fceb41e3d546d0bd83421d3409b1460cc7444cd389341a4c880fe7a042cb3d7" dependencies = [ "proc-macro2 1.0.86", "quote 1.0.36", @@ -1866,7 +1866,7 @@ checksum = "a4558b58466b9ad7ca0f102865eccc95938dca1a74a856f2b57b6629050da261" dependencies = [ "proc-macro2 1.0.86", "quote 1.0.36", - "syn 2.0.72", + "syn 2.0.74", ] [[package]] @@ -2067,5 +2067,5 @@ checksum = "fa4f8080344d4671fb4e831a13ad1e68092748387dfc4f55e356242fae12ce3e" dependencies = [ "proc-macro2 1.0.86", "quote 1.0.36", - "syn 2.0.72", + "syn 2.0.74", ] diff --git a/boards/communication/src/communication.rs b/boards/communication/src/communication.rs index 5f53cfef..3ca75041 100644 --- a/boards/communication/src/communication.rs +++ b/boards/communication/src/communication.rs @@ -1,3 +1,6 @@ +use core::any::Any; +use core::fmt::Debug; + use crate::data_manager::DataManager; use crate::types::*; use atsamd_hal::can::Dependencies; @@ -108,7 +111,7 @@ impl CanDevice0 { Dependencies::new(gclk0, pclk_can, ahb_clock, can_rx, can_tx, peripheral); let mut can = mcan::bus::CanConfigurable::new( - BitTiming {sjw: 1, phase_seg_1: 13, phase_seg_2: 2, bitrate: 500.kHz()}, + 200.kHz(), can_dependencies, can_memory, ) @@ -272,7 +275,7 @@ impl CanCommandManager { Dependencies::new(gclk0, pclk_can, ahb_clock, can_rx, can_tx, peripheral); let mut can = mcan::bus::CanConfigurable::new( - BitTiming {sjw: 1, phase_seg_1: 13, phase_seg_2: 2, bitrate: 500.kHz()}, // needs a prescaler of 6 to be changed in the mcan source because mcan is meh. + 200.kHz(), // needs a prescaler of 6 to be changed in the mcan source because mcan is meh. can_dependencies, can_memory, ) @@ -471,16 +474,22 @@ impl RadioManager { self.mav_sequence } pub fn receive_message(&mut self) -> Result { - let (_header, msg) = + 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) => { return Ok(postcard::from_bytes::(&msg.message)?); // weird Ok syntax to coerce to hydra error type. } + mavlink::uorocketry::MavMessage::COMMAND_MESSAGE(command) => { + info!("{}", command.command); + return Ok(postcard::from_bytes::(&command.command)?); + } _ => { - // error!("Error, ErrorContext::UnkownPostcardMessage"); + error!("Error, ErrorContext::UnkownPostcardMessage"); return Err(mavlink::error::MessageReadError::Io.into()); } } diff --git a/boards/communication/src/main.rs b/boards/communication/src/main.rs index 54f64fb3..2058c045 100644 --- a/boards/communication/src/main.rs +++ b/boards/communication/src/main.rs @@ -344,8 +344,9 @@ mod app { /** * Sends a message over CAN. */ - #[task(capacity = 5, shared = [can_command_manager, &em])] + #[task(priority = 3, capacity = 5, shared = [can_command_manager, &em])] fn send_command(mut cx: send_command::Context, m: Message) { + info!("{}", m.clone()); cx.shared.em.run(|| { cx.shared .can_command_manager @@ -499,14 +500,14 @@ mod app { /// We use a critical section to ensure that we are not interrupted while servicing the mavlink message. #[task(priority = 3, binds = SERCOM2_2, shared = [&em, radio_manager])] fn radio_rx(mut cx: radio_rx::Context) { - info!("Interrupt on Sercom2"); cx.shared.radio_manager.lock(|radio_manager| { cx.shared.em.run(|| { - let msg = cortex_m::interrupt::free(|cs| { - radio_manager.receive_message() + cortex_m::interrupt::free(|cs| { + let m = radio_manager.receive_message()?; + info!("Received message {}", m.clone()); + spawn!(send_command, m) + })?; - spawn!(send_command, msg.clone())?; - info!("Reading message {}", msg); Ok(()) }); }); diff --git a/boards/nav/src/main.rs b/boards/nav/src/main.rs index 230fbc10..60212808 100644 --- a/boards/nav/src/main.rs +++ b/boards/nav/src/main.rs @@ -104,7 +104,7 @@ mod app { info!("RCC enabled"); let ccdr = rcc .use_hse(48.MHz()) // check the clock hardware - .sys_ck(300.MHz()) + .sys_ck(200.MHz()) .pll1_strategy(rcc::PllConfigStrategy::Iterative) .pll1_q_ck(32.MHz()) .freeze(pwrcfg, &ctx.device.SYSCFG); @@ -115,7 +115,7 @@ mod app { .kernel_clk_mux(rec::FdcanClkSel::Pll1Q); let btr = NominalBitTiming { - prescaler: NonZeroU16::new(4).unwrap(), + prescaler: NonZeroU16::new(10).unwrap(), seg1: NonZeroU8::new(13).unwrap(), seg2: NonZeroU8::new(2).unwrap(), sync_jump_width: NonZeroU8::new(1).unwrap(), @@ -281,7 +281,7 @@ mod app { // let mono = Systick::new(core.SYST, ccdr.clocks.sysclk().to_Hz()); // blink::spawn().ok(); // display_data::spawn().ok(); - Mono::start(core.SYST, 300_000_000); + Mono::start(core.SYST, 200_000_000); info!("Online"); let data_manager = DataManager::new(); diff --git a/boards/nav/src/sbg_manager.rs b/boards/nav/src/sbg_manager.rs index 1195e709..5c7be67c 100644 --- a/boards/nav/src/sbg_manager.rs +++ b/boards/nav/src/sbg_manager.rs @@ -206,7 +206,7 @@ pub fn sbg_dma(mut cx: crate::app::sbg_dma::Context) { unsafe { (*core::ptr::addr_of_mut!(SBG_BUFFER)).assume_init_mut() }, // Uninitialised memory ); sbg.sbg_device.read_data(&data); - crate::app::sbg_sd_task::spawn(data).ok(); + // crate::app::sbg_sd_task::spawn(data).ok(); } } None => { diff --git a/boards/recovery/src/communication.rs b/boards/recovery/src/communication.rs index d07e37e2..392ddf79 100644 --- a/boards/recovery/src/communication.rs +++ b/boards/recovery/src/communication.rs @@ -77,7 +77,7 @@ impl CanDevice0 { Dependencies::new(gclk0, pclk_can, ahb_clock, can_rx, can_tx, peripheral); let mut can = - mcan::bus::CanConfigurable::new(BitTiming {sjw: 1, phase_seg_1: 13, phase_seg_2: 2, bitrate: 500.kHz()}, can_dependencies, can_memory).unwrap(); + mcan::bus::CanConfigurable::new(200.kHz(), can_dependencies, can_memory).unwrap(); can.config().mode = Mode::Fd { allow_bit_rate_switching: false, data_phase_timing: BitTiming::new(500.kHz()), @@ -218,7 +218,7 @@ impl CanCommandManager { Dependencies::new(gclk0, pclk_can, ahb_clock, can_rx, can_tx, peripheral); let mut can = - mcan::bus::CanConfigurable::new(BitTiming {sjw: 1, phase_seg_1: 13, phase_seg_2: 2, bitrate: 500.kHz()}, can_dependencies, can_memory).unwrap(); + mcan::bus::CanConfigurable::new(200.kHz(), can_dependencies, can_memory).unwrap(); can.config().mode = Mode::Fd { allow_bit_rate_switching: false, data_phase_timing: BitTiming::new(500.kHz()), diff --git a/boards/recovery/src/data_manager.rs b/boards/recovery/src/data_manager.rs index 1659d01f..fb5affcd 100644 --- a/boards/recovery/src/data_manager.rs +++ b/boards/recovery/src/data_manager.rs @@ -182,7 +182,7 @@ impl DataManager { match command.data { messages::Data::Command(command) => match command.data { messages::command::CommandData::DeployDrogue(_) => { - spawn!(fire_drogue)?; // need someway to capture this error. + spawn!(fire_drogue)?; } messages::command::CommandData::DeployMain(_) => { spawn!(fire_main)?; diff --git a/boards/recovery/src/main.rs b/boards/recovery/src/main.rs index 234722ef..51e1f8c2 100644 --- a/boards/recovery/src/main.rs +++ b/boards/recovery/src/main.rs @@ -270,6 +270,7 @@ mod app { #[task(priority = 3, local = [fired: bool = false], shared=[gpio, &em])] fn fire_drogue(mut cx: fire_drogue::Context) { + info!("Firing drogue"); cx.shared.em.run(|| { if !(*cx.local.fired) { cx.shared.gpio.lock(|gpio| { @@ -288,6 +289,7 @@ mod app { #[task(priority = 3, local = [fired: bool = false], shared=[gpio, &em])] fn fire_main(mut cx: fire_main::Context) { + info!("Firing main"); cx.shared.em.run(|| { if !(*cx.local.fired) { cx.shared.gpio.lock(|gpio| { From 6f9ef1f36e02d3a27a5569de28fb7624c7178e6a Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Sun, 11 Aug 2024 23:47:00 -0400 Subject: [PATCH 24/47] WIP --- .vscode/launch.json | 4 +-- Embed.toml | 2 +- boards/communication/src/communication.rs | 2 +- boards/communication/src/main.rs | 2 +- boards/nav/src/main.rs | 42 ++++++++++++++++++----- boards/nav/src/sbg_manager.rs | 3 +- boards/recovery/src/communication.rs | 4 +-- boards/recovery/src/main.rs | 7 ++-- 8 files changed, 48 insertions(+), 18 deletions(-) diff --git a/.vscode/launch.json b/.vscode/launch.json index c98b2496..1f3b7eea 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -94,8 +94,8 @@ "coreConfigs": [ { "programBinary": "${workspaceFolder}/target/thumbv7em-none-eabihf/release/nav", - "rttEnabled": true, - "coreIndex": 0, + // "rttEnabled": true, + // "coreIndex": 0, } ] }, diff --git a/Embed.toml b/Embed.toml index 5c5defed..a62c5357 100644 --- a/Embed.toml +++ b/Embed.toml @@ -10,7 +10,7 @@ protocol = "Swd" enabled = true [default.general] -chip = "ATSAME51J20A" +chip = "STM32H733VGTx" [default.probe] protocol = "Swd" diff --git a/boards/communication/src/communication.rs b/boards/communication/src/communication.rs index 3ca75041..e02f22b7 100644 --- a/boards/communication/src/communication.rs +++ b/boards/communication/src/communication.rs @@ -214,7 +214,7 @@ impl CanDevice0 { for message in &mut self.can.rx_fifo_0 { match from_bytes::(message.data()) { Ok(data) => { - info!("Received message {}", data.clone()); + // info!("Received message {}", data.clone()); data_manager.handle_data(data); } diff --git a/boards/communication/src/main.rs b/boards/communication/src/main.rs index 2058c045..903e5f8d 100644 --- a/boards/communication/src/main.rs +++ b/boards/communication/src/main.rs @@ -322,7 +322,7 @@ mod app { /// Handles the CAN0 interrupt. #[task(priority = 3, binds = CAN0, shared = [can0, data_manager])] fn can0(mut cx: can0::Context) { - info!("CAN0 interrupt"); + // info!("CAN0 interrupt"); cx.shared.can0.lock(|can| { cx.shared .data_manager diff --git a/boards/nav/src/main.rs b/boards/nav/src/main.rs index 60212808..65ae9c00 100644 --- a/boards/nav/src/main.rs +++ b/boards/nav/src/main.rs @@ -46,6 +46,7 @@ systick_monotonic!(Mono, 500); #[inline(never)] #[defmt::panic_handler] fn panic() -> ! { + // stm32h7xx_hal::pac::SCB::sys_reset() cortex_m::asm::udf() } @@ -69,12 +70,13 @@ mod app { sbg_manager: sbg_manager::SBGManager, can_command_manager: CanCommandManager, can_data_manager: CanDataManager, + sbg_power: PB4>, + } #[local] struct LocalResources { led_red: PA2>, led_green: PA3>, - sbg_power: PB4>, } #[init] @@ -96,12 +98,22 @@ mod app { let backup = pwrcfg.backup().unwrap(); info!("Backup domain enabled"); // RCC - let rcc = ctx.device.RCC.constrain(); + let mut rcc = ctx.device.RCC.constrain(); + let reset = rcc.get_reset_reason(); + info!("Reset reason: {:?}", reset); + // match reset { + // // rcc::ResetReason::PowerOnReset => { + // // stm32h7xx_hal::pac::SCB::sys_reset(); + // // info!("Power on reset"); + // // } + // _ => { + // info!("Reset reason: {:?}", reset); + // } + // } let fdcan_prec_unsafe = unsafe { rcc.steal_peripheral_rec() } .FDCAN .kernel_clk_mux(rec::FdcanClkSel::Pll1Q); - info!("RCC enabled"); let ccdr = rcc .use_hse(48.MHz()) // check the clock hardware .sys_ck(200.MHz()) @@ -148,7 +160,7 @@ mod app { // c0.enable(); info!("PWM enabled"); - assert_eq!(ccdr.clocks.pll1_q_ck().unwrap().raw(), 32_000_000); + // 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 @@ -291,6 +303,7 @@ mod app { blink::spawn().ok(); send_data_internal::spawn(r).ok(); display_data::spawn(s).ok(); + // sbg_power_on::spawn().ok(); ( SharedResources { @@ -300,11 +313,12 @@ mod app { sbg_manager, can_command_manager, can_data_manager, + sbg_power, + }, LocalResources { led_red, led_green, - sbg_power, }, ) } @@ -384,6 +398,16 @@ 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; + } + } + // Might be the wrong interrupt #[task(priority = 3, binds = FDCAN2_IT0, shared = [can_data_manager, data_manager])] fn can_data(mut cx: can_data::Context) { @@ -436,10 +460,12 @@ mod app { } } - #[task(priority = 3, local = [sbg_power], shared = [&em])] - async fn sleep_system(cx: sleep_system::Context) { + #[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.local.sbg_power.set_low(); + cx.shared.sbg_power.lock(|sbg| { + sbg.set_low(); + }); } extern "Rust" { diff --git a/boards/nav/src/sbg_manager.rs b/boards/nav/src/sbg_manager.rs index 5c7be67c..efa982b0 100644 --- a/boards/nav/src/sbg_manager.rs +++ b/boards/nav/src/sbg_manager.rs @@ -108,7 +108,7 @@ impl SBGManager { }); info!("Transfer started"); - while !transfer.get_transfer_complete_flag() {} + // while !transfer.get_transfer_complete_flag() {} // info!("Transfer complete"); // info!("{}", unsafe { SBG_BUFFER.assume_init_read() }); @@ -196,6 +196,7 @@ pub async fn sbg_sd_task( * Handles the SBG data. */ pub fn sbg_dma(mut cx: crate::app::sbg_dma::Context) { + info!("DMA interrupt"); cx.shared.sbg_manager.lock(|sbg| { match &mut sbg.xfer { Some(xfer) => { diff --git a/boards/recovery/src/communication.rs b/boards/recovery/src/communication.rs index 392ddf79..788b2607 100644 --- a/boards/recovery/src/communication.rs +++ b/boards/recovery/src/communication.rs @@ -161,7 +161,7 @@ impl CanDevice0 { for message in &mut self.can.rx_fifo_0 { match from_bytes::(message.data()) { Ok(data) => { - info!("Sender: {:?}", data.clone()); + // info!("Sender: {:?}", data.clone()); data_manager.handle_data(data)?; } Err(e) => { @@ -174,7 +174,7 @@ impl CanDevice0 { for message in &mut self.can.rx_fifo_1 { match from_bytes::(message.data()) { Ok(data) => { - info!("CAN Command: {:?}", data.clone()); + // info!("CAN Command: {:?}", data.clone()); data_manager.handle_data(data)?; } Err(e) => { diff --git a/boards/recovery/src/main.rs b/boards/recovery/src/main.rs index 51e1f8c2..b6670f4f 100644 --- a/boards/recovery/src/main.rs +++ b/boards/recovery/src/main.rs @@ -149,9 +149,13 @@ mod app { /* GPIO config */ let led_green = pins.pa03.into_push_pull_output(); let led_red = pins.pb04.into_push_pull_output(); + let mut main_ematch = pins.pb12.into_push_pull_output(); + main_ematch.set_high().ok(); + loop {} + let gpio = GPIOManager::new( // pins switched from schematic - pins.pb12.into_push_pull_output(), + main_ematch, pins.pb11.into_push_pull_output(), ); /* State Machine config */ @@ -323,7 +327,6 @@ mod app { /// Handles the CAN0 interrupt. #[task(binds = CAN0, shared = [can0, data_manager, &em])] fn can0(mut cx: can0::Context) { - info!("CAN0 interrupt"); cx.shared.can0.lock(|can| { cx.shared.data_manager.lock(|data_manager| { cx.shared.em.run(|| { From 21d169ab3dacb0d11766ff756062312b5a4f23f6 Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Mon, 12 Aug 2024 17:53:41 -0400 Subject: [PATCH 25/47] Add recovery sensing and cleanup. --- boards/communication/src/communication.rs | 22 +--- boards/communication/src/data_manager.rs | 8 +- boards/communication/src/main.rs | 40 ++----- boards/communication/src/types.rs | 2 +- boards/recovery/src/main.rs | 69 +++++++++-- .../common-arm/src/health/health_manager.rs | 77 ------------ .../common-arm/src/health/health_monitor.rs | 110 ------------------ libraries/common-arm/src/health/mod.rs | 2 - libraries/common-arm/src/lib.rs | 3 - libraries/messages/src/health.rs | 53 --------- libraries/messages/src/lib.rs | 3 - libraries/messages/src/sensor.rs | 34 ++---- 12 files changed, 94 insertions(+), 329 deletions(-) delete mode 100644 libraries/common-arm/src/health/health_manager.rs delete mode 100644 libraries/common-arm/src/health/health_monitor.rs delete mode 100644 libraries/common-arm/src/health/mod.rs delete mode 100644 libraries/messages/src/health.rs diff --git a/boards/communication/src/communication.rs b/boards/communication/src/communication.rs index e02f22b7..61249302 100644 --- a/boards/communication/src/communication.rs +++ b/boards/communication/src/communication.rs @@ -1,28 +1,18 @@ -use core::any::Any; -use core::fmt::Debug; use crate::data_manager::DataManager; use crate::types::*; use atsamd_hal::can::Dependencies; use atsamd_hal::clock::v2::ahb::AhbClk; -use atsamd_hal::clock::v2::gclk::{Gclk0Id, Gclk1Id}; +use atsamd_hal::clock::v2::gclk::{Gclk0Id}; use atsamd_hal::clock::v2::pclk::Pclk; -use atsamd_hal::clock::v2::pclk::PclkToken; use atsamd_hal::clock::v2::types::{Can0, Can1}; use atsamd_hal::clock::v2::Source; -use atsamd_hal::gpio::PA09; use atsamd_hal::gpio::{ - Alternate, AlternateI, Disabled, Floating, Pin, I, PA12, PA13, PA22, PA23, PB16, PB17, + Alternate, AlternateI, Pin, I, PA22, PA23, }; -use atsamd_hal::gpio::{AlternateH, H, PA08, PB14, PB15}; -use atsamd_hal::pac::MCLK; -use atsamd_hal::pac::SERCOM0; -use atsamd_hal::pac::SERCOM2; -use atsamd_hal::pac::SERCOM4; -use atsamd_hal::pac::SERCOM5; +use atsamd_hal::gpio::{AlternateH, H, PB14, PB15}; use atsamd_hal::pac::{CAN0, CAN1}; use atsamd_hal::prelude::*; -use atsamd_hal::sercom; use atsamd_hal::sercom::uart; use atsamd_hal::sercom::uart::Uart; use atsamd_hal::sercom::uart::{RxDuplex, TxDuplex}; @@ -35,7 +25,6 @@ use defmt::error; use defmt::info; use heapless::HistoryBuffer; use heapless::Vec; -use mavlink::embedded::Read; use mavlink::peek_reader::PeekReader; use mcan::bus::Can; use mcan::embedded_can as ecan; @@ -48,8 +37,7 @@ use mcan::{ filter::{Action, Filter}, }; use messages::mavlink::uorocketry::MavMessage; -use messages::mavlink::{self, MessageData}; -use messages::ErrorContext; +use messages::mavlink::{self}; use messages::Message; use postcard::from_bytes; use systick_monotonic::*; @@ -413,7 +401,7 @@ pub struct RadioDevice { impl RadioDevice { pub fn new( - mut uart: atsamd_hal::sercom::uart::Uart< + uart: atsamd_hal::sercom::uart::Uart< GroundStationUartConfig, atsamd_hal::sercom::uart::Duplex, >, diff --git a/boards/communication/src/data_manager.rs b/boards/communication/src/data_manager.rs index 5e48b58a..e093a425 100644 --- a/boards/communication/src/data_manager.rs +++ b/boards/communication/src/data_manager.rs @@ -20,6 +20,7 @@ pub struct DataManager { pub gps_pos_acc: Option, pub state: Option, pub logging_rate: Option, + pub recovery_sensing: Option, } impl DataManager { @@ -40,6 +41,7 @@ impl DataManager { gps_pos_acc: None, state: None, logging_rate: Some(RadioRate::Slow), // start slow. + recovery_sensing: None, } } @@ -54,7 +56,7 @@ impl DataManager { } /// Do not clone instead take to reduce CPU load. - pub fn take_sensors(&mut self) -> [Option; 13] { + pub fn take_sensors(&mut self) -> [Option; 14] { [ self.air.take(), self.ekf_nav_1.take(), @@ -69,6 +71,7 @@ impl DataManager { self.gps_pos_1.take(), self.gps_pos_2.take(), self.gps_pos_acc.take(), + self.recovery_sensing.take(), ] } @@ -131,6 +134,9 @@ impl DataManager { messages::sensor::SensorData::GpsPos2(_) => { self.gps_pos_2 = Some(data); } + messages::sensor::SensorData::RecoverySensing(_) => { + self.recovery_sensing = Some(data); + } }, messages::Data::State(state) => { self.state = Some(state.data); diff --git a/boards/communication/src/main.rs b/boards/communication/src/main.rs index 903e5f8d..abd31011 100644 --- a/boards/communication/src/main.rs +++ b/boards/communication/src/main.rs @@ -7,49 +7,33 @@ mod types; use atsamd_hal as hal; use atsamd_hal::rtc::Count32Mode; -use common_arm::HealthManager; -use common_arm::HealthMonitor; -use common_arm::SdManager; use common_arm::*; use common_arm_atsame::mcan; // use panic_halt as _; -use atsamd_hal::time::*; -use atsamd_hal::{ - clock::v2::{ - clock_system_at_reset, - xosc::{CrystalCurrent, SafeClockDiv, StartUpDelay, Xosc}, - }, - pac::Peripherals, -}; use communication::Capacities; use communication::{CanCommandManager, RadioDevice, RadioManager}; use core::cell::RefCell; use cortex_m::interrupt::Mutex; -use cortex_m_rt::exception; -use cortex_m_rt::ExceptionFrame; use data_manager::DataManager; use defmt::info; use defmt_rtt as _; // global logger -use fugit::ExtU32; use fugit::ExtU64; use fugit::RateExtU32; -use hal::adc::Adc; use hal::clock::v2::pclk::Pclk; use hal::clock::v2::Source; use hal::gpio::Pins; use hal::gpio::{ - Alternate, Output, Pin, PushPull, PushPullOutput, C, D, PA02, PA03, PA04, PA05, PA06, PA07, - PA08, PA09, PB12, PB13, PB14, PB15, + Alternate, Pin, PushPullOutput, C, PA02, PA03, + PA08, PA09, }; use hal::prelude::*; use hal::sercom::uart; use hal::sercom::{ - spi, spi::Config, spi::Duplex, spi::Pads, spi::Spi, IoSet1, IoSet3, Sercom0, Sercom2, Sercom4, + IoSet3, }; use mcan::messageram::SharedMemory; use messages::command::RadioRate; -use messages::health::Health; use messages::state::State; use messages::*; use panic_probe as _; @@ -77,12 +61,12 @@ static RTC: Mutex>>> = Mutex::new(RefC #[rtic::app(device = hal::pac, peripherals = true, dispatchers = [EVSYS_0, EVSYS_1, EVSYS_2])] mod app { - use atsamd_hal::clock::v2::gclk::{self, Gclk}; - use command::{Command, CommandData, RadioRateChange}; - use fugit::RateExtU64; - use mavlink::embedded::Read; + + use command::{Command, CommandData}; + + use sender::Sender; - use state::StateData; + use super::*; @@ -128,7 +112,7 @@ mod app { ])] fn init(cx: init::Context) -> (Shared, Local, init::Monotonics) { let mut peripherals = cx.device; - let mut core = cx.core; + let core = cx.core; let pins = Pins::new(peripherals.PORT); // let mut dmac = DmaController::init(peripherals.DMAC, &mut peripherals.PM); @@ -203,7 +187,7 @@ mod app { let pads = uart::Pads::::default() .rx(rx) .tx(tx); - let mut uart = + let uart = GroundStationUartConfig::new(&mclk, peripherals.SERCOM2, pads, pclk_radio.freq()) .baud( RateExtU32::Hz(57600), @@ -238,7 +222,7 @@ mod app { .rx(pins.pa12) .tx(pins.pa13); - let mut gps_uart = GpsUartConfig::new(&mclk, peripherals.SERCOM4, pads, pclk_gps.freq()) + let gps_uart = GpsUartConfig::new(&mclk, peripherals.SERCOM4, pads, pclk_gps.freq()) .baud( RateExtU32::Hz(9600), uart::BaudMode::Fractional(uart::Oversampling::Bits16), @@ -255,7 +239,7 @@ mod app { /* Spawn tasks */ // sensor_send::spawn().ok(); // state_send::spawn().ok(); - let mut rtc = hal::rtc::Rtc::clock_mode( + let rtc = hal::rtc::Rtc::clock_mode( peripherals.RTC, atsamd_hal::fugit::RateExtU32::Hz(1024), &mut mclk, diff --git a/boards/communication/src/types.rs b/boards/communication/src/types.rs index bc688857..cba8a5a6 100644 --- a/boards/communication/src/types.rs +++ b/boards/communication/src/types.rs @@ -1,6 +1,6 @@ use atsamd_hal::gpio::*; use atsamd_hal::sercom::uart::EightBit; -use atsamd_hal::sercom::{uart, uart::Duplex, IoSet1, IoSet3, Sercom0, Sercom2, Sercom4, Sercom5}; +use atsamd_hal::sercom::{uart, uart::Duplex, IoSet3, Sercom2, Sercom4}; use messages::sender::Sender; use messages::sender::Sender::CommunicationBoard; diff --git a/boards/recovery/src/main.rs b/boards/recovery/src/main.rs index b6670f4f..3208dda9 100644 --- a/boards/recovery/src/main.rs +++ b/boards/recovery/src/main.rs @@ -54,7 +54,8 @@ unsafe fn HardFault(_frame: &cortex_m_rt::ExceptionFrame) -> ! { #[rtic::app(device = hal::pac, peripherals = true, dispatchers = [EVSYS_0, EVSYS_1, EVSYS_2])] mod app { - use atsamd_hal::gpio::{PA03, PB04}; + + use atsamd_hal::gpio::{B, PA03, PB04, PB06, PB07, PB08, PB09}; use super::*; @@ -71,6 +72,11 @@ mod app { struct Local { led_green: Pin, led_red: Pin, + drogue_current_sense: Pin>, + main_current_sense: Pin>, + drogue_sense: Pin>, + main_sense: Pin>, + adc1: hal::adc::Adc, state_machine: StateMachine, barometer: MS5611_01BA< Spi< @@ -149,15 +155,10 @@ mod app { /* GPIO config */ let led_green = pins.pa03.into_push_pull_output(); let led_red = pins.pb04.into_push_pull_output(); - let mut main_ematch = pins.pb12.into_push_pull_output(); - main_ematch.set_high().ok(); - loop {} + let main_ematch = pins.pb12.into_push_pull_output(); + let drogue_ematch = pins.pb11.into_push_pull_output(); - let gpio = GPIOManager::new( - // pins switched from schematic - main_ematch, - pins.pb11.into_push_pull_output(), - ); + let gpio = GPIOManager::new(main_ematch, drogue_ematch); /* State Machine config */ let state_machine = StateMachine::new(); @@ -187,10 +188,20 @@ mod app { }); // info!("RTC done"); + // ADC sensing setup + let (pclk_adc, gclk0) = Pclk::enable(tokens.pclks.adc1, gclk0); + let adc1 = hal::adc::Adc::adc1(peripherals.ADC1, &mut mclk); + let drogue_current_sense = pins.pb06.into_alternate(); + let main_current_sense = pins.pb07.into_alternate(); + let drogue_sense = pins.pb08.into_alternate(); + let main_sense = pins.pb09.into_alternate(); + // let data = adc1.read(&mut drogue_sense); + /* Spawn tasks */ run_sm::spawn().ok(); read_barometer::spawn().ok(); state_send::spawn().ok(); + ejection_sense::spawn().ok(); blink::spawn().ok(); // fire_main::spawn_after(ExtU64::secs(1000)).ok(); // fire_drogue::spawn_after(ExtU64::secs(15)).ok(); @@ -210,6 +221,11 @@ mod app { led_green, led_red, state_machine, + adc1, + drogue_current_sense, + main_current_sense, + drogue_sense, + main_sense, barometer, }, init::Monotonics(mono), @@ -222,6 +238,41 @@ mod app { loop {} } + #[task(local = [adc1, main_sense, drogue_sense, main_current_sense, drogue_current_sense], shared = [&em])] + fn ejection_sense(mut cx: ejection_sense::Context) { + cx.shared.em.run(|| { + let data_drogue_current: u16 = + cx.local.adc1.read(cx.local.drogue_current_sense).unwrap(); + let data_main_current: u16 = cx.local.adc1.read(cx.local.main_current_sense).unwrap(); + let data_drogue_sense: u16 = cx.local.adc1.read(cx.local.drogue_sense).unwrap(); + let data_main_sense: u16 = cx.local.adc1.read(cx.local.main_sense).unwrap(); + let sensing = messages::sensor::Sensor { + data: messages::sensor::SensorData::RecoverySensing( + messages::sensor::RecoverySensing { + drogue_current: data_drogue_current, + main_current: data_main_current, + drogue_voltage: data_drogue_sense, + main_voltage: data_main_sense, + }, + ), + }; + + let message = Message::new( + cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + rtc.count32() + }), + COM_ID, + sensing, + ); + // info!("Drogue current: {} Main current: {} Drogue sense: {} Main sense: {}", data_drogue_current, data_main_current, data_drogue_sense, data_main_sense); + spawn!(send_internal, message).ok(); + spawn_after!(ejection_sense, ExtU64::secs(1)).ok(); + Ok(()) + }); + } + /// Handles the CAN0 interrupt. #[task(priority = 3, binds = CAN1, shared = [can_command_manager, data_manager])] fn can_command(mut cx: can_command::Context) { diff --git a/libraries/common-arm/src/health/health_manager.rs b/libraries/common-arm/src/health/health_manager.rs deleted file mode 100644 index 7b059ae4..00000000 --- a/libraries/common-arm/src/health/health_manager.rs +++ /dev/null @@ -1,77 +0,0 @@ -use crate::health::health_monitor::{HealthMonitor, HealthMonitorChannels}; -use core::ops::RangeInclusive; -use defmt::warn; -use messages::health::HealthState; - -/// Manages and reports the health of the system. -/// I think OutputPin trait is the right one that should be good for atsame and stm32. -pub struct HealthManager { - pub monitor: HealthMonitor, -} - -impl HealthManager -where - T: HealthMonitorChannels, -{ - /// Create a new health manager. - /// Divider is the ohmage of the resistor and resolution is the resolution of the ADC. - pub fn new(monitor: HealthMonitor) -> Self { - Self { monitor } - } - /// !! Values are not correct !! - /// They need to be updated with the proper dividers - /// Evaluate the health of the system. - /// All values must be nominal for the system to be in a nominal state. - /// could be nice to send out a log message if something fails, tell the system what failed. - pub fn evaluate(&mut self) -> HealthState { - self.monitor.update(); // We need new values before we can evaluate the health status. - let data = self.monitor.data.clone(); - // This is ugly... - let v5_status = get_status(data.v5, &self.monitor.range_5v); - let v3_3_status = get_status(data.v3_3, &self.monitor.range_3v3); - let ext_3v3 = get_status(data.ext_3v3, &self.monitor.range_ext_3v3); - let ext_v5_status = get_status(data.ext_v5, &self.monitor.range_ext_5v); - let int_v3_3_status = get_status(data.int_v3_3, &self.monitor.range_int_3v3); - let int_5v_status = get_status(data.int_v5, &self.monitor.range_int_5v); - let pyro_status = get_status(data.pyro_sense, &self.monitor.range_pyro); - let vcc_status = get_status(data.vcc_sense, &self.monitor.range_vcc); - let failover_status = get_status(data.failover_sense, &self.monitor.range_failover); - - for status in [ - v5_status, - v3_3_status, - ext_3v3, - ext_v5_status, - int_v3_3_status, - int_5v_status, - pyro_status, - vcc_status, - failover_status, - ] { - match status { - HealthState::Error => return HealthState::Error, - HealthState::Warning => return HealthState::Warning, - _ => continue, - } - } - - HealthState::Nominal - } -} - -fn get_status(data: Option, nominal: &RangeInclusive) -> HealthState { - match data { - Some(x) => { - if nominal.contains(&x) { - return HealthState::Nominal; - } else { - // warn!("Unsafe Voltage"); - HealthState::Error - } - } - None => { - warn!("No data"); - return HealthState::Warning; - } - } -} diff --git a/libraries/common-arm/src/health/health_monitor.rs b/libraries/common-arm/src/health/health_monitor.rs deleted file mode 100644 index 29d61afe..00000000 --- a/libraries/common-arm/src/health/health_monitor.rs +++ /dev/null @@ -1,110 +0,0 @@ -use core::ops::RangeInclusive; -/// Health monitor module -/// This may not actually be feasible. -/// So, we have a health monitor module that is responsible for monitoring the health of the system. -/// But if we want this to be generic to the point where we can use it on any board, then we need to -/// ensure that the ADC channels will accept the pins that we want to use. -use messages::health::HealthStatus; - -/// Add a note about what each means and what the range is. -pub trait HealthMonitorChannels { - fn get_5v(&mut self) -> Option; - fn get_3v3(&mut self) -> Option; - fn get_pyro(&mut self) -> Option; - fn get_vcc(&mut self) -> Option; - fn get_int_5v(&mut self) -> Option; - fn get_int_3v3(&mut self) -> Option; - fn get_ext_5v(&mut self) -> Option; - fn get_ext_3v3(&mut self) -> Option; - fn get_failover(&mut self) -> Option; -} - -pub struct HealthMonitor { - // we will store a set of readings here that can be propagated up to the manager - // we also - channels: T, - pub data: HealthStatus, - pub range_5v: RangeInclusive, - pub range_3v3: RangeInclusive, - pub range_pyro: RangeInclusive, - pub range_vcc: RangeInclusive, - pub range_int_5v: RangeInclusive, - pub range_int_3v3: RangeInclusive, - pub range_ext_5v: RangeInclusive, - pub range_ext_3v3: RangeInclusive, - pub range_failover: RangeInclusive, -} - -impl HealthMonitor -where - T: HealthMonitorChannels, -{ - pub fn new(channels: T, divider1: u16, divider2: u16, resolution: u16) -> Self { - let range_5v = ((resolution as f32 / calculate_voltage(divider1, divider2, 4.9)) as u16) - ..=((resolution as f32 / calculate_voltage(divider1, divider2, 5.1)) as u16); - let range_3v3 = ((resolution as f32 / calculate_voltage(divider1, divider2, 3.2)) as u16) - ..=((resolution as f32 / calculate_voltage(divider1, divider2, 3.4)) as u16); - let range_pyro = ((resolution as f32 / calculate_voltage(divider1, divider2, 8.9)) as u16) - ..=((resolution as f32 / calculate_voltage(divider1, divider2, 9.1)) as u16); - let range_vcc = ((resolution as f32 / calculate_voltage(divider1, divider2, 11.9)) as u16) - ..=((resolution as f32 / calculate_voltage(divider1, divider2, 12.1)) as u16); - let range_int_5v = ((resolution as f32 / calculate_voltage(divider1, divider2, 4.9)) as u16) - ..=((resolution as f32 / calculate_voltage(divider1, divider2, 5.1)) as u16); - let range_int_3v3 = ((resolution as f32 / calculate_voltage(divider1, divider2, 3.2)) - as u16) - ..=((resolution as f32 / calculate_voltage(divider1, divider2, 3.4)) as u16); - let range_ext_5v = ((resolution as f32 / calculate_voltage(divider1, divider2, 4.9)) as u16) - ..=((resolution as f32 / calculate_voltage(divider1, divider2, 5.1)) as u16); - let range_ext_3v3 = ((resolution as f32 / calculate_voltage(divider1, divider2, 3.2)) - as u16) - ..=((resolution as f32 / calculate_voltage(divider1, divider2, 3.4)) as u16); - // I'm not certain that failover is actually 3.3v - let range_failover = ((resolution as f32 / calculate_voltage(divider1, divider2, 3.2)) - as u16) - ..=((resolution as f32 / calculate_voltage(divider1, divider2, 3.4)) as u16); - Self { - channels, - data: HealthStatus { - v5: None, - v3_3: None, - pyro_sense: None, - vcc_sense: None, - int_v5: None, - int_v3_3: None, - ext_v5: None, - ext_3v3: None, - failover_sense: None, - }, - range_5v, - range_3v3, - range_pyro, - range_vcc, - range_int_5v, - range_int_3v3, - range_ext_5v, - range_ext_3v3, - range_failover, - } - } - - /// Should this really be unwrap? - /// Or should we return this to the manager and let it decide what to do with it? - /// For now we will just unwrap it. - /// Also update will be called by the manager. - /// This could be intensive if we are reading all of the channels at once. - pub fn update(&mut self) { - self.data.v5 = self.channels.get_5v(); - self.data.v3_3 = self.channels.get_3v3(); - self.data.pyro_sense = self.channels.get_pyro(); - self.data.vcc_sense = self.channels.get_vcc(); - self.data.int_v5 = self.channels.get_int_5v(); - self.data.int_v3_3 = self.channels.get_int_3v3(); - self.data.ext_v5 = self.channels.get_ext_5v(); - self.data.ext_3v3 = self.channels.get_ext_3v3(); - self.data.failover_sense = self.channels.get_failover(); - } -} - -fn calculate_voltage(r1: u16, r2: u16, v_source: f32) -> f32 { - v_source * (r2 as f32 / (r1 as f32 + r2 as f32)) -} diff --git a/libraries/common-arm/src/health/mod.rs b/libraries/common-arm/src/health/mod.rs deleted file mode 100644 index e5de5966..00000000 --- a/libraries/common-arm/src/health/mod.rs +++ /dev/null @@ -1,2 +0,0 @@ -pub mod health_manager; -pub mod health_monitor; diff --git a/libraries/common-arm/src/lib.rs b/libraries/common-arm/src/lib.rs index 8209122f..643477b7 100644 --- a/libraries/common-arm/src/lib.rs +++ b/libraries/common-arm/src/lib.rs @@ -7,14 +7,11 @@ //! mod error; -mod health; mod logging; mod sd_manager; pub use crate::error::error_manager::ErrorManager; pub use crate::error::hydra_error::{ErrorContextTrait, HydraError, SpawnError}; -pub use crate::health::health_manager::HealthManager; -pub use crate::health::health_monitor::{HealthMonitor, HealthMonitorChannels}; pub use crate::logging::HydraLogging; pub use crate::sd_manager::SdManager; diff --git a/libraries/messages/src/health.rs b/libraries/messages/src/health.rs deleted file mode 100644 index 3ff228ef..00000000 --- a/libraries/messages/src/health.rs +++ /dev/null @@ -1,53 +0,0 @@ -use derive_more::From; -use messages_proc_macros_lib::common_derives; - -#[common_derives] -#[derive(From)] -pub struct Health { - pub data: HealthData, - pub status: HealthState, -} - -/// The health status of a component. -/// Nomial: everything is fine -/// Warning: something is wrong, but it's not critical -/// Error: something is wrong, and it's critical -/// We need some way to quantify this concept of good and bad health. -/// Could be current range or voltage range. If failover happens on the regulators that is warning. -#[common_derives] -#[derive(From)] -pub enum HealthState { - Nominal, - Warning, - Error, -} - -#[common_derives] -#[derive(From)] -pub enum HealthData { - // RegulatorStatus(RegulatorStatus), - HealthStatus(HealthStatus), -} - -#[common_derives] -#[derive(From)] -pub struct HealthStatus { - pub v5: Option, - pub v3_3: Option, - pub pyro_sense: Option, - pub vcc_sense: Option, - pub int_v5: Option, - pub int_v3_3: Option, - pub ext_v5: Option, - pub ext_3v3: Option, - pub failover_sense: Option, -} - -impl Health { - pub fn new(data: impl Into, status: impl Into) -> Self { - Health { - data: data.into(), - status: status.into(), - } - } -} diff --git a/libraries/messages/src/lib.rs b/libraries/messages/src/lib.rs index 1be9e154..70ac890d 100644 --- a/libraries/messages/src/lib.rs +++ b/libraries/messages/src/lib.rs @@ -6,7 +6,6 @@ //! and ground-station communication. use crate::command::Command; -use crate::health::Health; use crate::sender::Sender; use crate::sensor::Sensor; use crate::state::State; @@ -16,7 +15,6 @@ pub use mavlink; use messages_proc_macros_lib::common_derives; pub mod command; -pub mod health; mod logging; pub mod sender; pub mod sensor; @@ -50,7 +48,6 @@ pub enum Data { Sensor(Sensor), Log(Log), Command(Command), - Health(Health), } impl Message { diff --git a/libraries/messages/src/sensor.rs b/libraries/messages/src/sensor.rs index 580bb1c3..81639246 100644 --- a/libraries/messages/src/sensor.rs +++ b/libraries/messages/src/sensor.rs @@ -27,31 +27,7 @@ pub enum SensorData { GpsPos1(GpsPos1), GpsPos2(GpsPos2), GpsPosAcc(GpsPosAcc), -} - -/* Replace with new health monitor */ - -#[common_derives] -pub struct Regulator { - pub status: bool, -} - -#[common_derives] -pub struct Voltage { - pub voltage: f32, - pub rolling_avg: f32, -} - -#[common_derives] -pub struct Current { - pub current: f32, - pub rolling_avg: f32, -} - -#[common_derives] -pub struct Temperature { - pub temperature: f32, - pub rolling_avg: f32, + RecoverySensing(RecoverySensing), } /* Replace with new health monitor */ @@ -218,6 +194,14 @@ pub struct GpsVelAcc { pub velocity_acc: Option<[f32; 3usize]>, } +#[common_derives] +pub struct RecoverySensing { + pub drogue_current: u16, + pub main_current: u16, + pub drogue_voltage: u16, + pub main_voltage: u16, +} + impl Sensor { pub fn new(data: impl Into) -> Self { Sensor { From 7f1dc918a2e10592c60d9470c1be31bf9df4397b Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Mon, 12 Aug 2024 23:11:50 -0400 Subject: [PATCH 26/47] Please work GPS :) --- .cargo/config.toml | 4 ++-- boards/nav/src/main.rs | 7 +++--- boards/nav/src/sbg_manager.rs | 20 ++++++++++------ libraries/sbg-rs/src/data_conversion.rs | 2 +- libraries/sbg-rs/src/sbg.rs | 31 +++++++++++++++++++------ 5 files changed, 43 insertions(+), 21 deletions(-) diff --git a/.cargo/config.toml b/.cargo/config.toml index e3b9e473..4951091d 100644 --- a/.cargo/config.toml +++ b/.cargo/config.toml @@ -1,7 +1,7 @@ [target.'cfg(all(target_arch = "arm", target_os = "none"))'] # This runner needs to be parametric so we can pass in the chip name -#runner = "probe-rs run --chip STM32H733VGTx --protocol swd" -runner = "probe-rs run --chip ATSAME51J18A --protocol swd" +runner = "probe-rs run --chip STM32H733VGTx --protocol swd" +#runner = "probe-rs run --chip ATSAME51J18A --protocol swd" rustflags = [ "-C", "link-arg=-Tlink.x", "-C", "link-arg=-Tdefmt.x", diff --git a/boards/nav/src/main.rs b/boards/nav/src/main.rs index 65ae9c00..1a732a7d 100644 --- a/boards/nav/src/main.rs +++ b/boards/nav/src/main.rs @@ -155,7 +155,7 @@ mod app { .TIM12 .pwm(pins, 4.kHz(), ccdr.peripheral.TIM12, &ccdr.clocks); - c0.set_duty(c0.get_max_duty() / 2); + c0.set_duty(c0.get_max_duty() / 4); // PWM outputs are disabled by default // c0.enable(); @@ -268,7 +268,7 @@ mod app { .UART4 .serial((tx, rx), 115_200.bps(), ccdr.peripheral.UART4, &ccdr.clocks) .unwrap(); - let sbg_manager = sbg_manager::SBGManager::new(uart_sbg, stream_tuple); + let mut sbg_manager = sbg_manager::SBGManager::new(uart_sbg, stream_tuple); let mut rtc = stm32h7xx_hal::rtc::Rtc::open_or_init( ctx.device.RTC, @@ -341,7 +341,7 @@ mod app { .shared .data_manager .lock(|manager| manager.clone_sensors()); - // info!("{:?}", data[4].clone()); + info!("{:?}", data.clone()); let messages = data.into_iter().flatten().map(|x| { Message::new( cortex_m::interrupt::free(|cs| { @@ -411,7 +411,6 @@ mod app { // Might be the wrong interrupt #[task(priority = 3, binds = FDCAN2_IT0, shared = [can_data_manager, data_manager])] fn can_data(mut cx: can_data::Context) { - info!("CAN Data"); cx.shared.can_data_manager.lock(|can| { cx.shared .data_manager diff --git a/boards/nav/src/sbg_manager.rs b/boards/nav/src/sbg_manager.rs index efa982b0..b17ee58d 100644 --- a/boards/nav/src/sbg_manager.rs +++ b/boards/nav/src/sbg_manager.rs @@ -36,7 +36,7 @@ pub static mut SBG_BUFFER: MaybeUninit<[u8; SBG_BUFFER_SIZE]> = MaybeUninit::uni static HEAP: Heap = Heap::empty(); pub struct SBGManager { - sbg_device: SBG, + pub sbg_device: SBG, xfer: Option< Transfer< StreamX, @@ -84,7 +84,7 @@ impl SBGManager { let config = DmaConfig::default() .memory_increment(true) - .transfer_complete_interrupt(true); + .transfer_complete_interrupt(true).peripheral_burst(stm32h7xx_hal::dma::config::BurstMode::Burst4); let mut transfer: Transfer< StreamX, Rx, @@ -124,7 +124,8 @@ impl SBGManager { sbg_flush::spawn().ok(); }, ); - sbg.read_data(&unsafe { SBG_BUFFER.assume_init_read() }); + // sbg.setup(); + // sbg.read_data(&unsafe { SBG_BUFFER.assume_init_read() }); SBGManager { sbg_device: sbg, @@ -160,6 +161,7 @@ pub fn sbg_get_time() -> u32 { } pub async fn sbg_handle_data(mut cx: sbg_handle_data::Context<'_>, data: CallbackData) { + cx.shared.data_manager.lock(|manager| match data { CallbackData::UtcTime(x) => manager.utc_time = Some(x), CallbackData::Air(x) => manager.air = Some(x), @@ -167,7 +169,11 @@ pub async fn sbg_handle_data(mut cx: sbg_handle_data::Context<'_>, data: Callbac CallbackData::EkfNav(x) => manager.ekf_nav = Some(x), CallbackData::Imu(x) => manager.imu = Some(x), CallbackData::GpsVel(x) => manager.gps_vel = Some(x), - CallbackData::GpsPos(x) => manager.gps_pos = Some(x), + CallbackData::GpsPos(x) => { + info!("{}", x.0.latitude.unwrap()); + panic!("fucl"); + manager.gps_pos = Some(x) + }, }); } @@ -196,16 +202,16 @@ pub async fn sbg_sd_task( * Handles the SBG data. */ pub fn sbg_dma(mut cx: crate::app::sbg_dma::Context) { - info!("DMA interrupt"); cx.shared.sbg_manager.lock(|sbg| { match &mut sbg.xfer { Some(xfer) => { if xfer.get_transfer_complete_flag() { - let data = unsafe { SBG_BUFFER.assume_init_read() }; - xfer.clear_transfer_complete_interrupt(); + let data = unsafe { SBG_BUFFER.assume_init_read() }.clone(); xfer.next_transfer( unsafe { (*core::ptr::addr_of_mut!(SBG_BUFFER)).assume_init_mut() }, // Uninitialised memory ); + // info!("{}", data); + xfer.clear_transfer_complete_interrupt(); sbg.sbg_device.read_data(&data); // crate::app::sbg_sd_task::spawn(data).ok(); } diff --git a/libraries/sbg-rs/src/data_conversion.rs b/libraries/sbg-rs/src/data_conversion.rs index f54c8c6d..56f9af82 100644 --- a/libraries/sbg-rs/src/data_conversion.rs +++ b/libraries/sbg-rs/src/data_conversion.rs @@ -29,7 +29,7 @@ impl From for (GpsPos1, GpsPos2, GpsPosAcc) { let status = GpsPositionStatus::new(value.status); let valid = matches!(status.get_status(), Some(GpsPositionStatusE::SolComputed)); - + // defmt::info!("GpsPos: {:?}", value.latitude); ( GpsPos1 { latitude: if valid { Some(value.latitude) } else { None }, diff --git a/libraries/sbg-rs/src/sbg.rs b/libraries/sbg-rs/src/sbg.rs index 76199e3a..a6041c5a 100644 --- a/libraries/sbg-rs/src/sbg.rs +++ b/libraries/sbg-rs/src/sbg.rs @@ -1,10 +1,5 @@ use crate::bindings::{ - self, _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_WARNING, _SbgEComLog_SBG_ECOM_LOG_AIR_DATA, - _SbgEComLog_SBG_ECOM_LOG_EKF_NAV, _SbgEComLog_SBG_ECOM_LOG_GPS1_POS, - _SbgEComLog_SBG_ECOM_LOG_GPS1_VEL, _SbgEComLog_SBG_ECOM_LOG_UTC_TIME, - _SbgEComOutputMode_SBG_ECOM_OUTPUT_MODE_DIV_40, _SbgErrorCode_SBG_NO_ERROR, - _SbgErrorCode_SBG_NULL_POINTER, _SbgErrorCode_SBG_READ_ERROR, _SbgErrorCode_SBG_WRITE_ERROR, - sbgEComCmdOutputSetConf, sbgEComHandle, + self, _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_WARNING, _SbgEComLog_SBG_ECOM_LOG_AIR_DATA, _SbgEComLog_SBG_ECOM_LOG_EKF_NAV, _SbgEComLog_SBG_ECOM_LOG_GPS1_POS, _SbgEComLog_SBG_ECOM_LOG_GPS1_RAW, _SbgEComLog_SBG_ECOM_LOG_GPS1_VEL, _SbgEComLog_SBG_ECOM_LOG_UTC_TIME, _SbgEComOutputMode_SBG_ECOM_OUTPUT_MODE_DIV_40, _SbgErrorCode_SBG_NO_ERROR, _SbgErrorCode_SBG_NULL_POINTER, _SbgErrorCode_SBG_READ_ERROR, _SbgErrorCode_SBG_WRITE_ERROR, sbgEComCmdOutputSetConf, sbgEComHandle }; use crate::bindings::{ _SbgBinaryLogData, _SbgDebugLogType, _SbgEComClass_SBG_ECOM_CLASS_LOG_ECOM_0, _SbgEComHandle, @@ -190,6 +185,19 @@ impl SBG { warn!("Unable to configure UTC Time logs to 40 cycles"); } + let errorCode: _SbgErrorCode = unsafe { + sbgEComCmdOutputSetConf( + &mut self.handle, + _SbgEComOutputPort_SBG_ECOM_OUTPUT_PORT_A, + _SbgEComClass_SBG_ECOM_CLASS_LOG_ECOM_0, + _SbgEComLog_SBG_ECOM_LOG_GPS1_POS, + _SbgEComOutputMode_SBG_ECOM_OUTPUT_MODE_DIV_40, + ) + }; + if errorCode != _SbgErrorCode_SBG_NO_ERROR { + warn!("Unable to configure UTC Time logs to 40 cycles"); + } + // SAFETY: We are calling a C function. // This is safe because it is assumed the SBG library is safe. let errorCode: _SbgErrorCode = unsafe { @@ -374,9 +382,18 @@ impl SBG { callback(CallbackData::EkfNav((*pLogData).ekfNavData.into())) } _SbgEComLog_SBG_ECOM_LOG_GPS1_POS => { + info!("GPS Data"); callback(CallbackData::GpsPos((*pLogData).gpsPosData.into())) } - _ => (), + _SbgEComLog_SBG_ECOM_LOG_GPS1_VEL => { + + callback(CallbackData::GpsVel((*pLogData).gpsVelData.into())) + } + _SbgEComLog_SBG_ECOM_LOG_GPS1_HDT => { + // info!("Heading Data"); + } + _ => { + }, } } } From b8e1cb1cefbc12d060803c0070229622d081b551 Mon Sep 17 00:00:00 2001 From: NoahSprenger Date: Tue, 13 Aug 2024 18:00:29 +0000 Subject: [PATCH 27/47] Add GPS board. --- Cargo.lock | 59 +++- boards/gps/Cargo.toml | 26 ++ boards/gps/src/communication.rs | 388 ++++++++++++++++++++++++++ boards/gps/src/data_manager.rs | 161 +++++++++++ boards/gps/src/main.rs | 466 ++++++++++++++++++++++++++++++++ boards/gps/src/types.rs | 17 ++ 6 files changed, 1111 insertions(+), 6 deletions(-) create mode 100644 boards/gps/Cargo.toml create mode 100644 boards/gps/src/communication.rs create mode 100644 boards/gps/src/data_manager.rs create mode 100644 boards/gps/src/main.rs create mode 100644 boards/gps/src/types.rs diff --git a/Cargo.lock b/Cargo.lock index 35f99efe..b6b54041 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -386,9 +386,9 @@ dependencies = [ [[package]] name = "cpufeatures" -version = "0.2.12" +version = "0.2.13" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "53fe5e26ff1b7aef8bca9c6080520cfb8d9333c7568e1829cef191a9723e5504" +checksum = "51e852e6dc9a5bed1fae92dd2375037bf2b768725bf3be87811edee3249d09ad" dependencies = [ "libc", ] @@ -706,6 +706,29 @@ dependencies = [ "wasi", ] +[[package]] +name = "gps" +version = "0.1.0" +dependencies = [ + "atsamd-hal", + "common-arm", + "common-arm-atsame", + "cortex-m", + "cortex-m-rt", + "cortex-m-rtic", + "defmt", + "defmt-rtt", + "embedded-alloc", + "embedded-sdmmc 0.8.0", + "heapless 0.7.17", + "messages", + "panic-probe", + "postcard", + "systick-monotonic", + "typenum", + "ublox", +] + [[package]] name = "hash32" version = "0.2.1" @@ -1592,9 +1615,9 @@ checksum = "a3f0bf26fd526d2a95683cd0f87bf103b8539e2ca1ef48ce002d67aad59aa0b4" [[package]] name = "serde" -version = "1.0.206" +version = "1.0.207" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5b3e4cd94123dd520a128bcd11e34d9e9e423e7e3e50425cb1b4b1e3549d0284" +checksum = "5665e14a49a4ea1b91029ba7d3bca9f299e1f7cfa194388ccc20f14743e784f2" dependencies = [ "serde_derive", ] @@ -1610,9 +1633,9 @@ dependencies = [ [[package]] name = "serde_derive" -version = "1.0.206" +version = "1.0.207" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fabfb6138d2383ea8208cf98ccf69cdfb1aff4088460681d84189aa259762f97" +checksum = "6aea2634c86b0e8ef2cfdc0c340baede54ec27b1e46febd7f80dffb2aa44a00e" dependencies = [ "proc-macro2 1.0.86", "quote 1.0.36", @@ -1898,6 +1921,30 @@ version = "1.17.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "42ff0bf0c66b8238c6f3b578df37d0b7848e55df8577b3f74f92a69acceeb825" +[[package]] +name = "ublox" +version = "0.4.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0ad0c3df1bc466bd97f94ab437aab9116d5c15909c8a5cb9905ee3c11df998e8" +dependencies = [ + "bitflags 2.6.0", + "chrono", + "num-traits", + "serde", + "ublox_derive", +] + +[[package]] +name = "ublox_derive" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dea60f23c86d2329b7162f6ebb82854bc77b992a9984e1fdfe8707f88e0e3e69" +dependencies = [ + "proc-macro2 1.0.86", + "quote 1.0.36", + "syn 1.0.109", +] + [[package]] name = "unarray" version = "0.1.4" diff --git a/boards/gps/Cargo.toml b/boards/gps/Cargo.toml new file mode 100644 index 00000000..5447d4b9 --- /dev/null +++ b/boards/gps/Cargo.toml @@ -0,0 +1,26 @@ +[package] +name = "gps" +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 = "^0.7.0" +cortex-m-rtic = "1.1.3" +systick-monotonic = "1.0.1" +postcard = "1.0.2" +heapless = "0.7.16" +common-arm-atsame = { path = "../../libraries/common-arm-atsame" } +common-arm = { path = "../../libraries/common-arm" } +atsamd-hal = { workspace = true } +messages = { path = "../../libraries/messages" } +typenum = "1.16.0" +embedded-sdmmc = "0.8.0" +#panic-halt = "0.2.0" +defmt = "0.3" +defmt-rtt = "0.4" +panic-probe = { version = "0.3", features = ["print-defmt"] } +ublox = {version = "0.4.5", features = ['serde', 'alloc'], default-features = false} +embedded-alloc = "0.5.0" diff --git a/boards/gps/src/communication.rs b/boards/gps/src/communication.rs new file mode 100644 index 00000000..bd81b73b --- /dev/null +++ b/boards/gps/src/communication.rs @@ -0,0 +1,388 @@ + +use crate::data_manager::DataManager; +use atsamd_hal::can::Dependencies; +use atsamd_hal::clock::v2::ahb::AhbClk; +use atsamd_hal::clock::v2::gclk::{Gclk0Id}; +use atsamd_hal::clock::v2::pclk::Pclk; +use atsamd_hal::clock::v2::types::{Can0, Can1}; +use atsamd_hal::clock::v2::Source; +use atsamd_hal::gpio::{ + Alternate, AlternateI, Pin, I, PA22, PA23, +}; +use atsamd_hal::gpio::{AlternateH, H, PB14, PB15}; +use atsamd_hal::pac::{CAN0, CAN1}; +use atsamd_hal::prelude::*; +use atsamd_hal::typelevel::Increment; +use common_arm::HydraError; +use common_arm_atsame::mcan; +use common_arm_atsame::mcan::message::{rx, Raw}; +use common_arm_atsame::mcan::tx_buffers::DynTx; +use defmt::error; +use defmt::info; +use heapless::Vec; +use mcan::bus::Can; +use mcan::embedded_can as ecan; +use mcan::interrupt::state::EnabledLine0; +use mcan::interrupt::{Interrupt, OwnedInterruptSet}; +use mcan::message::tx; +use mcan::messageram::SharedMemory; +use mcan::{ + config::{BitTiming, Mode}, + filter::{Action, Filter}, +}; +use messages::mavlink::{self}; +use messages::Message; +use postcard::from_bytes; +use systick_monotonic::*; +use typenum::{U0, U128, U32, U64}; +pub struct Capacities; + +impl mcan::messageram::Capacities for Capacities { + type StandardFilters = U128; + type ExtendedFilters = U64; + type RxBufferMessage = rx::Message<64>; + type DedicatedRxBuffers = U64; + type RxFifo0Message = rx::Message<64>; + type RxFifo0 = U64; + type RxFifo1Message = rx::Message<64>; + type RxFifo1 = U64; + type TxMessage = tx::Message<64>; + type TxBuffers = U32; + type DedicatedTxBuffers = U0; + type TxEventFifo = U32; +} + +// macro_rules! create_filter { +// ($can:expr, $action:expr, $sender:expr) => { +// $can.filters_standard() +// .push(Filter::Classic { +// action: $action, +// filter: ecan::StandardId::new($sender.into()).unwrap(), +// mask: ecan::StandardId::ZERO, +// }) +// .unwrap_or_else(|_| panic!("Filter Error")); +// }; +// } + +pub struct CanDevice0 { + pub can: Can< + 'static, + Can0, + Dependencies>, Pin>, CAN0>, + Capacities, + >, + line_interrupts: OwnedInterruptSet, +} + +impl CanDevice0 { + pub fn new( + can_rx: Pin, + can_tx: Pin, + pclk_can: Pclk, + ahb_clock: AhbClk, + peripheral: CAN0, + gclk0: S, + can_memory: &'static mut SharedMemory, + loopback: bool, + ) -> (Self, S::Inc) + where + S: Source + Increment, + { + let (can_dependencies, gclk0) = + Dependencies::new(gclk0, pclk_can, ahb_clock, can_rx, can_tx, peripheral); + + let mut can = mcan::bus::CanConfigurable::new( + 200.kHz(), + can_dependencies, + can_memory, + ) + .unwrap(); + can.config().mode = Mode::Fd { + allow_bit_rate_switching: false, + data_phase_timing: BitTiming::new(fugit::RateExtU32::kHz(500)), + }; + + if loopback { + can.config().loopback = true; + } + + let interrupts_to_be_enabled = can + .interrupts() + .split( + [ + Interrupt::RxFifo0NewMessage, + Interrupt::RxFifo0Full, + Interrupt::RxFifo0MessageLost, + Interrupt::RxFifo1NewMessage, + Interrupt::RxFifo1Full, + Interrupt::RxFifo1MessageLost, + ] + .into_iter() + .collect(), + ) + .unwrap(); + + // Line 0 and 1 are connected to the same interrupt line + let line_interrupts = can + .interrupt_configuration() + .enable_line_0(interrupts_to_be_enabled); + + can.filters_standard() + .push(Filter::Classic { + action: Action::StoreFifo0, + filter: ecan::StandardId::new(messages::sender::Sender::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::sender::Sender::SensorBoard.into()) + .unwrap(), + mask: ecan::StandardId::ZERO, + }) + .unwrap_or_else(|_| panic!("Sensor filter")); + + can.filters_standard() + .push(Filter::Classic { + action: Action::StoreFifo0, + filter: ecan::StandardId::new(messages::sender::Sender::PowerBoard.into()).unwrap(), + mask: ecan::StandardId::ZERO, + }) + .unwrap_or_else(|_| panic!("Power filter")); + + can.filters_standard() + .push(Filter::Classic { + action: Action::StoreFifo0, + filter: ecan::StandardId::new(messages::sender::Sender::GroundStation.into()) + .unwrap(), + mask: ecan::StandardId::ZERO, + }) + .unwrap_or_else(|_| panic!("Ground Station filter")); + + let can = can.finalize().unwrap(); + ( + CanDevice0 { + can, + line_interrupts, + }, + gclk0, + ) + } + pub fn send_message(&mut self, m: Message) -> Result<(), HydraError> { + let payload: Vec = postcard::to_vec(&m)?; + self.can.tx.transmit_queued( + tx::MessageBuilder { + id: ecan::Id::Standard(ecan::StandardId::new(m.sender.into()).unwrap()), + frame_type: tx::FrameType::FlexibleDatarate { + payload: &payload[..], + bit_rate_switching: false, + force_error_state_indicator: false, + }, + store_tx_event: None, + } + .build()?, + )?; + Ok(()) + } + pub fn process_data(&mut self, data_manager: &mut DataManager) { + let line_interrupts = &self.line_interrupts; + for interrupt in line_interrupts.iter_flagged() { + match interrupt { + Interrupt::RxFifo0NewMessage => { + for message in &mut self.can.rx_fifo_0 { + match from_bytes::(message.data()) { + Ok(data) => { + // info!("Received message {}", data.clone()); + + data_manager.handle_data(data); + } + Err(_) => { + // error!("Error, ErrorContext::UnkownCanMessage"); + } + } + } + } + Interrupt::RxFifo1NewMessage => { + for message in &mut self.can.rx_fifo_1 { + match from_bytes::(message.data()) { + Ok(data) => { + info!("Received message {}", data.clone()); + + data_manager.handle_data(data); + } + Err(_) => { + // error!("Error, ErrorContext::UnkownCanMessage"); + } + } + } + } + _ => (), + } + } + } +} + +// So I really am not a fan of this can device 0 and can device 1, I think it would be better to have a single generic can manager +// that can also take a list of filters and apply them. + +pub struct CanCommandManager { + pub can: Can< + 'static, + Can1, + Dependencies>, Pin>, CAN1>, + Capacities, + >, + line_interrupts: OwnedInterruptSet, +} + +impl CanCommandManager { + pub fn new( + can_rx: Pin, + can_tx: Pin, + pclk_can: Pclk, + ahb_clock: AhbClk, + peripheral: CAN1, + gclk0: S, + can_memory: &'static mut SharedMemory, + loopback: bool, + ) -> (Self, S::Inc) + where + S: Source + Increment, + { + let (can_dependencies, gclk0) = + Dependencies::new(gclk0, pclk_can, ahb_clock, can_rx, can_tx, peripheral); + + let mut can = mcan::bus::CanConfigurable::new( + 200.kHz(), // needs a prescaler of 6 to be changed in the mcan source because mcan is meh. + can_dependencies, + can_memory, + ) + .unwrap(); + can.config().mode = Mode::Fd { + allow_bit_rate_switching: false, + data_phase_timing: BitTiming::new(fugit::RateExtU32::kHz(500)), + }; + + if loopback { + can.config().loopback = true; + } + + let interrupts_to_be_enabled = can + .interrupts() + .split( + [ + Interrupt::RxFifo0NewMessage, + Interrupt::RxFifo0Full, + // Interrupt::RxFifo0MessageLost, + Interrupt::RxFifo1NewMessage, + Interrupt::RxFifo1Full, + // Interrupt::RxFifo1MessageLost, + ] + .into_iter() + .collect(), + ) + .unwrap(); + + // Line 0 and 1 are connected to the same interrupt line + let line_interrupts = can + .interrupt_configuration() + .enable_line_0(interrupts_to_be_enabled); + + can.filters_standard() + .push(Filter::Classic { + action: Action::StoreFifo0, + filter: ecan::StandardId::new(messages::sender::Sender::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::sender::Sender::SensorBoard.into()) + .unwrap(), + mask: ecan::StandardId::ZERO, + }) + .unwrap_or_else(|_| panic!("Sensor filter")); + + can.filters_standard() + .push(Filter::Classic { + action: Action::StoreFifo0, + filter: ecan::StandardId::new(messages::sender::Sender::PowerBoard.into()).unwrap(), + mask: ecan::StandardId::ZERO, + }) + .unwrap_or_else(|_| panic!("Power filter")); + + can.filters_standard() + .push(Filter::Classic { + action: Action::StoreFifo0, + filter: ecan::StandardId::new(messages::sender::Sender::GroundStation.into()) + .unwrap(), + mask: ecan::StandardId::ZERO, + }) + .unwrap_or_else(|_| panic!("Ground Station filter")); + + let can = can.finalize().unwrap(); + ( + CanCommandManager { + can, + line_interrupts, + }, + gclk0, + ) + } + pub fn send_message(&mut self, m: Message) -> Result<(), HydraError> { + let payload: Vec = postcard::to_vec(&m)?; + self.can.tx.transmit_queued( + tx::MessageBuilder { + id: ecan::Id::Standard(ecan::StandardId::new(m.sender.into()).unwrap()), + frame_type: tx::FrameType::FlexibleDatarate { + payload: &payload[..], + bit_rate_switching: false, + force_error_state_indicator: false, + }, + store_tx_event: None, + } + .build()?, + )?; + Ok(()) + } + pub fn process_data(&mut self, data_manager: &mut DataManager) { + let line_interrupts = &self.line_interrupts; + for interrupt in line_interrupts.iter_flagged() { + match interrupt { + Interrupt::RxFifo0NewMessage => { + for message in &mut self.can.rx_fifo_0 { + match from_bytes::(message.data()) { + Ok(data) => { + info!("Received message {}", data.clone()); + data_manager.handle_command(data); + } + Err(_) => { + error!("Error, ErrorContext::UnkownCanMessage"); + } + } + } + } + Interrupt::RxFifo1NewMessage => { + for message in &mut self.can.rx_fifo_1 { + match from_bytes::(message.data()) { + Ok(data) => { + info!("Received message {}", data.clone()); + data_manager.handle_command(data); + } + Err(_) => { + error!("Error, ErrorContext::UnkownCanMessage"); + } + } + } + } + _ => (), + } + } + } +} \ No newline at end of file diff --git a/boards/gps/src/data_manager.rs b/boards/gps/src/data_manager.rs new file mode 100644 index 00000000..e093a425 --- /dev/null +++ b/boards/gps/src/data_manager.rs @@ -0,0 +1,161 @@ +use defmt::info; +use messages::command::RadioRate; +use messages::state::StateData; +use messages::Message; + +#[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 logging_rate: Option, + pub recovery_sensing: 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, + logging_rate: Some(RadioRate::Slow), // start slow. + recovery_sensing: 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); + return RadioRate::Slow; + } + + /// Do not clone instead take to reduce CPU load. + pub fn take_sensors(&mut self) -> [Option; 14] { + [ + 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.recovery_sensing.take(), + ] + } + + pub fn clone_states(&self) -> [Option; 1] { + [self.state.clone()] + } + pub fn handle_command(&mut self, command: Message) { + info!("Handling command"); + match command.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(_) => {} + }, + _ => {} + } + } + pub fn handle_data(&mut self, data: Message) { + 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::sensor::SensorData::RecoverySensing(_) => { + self.recovery_sensing = Some(data); + } + }, + messages::Data::State(state) => { + self.state = Some(state.data); + } + // messages::Data::Command(command) => match command.data { + // messages::command::CommandData::RadioRateChange(command_data) => { + // self.logging_rate = Some(command_data.rate); + // } + // 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/gps/src/main.rs b/boards/gps/src/main.rs new file mode 100644 index 00000000..05f8ea7a --- /dev/null +++ b/boards/gps/src/main.rs @@ -0,0 +1,466 @@ +#![no_std] +#![no_main] + +mod communication; +mod data_manager; +mod types; + +use atsamd_hal as hal; +use atsamd_hal::rtc::Count32Mode; +use common_arm::*; +use common_arm_atsame::mcan; + +use communication::Capacities; +use communication::CanCommandManager; +use core::cell::RefCell; +use cortex_m::interrupt::Mutex; +use data_manager::DataManager; +use defmt::info; +use defmt_rtt as _; // global logger +use fugit::ExtU64; +use fugit::RateExtU32; +use hal::clock::v2::pclk::Pclk; +use hal::clock::v2::Source; +use hal::gpio::Pins; +use hal::gpio::{ + Pin, PushPullOutput, PA02, PA03, +}; +use hal::prelude::*; +use hal::sercom::uart; +use hal::sercom::IoSet1; +use mcan::messageram::SharedMemory; +use messages::*; +use panic_probe as _; +use systick_monotonic::*; +use types::*; +use mavlink::embedded::Read; + + +#[inline(never)] +#[defmt::panic_handler] +fn panic() -> ! { + cortex_m::asm::udf() +} + +#[global_allocator] +static HEAP: embedded_alloc::Heap = embedded_alloc::Heap::empty(); + +/// Hardfault handler. +/// +/// Terminates the application and makes a semihosting-capable debug tool exit +/// with an error. This seems better than the default, which is to spin in a +/// loop. +#[cortex_m_rt::exception] +unsafe fn HardFault(_frame: &cortex_m_rt::ExceptionFrame) -> ! { + loop {} +} + +static RTC: Mutex>>> = Mutex::new(RefCell::new(None)); + +#[rtic::app(device = hal::pac, peripherals = true, dispatchers = [EVSYS_0, EVSYS_1, EVSYS_2])] +mod app { + + use ublox::PacketRef; + + use super::*; + + #[shared] + struct Shared { + em: ErrorManager, + data_manager: DataManager, + can0: communication::CanDevice0, + can_command_manager: CanCommandManager, + } + + #[local] + struct Local { + led_green: Pin, + led_red: Pin, + gps_uart: GpsUart, + // sd_manager: SdManager< + // Spi< + // Config< + // Pads< + // hal::pac::SERCOM0, + // IoSet3, + // Pin>, + // Pin>, + // Pin>, + // >, + // >, + // Duplex, + // >, + // Pin>, + // >, + } + + #[monotonic(binds = SysTick, default = true)] + type MyMono = Systick<100>; // 100 Hz / 10 ms granularity + + #[init(local = [ + #[link_section = ".can"] + can_memory: SharedMemory = SharedMemory::new(), + #[link_section = ".can_command"] + can_command_memory: SharedMemory = SharedMemory::new() + ])] + fn init(cx: init::Context) -> (Shared, Local, init::Monotonics) { + { + use core::mem::MaybeUninit; + const HEAP_SIZE: usize = 1024; + static mut HEAP_MEM: [MaybeUninit; HEAP_SIZE] = [MaybeUninit::uninit(); HEAP_SIZE]; + unsafe { HEAP.init(HEAP_MEM.as_ptr() as usize, HEAP_SIZE) } + } + let mut peripherals = cx.device; + let core = cx.core; + let pins = Pins::new(peripherals.PORT); + + // let mut dmac = DmaController::init(peripherals.DMAC, &mut peripherals.PM); + // let dmaChannels = dmac.split(); + + /* Logging Setup */ + HydraLogging::set_ground_station_callback(queue_gs_message); + + /* Clock setup */ + let (_, clocks, tokens) = atsamd_hal::clock::v2::clock_system_at_reset( + peripherals.OSCCTRL, + peripherals.OSC32KCTRL, + peripherals.GCLK, + peripherals.MCLK, + &mut peripherals.NVMCTRL, + ); + + let gclk0 = clocks.gclk0; + + // SAFETY: Misusing the PAC API can break the system. + // This is safe because we only steal the MCLK. + let (_, _, _, mut mclk) = unsafe { clocks.pac.steal() }; + + /* CAN config */ + + let (pclk_can, gclk0) = Pclk::enable(tokens.pclks.can0, gclk0); + let (can0, gclk0) = communication::CanDevice0::new( + pins.pa23.into_mode(), + pins.pa22.into_mode(), + pclk_can, + clocks.ahbs.can0, + peripherals.CAN0, + gclk0, + cx.local.can_memory, + false, + ); + + let (pclk_can_command, gclk0) = Pclk::enable(tokens.pclks.can1, gclk0); + let (can_command_manager, gclk0) = CanCommandManager::new( + pins.pb15.into_mode(), + pins.pb14.into_mode(), + pclk_can_command, + clocks.ahbs.can1, + peripherals.CAN1, + gclk0, + cx.local.can_command_memory, + false, + ); + + // setup external osc + // let xosc0 = atsamd_hal::clock::v2::xosc::Xosc::from_crystal( + // tokens.xosc0, + // pins.pa14, + // pins.pa15, + // 48_000_000.Hz(), + // ).current(CrystalCurrent::Medium) + // .loop_control(true) + // .low_buf_gain(true) + // .start_up_delay(StartUpDelay::Delay488us).enable(); + // while !xosc0.is_ready() { + // info!("Waiting for XOSC0 to stabilize"); + // } + + // let (mut gclk0, dfll) = + // hal::clock::v2::gclk::Gclk::from_source(tokens.gclks.gclk2, xosc0); + // let gclk2 = gclk2.div(gclk::GclkDiv8::Div(1)).enable(); + + // /* SD config */ + // let (mut gclk1, dfll) = + // hal::clock::v2::gclk::Gclk::from_source(tokens.gclks.gclk1, clocks.dfll); + // let gclk1 = gclk1.div(gclk::GclkDiv16::Div(3)).enable(); // 48 / 3 = 16 MHzs + // let (pclk_sd, gclk1) = Pclk::enable(tokens.pclks.sercom0, gclk1); + + // let pads_spi = spi::Pads::::default() + // .sclk(pins.pa05) + // .data_in(pins.pa07) + // .data_out(pins.pa04); + // let sdmmc_spi = spi::Config::new(&mclk, peripherals.SERCOM0, pads_spi, pclk_sd.freq()) + // .length::() + // .bit_order(spi::BitOrder::MsbFirst) + // .spi_mode(spi::MODE_0) + // .enable(); + // let sd_manager = SdManager::new(sdmmc_spi, pins.pa06.into_push_pull_output()); + + let (pclk_gps, gclk0) = Pclk::enable(tokens.pclks.sercom2, gclk0); + //info!("here"); + let pads = hal::sercom::uart::Pads::::default() + .rx(pins.pa13) + .tx(pins.pa12); + + let mut gps_uart = GpsUartConfig::new(&mclk, peripherals.SERCOM2, pads, pclk_gps.freq()) + .baud( + RateExtU32::Hz(9600), + uart::BaudMode::Fractional(uart::Oversampling::Bits16), + ) + .enable(); + gps_uart.enable_interrupts(hal::sercom::uart::Flags::RXC); + + /* Status LED */ + // info!("Setting up LED"); + // let led = pins.pa02.into_push_pull_output(); + let mut gps_enable = pins.pb09.into_push_pull_output(); + gps_enable.set_high().ok(); + // gps_enable.set_low().ok(); + /* Spawn tasks */ + // sensor_send::spawn().ok(); + // state_send::spawn().ok(); + let rtc = hal::rtc::Rtc::clock_mode( + peripherals.RTC, + atsamd_hal::fugit::RateExtU32::Hz(1024), + &mut mclk, + ); + let mut rtc = rtc.into_count32_mode(); // hal bug this must be done + rtc.set_count32(0); + cortex_m::interrupt::free(|cs| { + RTC.borrow(cs).replace(Some(rtc)); + }); + let mut led_green = pins.pa03.into_push_pull_output(); + let mut led_red = pins.pa02.into_push_pull_output(); + led_green.set_low(); + led_red.set_low(); + blink::spawn().ok(); + let mono = Systick::new(core.SYST, gclk0.freq().to_Hz()); + + ( + Shared { + em: ErrorManager::new(), + data_manager: DataManager::new(), + can0, + can_command_manager, + }, + Local { + led_green, + led_red, + gps_uart, + // sd_manager, + }, + init::Monotonics(mono), + ) + } + + /// Idle task for when no other tasks are running. + #[idle] + fn idle(_: idle::Context) -> ! { + loop {} + } + + #[task(binds = SERCOM2_2, local = [gps_uart], shared = [&em])] + fn gps_rx(cx: gps_rx::Context) { + cx.shared.em.run(|| { + cortex_m::interrupt::free(|cs| { + let mut buf: [u8; 256] = [0; 256]; + let mut bytes: [u8; 256] = [0; 256]; + for i in 0..buf.len() { + let item = nb::block!(cx.local.gps_uart.read()).unwrap(); + bytes[i] = item; + } + let buf = ublox::FixedLinearBuffer::new(&mut buf[..]); + let mut parser = ublox::Parser::new(buf); + let mut msgs = parser.consume(&bytes); + while let Some(msg) = msgs.next() { + match msg { + Ok(msg) => { + match msg { + ublox::PacketRef::NavPosLlh(x) => { + info!("GPS latitude: {:?}", x.lat_degrees()); + } + ublox::PacketRef::NavStatus(x) => { + info!("GPS fix stat: {:?}", x.fix_stat_raw()); + } + ublox::PacketRef::NavDop(x) => { + info!("GPS geometric drop: {:?}", x.geometric_dop()); + } + ublox::PacketRef::NavSat(x) => { + info!("GPS num sats used: {:?}", x.num_svs()); + } + ublox::PacketRef::NavVelNed(x) => { + info!("GPS velocity north: {:?}", x.vel_north()); + } + ublox::PacketRef::NavPvt(x) => { + info!("GPS nun sats PVT: {:?}", x.num_satellites()); + } + _ => { + info!("GPS Message not handled."); + } + } + } + Err(e) => { + info!("GPS parse Error"); + } + } + } + + }); + Ok(()) + }); + } + + /// Handles the CAN1 interrupt. + #[task(priority = 3, binds = CAN1, shared = [can_command_manager, data_manager])] + fn can_command(mut cx: can_command::Context) { + info!("CAN1 interrupt"); + cx.shared.can_command_manager.lock(|can| { + cx.shared + .data_manager + .lock(|data_manager| can.process_data(data_manager)); + }); + } + + /// Handles the CAN0 interrupt. + #[task(priority = 3, binds = CAN0, shared = [can0, data_manager])] + fn can0(mut cx: can0::Context) { + // info!("CAN0 interrupt"); + cx.shared.can0.lock(|can| { + cx.shared + .data_manager + .lock(|data_manager| can.process_data(data_manager)); + }); + } + /** + * Sends a message over CAN. + */ + #[task(capacity = 10, shared = [can0, &em])] + fn send_internal(mut cx: send_internal::Context, m: Message) { + info!("{}", m.clone()); + cx.shared.em.run(|| { + cx.shared.can0.lock(|can| can.send_message(m))?; + Ok(()) + }); + } + + /** + * Sends a message over CAN. + */ + #[task(priority = 3, capacity = 5, shared = [can_command_manager, &em])] + fn send_command(mut cx: send_command::Context, m: Message) { + info!("{}", m.clone()); + cx.shared.em.run(|| { + cx.shared + .can_command_manager + .lock(|can| can.send_message(m))?; + 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) { + let message = Message::new( + cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + rtc.count32() + }), + COM_ID, + d.into(), + ); + + send_internal::spawn(message).ok(); + } + + // #[task(capacity = 10, local = [sd_manager], shared = [&em])] + // fn sd_dump(cx: sd_dump::Context, m: Message) { + // let manager = cx.local.sd_manager; + // cx.shared.em.run(|| { + // let mut buf: [u8; 255] = [0; 255]; + // let msg_ser = postcard::to_slice_cobs(&m, &mut buf)?; + // if let Some(mut file) = manager.file.take() { + // manager.write(&mut file, &msg_ser)?; + // manager.file = Some(file); + // } else if let Ok(mut file) = manager.open_file("log.txt") { + // manager.write(&mut file, &msg_ser)?; + // manager.file = Some(file); + // } + // Ok(()) + // }); + // } + + // /** + // * Sends information about the sensors. + // */ + // #[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())); + + // cx.shared.em.run(|| { + // for msg in sensors { + // match msg { + // Some(x) => { + // spawn!(send_gs, x.clone())?; + // // spawn!(sd_dump, x)?; + // } + // None => { + // continue; + // } + // } + // } + // Ok(()) + // }); + // match logging_rate { + // RadioRate::Fast => { + // spawn_after!(sensor_send, ExtU64::millis(100)).ok(); + // } + // RadioRate::Slow => { + // spawn_after!(sensor_send, ExtU64::millis(250)).ok(); + // } + // } + // } + + // /// Handles the radio interrupt. + // /// This only needs to be called when the radio has data to read, this is why an interrupt handler is used above polling which would waste resources. + // /// We use a critical section to ensure that we are not interrupted while servicing the mavlink message. + // #[task(priority = 3, binds = SERCOM2_2, shared = [&em, radio_manager])] + // fn radio_rx(mut cx: radio_rx::Context) { + // cx.shared.radio_manager.lock(|radio_manager| { + // cx.shared.em.run(|| { + // cortex_m::interrupt::free(|cs| { + // let m = radio_manager.receive_message()?; + // info!("Received message {}", m.clone()); + // spawn!(send_command, m) + + // })?; + // Ok(()) + // }); + // }); + // } + + /** + * Simple blink task to test the system. + * Acts as a heartbeat for the system. + */ + #[task(local = [led_green, led_red], shared = [&em])] + fn blink(cx: blink::Context) { + cx.shared.em.run(|| { + if cx.shared.em.has_error() { + cx.local.led_red.toggle()?; + spawn_after!(blink, ExtU64::millis(200))?; + } else { + cx.local.led_green.toggle()?; + spawn_after!(blink, ExtU64::secs(1))?; + } + Ok(()) + }); + } + + // extern "Rust" { + // #[task(binds = DMAC_0, shared=[&em], local=[radio_manager])] + // fn radio_dma(context: radio_dma::Context); + // } +} diff --git a/boards/gps/src/types.rs b/boards/gps/src/types.rs new file mode 100644 index 00000000..3a408f07 --- /dev/null +++ b/boards/gps/src/types.rs @@ -0,0 +1,17 @@ +use atsamd_hal::gpio::*; +use atsamd_hal::sercom::uart::EightBit; +use atsamd_hal::sercom::{uart, uart::Duplex, IoSet1, Sercom2}; +use messages::sender::Sender; +use messages::sender::Sender::CommunicationBoard; + +// ------- +// Sender ID +// ------- +pub static COM_ID: Sender = CommunicationBoard; + +// ------- +// GPS UART +// ------- +pub type GpsPads = uart::PadsFromIds; +pub type GpsUartConfig = uart::Config; +pub type GpsUart = uart::Uart; From 45222310ab79b985d35946dfd60e6c1b3c51e8f1 Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Wed, 14 Aug 2024 00:25:32 -0400 Subject: [PATCH 28/47] WIP --- .cargo/config.toml | 4 +- boards/gps/src/main.rs | 107 +++++++++++++++++++++++++++++++++--- boards/gps/src/types.rs | 7 ++- boards/nav/src/main.rs | 6 +- libraries/sbg-rs/src/sbg.rs | 8 +-- 5 files changed, 113 insertions(+), 19 deletions(-) diff --git a/.cargo/config.toml b/.cargo/config.toml index 4951091d..e3b9e473 100644 --- a/.cargo/config.toml +++ b/.cargo/config.toml @@ -1,7 +1,7 @@ [target.'cfg(all(target_arch = "arm", target_os = "none"))'] # This runner needs to be parametric so we can pass in the chip name -runner = "probe-rs run --chip STM32H733VGTx --protocol swd" -#runner = "probe-rs run --chip ATSAME51J18A --protocol swd" +#runner = "probe-rs run --chip STM32H733VGTx --protocol swd" +runner = "probe-rs run --chip ATSAME51J18A --protocol swd" rustflags = [ "-C", "link-arg=-Tlink.x", "-C", "link-arg=-Tdefmt.x", diff --git a/boards/gps/src/main.rs b/boards/gps/src/main.rs index 05f8ea7a..f8705883 100644 --- a/boards/gps/src/main.rs +++ b/boards/gps/src/main.rs @@ -23,7 +23,7 @@ use hal::clock::v2::pclk::Pclk; use hal::clock::v2::Source; use hal::gpio::Pins; use hal::gpio::{ - Pin, PushPullOutput, PA02, PA03, + Pin, PushPullOutput, PA02, PA03, PA08, PA09, Alternate, C, }; use hal::prelude::*; use hal::sercom::uart; @@ -34,7 +34,7 @@ use panic_probe as _; use systick_monotonic::*; use types::*; use mavlink::embedded::Read; - +use ublox::{CfgPrtUartBuilder, UartPortId, UartMode, DataBits, Parity, StopBits, InProtoMask, OutProtoMask}; #[inline(never)] #[defmt::panic_handler] @@ -60,7 +60,7 @@ static RTC: Mutex>>> = Mutex::new(RefC #[rtic::app(device = hal::pac, peripherals = true, dispatchers = [EVSYS_0, EVSYS_1, EVSYS_2])] mod app { - use ublox::PacketRef; + use ublox::{CfgMsgAllPortsBuilder, CfgMsgSinglePortBuilder, NavPosLlh, NavPvt, PacketRef, UbxPacketRequest}; use super::*; @@ -195,27 +195,114 @@ mod app { // .spi_mode(spi::MODE_0) // .enable(); // let sd_manager = SdManager::new(sdmmc_spi, pins.pa06.into_push_pull_output()); + // let (pclk_radio, gclk0) = Pclk::enable(tokens.pclks.sercom2, gclk0); + // /* Radio config */ + + // let rx: Pin> = pins.pa08.into_alternate(); + // let tx: Pin> = pins.pa09.into_alternate(); + // let pads = uart::Pads::::default() + // .rx(rx) + // .tx(tx); + // let mut uart = + // GroundStationUartConfig::new(&mclk, peripherals.SERCOM2, pads, pclk_radio.freq()) + // .baud( + // RateExtU32::Hz(57600), + // uart::BaudMode::Fractional(uart::Oversampling::Bits16), + // ) + // .enable(); + + // loop { + // nb::block!(uart.write(0x55)).unwrap(); + // } + let (pclk_gps, gclk0) = Pclk::enable(tokens.pclks.sercom2, gclk0); //info!("here"); + // let mut rx = pins.pa13.into_push_pull_output(); + // loop { + // rx.set_high().ok(); + // cortex_m::asm::delay(300_000); + // rx.set_low().ok(); + // cortex_m::asm::delay(300_000); + // } + let pads = hal::sercom::uart::Pads::::default() .rx(pins.pa13) .tx(pins.pa12); - let mut gps_uart = GpsUartConfig::new(&mclk, peripherals.SERCOM2, pads, pclk_gps.freq()) + let mut gps_uart_config = GpsUartConfig::new(&mclk, peripherals.SERCOM2, pads, pclk_gps.freq()) .baud( RateExtU32::Hz(9600), uart::BaudMode::Fractional(uart::Oversampling::Bits16), - ) - .enable(); - gps_uart.enable_interrupts(hal::sercom::uart::Flags::RXC); + ); + // loop { + // let (x,y) = gps_uart_config.get_baud(); + // info!("Baud: {}", x); + // } + + gps_uart_config.set_bit_order(uart::BitOrder::LsbFirst); + gps_uart_config.set_parity(uart::Parity::None); + gps_uart_config.set_stop_bits(uart::StopBits::OneBit); + + let mut gps_uart = gps_uart_config.enable(); + + // .enable(); + // gps_uart.enable_interrupts(hal::sercom::uart::Flags::RXC); + /* Status LED */ // info!("Setting up LED"); // let led = pins.pa02.into_push_pull_output(); let mut gps_enable = pins.pb09.into_push_pull_output(); - gps_enable.set_high().ok(); - // gps_enable.set_low().ok(); + let mut gps_reset = pins.pb07.into_push_pull_output(); + gps_reset.set_low().ok(); + cortex_m::asm::delay(300_000); + gps_reset.set_high().ok(); + + // gps_reset.set_low().ok(); + // gps_enable.set_high().ok(); + gps_enable.set_low().ok(); + let packet: [u8; 28] = CfgPrtUartBuilder { + portid: UartPortId::Uart1, + reserved0: 0, + tx_ready: 1, + mode: UartMode::new(DataBits::Eight, Parity::None, StopBits::One), + baud_rate: 9600, + in_proto_mask: InProtoMask::all(), + out_proto_mask: OutProtoMask::UBLOX, + flags: 0, + reserved5: 0, + }.into_packet_bytes(); + + loop { + for byte in packet { + info!("Byte: {}", byte); + nb::block!(gps_uart.write(byte)).unwrap(); + } + } + + + // let packet_two = CfgMsgAllPortsBuilder::set_rate_for::([1, 0, 0, 0, 0, 0]).into_packet_bytes(); + // for byte in packet_two { + // nb::block!(gps_uart.write(byte)).unwrap(); + // } + + let request = UbxPacketRequest::request_for::().into_packet_bytes(); + for byte in request { + nb::block!(gps_uart.write(byte)).unwrap(); + } + loop { + info!("reading" ); + let byte = nb::block!(gps_uart.read()); + match byte { + Ok(byte) => { + info!("Byte: {}", byte); + } + Err(e) => { + info!("Error {}", e); + } + } + } /* Spawn tasks */ // sensor_send::spawn().ok(); // state_send::spawn().ok(); @@ -259,8 +346,10 @@ mod app { loop {} } + #[task(binds = SERCOM2_2, local = [gps_uart], shared = [&em])] fn gps_rx(cx: gps_rx::Context) { + info!("GPS interrupt"); cx.shared.em.run(|| { cortex_m::interrupt::free(|cs| { let mut buf: [u8; 256] = [0; 256]; diff --git a/boards/gps/src/types.rs b/boards/gps/src/types.rs index 3a408f07..ba5d2230 100644 --- a/boards/gps/src/types.rs +++ b/boards/gps/src/types.rs @@ -1,6 +1,6 @@ use atsamd_hal::gpio::*; use atsamd_hal::sercom::uart::EightBit; -use atsamd_hal::sercom::{uart, uart::Duplex, IoSet1, Sercom2}; +use atsamd_hal::sercom::{uart, uart::Duplex, IoSet1, Sercom2, IoSet3}; use messages::sender::Sender; use messages::sender::Sender::CommunicationBoard; @@ -15,3 +15,8 @@ pub static COM_ID: Sender = CommunicationBoard; pub type GpsPads = uart::PadsFromIds; pub type GpsUartConfig = uart::Config; pub type GpsUart = uart::Uart; +// ------- +// Ground Station +// ------- +pub type GroundStationPads = uart::PadsFromIds; +pub type GroundStationUartConfig = uart::Config; diff --git a/boards/nav/src/main.rs b/boards/nav/src/main.rs index 1a732a7d..0772d76c 100644 --- a/boards/nav/src/main.rs +++ b/boards/nav/src/main.rs @@ -359,9 +359,9 @@ mod app { Sensor::new(x), ) }); - for msg in messages { - sender.send(msg).await; - } + // for msg in messages { + // sender.send(msg).await; + // } Mono::delay(100.millis()).await; // what if there was no delay and we used chanenls to control the rate of messages. } } diff --git a/libraries/sbg-rs/src/sbg.rs b/libraries/sbg-rs/src/sbg.rs index a6041c5a..7833284b 100644 --- a/libraries/sbg-rs/src/sbg.rs +++ b/libraries/sbg-rs/src/sbg.rs @@ -382,11 +382,11 @@ impl SBG { callback(CallbackData::EkfNav((*pLogData).ekfNavData.into())) } _SbgEComLog_SBG_ECOM_LOG_GPS1_POS => { - info!("GPS Data"); + panic!("GPS Data"); callback(CallbackData::GpsPos((*pLogData).gpsPosData.into())) } _SbgEComLog_SBG_ECOM_LOG_GPS1_VEL => { - + panic!("GPS Velocity Data"); callback(CallbackData::GpsVel((*pLogData).gpsVelData.into())) } _SbgEComLog_SBG_ECOM_LOG_GPS1_HDT => { @@ -483,13 +483,13 @@ pub unsafe extern "C" fn sbgPlatformDebugLogMsg( let category = unsafe { CStr::from_ptr(pCategory).to_str().unwrap() }; let format = unsafe { CStr::from_ptr(pFormat).to_str().unwrap() }; - info!("{}:{}:{}:{}:{}:{}", file, function, line, category, errorCode, format); + // info!("{}:{}:{}:{}:{}:{}", file, function, line, category, errorCode, format); match logType { // silently handle errors // _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_ERROR => error!("SBG Error"), _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_WARNING => warn!("SBG Warning"), - _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_INFO => info!("SBG Info "), + // _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_INFO => info!("SBG Info "), _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_DEBUG => debug!("SBG Debug "), _ => (), }; From c920181e3d02e94d4bfee6050d57b437a57fce0c Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Wed, 14 Aug 2024 16:02:43 -0400 Subject: [PATCH 29/47] WIP --- boards/gps/src/main.rs | 204 +++++++++++++++++++++++++--------------- boards/gps/src/types.rs | 3 + 2 files changed, 132 insertions(+), 75 deletions(-) diff --git a/boards/gps/src/main.rs b/boards/gps/src/main.rs index f8705883..65c9698f 100644 --- a/boards/gps/src/main.rs +++ b/boards/gps/src/main.rs @@ -6,12 +6,14 @@ mod data_manager; mod types; use atsamd_hal as hal; +use atsamd_hal::dmac; +use atsamd_hal::dmac::DmaController; +use atsamd_hal::dmac::Transfer; use atsamd_hal::rtc::Count32Mode; use common_arm::*; use common_arm_atsame::mcan; - -use communication::Capacities; use communication::CanCommandManager; +use communication::Capacities; use core::cell::RefCell; use cortex_m::interrupt::Mutex; use data_manager::DataManager; @@ -22,19 +24,22 @@ use fugit::RateExtU32; use hal::clock::v2::pclk::Pclk; use hal::clock::v2::Source; use hal::gpio::Pins; -use hal::gpio::{ - Pin, PushPullOutput, PA02, PA03, PA08, PA09, Alternate, C, -}; +use hal::gpio::{Alternate, Pin, PushPullOutput, C, PA02, PA03, PA08, PA09}; use hal::prelude::*; use hal::sercom::uart; use hal::sercom::IoSet1; +use mavlink::embedded::Read; use mcan::messageram::SharedMemory; use messages::*; use panic_probe as _; use systick_monotonic::*; +use types::GPSBUFFER; use types::*; -use mavlink::embedded::Read; -use ublox::{CfgPrtUartBuilder, UartPortId, UartMode, DataBits, Parity, StopBits, InProtoMask, OutProtoMask}; +use ublox::{ + CfgPrtUartBuilder, DataBits, InProtoMask, OutProtoMask, Parity, StopBits, UartMode, UartPortId, +}; + +pub static mut BUF_DST: GPSBUFFER = &mut [0; 256]; #[inline(never)] #[defmt::panic_handler] @@ -60,7 +65,12 @@ static RTC: Mutex>>> = Mutex::new(RefC #[rtic::app(device = hal::pac, peripherals = true, dispatchers = [EVSYS_0, EVSYS_1, EVSYS_2])] mod app { - use ublox::{CfgMsgAllPortsBuilder, CfgMsgSinglePortBuilder, NavPosLlh, NavPvt, PacketRef, UbxPacketRequest}; + use atsamd_hal::sercom::Sercom; + use cortex_m::asm; + use ublox::{ + CfgMsgAllPortsBuilder, CfgMsgSinglePortBuilder, NavPosLlh, NavPvt, PacketRef, + UbxPacketRequest, + }; use super::*; @@ -114,8 +124,8 @@ mod app { let core = cx.core; let pins = Pins::new(peripherals.PORT); - // let mut dmac = DmaController::init(peripherals.DMAC, &mut peripherals.PM); - // let dmaChannels = dmac.split(); + let mut dmac = DmaController::init(peripherals.DMAC, &mut peripherals.PM); + let dmaChannels = dmac.split(); /* Logging Setup */ HydraLogging::set_ground_station_callback(queue_gs_message); @@ -197,7 +207,6 @@ mod app { // let sd_manager = SdManager::new(sdmmc_spi, pins.pa06.into_push_pull_output()); // let (pclk_radio, gclk0) = Pclk::enable(tokens.pclks.sercom2, gclk0); // /* Radio config */ - // let rx: Pin> = pins.pa08.into_alternate(); // let tx: Pin> = pins.pa09.into_alternate(); // let pads = uart::Pads::::default() @@ -215,7 +224,6 @@ mod app { // nb::block!(uart.write(0x55)).unwrap(); // } - let (pclk_gps, gclk0) = Pclk::enable(tokens.pclks.sercom2, gclk0); //info!("here"); // let mut rx = pins.pa13.into_push_pull_output(); @@ -230,33 +238,30 @@ mod app { .rx(pins.pa13) .tx(pins.pa12); - let mut gps_uart_config = GpsUartConfig::new(&mclk, peripherals.SERCOM2, pads, pclk_gps.freq()) - .baud( + let mut gps_uart_config = + GpsUartConfig::new(&mclk, peripherals.SERCOM2, pads, pclk_gps.freq()).baud( RateExtU32::Hz(9600), uart::BaudMode::Fractional(uart::Oversampling::Bits16), ); + gps_uart_config = gps_uart_config.immediate_overflow_notification(false); + // loop { - // let (x,y) = gps_uart_config.get_baud(); + // let (x,y) = gps_uart_config.get_baud(); // info!("Baud: {}", x); // } - gps_uart_config.set_bit_order(uart::BitOrder::LsbFirst); - gps_uart_config.set_parity(uart::Parity::None); - gps_uart_config.set_stop_bits(uart::StopBits::OneBit); - - let mut gps_uart = gps_uart_config.enable(); - - // .enable(); + let mut gps_uart = gps_uart_config.enable(); + let (mut gps_rx, mut gps_tx) = gps_uart.split(); // gps_uart.enable_interrupts(hal::sercom::uart::Flags::RXC); - + /* Status LED */ // info!("Setting up LED"); // let led = pins.pa02.into_push_pull_output(); let mut gps_enable = pins.pb09.into_push_pull_output(); let mut gps_reset = pins.pb07.into_push_pull_output(); gps_reset.set_low().ok(); - cortex_m::asm::delay(300_000); + cortex_m::asm::delay(300_000); gps_reset.set_high().ok(); // gps_reset.set_low().ok(); @@ -265,41 +270,95 @@ mod app { let packet: [u8; 28] = CfgPrtUartBuilder { portid: UartPortId::Uart1, reserved0: 0, - tx_ready: 1, + tx_ready: 0, mode: UartMode::new(DataBits::Eight, Parity::None, StopBits::One), baud_rate: 9600, in_proto_mask: InProtoMask::all(), out_proto_mask: OutProtoMask::UBLOX, flags: 0, reserved5: 0, - }.into_packet_bytes(); - - loop { - for byte in packet { - info!("Byte: {}", byte); - nb::block!(gps_uart.write(byte)).unwrap(); - } } + .into_packet_bytes(); + for byte in packet { + // info!("Byte: {}", byte); + nb::block!(gps_tx.write(byte)).unwrap(); + } // let packet_two = CfgMsgAllPortsBuilder::set_rate_for::([1, 0, 0, 0, 0, 0]).into_packet_bytes(); // for byte in packet_two { // nb::block!(gps_uart.write(byte)).unwrap(); // } + let mut dmaCh0 = dmaChannels.0.init(dmac::PriorityLevel::LVL3); + /* DMAC config */ + dmaCh0 + .as_mut() + .enable_interrupts(dmac::InterruptFlags::new().with_tcmpl(true)); + let mut xfer = Transfer::new(dmaCh0, gps_rx, unsafe { &mut *BUF_DST }, false) + .expect("DMA err") + .begin( + atsamd_hal::sercom::Sercom2::DMA_RX_TRIGGER, + dmac::TriggerAction::BURST, + ); - let request = UbxPacketRequest::request_for::().into_packet_bytes(); - for byte in request { - nb::block!(gps_uart.write(byte)).unwrap(); - } loop { - info!("reading" ); - let byte = nb::block!(gps_uart.read()); - match byte { - Ok(byte) => { - info!("Byte: {}", byte); + if xfer.complete() { + let (chan0, source, buf) = xfer.stop(); + xfer = dmac::Transfer::new(chan0, source, unsafe { &mut *BUF_DST }, false) + .unwrap() + .begin( + atsamd_hal::sercom::Sercom2::DMA_RX_TRIGGER, + dmac::TriggerAction::BURST, + ); + let buf_clone = buf.clone(); + + unsafe { BUF_DST.copy_from_slice(&[0; 256]) }; + xfer.block_transfer_interrupt(); + let request = + UbxPacketRequest::request_for::().into_packet_bytes(); + for byte in request { + nb::block!(gps_tx.write(byte)).unwrap(); } - Err(e) => { - info!("Error {}", e); + cortex_m::asm::delay(300_000); + let mut buf: [u8; 256] = [0; 256]; + let mut bytes: [u8; 256] = [0; 256]; + // for i in 0..buf.len() { + // let item = nb::block!(gps_uart.read()).unwrap(); + // // info!("Byte: {}", item); + // bytes[i] = item; + // } + let buf = ublox::FixedLinearBuffer::new(&mut buf[..]); + let mut parser = ublox::Parser::new(buf); + let mut msgs = parser.consume(&buf_clone); + while let Some(msg) = msgs.next() { + match msg { + Ok(msg) => match msg { + ublox::PacketRef::NavPosLlh(x) => { + info!("GPS latitude: {:?}, longitude {:?}", x.lat_degrees(), x.lon_degrees()); + } + ublox::PacketRef::NavStatus(x) => { + info!("GPS fix stat: {:?}", x.fix_stat_raw()); + } + ublox::PacketRef::NavDop(x) => { + info!("GPS geometric drop: {:?}", x.geometric_dop()); + } + ublox::PacketRef::NavSat(x) => { + info!("GPS num sats used: {:?}", x.num_svs()); + } + ublox::PacketRef::NavVelNed(x) => { + info!("GPS velocity north: {:?}", x.vel_north()); + } + ublox::PacketRef::NavPvt(x) => { + info!("GPS nun sats PVT: {:?}", x.num_satellites()); + } + _ => { + info!("GPS Message not handled."); + } + }, + Err(e) => { + info!("GPS parse Error"); + } + } } } } @@ -333,7 +392,7 @@ mod app { Local { led_green, led_red, - gps_uart, + gps_uart, // sd_manager, }, init::Monotonics(mono), @@ -346,10 +405,8 @@ mod app { loop {} } - #[task(binds = SERCOM2_2, local = [gps_uart], shared = [&em])] fn gps_rx(cx: gps_rx::Context) { - info!("GPS interrupt"); cx.shared.em.run(|| { cortex_m::interrupt::free(|cs| { let mut buf: [u8; 256] = [0; 256]; @@ -363,38 +420,35 @@ mod app { let mut msgs = parser.consume(&bytes); while let Some(msg) = msgs.next() { match msg { - Ok(msg) => { - match msg { - ublox::PacketRef::NavPosLlh(x) => { - info!("GPS latitude: {:?}", x.lat_degrees()); - } - ublox::PacketRef::NavStatus(x) => { - info!("GPS fix stat: {:?}", x.fix_stat_raw()); - } - ublox::PacketRef::NavDop(x) => { - info!("GPS geometric drop: {:?}", x.geometric_dop()); - } - ublox::PacketRef::NavSat(x) => { - info!("GPS num sats used: {:?}", x.num_svs()); - } - ublox::PacketRef::NavVelNed(x) => { - info!("GPS velocity north: {:?}", x.vel_north()); - } - ublox::PacketRef::NavPvt(x) => { - info!("GPS nun sats PVT: {:?}", x.num_satellites()); - } - _ => { - info!("GPS Message not handled."); - } + Ok(msg) => match msg { + ublox::PacketRef::NavPosLlh(x) => { + info!("GPS latitude: {:?}", x.lat_degrees()); } - } + ublox::PacketRef::NavStatus(x) => { + info!("GPS fix stat: {:?}", x.fix_stat_raw()); + } + ublox::PacketRef::NavDop(x) => { + info!("GPS geometric drop: {:?}", x.geometric_dop()); + } + ublox::PacketRef::NavSat(x) => { + info!("GPS num sats used: {:?}", x.num_svs()); + } + ublox::PacketRef::NavVelNed(x) => { + info!("GPS velocity north: {:?}", x.vel_north()); + } + ublox::PacketRef::NavPvt(x) => { + info!("GPS nun sats PVT: {:?}", x.num_satellites()); + } + _ => { + info!("GPS Message not handled."); + } + }, Err(e) => { info!("GPS parse Error"); } } } - - }); + }); Ok(()) }); } @@ -513,8 +567,8 @@ mod app { // } // /// Handles the radio interrupt. - // /// This only needs to be called when the radio has data to read, this is why an interrupt handler is used above polling which would waste resources. - // /// We use a critical section to ensure that we are not interrupted while servicing the mavlink message. + // /// This only needs to be called when the radio has data to read, this is why an interrupt handler is used above polling which would waste resources. + // /// We use a critical section to ensure that we are not interrupted while servicing the mavlink message. // #[task(priority = 3, binds = SERCOM2_2, shared = [&em, radio_manager])] // fn radio_rx(mut cx: radio_rx::Context) { // cx.shared.radio_manager.lock(|radio_manager| { @@ -522,7 +576,7 @@ mod app { // cortex_m::interrupt::free(|cs| { // let m = radio_manager.receive_message()?; // info!("Received message {}", m.clone()); - // spawn!(send_command, m) + // spawn!(send_command, m) // })?; // Ok(()) diff --git a/boards/gps/src/types.rs b/boards/gps/src/types.rs index ba5d2230..dbf53c90 100644 --- a/boards/gps/src/types.rs +++ b/boards/gps/src/types.rs @@ -15,6 +15,9 @@ pub static COM_ID: Sender = CommunicationBoard; pub type GpsPads = uart::PadsFromIds; pub type GpsUartConfig = uart::Config; pub type GpsUart = uart::Uart; + +pub type GPSBUFFER = &'static mut [u8; 256]; + // ------- // Ground Station // ------- From 1313ecf4eecbe25a9a9a27f30ed3664b3bc1e8f6 Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Thu, 15 Aug 2024 01:10:41 -0400 Subject: [PATCH 30/47] WIP --- boards/recovery/src/gpio_manager.rs | 4 ++-- boards/recovery/src/main.rs | 9 ++++++--- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/boards/recovery/src/gpio_manager.rs b/boards/recovery/src/gpio_manager.rs index 01add0b6..ee871fbe 100644 --- a/boards/recovery/src/gpio_manager.rs +++ b/boards/recovery/src/gpio_manager.rs @@ -11,8 +11,8 @@ impl GPIOManager { mut main_ematch: Pin, mut drogue_ematch: Pin, ) -> Self { - drogue_ematch.set_low().ok(); - main_ematch.set_low().ok(); + // drogue_ematch.set_low().ok(); + // main_ematch.set_low().ok(); Self { main_ematch, drogue_ematch, diff --git a/boards/recovery/src/main.rs b/boards/recovery/src/main.rs index 3208dda9..331f77f4 100644 --- a/boards/recovery/src/main.rs +++ b/boards/recovery/src/main.rs @@ -155,8 +155,11 @@ mod app { /* GPIO config */ let led_green = pins.pa03.into_push_pull_output(); let led_red = pins.pb04.into_push_pull_output(); - let main_ematch = pins.pb12.into_push_pull_output(); - let drogue_ematch = pins.pb11.into_push_pull_output(); + let mut main_ematch = pins.pb12.into_push_pull_output(); + let mut drogue_ematch = pins.pb11.into_push_pull_output(); + main_ematch.set_low().ok(); + drogue_ematch.set_low().ok(); + let gpio = GPIOManager::new(main_ematch, drogue_ematch); /* State Machine config */ @@ -203,7 +206,7 @@ mod app { state_send::spawn().ok(); ejection_sense::spawn().ok(); blink::spawn().ok(); - // fire_main::spawn_after(ExtU64::secs(1000)).ok(); + // fire_main::spawn_after(ExtU64::secs(15)).ok(); // fire_drogue::spawn_after(ExtU64::secs(15)).ok(); /* Monotonic clock */ From 563fa09ad9852c5ae4b41c080a5d29220448d880 Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Thu, 15 Aug 2024 22:57:02 -0400 Subject: [PATCH 31/47] WIP --- boards/communication/src/main.rs | 86 ++++++++++++------------- boards/nav/src/main.rs | 12 ++-- boards/nav/src/sbg_manager.rs | 2 +- boards/recovery/src/data_manager.rs | 5 +- libraries/sbg-rs/src/data_conversion.rs | 1 - libraries/sbg-rs/src/sbg.rs | 9 +-- 6 files changed, 59 insertions(+), 56 deletions(-) diff --git a/boards/communication/src/main.rs b/boards/communication/src/main.rs index abd31011..c16e421b 100644 --- a/boards/communication/src/main.rs +++ b/boards/communication/src/main.rs @@ -10,7 +10,12 @@ use atsamd_hal::rtc::Count32Mode; use common_arm::*; use common_arm_atsame::mcan; // use panic_halt as _; - +use atsamd_hal::gpio::{Alternate, Output, PushPull, D, PA04, PA05, PA06, PA07}; +use atsamd_hal::sercom::{ + spi, + spi::{Config, Duplex, Pads, Spi}, + Sercom0, +}; use communication::Capacities; use communication::{CanCommandManager, RadioDevice, RadioManager}; use core::cell::RefCell; @@ -23,15 +28,10 @@ use fugit::RateExtU32; use hal::clock::v2::pclk::Pclk; use hal::clock::v2::Source; use hal::gpio::Pins; -use hal::gpio::{ - Alternate, Pin, PushPullOutput, C, PA02, PA03, - PA08, PA09, -}; +use hal::gpio::{Pin, PushPullOutput, C, PA02, PA03, PA08, PA09}; use hal::prelude::*; use hal::sercom::uart; -use hal::sercom::{ - IoSet3, -}; +use hal::sercom::IoSet3; use mcan::messageram::SharedMemory; use messages::command::RadioRate; use messages::state::State; @@ -61,12 +61,9 @@ static RTC: Mutex>>> = Mutex::new(RefC #[rtic::app(device = hal::pac, peripherals = true, dispatchers = [EVSYS_0, EVSYS_1, EVSYS_2])] mod app { - use command::{Command, CommandData}; - - + use sender::Sender; - use super::*; @@ -203,7 +200,7 @@ mod app { // let (mut gclk1, dfll) = // hal::clock::v2::gclk::Gclk::from_source(tokens.gclks.gclk1, clocks.dfll); // let gclk1 = gclk1.div(gclk::GclkDiv16::Div(3)).enable(); // 48 / 3 = 16 MHzs - // let (pclk_sd, gclk1) = Pclk::enable(tokens.pclks.sercom0, gclk1); + // let (pclk_sd, gclk0) = Pclk::enable(tokens.pclks.sercom0, gclk0); // let pads_spi = spi::Pads::::default() // .sclk(pins.pa05) @@ -306,7 +303,7 @@ mod app { /// Handles the CAN0 interrupt. #[task(priority = 3, binds = CAN0, shared = [can0, data_manager])] fn can0(mut cx: can0::Context) { - // info!("CAN0 interrupt"); + info!("CAN0 interrupt"); cx.shared.can0.lock(|can| { cx.shared .data_manager @@ -318,7 +315,7 @@ mod app { */ #[task(capacity = 10, shared = [can0, &em])] fn send_internal(mut cx: send_internal::Context, m: Message) { - info!("{}", m.clone()); + // info!("{}", m.clone()); cx.shared.em.run(|| { cx.shared.can0.lock(|can| can.send_message(m))?; Ok(()) @@ -420,25 +417,24 @@ mod app { } } - // #[task(shared = [&em])] - // fn generate_random_messages(cx: generate_random_messages::Context) { - // cx.shared.em.run(|| { - // let message = Message::new( - // cortex_m::interrupt::free(|cs| { - // let mut rc = RTC.borrow(cs).borrow_mut(); - // let rtc = rc.as_mut().unwrap(); - // rtc.count32() - // }), - // COM_ID, - // State::new(StateData::Initializing), - // ); - // // spawn!(send_gs, message.clone())?; - // spawn!(send_internal, message)?; - // Ok(()) - // }); - // spawn_after!(generate_random_messages, ExtU64::millis(200)).ok(); - - // } + #[task(shared = [&em])] + fn generate_random_messages(cx: generate_random_messages::Context) { + cx.shared.em.run(|| { + let message = Message::new( + cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + rtc.count32() + }), + COM_ID, + State::new(messages::state::StateData::Initializing), + ); + // spawn!(send_gs, message.clone())?; + spawn!(send_internal, message)?; + Ok(()) + }); + spawn_after!(generate_random_messages, ExtU64::millis(200)).ok(); + } #[task(shared = [&em])] fn generate_random_command(cx: generate_random_command::Context) { @@ -468,10 +464,15 @@ mod app { .lock(|data_manager| data_manager.state.clone()); cx.shared.em.run(|| { if let Some(x) = state_data { - let message = Message::new(cortex_m::interrupt::free(|cs| { - let mut rc = RTC.borrow(cs).borrow_mut(); - let rtc = rc.as_mut().unwrap(); - rtc.count32()}), COM_ID, State::new(x)); + let message = Message::new( + cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + rtc.count32() + }), + COM_ID, + State::new(x), + ); // spawn!(send_gs, message)?; } // if there is none we still return since we simply don't have data yet. Ok(()) @@ -480,17 +481,16 @@ mod app { } /// Handles the radio interrupt. - /// This only needs to be called when the radio has data to read, this is why an interrupt handler is used above polling which would waste resources. - /// We use a critical section to ensure that we are not interrupted while servicing the mavlink message. + /// This only needs to be called when the radio has data to read, this is why an interrupt handler is used above polling which would waste resources. + /// We use a critical section to ensure that we are not interrupted while servicing the mavlink message. #[task(priority = 3, binds = SERCOM2_2, shared = [&em, radio_manager])] fn radio_rx(mut cx: radio_rx::Context) { cx.shared.radio_manager.lock(|radio_manager| { cx.shared.em.run(|| { - cortex_m::interrupt::free(|cs| { + cortex_m::interrupt::free(|cs| { let m = radio_manager.receive_message()?; info!("Received message {}", m.clone()); - spawn!(send_command, m) - + spawn!(send_command, m) })?; Ok(()) }); diff --git a/boards/nav/src/main.rs b/boards/nav/src/main.rs index 0772d76c..294956ac 100644 --- a/boards/nav/src/main.rs +++ b/boards/nav/src/main.rs @@ -54,6 +54,7 @@ static RTC: Mutex>> = Mutex::new(RefCell::new(None)); #[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, dispatchers = [EXTI0, EXTI1, EXTI2, SPI3, SPI2])] mod app { + use chrono::Timelike; use messages::Message; use stm32h7xx_hal::gpio::{Alternate, Pin}; @@ -352,16 +353,15 @@ mod app { NaiveDate::from_ymd_opt(2024, 1, 1).unwrap(), NaiveTime::from_hms_milli_opt(0, 0, 0, 0).unwrap(), )) - .and_utc() - .timestamp_subsec_millis() + .and_utc().timestamp_subsec_millis() }), COM_ID, Sensor::new(x), ) }); - // for msg in messages { - // sender.send(msg).await; - // } + for msg in messages { + sender.send(msg).await; + } Mono::delay(100.millis()).await; // what if there was no delay and we used chanenls to control the rate of messages. } } @@ -474,7 +474,7 @@ mod app { #[task(priority = 3, binds = DMA1_STR1, shared = [&em, sbg_manager])] fn sbg_dma(mut context: sbg_dma::Context); - #[task(priority = 3, shared = [data_manager])] + #[task(priority = 2, shared = [data_manager])] async fn sbg_handle_data(context: sbg_handle_data::Context, data: CallbackData); #[task(priority = 1, shared = [&em, sbg_manager])] diff --git a/boards/nav/src/sbg_manager.rs b/boards/nav/src/sbg_manager.rs index b17ee58d..a2b7cf28 100644 --- a/boards/nav/src/sbg_manager.rs +++ b/boards/nav/src/sbg_manager.rs @@ -84,7 +84,7 @@ impl SBGManager { let config = DmaConfig::default() .memory_increment(true) - .transfer_complete_interrupt(true).peripheral_burst(stm32h7xx_hal::dma::config::BurstMode::Burst4); + .transfer_complete_interrupt(true); let mut transfer: Transfer< StreamX, Rx, diff --git a/boards/recovery/src/data_manager.rs b/boards/recovery/src/data_manager.rs index fb5affcd..34045dd4 100644 --- a/boards/recovery/src/data_manager.rs +++ b/boards/recovery/src/data_manager.rs @@ -45,7 +45,10 @@ impl DataManager { let mut avg_sum: f32 = 0.0; let mut prev = last; for i in buf { - let time_diff: f32 = (i.1 - prev.1) as f32 / 1_000_000.0; + + let time_diff: f32 = (i.1 - prev.1) as f32 / 100_000_0.0; + info!("last: {:?}, timestamp: {}, time diff {}", last, i.1, time_diff); + if time_diff == 0.0 { continue; } diff --git a/libraries/sbg-rs/src/data_conversion.rs b/libraries/sbg-rs/src/data_conversion.rs index 56f9af82..3c71db14 100644 --- a/libraries/sbg-rs/src/data_conversion.rs +++ b/libraries/sbg-rs/src/data_conversion.rs @@ -29,7 +29,6 @@ impl From for (GpsPos1, GpsPos2, GpsPosAcc) { let status = GpsPositionStatus::new(value.status); let valid = matches!(status.get_status(), Some(GpsPositionStatusE::SolComputed)); - // defmt::info!("GpsPos: {:?}", value.latitude); ( GpsPos1 { latitude: if valid { Some(value.latitude) } else { None }, diff --git a/libraries/sbg-rs/src/sbg.rs b/libraries/sbg-rs/src/sbg.rs index 7833284b..6c436997 100644 --- a/libraries/sbg-rs/src/sbg.rs +++ b/libraries/sbg-rs/src/sbg.rs @@ -31,7 +31,7 @@ static mut BUF_INDEX: AtomicUsize = AtomicUsize::new(0); */ static mut BUF: &[u8; SBG_BUFFER_SIZE] = &[0; SBG_BUFFER_SIZE]; -static mut DEQ: Deque = Deque::new(); +static mut DEQ: Deque = Deque::new(); static mut DATA_CALLBACK: Option = None; @@ -382,17 +382,18 @@ impl SBG { callback(CallbackData::EkfNav((*pLogData).ekfNavData.into())) } _SbgEComLog_SBG_ECOM_LOG_GPS1_POS => { - panic!("GPS Data"); + // panic!("GPS Data"); callback(CallbackData::GpsPos((*pLogData).gpsPosData.into())) } _SbgEComLog_SBG_ECOM_LOG_GPS1_VEL => { - panic!("GPS Velocity Data"); + // panic!("GPS Velocity Data"); callback(CallbackData::GpsVel((*pLogData).gpsVelData.into())) } _SbgEComLog_SBG_ECOM_LOG_GPS1_HDT => { // info!("Heading Data"); } _ => { + panic!("unkown") }, } } @@ -487,7 +488,7 @@ pub unsafe extern "C" fn sbgPlatformDebugLogMsg( match logType { // silently handle errors - // _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_ERROR => error!("SBG Error"), + _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_ERROR => error!("SBG Error"), _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_WARNING => warn!("SBG Warning"), // _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_INFO => info!("SBG Info "), _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_DEBUG => debug!("SBG Debug "), From 54adb71152e80548260c88d885fac2d31ee740f8 Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Fri, 16 Aug 2024 22:15:29 -0400 Subject: [PATCH 32/47] WIP --- Cargo.lock | 29 +- Cargo.toml | 3 +- boards/link/Cargo.toml | 27 ++ boards/link/src/communication.rs | 201 +++++++++++++ boards/link/src/data_manager.rs | 181 +++++++++++ boards/link/src/main.rs | 501 +++++++++++++++++++++++++++++++ boards/link/src/types.rs | 3 + libraries/messages/src/sensor.rs | 32 ++ 8 files changed, 972 insertions(+), 5 deletions(-) create mode 100644 boards/link/Cargo.toml create mode 100644 boards/link/src/communication.rs create mode 100644 boards/link/src/data_manager.rs create mode 100644 boards/link/src/main.rs create mode 100644 boards/link/src/types.rs diff --git a/Cargo.lock b/Cargo.lock index b6b54041..abda92a6 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -830,6 +830,30 @@ version = "0.2.8" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "4ec2a862134d2a7d32d7983ddcdd1c4923530833c9f2ea1a44fc5fa473989058" +[[package]] +name = "link" +version = "0.1.0" +dependencies = [ + "chrono", + "common-arm", + "common-arm-stm32h7", + "cortex-m", + "cortex-m-rt", + "defmt", + "defmt-rtt", + "embedded-alloc", + "fdcan", + "heapless 0.7.17", + "messages", + "panic-probe", + "postcard", + "rtic", + "rtic-monotonics", + "rtic-sync", + "stm32h7xx-hal", + "systick-monotonic", +] + [[package]] name = "linked_list_allocator" version = "0.10.5" @@ -1748,9 +1772,8 @@ dependencies = [ [[package]] name = "stm32h7xx-hal" -version = "0.15.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e08bcfbdbe4458133f2fd55994a5c4f1b4bf28084f0218e93cdbc19d7c70219f" +version = "0.16.0" +source = "git+https://github.com/uorocketry/stm32h7xx-hal#5166da8a5485d51e60a42d3a564d1edae0c8e164" dependencies = [ "bare-metal 1.0.0", "cast", diff --git a/Cargo.toml b/Cargo.toml index 676a558f..309c3d43 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -11,8 +11,7 @@ default-members = ["boards/*", "examples/*"] version = "0.2.7" [workspace.dependencies.stm32h7xx-hal] -#git = "https://github.com/stm32-rs/stm32h7xx-hal" -version = "0.15.1" +git = "https://github.com/uorocketry/stm32h7xx-hal" # We use 35 even though we have the 33. features = ["defmt", "rt", "stm32h735", "can", "rtc"] diff --git a/boards/link/Cargo.toml b/boards/link/Cargo.toml new file mode 100644 index 00000000..7e6c48ff --- /dev/null +++ b/boards/link/Cargo.toml @@ -0,0 +1,27 @@ +[package] +name = "link" +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 = "0.7.4" +cortex-m-rt = "0.7.1" +rtic = {version = "2.0.0", features = ["thumbv7-backend"]} +rtic-monotonics = {version = "2.0.2", features = ["cortex-m-systick", "stm32h733vg"]} +common-arm-stm32h7 = { path = "../../libraries/common-arm-stm32h7" } +common-arm = { path = "../../libraries/common-arm" } +stm32h7xx-hal = { workspace = true } +postcard = "1.0.2" +messages = { path = "../../libraries/messages" } +systick-monotonic = "1.0.1" +defmt = "0.3.2" +fdcan = "0.2" +embedded-alloc = "0.5.0" +heapless = "0.7.16" +rtic-sync = "1.3.0" +defmt-rtt = "0.4" +panic-probe = { version = "0.3", features = ["print-defmt"] } +chrono = {version = "0.4.0", default-features = false} \ No newline at end of file diff --git a/boards/link/src/communication.rs b/boards/link/src/communication.rs new file mode 100644 index 00000000..34d0456d --- /dev/null +++ b/boards/link/src/communication.rs @@ -0,0 +1,201 @@ +use crate::data_manager::DataManager; +use crate::types::COM_ID; +use common_arm::HydraError; +use fdcan::{ + frame::{FrameFormat, TxFrameHeader}, + id::StandardId, +}; +use messages::Message; +use postcard::from_bytes; +use stm32h7xx_hal::prelude::*; +use mavlink::peek_reader::PeekReader; +use messages::mavlink::uorocketry::MavMessage; +use messages::mavlink::{self}; +use defmt::{info, error}; + +/// 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]; + for message in self.can.receive0(&mut buf) { + match from_bytes::(&buf) { + Ok(data) => { + info!("Received message {}", data.clone()); + data_manager.handle_command(data)?; + } + Err(e) => { + info!("Error: {:?}", e) + } + } + } + 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, data_manager: &mut DataManager) -> Result<(), HydraError> { + let mut buf = [0u8; 64]; + for message in self.can.receive0(&mut buf) { + match from_bytes::(&buf) { + Ok(data) => { + info!("Received message {}", data.clone()); + data_manager.handle_data(data); + } + Err(e) => { + info!("Error: {:?}", e) + } + } + } + self.can.clear_interrupt(fdcan::interrupt::Interrupt::RxFifo0NewMsg); + Ok(()) + } +} + + +pub struct RadioDevice { + transmitter: stm32h7xx_hal::serial::Tx, + 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 { + 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) => { + return Ok(postcard::from_bytes::(&msg.message)?); + // weird Ok syntax to coerce to hydra error type. + } + mavlink::uorocketry::MavMessage::COMMAND_MESSAGE(command) => { + info!("{}", command.command); + return Ok(postcard::from_bytes::(&command.command)?); + } + _ => { + error!("Error, ErrorContext::UnkownPostcardMessage"); + return Err(mavlink::error::MessageReadError::Io.into()); + } + } + } +} diff --git a/boards/link/src/data_manager.rs b/boards/link/src/data_manager.rs new file mode 100644 index 00000000..6eeb058c --- /dev/null +++ b/boards/link/src/data_manager.rs @@ -0,0 +1,181 @@ +use messages::command::RadioRate; +use messages::state::StateData; +use messages::Message; +use common_arm::HydraError; +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, +} + +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, + } + } + + 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); + return RadioRate::Slow; + } + + /// Do not clone instead take to reduce CPU load. + pub fn take_sensors(&mut self) -> [Option; 14] { + [ + 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.recovery_sensing.take(), + ] + } + + pub fn clone_states(&self) -> [Option; 1] { + [self.state.clone()] + } + + pub fn clone_reset_reason(&self) -> Option { + self.reset_reason.clone() + } + + 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::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::sensor::SensorData::RecoverySensing(_) => { + self.recovery_sensing = 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/link/src/main.rs b/boards/link/src/main.rs new file mode 100644 index 00000000..bd36f0c9 --- /dev/null +++ b/boards/link/src/main.rs @@ -0,0 +1,501 @@ +#![no_std] +#![no_main] + +mod communication; +mod data_manager; +mod types; + +use chrono::{NaiveDate, NaiveDateTime, NaiveTime}; +use common_arm::*; +use communication::{CanCommandManager, CanDataManager}; +use communication::{RadioDevice, RadioManager}; +use stm32h7xx_hal::rcc::ResetReason; +use core::cell::RefCell; +use core::num::{NonZeroU16, NonZeroU8}; +use cortex_m::interrupt::Mutex; +use data_manager::DataManager; +use defmt::info; +use defmt_rtt as _; +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, PA4}; +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::spi; +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() +} + +static RTC: Mutex>> = Mutex::new(RefCell::new(None)); + +#[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>, + } + #[local] + struct LocalResources { + led_red: PA2>, + led_green: PA3>, + } + + #[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 gpioc = ctx.device.GPIOC.split(ccdr.peripheral.GPIOC); + 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(), + ); + + let config = can_data + .get_config() + .set_frame_transmit(fdcan::config::FrameTransmissionConfig::AllowFdCan); + can_data.apply_config(config); + + can_data.enable_interrupt(fdcan::interrupt::Interrupt::RxFifo0NewMsg); + + can_data.enable_interrupt_line(fdcan::interrupt::InterruptLine::_0, true); + let can_data_manager = CanDataManager::new(can_data.into_normal()); + + /// SAFETY: This is done as a result of a single memory mapped bit in hardware. Safe in this context. + 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(), + ); + + 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); + can_command.enable_interrupt(fdcan::interrupt::Interrupt::RxFifo0NewMsg); + + can_command.enable_interrupt_line(fdcan::interrupt::InterruptLine::_0, true); + + 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), 115_200.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); + + cortex_m::interrupt::free(|cs| { + RTC.borrow(cs).replace(Some(rtc)); + }); + + /* 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(); + info!("Online"); + + ( + SharedResources { + data_manager, + em, + sd_manager, + radio_manager, + can_command_manager, + can_data_manager, + sbg_power, + }, + LocalResources { led_red, led_green }, + ) + } + + #[idle] + fn idle(ctx: idle::Context) -> ! { + loop { + // info!("Idle"); + // cortex_m::asm::wfi(); // could case issue with CAN Bus. Was an issue with ATSAME51. + } + } + + #[task(priority = 3, shared = [data_manager, &em])] + 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() + }); + let message = messages::Message::new( + cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + rtc.date_time() + .unwrap_or(NaiveDateTime::new( + NaiveDate::from_ymd_opt(2024, 1, 1).unwrap(), + NaiveTime::from_hms_milli_opt(0, 0, 0, 0).unwrap(), + )) + .and_utc() + .timestamp_subsec_millis() + }), + COM_ID, + reason.into(), + ); + cx.shared.em.run(|| { + spawn!(send_gs, message)?; + Ok(()) + }) + } + + /** + * Sends information about the sensors. + */ + #[task(priority = 2, 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) => { + spawn!(send_gs, x.clone())?; + // spawn!(sd_dump, x)?; + } + None => { + continue; + } + } + } + + Ok(()) + }); + match logging_rate { + RadioRate::Fast => { + Mono::delay(100.millis()).await; + spawn!(sensor_send).ok(); + } + RadioRate::Slow => { + Mono::delay(250.millis()).await; + spawn!(sensor_send).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"); + let message = messages::Message::new( + cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + rtc.date_time() + .unwrap_or(NaiveDateTime::new( + NaiveDate::from_ymd_opt(2024, 1, 1).unwrap(), + NaiveTime::from_hms_milli_opt(0, 0, 0, 0).unwrap(), + )) + .and_utc() + .timestamp_subsec_millis() + }), + COM_ID, + d.into(), + ); + info!("{:?}", message); + // send_in::spawn(message).ok(); + } + + #[task(priority = 3, 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) { + cx.shared.radio_manager.lock(|radio_manager| { + cx.shared.em.run(|| { + let mut buf = [0; 255]; + let data = postcard::to_slice(&m, &mut buf)?; + radio_manager.send_message(data)?; + Ok(()) + }) + }); + } + + #[task(priority = 3, binds = UART4, shared = [&em, radio_manager])] + fn radio_rx(mut cx: radio_rx::Context) { + cx.shared.radio_manager.lock(|radio_manager| { + cx.shared.em.run(|| { + cortex_m::interrupt::free(|cs| { + let m = radio_manager.receive_message()?; + info!("Received message {}", m.clone()); + spawn!(send_command_internal, m) + })?; + Ok(()) + }); + }); + } + + // Might be the wrong interrupt + #[task(priority = 3, binds = FDCAN2_IT0, shared = [can_data_manager, data_manager])] + fn can_data(mut cx: can_data::Context) { + cx.shared.can_data_manager.lock(|can| { + cx.shared + .data_manager + .lock(|data_manager| can.process_data(data_manager)); + }); + } + + #[task(priority = 2, shared = [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| { + can.send_message(m); + }); + } + } + } + + #[task(priority = 2, shared = [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| { + can.send_message(m); + }); + // } + } + + #[task(priority = 1, local = [led_red, led_green], shared = [&em])] + async fn blink(cx: blink::Context) { + loop { + cx.shared.em.run(|| { + if cx.shared.em.has_error() { + cx.local.led_red.toggle(); + } else { + cx.local.led_green.toggle(); + } + Ok(()) + }); + Mono::delay(1000.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/link/src/types.rs b/boards/link/src/types.rs new file mode 100644 index 00000000..e362bf9c --- /dev/null +++ b/boards/link/src/types.rs @@ -0,0 +1,3 @@ +use messages::sender::{Sender, Sender::CommunicationBoard}; + +pub static COM_ID: Sender = CommunicationBoard; \ No newline at end of file diff --git a/libraries/messages/src/sensor.rs b/libraries/messages/src/sensor.rs index 81639246..b142550d 100644 --- a/libraries/messages/src/sensor.rs +++ b/libraries/messages/src/sensor.rs @@ -27,11 +27,43 @@ pub enum SensorData { GpsPos1(GpsPos1), GpsPos2(GpsPos2), GpsPosAcc(GpsPosAcc), + ResetReason, RecoverySensing(RecoverySensing), } /* Replace with new health monitor */ +#[common_derives] +pub enum ResetReason { + /// The mcu went from not having power to having power and resetting + PowerOnReset, + /// The reset pin was asserted + PinReset, + /// The brownout detector triggered + BrownoutReset, + /// The software did a soft reset through the SCB peripheral + SystemReset, + /// The software did a soft reset through the RCC periperal + CpuReset, + /// The window watchdog triggered + WindowWatchdogReset, + /// The independent watchdog triggered + IndependentWatchdogReset, + /// Either of the two watchdogs triggered (but we don't know which one) + GenericWatchdogReset, + /// The DStandby mode was exited + D1ExitsDStandbyMode, + /// The DStandby mode was exited + D2ExitsDStandbyMode, + /// A state has been entered erroneously + D1EntersDStandbyErroneouslyOrCpuEntersCStopErroneously, + /// The reason could not be determined + Unknown { + /// The raw register value + rcc_rsr: u32, + }, +} + #[common_derives] pub struct GpsPos1 { #[doc = "< Latitude in degrees, positive north."] From d05fcbafa57b66b8da986a260db40a86ad2ff4f4 Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Fri, 16 Aug 2024 23:43:24 -0400 Subject: [PATCH 33/47] Add reset reason message and convert nav to a link board. --- Cargo.lock | 38 ++++++++----- Cargo.toml | 4 +- boards/link/src/data_manager.rs | 2 +- boards/link/src/main.rs | 98 +++++++++++++++++++++++++------- libraries/messages/src/sensor.rs | 4 +- 5 files changed, 105 insertions(+), 41 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index abda92a6..b5b103ce 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -187,9 +187,12 @@ checksum = "37b2a672a2cb129a2e41c10b1224bb368f9f37a2b16b612598138befd7b37eb5" [[package]] name = "cc" -version = "1.1.10" +version = "1.1.13" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e9e8aabfac534be767c909e0690571677d49f41bd8465ae876fe043d52ba5292" +checksum = "72db2f7947ecee9b03b510377e8bb9077afa27176fdbff55c51027e976fdcc48" +dependencies = [ + "shlex", +] [[package]] name = "cfg-if" @@ -217,9 +220,9 @@ dependencies = [ [[package]] name = "cmake" -version = "0.1.50" +version = "0.1.51" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a31c789563b815f77f4250caee12365734369f942439b7defd71e18a48197130" +checksum = "fb1e43aa7fd152b1f968787f7dbcdeb306d1867ff373c69955211876c053f91a" dependencies = [ "cc", ] @@ -795,9 +798,9 @@ dependencies = [ [[package]] name = "indexmap" -version = "2.3.0" +version = "2.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "de3fc2e30ba82dd1b3911c8de1ffc143c74a914a14e99514d7637e3099df5ea0" +checksum = "93ead53efc7ea8ed3cfb0c79fc8023fbb782a5432b52830b6518941cebe6505c" dependencies = [ "equivalent", "hashbrown 0.14.5", @@ -820,9 +823,9 @@ checksum = "bbd2bcb4c963f2ddae06a2efc7e9f3591312473c50c6685e1f298068316e66fe" [[package]] name = "libc" -version = "0.2.155" +version = "0.2.156" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "97b3888a4aecf77e811145cadf6eef5901f4782c53886191b2f693f24761847c" +checksum = "a5f43f184355eefb8d17fc948dbecf6c13be3c141f20d834ae842193a448c72a" [[package]] name = "libm" @@ -1433,7 +1436,7 @@ version = "2.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "54053598ea24b1b74937724e366558412a1777eb2680b91ef646db540982789a" dependencies = [ - "indexmap 2.3.0", + "indexmap 2.4.0", "proc-macro-error", "proc-macro2 1.0.86", "quote 1.0.36", @@ -1639,9 +1642,9 @@ checksum = "a3f0bf26fd526d2a95683cd0f87bf103b8539e2ca1ef48ce002d67aad59aa0b4" [[package]] name = "serde" -version = "1.0.207" +version = "1.0.208" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5665e14a49a4ea1b91029ba7d3bca9f299e1f7cfa194388ccc20f14743e784f2" +checksum = "cff085d2cb684faa248efb494c39b68e522822ac0de72ccf08109abde717cfb2" dependencies = [ "serde_derive", ] @@ -1657,9 +1660,9 @@ dependencies = [ [[package]] name = "serde_derive" -version = "1.0.207" +version = "1.0.208" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6aea2634c86b0e8ef2cfdc0c340baede54ec27b1e46febd7f80dffb2aa44a00e" +checksum = "24008e81ff7613ed8e5ba0cfaf24e2c2f1e5b8a0495711e44fcd4882fca62bcf" dependencies = [ "proc-macro2 1.0.86", "quote 1.0.36", @@ -1708,6 +1711,12 @@ dependencies = [ "serial-core", ] +[[package]] +name = "shlex" +version = "1.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0fda2ff0d084019ba4d7c6f371c95d8fd75ce3524c3cb8fb653a3023f6323e64" + [[package]] name = "simple_example" version = "0.1.0" @@ -1773,7 +1782,8 @@ dependencies = [ [[package]] name = "stm32h7xx-hal" version = "0.16.0" -source = "git+https://github.com/uorocketry/stm32h7xx-hal#5166da8a5485d51e60a42d3a564d1edae0c8e164" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3bd869329be25440b24e2b3583a1c016151b4a54bc36d96d82af7fcd9d010b98" dependencies = [ "bare-metal 1.0.0", "cast", diff --git a/Cargo.toml b/Cargo.toml index 309c3d43..317c9144 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -11,10 +11,10 @@ default-members = ["boards/*", "examples/*"] version = "0.2.7" [workspace.dependencies.stm32h7xx-hal] -git = "https://github.com/uorocketry/stm32h7xx-hal" +#git = "https://github.com/uorocketry/stm32h7xx-hal" +version = "0.16.0" # We use 35 even though we have the 33. features = ["defmt", "rt", "stm32h735", "can", "rtc"] - [workspace.dependencies.atsamd-hal] git = "https://github.com/uorocketry/atsamd" #git = "https://github.com/atsamd-rs/atsamd" diff --git a/boards/link/src/data_manager.rs b/boards/link/src/data_manager.rs index 6eeb058c..8f09ac9f 100644 --- a/boards/link/src/data_manager.rs +++ b/boards/link/src/data_manager.rs @@ -154,7 +154,7 @@ impl DataManager { messages::sensor::SensorData::RecoverySensing(_) => { self.recovery_sensing = Some(data); } - messages::sensor::SensorData::ResetReason => { + messages::sensor::SensorData::ResetReason(_) => { } }, diff --git a/boards/link/src/main.rs b/boards/link/src/main.rs index bd36f0c9..d655cf0d 100644 --- a/boards/link/src/main.rs +++ b/boards/link/src/main.rs @@ -9,7 +9,6 @@ use chrono::{NaiveDate, NaiveDateTime, NaiveTime}; use common_arm::*; use communication::{CanCommandManager, CanDataManager}; use communication::{RadioDevice, RadioManager}; -use stm32h7xx_hal::rcc::ResetReason; use core::cell::RefCell; use core::num::{NonZeroU16, NonZeroU8}; use cortex_m::interrupt::Mutex; @@ -30,6 +29,7 @@ use stm32h7xx_hal::gpio::gpiob::PB4; use stm32h7xx_hal::gpio::Speed; use stm32h7xx_hal::gpio::{Output, PushPull}; use stm32h7xx_hal::prelude::*; +use stm32h7xx_hal::rcc::ResetReason; use stm32h7xx_hal::rtc; use stm32h7xx_hal::spi; use stm32h7xx_hal::{rcc, rcc::rec}; @@ -283,6 +283,9 @@ mod app { let em = ErrorManager::new(); blink::spawn().ok(); send_data_internal::spawn(r).ok(); + reset_reason_send::spawn().ok(); + state_send::spawn().ok(); + sensor_send::spawn().ok(); info!("Online"); ( @@ -309,28 +312,80 @@ mod app { #[task(priority = 3, shared = [data_manager, &em])] 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() - }); - let message = messages::Message::new( - cortex_m::interrupt::free(|cs| { - let mut rc = RTC.borrow(cs).borrow_mut(); - let rtc = rc.as_mut().unwrap(); - rtc.date_time() - .unwrap_or(NaiveDateTime::new( - NaiveDate::from_ymd_opt(2024, 1, 1).unwrap(), - NaiveTime::from_hms_milli_opt(0, 0, 0, 0).unwrap(), - )) - .and_utc() - .timestamp_subsec_millis() - }), - COM_ID, - reason.into(), - ); + 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( + cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + rtc.date_time() + .unwrap_or(NaiveDateTime::new( + NaiveDate::from_ymd_opt(2024, 1, 1).unwrap(), + NaiveTime::from_hms_milli_opt(0, 0, 0, 0).unwrap(), + )) + .and_utc() + .timestamp_subsec_millis() + }), + COM_ID, + sensor::Sensor::new(x), + ); + + cx.shared.em.run(|| { + spawn!(send_gs, message)?; + Ok(()) + }) + } + None => return, + } + } + + #[task(priority = 3, shared = [data_manager, &em])] + 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(|| { - spawn!(send_gs, message)?; + if let Some(x) = state_data { + let message = Message::new( + cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + rtc.date_time() + .unwrap_or(NaiveDateTime::new( + NaiveDate::from_ymd_opt(2024, 1, 1).unwrap(), + NaiveTime::from_hms_milli_opt(0, 0, 0, 0).unwrap(), + )) + .and_utc() + .timestamp_subsec_millis() + }), + 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(); } /** @@ -498,4 +553,3 @@ mod app { }); } } - diff --git a/libraries/messages/src/sensor.rs b/libraries/messages/src/sensor.rs index b142550d..dd3b8428 100644 --- a/libraries/messages/src/sensor.rs +++ b/libraries/messages/src/sensor.rs @@ -27,7 +27,7 @@ pub enum SensorData { GpsPos1(GpsPos1), GpsPos2(GpsPos2), GpsPosAcc(GpsPosAcc), - ResetReason, + ResetReason(ResetReason), RecoverySensing(RecoverySensing), } @@ -241,4 +241,4 @@ impl Sensor { data: data.into(), } } -} +} \ No newline at end of file From 8b8dd249940ada32756e577f011dd272b5386ea2 Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Sat, 17 Aug 2024 00:15:11 -0400 Subject: [PATCH 34/47] Change timestamp to ticks from SysTick 2ms granularity. --- boards/link/src/main.rs | 36 +++------------------------- boards/nav/src/main.rs | 52 +++++++++-------------------------------- 2 files changed, 14 insertions(+), 74 deletions(-) diff --git a/boards/link/src/main.rs b/boards/link/src/main.rs index d655cf0d..d3e5cdd8 100644 --- a/boards/link/src/main.rs +++ b/boards/link/src/main.rs @@ -333,17 +333,7 @@ mod app { stm32h7xx_hal::rcc::ResetReason::WindowWatchdogReset => sensor::ResetReason::WindowWatchdogReset, }; let message = messages::Message::new( - cortex_m::interrupt::free(|cs| { - let mut rc = RTC.borrow(cs).borrow_mut(); - let rtc = rc.as_mut().unwrap(); - rtc.date_time() - .unwrap_or(NaiveDateTime::new( - NaiveDate::from_ymd_opt(2024, 1, 1).unwrap(), - NaiveTime::from_hms_milli_opt(0, 0, 0, 0).unwrap(), - )) - .and_utc() - .timestamp_subsec_millis() - }), + Mono::now().ticks(), COM_ID, sensor::Sensor::new(x), ); @@ -366,17 +356,7 @@ mod app { cx.shared.em.run(|| { if let Some(x) = state_data { let message = Message::new( - cortex_m::interrupt::free(|cs| { - let mut rc = RTC.borrow(cs).borrow_mut(); - let rtc = rc.as_mut().unwrap(); - rtc.date_time() - .unwrap_or(NaiveDateTime::new( - NaiveDate::from_ymd_opt(2024, 1, 1).unwrap(), - NaiveTime::from_hms_milli_opt(0, 0, 0, 0).unwrap(), - )) - .and_utc() - .timestamp_subsec_millis() - }), + Mono::now().ticks(), COM_ID, messages::state::State::new(x), ); @@ -430,17 +410,7 @@ mod app { pub fn queue_gs_message(d: impl Into) { info!("Queueing message"); let message = messages::Message::new( - cortex_m::interrupt::free(|cs| { - let mut rc = RTC.borrow(cs).borrow_mut(); - let rtc = rc.as_mut().unwrap(); - rtc.date_time() - .unwrap_or(NaiveDateTime::new( - NaiveDate::from_ymd_opt(2024, 1, 1).unwrap(), - NaiveTime::from_hms_milli_opt(0, 0, 0, 0).unwrap(), - )) - .and_utc() - .timestamp_subsec_millis() - }), + Mono::now().ticks(), COM_ID, d.into(), ); diff --git a/boards/nav/src/main.rs b/boards/nav/src/main.rs index 294956ac..59a59bc1 100644 --- a/boards/nav/src/main.rs +++ b/boards/nav/src/main.rs @@ -16,7 +16,7 @@ use data_manager::DataManager; use defmt::info; use defmt_rtt as _; use fdcan::{ - config::{NominalBitTiming, DataBitTiming}, + config::{DataBitTiming, NominalBitTiming}, filter::{StandardFilter, StandardFilterSlot}, }; use heapless::Vec; @@ -72,7 +72,6 @@ mod app { can_command_manager: CanCommandManager, can_data_manager: CanDataManager, sbg_power: PB4>, - } #[local] struct LocalResources { @@ -142,7 +141,6 @@ mod app { // transceiver_delay_compensation: true, // }; - info!("CAN enabled"); // GPIO let gpioc = ctx.device.GPIOC.split(ccdr.peripheral.GPIOC); @@ -179,7 +177,7 @@ mod app { can_data.set_nominal_bit_timing(btr); - // can_data.set_automatic_retransmit(false); // data can be dropped due to its volume. + // can_data.set_automatic_retransmit(false); // data can be dropped due to its volume. // can_command.set_data_bit_timing(data_bit_timing); @@ -218,7 +216,7 @@ mod app { StandardFilter::accept_all_into_fifo0(), ); // can_data.set_data_bit_timing(data_bit_timing); - + let config = can_command .get_config() .set_frame_transmit(fdcan::config::FrameTransmissionConfig::AllowFdCanAndBRS); // check this maybe don't bit switch allow. @@ -226,10 +224,8 @@ mod app { can_command.enable_interrupt(fdcan::interrupt::Interrupt::RxFifo0NewMsg); can_command.enable_interrupt_line(fdcan::interrupt::InterruptLine::_0, true); - - let can_command_manager = CanCommandManager::new(can_command.into_normal()); - + let can_command_manager = CanCommandManager::new(can_command.into_normal()); let spi_sd: stm32h7xx_hal::spi::Spi< stm32h7xx_hal::stm32::SPI1, @@ -305,7 +301,7 @@ mod app { send_data_internal::spawn(r).ok(); display_data::spawn(s).ok(); // sbg_power_on::spawn().ok(); - + ( SharedResources { data_manager, @@ -315,12 +311,8 @@ mod app { can_command_manager, can_data_manager, sbg_power, - - }, - LocalResources { - led_red, - led_green, }, + LocalResources { led_red, led_green }, ) } @@ -343,22 +335,10 @@ mod app { .data_manager .lock(|manager| manager.clone_sensors()); info!("{:?}", data.clone()); - let messages = data.into_iter().flatten().map(|x| { - Message::new( - cortex_m::interrupt::free(|cs| { - let mut rc = RTC.borrow(cs).borrow_mut(); - let rtc = rc.as_mut().unwrap(); - rtc.date_time() - .unwrap_or(NaiveDateTime::new( - NaiveDate::from_ymd_opt(2024, 1, 1).unwrap(), - NaiveTime::from_hms_milli_opt(0, 0, 0, 0).unwrap(), - )) - .and_utc().timestamp_subsec_millis() - }), - COM_ID, - Sensor::new(x), - ) - }); + let messages = data + .into_iter() + .flatten() + .map(|x| Message::new(Mono::now().ticks(), COM_ID, Sensor::new(x))); for msg in messages { sender.send(msg).await; } @@ -370,17 +350,7 @@ mod app { pub fn queue_gs_message(d: impl Into) { info!("Queueing message"); let message = messages::Message::new( - cortex_m::interrupt::free(|cs| { - let mut rc = RTC.borrow(cs).borrow_mut(); - let rtc = rc.as_mut().unwrap(); - rtc.date_time() - .unwrap_or(NaiveDateTime::new( - NaiveDate::from_ymd_opt(2024, 1, 1).unwrap(), - NaiveTime::from_hms_milli_opt(0, 0, 0, 0).unwrap(), - )) - .and_utc() - .timestamp_subsec_millis() - }), + Mono::now().ticks(), COM_ID, d.into(), ); From 8180fe71d571e41393151f52d3bdca718d49f408 Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Sat, 17 Aug 2024 00:37:25 -0400 Subject: [PATCH 35/47] Add GPS nav message type --- boards/gps/src/data_manager.rs | 116 ------------------------------- boards/gps/src/main.rs | 7 +- boards/link/src/data_manager.rs | 8 ++- libraries/messages/src/sensor.rs | 10 +++ 4 files changed, 23 insertions(+), 118 deletions(-) diff --git a/boards/gps/src/data_manager.rs b/boards/gps/src/data_manager.rs index e093a425..522f4758 100644 --- a/boards/gps/src/data_manager.rs +++ b/boards/gps/src/data_manager.rs @@ -5,85 +5,21 @@ use messages::Message; #[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 logging_rate: Option, - pub recovery_sensing: 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, - logging_rate: Some(RadioRate::Slow), // start slow. - recovery_sensing: 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); - return RadioRate::Slow; - } - - /// Do not clone instead take to reduce CPU load. - pub fn take_sensors(&mut self) -> [Option; 14] { - [ - 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.recovery_sensing.take(), - ] - } - - pub fn clone_states(&self) -> [Option; 1] { - [self.state.clone()] - } pub fn handle_command(&mut self, command: Message) { info!("Handling command"); match command.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(_) => {} @@ -94,61 +30,9 @@ impl DataManager { } pub fn handle_data(&mut self, data: Message) { 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::sensor::SensorData::RecoverySensing(_) => { - self.recovery_sensing = Some(data); - } - }, messages::Data::State(state) => { self.state = Some(state.data); } - // messages::Data::Command(command) => match command.data { - // messages::command::CommandData::RadioRateChange(command_data) => { - // self.logging_rate = Some(command_data.rate); - // } - // messages::command::CommandData::DeployDrogue(_) => {} - // messages::command::CommandData::DeployMain(_) => {} - // messages::command::CommandData::PowerDown(_) => {} - // }, _ => {} } } diff --git a/boards/gps/src/main.rs b/boards/gps/src/main.rs index 65c9698f..f0d7da23 100644 --- a/boards/gps/src/main.rs +++ b/boards/gps/src/main.rs @@ -334,7 +334,12 @@ mod app { match msg { Ok(msg) => match msg { ublox::PacketRef::NavPosLlh(x) => { - info!("GPS latitude: {:?}, longitude {:?}", x.lat_degrees(), x.lon_degrees()); + let message_data = messages::sensor::NavPosLlh { + height_msl: x.height_msl(), + longitude: x.lon_degrees(), + latitude: x.lat_degrees(), + }; + // info!("GPS latitude: {:?}, longitude {:?}", x.lat_degrees(), x.lon_degrees()); } ublox::PacketRef::NavStatus(x) => { info!("GPS fix stat: {:?}", x.fix_stat_raw()); diff --git a/boards/link/src/data_manager.rs b/boards/link/src/data_manager.rs index 8f09ac9f..98e99414 100644 --- a/boards/link/src/data_manager.rs +++ b/boards/link/src/data_manager.rs @@ -23,6 +23,7 @@ pub struct DataManager { pub reset_reason: Option, pub logging_rate: Option, pub recovery_sensing: Option, + pub nav_pos_l1h: Option, } impl DataManager { @@ -45,6 +46,7 @@ impl DataManager { reset_reason: None, logging_rate: Some(RadioRate::Slow), // start slow. recovery_sensing: None, + nav_pos_l1h: None, } } @@ -59,7 +61,7 @@ impl DataManager { } /// Do not clone instead take to reduce CPU load. - pub fn take_sensors(&mut self) -> [Option; 14] { + pub fn take_sensors(&mut self) -> [Option; 15] { [ self.air.take(), self.ekf_nav_1.take(), @@ -74,6 +76,7 @@ impl DataManager { self.gps_pos_1.take(), self.gps_pos_2.take(), self.gps_pos_acc.take(), + self.nav_pos_l1h.take(), self.recovery_sensing.take(), ] } @@ -154,6 +157,9 @@ impl DataManager { messages::sensor::SensorData::RecoverySensing(_) => { self.recovery_sensing = Some(data); } + messages::sensor::SensorData::NavPosLlh(_) => { + self.nav_pos_l1h = Some(data); + } messages::sensor::SensorData::ResetReason(_) => { } diff --git a/libraries/messages/src/sensor.rs b/libraries/messages/src/sensor.rs index dd3b8428..3fc844f1 100644 --- a/libraries/messages/src/sensor.rs +++ b/libraries/messages/src/sensor.rs @@ -22,6 +22,7 @@ pub enum SensorData { EkfNavAcc(EkfNavAcc), Imu1(Imu1), Imu2(Imu2), + NavPosLlh(NavPosLlh), GpsVel(GpsVel), GpsVelAcc(GpsVelAcc), GpsPos1(GpsPos1), @@ -31,6 +32,13 @@ pub enum SensorData { RecoverySensing(RecoverySensing), } +#[common_derives] +pub struct NavPosLlh { + pub height_msl: f64, + pub longitude: f64, + pub latitude: f64, +} + /* Replace with new health monitor */ #[common_derives] @@ -72,6 +80,8 @@ pub struct GpsPos1 { pub longitude: Option, } + + #[common_derives] pub struct GpsPos2 { #[doc = "< GPS time of week in ms."] From 9911e0f186c0232a1c9cee4b517e160f75574d47 Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Sun, 18 Aug 2024 23:27:02 -0400 Subject: [PATCH 36/47] Add online message and refactor gps board. --- boards/communication/src/data_manager.rs | 6 +- boards/communication/src/main.rs | 5 +- boards/gps/src/data_manager.rs | 1 + boards/gps/src/main.rs | 154 ++++++++++++++--------- boards/gps/src/types.rs | 14 ++- boards/nav/src/main.rs | 14 ++- boards/recovery/src/main.rs | 11 +- libraries/messages/src/command.rs | 9 ++ libraries/messages/src/sender.rs | 2 + 9 files changed, 146 insertions(+), 70 deletions(-) diff --git a/boards/communication/src/data_manager.rs b/boards/communication/src/data_manager.rs index e093a425..5f093c02 100644 --- a/boards/communication/src/data_manager.rs +++ b/boards/communication/src/data_manager.rs @@ -1,4 +1,4 @@ -use defmt::info; +use defmt::{info, error}; use messages::command::RadioRate; use messages::state::StateData; use messages::Message; @@ -88,6 +88,7 @@ impl DataManager { messages::command::CommandData::DeployDrogue(_) => {} messages::command::CommandData::DeployMain(_) => {} messages::command::CommandData::PowerDown(_) => {} + messages::command::CommandData::Online(_) => {} }, _ => {} } @@ -137,6 +138,9 @@ impl DataManager { messages::sensor::SensorData::RecoverySensing(_) => { self.recovery_sensing = Some(data); } + _ => { + error!("Unknown sensor data type"); + } }, messages::Data::State(state) => { self.state = Some(state.data); diff --git a/boards/communication/src/main.rs b/boards/communication/src/main.rs index c16e421b..67d12b3b 100644 --- a/boards/communication/src/main.rs +++ b/boards/communication/src/main.rs @@ -252,7 +252,7 @@ mod app { led_red.set_low(); blink::spawn().ok(); sensor_send::spawn().ok(); - // generate_random_messages::spawn().ok(); + generate_random_messages::spawn().ok(); // generate_random_command::spawn().ok(); let mono = Systick::new(core.SYST, gclk0.freq().to_Hz()); @@ -429,8 +429,9 @@ mod app { COM_ID, State::new(messages::state::StateData::Initializing), ); + info!("Sending message {}", message.clone()); // spawn!(send_gs, message.clone())?; - spawn!(send_internal, message)?; + spawn!(send_gs, message)?; Ok(()) }); spawn_after!(generate_random_messages, ExtU64::millis(200)).ok(); diff --git a/boards/gps/src/data_manager.rs b/boards/gps/src/data_manager.rs index 522f4758..1e7ee142 100644 --- a/boards/gps/src/data_manager.rs +++ b/boards/gps/src/data_manager.rs @@ -24,6 +24,7 @@ impl DataManager { messages::command::CommandData::DeployDrogue(_) => {} messages::command::CommandData::DeployMain(_) => {} messages::command::CommandData::PowerDown(_) => {} + messages::command::CommandData::Online(_) => {} }, _ => {} } diff --git a/boards/gps/src/main.rs b/boards/gps/src/main.rs index f0d7da23..9e47d1f0 100644 --- a/boards/gps/src/main.rs +++ b/boards/gps/src/main.rs @@ -20,6 +20,7 @@ use data_manager::DataManager; use defmt::info; use defmt_rtt as _; // global logger use fugit::ExtU64; +use types::GpsUartTx; use fugit::RateExtU32; use hal::clock::v2::pclk::Pclk; use hal::clock::v2::Source; @@ -33,6 +34,7 @@ use mcan::messageram::SharedMemory; use messages::*; use panic_probe as _; use systick_monotonic::*; +use types::GPSTransfer; use types::GPSBUFFER; use types::*; use ublox::{ @@ -86,7 +88,8 @@ mod app { struct Local { led_green: Pin, led_red: Pin, - gps_uart: GpsUart, + gps_tx: GpsUartTx, + gps_dma_transfer: Option, // sd_manager: SdManager< // Spi< // Config< @@ -294,26 +297,28 @@ mod app { dmaCh0 .as_mut() .enable_interrupts(dmac::InterruptFlags::new().with_tcmpl(true)); - let mut xfer = Transfer::new(dmaCh0, gps_rx, unsafe { &mut *BUF_DST }, false) - .expect("DMA err") - .begin( - atsamd_hal::sercom::Sercom2::DMA_RX_TRIGGER, - dmac::TriggerAction::BURST, - ); + let mut gps_dma_transfer: GPSTransfer = + Transfer::new(dmaCh0, gps_rx, unsafe { &mut *BUF_DST }, false) + .expect("DMA err") + .begin( + atsamd_hal::sercom::Sercom2::DMA_RX_TRIGGER, + dmac::TriggerAction::BURST, + ); loop { - if xfer.complete() { - let (chan0, source, buf) = xfer.stop(); - xfer = dmac::Transfer::new(chan0, source, unsafe { &mut *BUF_DST }, false) - .unwrap() - .begin( - atsamd_hal::sercom::Sercom2::DMA_RX_TRIGGER, - dmac::TriggerAction::BURST, - ); + if gps_dma_transfer.complete() { + let (chan0, source, buf) = gps_dma_transfer.stop(); + gps_dma_transfer = + dmac::Transfer::new(chan0, source, unsafe { &mut *BUF_DST }, false) + .unwrap() + .begin( + atsamd_hal::sercom::Sercom2::DMA_RX_TRIGGER, + dmac::TriggerAction::BURST, + ); let buf_clone = buf.clone(); unsafe { BUF_DST.copy_from_slice(&[0; 256]) }; - xfer.block_transfer_interrupt(); + gps_dma_transfer.block_transfer_interrupt(); let request = UbxPacketRequest::request_for::().into_packet_bytes(); for byte in request { @@ -338,7 +343,7 @@ mod app { height_msl: x.height_msl(), longitude: x.lon_degrees(), latitude: x.lat_degrees(), - }; + }; // info!("GPS latitude: {:?}, longitude {:?}", x.lat_degrees(), x.lon_degrees()); } ublox::PacketRef::NavStatus(x) => { @@ -385,6 +390,16 @@ mod app { led_green.set_low(); led_red.set_low(); blink::spawn().ok(); + let message = Message::new( + 0, // technically true time is not known yet. + COM_ID, + messages::command::Command { + data: messages::command::CommandData::Online(messages::command::Online { + online: true, + }), + }, + ); + send_command::spawn(message).ok(); let mono = Systick::new(core.SYST, gclk0.freq().to_Hz()); ( @@ -397,7 +412,8 @@ mod app { Local { led_green, led_red, - gps_uart, + gps_dma_transfer: Some(gps_dma_transfer), + gps_tx, // sd_manager, }, init::Monotonics(mono), @@ -410,52 +426,68 @@ mod app { loop {} } - #[task(binds = SERCOM2_2, local = [gps_uart], shared = [&em])] - fn gps_rx(cx: gps_rx::Context) { - cx.shared.em.run(|| { - cortex_m::interrupt::free(|cs| { - let mut buf: [u8; 256] = [0; 256]; - let mut bytes: [u8; 256] = [0; 256]; - for i in 0..buf.len() { - let item = nb::block!(cx.local.gps_uart.read()).unwrap(); - bytes[i] = item; - } - let buf = ublox::FixedLinearBuffer::new(&mut buf[..]); - let mut parser = ublox::Parser::new(buf); - let mut msgs = parser.consume(&bytes); - while let Some(msg) = msgs.next() { - match msg { - Ok(msg) => match msg { - ublox::PacketRef::NavPosLlh(x) => { - info!("GPS latitude: {:?}", x.lat_degrees()); - } - ublox::PacketRef::NavStatus(x) => { - info!("GPS fix stat: {:?}", x.fix_stat_raw()); - } - ublox::PacketRef::NavDop(x) => { - info!("GPS geometric drop: {:?}", x.geometric_dop()); - } - ublox::PacketRef::NavSat(x) => { - info!("GPS num sats used: {:?}", x.num_svs()); - } - ublox::PacketRef::NavVelNed(x) => { - info!("GPS velocity north: {:?}", x.vel_north()); - } - ublox::PacketRef::NavPvt(x) => { - info!("GPS nun sats PVT: {:?}", x.num_satellites()); - } - _ => { - info!("GPS Message not handled."); - } - }, - Err(e) => { - info!("GPS parse Error"); + #[task(priority = 3, binds = DMAC_0, local = [gps_dma_transfer, gps_tx], shared = [&em, data_manager])] + fn gps_dma(mut cx: gps_dma::Context) { + let mut gps_dma_transfer = cx.local.gps_dma_transfer.take().unwrap(); + let mut gps_tx = cx.local.gps_tx; + if gps_dma_transfer.complete() { + let (chan0, source, buf) = gps_dma_transfer.stop(); + gps_dma_transfer = dmac::Transfer::new(chan0, source, unsafe { &mut *BUF_DST }, false) + .unwrap() + .begin( + atsamd_hal::sercom::Sercom2::DMA_RX_TRIGGER, + dmac::TriggerAction::BURST, + ); + let buf_clone = buf.clone(); + + unsafe { BUF_DST.copy_from_slice(&[0; 256]) }; + gps_dma_transfer.block_transfer_interrupt(); + let request = UbxPacketRequest::request_for::().into_packet_bytes(); + for byte in request { + nb::block!(gps_tx.write(byte)).unwrap(); + } + cortex_m::asm::delay(300_000); + let mut buf: [u8; 256] = [0; 256]; + let mut bytes: [u8; 256] = [0; 256]; + let buf = ublox::FixedLinearBuffer::new(&mut buf[..]); + let mut parser = ublox::Parser::new(buf); + let mut msgs = parser.consume(&buf_clone); + while let Some(msg) = msgs.next() { + match msg { + Ok(msg) => match msg { + ublox::PacketRef::NavPosLlh(x) => { + let message_data = messages::sensor::NavPosLlh { + height_msl: x.height_msl(), + longitude: x.lon_degrees(), + latitude: x.lat_degrees(), + }; + // info!("GPS latitude: {:?}, longitude {:?}", x.lat_degrees(), x.lon_degrees()); + } + ublox::PacketRef::NavStatus(x) => { + info!("GPS fix stat: {:?}", x.fix_stat_raw()); + } + ublox::PacketRef::NavDop(x) => { + info!("GPS geometric drop: {:?}", x.geometric_dop()); + } + ublox::PacketRef::NavSat(x) => { + info!("GPS num sats used: {:?}", x.num_svs()); + } + ublox::PacketRef::NavVelNed(x) => { + info!("GPS velocity north: {:?}", x.vel_north()); } + ublox::PacketRef::NavPvt(x) => { + info!("GPS nun sats PVT: {:?}", x.num_satellites()); + } + _ => { + info!("GPS Message not handled."); + } + }, + Err(e) => { + info!("GPS parse Error"); } } - }); - Ok(()) - }); + } + } } /// Handles the CAN1 interrupt. diff --git a/boards/gps/src/types.rs b/boards/gps/src/types.rs index dbf53c90..b7753ba1 100644 --- a/boards/gps/src/types.rs +++ b/boards/gps/src/types.rs @@ -1,13 +1,15 @@ use atsamd_hal::gpio::*; use atsamd_hal::sercom::uart::EightBit; -use atsamd_hal::sercom::{uart, uart::Duplex, IoSet1, Sercom2, IoSet3}; +use atsamd_hal::sercom::{uart, uart::Duplex, IoSet1, Sercom2, IoSet3, uart::TxDuplex}; use messages::sender::Sender; -use messages::sender::Sender::CommunicationBoard; +use messages::sender::Sender::GpsBoard; +use atsamd_hal::dmac; +use atsamd_hal::dmac::BufferPair; // ------- // Sender ID // ------- -pub static COM_ID: Sender = CommunicationBoard; +pub static COM_ID: Sender = GpsBoard; // ------- // GPS UART @@ -15,7 +17,11 @@ pub static COM_ID: Sender = CommunicationBoard; pub type GpsPads = uart::PadsFromIds; pub type GpsUartConfig = uart::Config; pub type GpsUart = uart::Uart; - +pub type GpsUartTx = uart::Uart; +pub type GPSTransfer = dmac::Transfer< + dmac::Channel, + BufferPair, GPSBUFFER>, +>; pub type GPSBUFFER = &'static mut [u8; 256]; // ------- diff --git a/boards/nav/src/main.rs b/boards/nav/src/main.rs index 59a59bc1..df16b637 100644 --- a/boards/nav/src/main.rs +++ b/boards/nav/src/main.rs @@ -82,7 +82,8 @@ mod app { #[init] fn init(ctx: init::Context) -> (SharedResources, LocalResources) { // channel setup - let (s, r) = make_channel!(Message, DATA_CHANNEL_CAPACITY); + let (mut s, mut r) = make_channel!(Message, DATA_CHANNEL_CAPACITY); + let (mut s_command, mut r_command) = make_channel!(Message, DATA_CHANNEL_CAPACITY); let core = ctx.core; @@ -301,6 +302,17 @@ mod app { send_data_internal::spawn(r).ok(); display_data::spawn(s).ok(); // sbg_power_on::spawn().ok(); + let message = Message::new( + 0,// technically true time is not known yet. + COM_ID, + messages::command::Command { + data: messages::command::CommandData::Online(messages::command::Online { online: true }), + }, + ); + send_command_internal::spawn(r_command).ok(); + async { + s_command.send(message).await; + }; ( SharedResources { diff --git a/boards/recovery/src/main.rs b/boards/recovery/src/main.rs index 331f77f4..be0accba 100644 --- a/boards/recovery/src/main.rs +++ b/boards/recovery/src/main.rs @@ -202,10 +202,19 @@ mod app { /* Spawn tasks */ run_sm::spawn().ok(); - read_barometer::spawn().ok(); + // read_barometer::spawn().ok(); state_send::spawn().ok(); ejection_sense::spawn().ok(); blink::spawn().ok(); + // send an online message to the com board. + let message = Message::new( + 0,// technically true time is not known yet. + COM_ID, + messages::command::Command { + data: messages::command::CommandData::Online(messages::command::Online { online: true }), + }, + ); + send_command::spawn(message).ok(); // fire_main::spawn_after(ExtU64::secs(15)).ok(); // fire_drogue::spawn_after(ExtU64::secs(15)).ok(); diff --git a/libraries/messages/src/command.rs b/libraries/messages/src/command.rs index 6ad0b804..8597b9ca 100644 --- a/libraries/messages/src/command.rs +++ b/libraries/messages/src/command.rs @@ -7,6 +7,7 @@ pub struct Command { pub data: CommandData, } + #[common_derives] #[derive(From)] pub enum CommandData { @@ -14,6 +15,14 @@ pub enum CommandData { DeployMain(DeployMain), PowerDown(PowerDown), RadioRateChange(RadioRateChange), + Online(Online), +} + + +// not really a command but this decreases the complexity of the change on ground station. +#[common_derives] +pub struct Online { + pub online: bool, } #[common_derives] diff --git a/libraries/messages/src/sender.rs b/libraries/messages/src/sender.rs index 2bc90999..06279a1c 100644 --- a/libraries/messages/src/sender.rs +++ b/libraries/messages/src/sender.rs @@ -11,6 +11,7 @@ pub enum Sender { PowerBoard, CameraBoard, BeaconBoard, + GpsBoard, } impl From for u16 { @@ -23,6 +24,7 @@ impl From for u16 { Sender::PowerBoard => 4, Sender::CameraBoard => 5, Sender::BeaconBoard => 6, + Sender::GpsBoard => 7, } } } From 8879740b4f6ecf4103803fac421609789640e3e0 Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Mon, 19 Aug 2024 15:37:00 -0400 Subject: [PATCH 37/47] WIP: GPS and Link fushion. --- Cargo.lock | 2 + boards/communication/Cargo.toml | 4 +- boards/communication/src/communication.rs | 74 ++---- boards/communication/src/gps_manager.rs | 109 +++++++++ boards/communication/src/main.rs | 281 ++++++++++++---------- boards/communication/src/types.rs | 13 +- boards/gps/src/main.rs | 5 +- 7 files changed, 312 insertions(+), 176 deletions(-) create mode 100644 boards/communication/src/gps_manager.rs diff --git a/Cargo.lock b/Cargo.lock index b5b103ce..65ee7864 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -302,6 +302,7 @@ dependencies = [ "cortex-m-rtic", "defmt", "defmt-rtt", + "embedded-alloc", "embedded-sdmmc 0.8.0", "heapless 0.7.17", "messages", @@ -309,6 +310,7 @@ dependencies = [ "postcard", "systick-monotonic", "typenum", + "ublox", ] [[package]] diff --git a/boards/communication/Cargo.toml b/boards/communication/Cargo.toml index 60811447..2297846c 100644 --- a/boards/communication/Cargo.toml +++ b/boards/communication/Cargo.toml @@ -21,4 +21,6 @@ embedded-sdmmc = "0.8.0" #panic-halt = "0.2.0" defmt = "0.3" defmt-rtt = "0.4" -panic-probe = { version = "0.3", features = ["print-defmt"] } \ No newline at end of file +panic-probe = { version = "0.3", features = ["print-defmt"] } +ublox = {version = "0.4.5", features = ['serde', 'alloc'], default-features = false} +embedded-alloc = "0.5.0" diff --git a/boards/communication/src/communication.rs b/boards/communication/src/communication.rs index 61249302..16c7220e 100644 --- a/boards/communication/src/communication.rs +++ b/boards/communication/src/communication.rs @@ -13,6 +13,7 @@ use atsamd_hal::gpio::{ use atsamd_hal::gpio::{AlternateH, H, PB14, PB15}; use atsamd_hal::pac::{CAN0, CAN1}; use atsamd_hal::prelude::*; +use atsamd_hal::sercom::spi::Duplex; use atsamd_hal::sercom::uart; use atsamd_hal::sercom::uart::Uart; use atsamd_hal::sercom::uart::{RxDuplex, TxDuplex}; @@ -394,42 +395,18 @@ impl CanCommandManager { } } -pub struct RadioDevice { - transmitter: Uart, - receiver: PeekReader>, -} - -impl RadioDevice { - pub fn new( - uart: atsamd_hal::sercom::uart::Uart< - GroundStationUartConfig, - atsamd_hal::sercom::uart::Duplex, - >, - ) -> Self { - let (mut rx, tx) = uart.split(); - - // setup interrupts - rx.enable_interrupts(uart::Flags::RXC); - - RadioDevice { - transmitter: tx, - receiver: PeekReader::new(rx), - } - } -} - pub struct RadioManager { buf: HistoryBuffer, - radio: RadioDevice, + radio: Option>, mav_sequence: u8, } impl RadioManager { - pub fn new(radio: RadioDevice) -> Self { + pub fn new(radio: atsamd_hal::sercom::uart::Uart) -> Self { let buf = HistoryBuffer::new(); RadioManager { buf, - radio, + radio: Some(radio), mav_sequence: 0, } } @@ -449,8 +426,9 @@ impl RadioManager { message: fixed_payload, }, ); + let mut radio = self.radio.take().unwrap(); mavlink::write_versioned_msg( - &mut self.radio.transmitter, + &mut radio, mavlink::MavlinkVersion::V2, mav_header, &mav_message, @@ -461,27 +439,27 @@ impl RadioManager { 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)?; + // 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) => { - return Ok(postcard::from_bytes::(&msg.message)?); - // weird Ok syntax to coerce to hydra error type. - } - mavlink::uorocketry::MavMessage::COMMAND_MESSAGE(command) => { - info!("{}", command.command); - return Ok(postcard::from_bytes::(&command.command)?); - } - _ => { - error!("Error, ErrorContext::UnkownPostcardMessage"); - return Err(mavlink::error::MessageReadError::Io.into()); - } - } - } + // // info!("{:?}", ); + // // Do we need the header? + // match msg { + // mavlink::uorocketry::MavMessage::POSTCARD_MESSAGE(msg) => { + // return Ok(postcard::from_bytes::(&msg.message)?); + // // weird Ok syntax to coerce to hydra error type. + // } + // mavlink::uorocketry::MavMessage::COMMAND_MESSAGE(command) => { + // info!("{}", command.command); + // return Ok(postcard::from_bytes::(&command.command)?); + // } + // _ => { + // error!("Error, ErrorContext::UnkownPostcardMessage"); + // return Err(mavlink::error::MessageReadError::Io.into()); + // } + // } + // } } // impl Read for RadioManager { diff --git a/boards/communication/src/gps_manager.rs b/boards/communication/src/gps_manager.rs new file mode 100644 index 00000000..6b044bda --- /dev/null +++ b/boards/communication/src/gps_manager.rs @@ -0,0 +1,109 @@ +use crate::app::gps_dma; +use atsamd_hal::dmac; +use atsamd_hal::gpio::{Output, Pin, PushPull, PB07, PB09}; +use atsamd_hal::sercom::uart::Uart; +use defmt::info; +use crate::types::{ + GPSTransfer, GpsUart, GpsUartConfig, GpsUartTx, GroundStationPads, GroundStationUartConfig, + GPSBUFFER, +}; +use ublox::{ + CfgPrtUartBuilder, DataBits, InProtoMask, OutProtoMask, Parity, StopBits, UartMode, UartPortId, UbxPacketRequest +}; +// use atsamd_hal::prelude::nb; +use rtic::Mutex; +use atsamd_hal::prelude::*; + +pub static mut BUF_DST: GPSBUFFER = &mut [0; 256]; + +/// GPS needs reset only once on startup and gps enable otherwise they are unused. This should not be in the constructor incase we are +/// We are using options right now because of a board issue and usage of two sercoms. +pub struct GpsManager { + pub gps_tx: Option, + pub transfer: Option, + gps_enable: Pin>, + gps_reset: Pin>, +} + +impl GpsManager { + pub fn new( + gps_tx: Option, + gps_reset: Pin>, + gps_enable: Pin>, + ) -> Self { + Self { + gps_tx, + gps_reset, + gps_enable, + transfer: None, + } + } +} + +pub fn gps_dma(mut cx: crate::app::gps_dma::Context) { + cx.shared.gps_manager.lock(|gps_manager| { + let mut gps_manager = gps_manager.take().unwrap(); + let mut gps_dma_transfer = gps_manager.transfer.take().unwrap(); + let mut gps_tx = gps_manager.gps_tx.take().unwrap(); + if gps_dma_transfer.complete() { + let (chan0, source, buf) = gps_dma_transfer.stop(); + // gps_dma_transfer = dmac::Transfer::new(chan0, source, unsafe { &mut *BUF_DST }, false) + // .unwrap() + // .begin( + // atsamd_hal::sercom::Sercom2::DMA_RX_TRIGGER, + // dmac::TriggerAction::BURST, + // ); + + let buf_clone = buf.clone(); + let mut uart = Uart::join(source, gps_tx); + unsafe { BUF_DST.copy_from_slice(&[0; 256]) }; + // gps_dma_transfer.block_transfer_interrupt(); + + let request = UbxPacketRequest::request_for::().into_packet_bytes(); + for byte in request { + nb::block!(uart.write(byte)).unwrap(); + } + cortex_m::asm::delay(300_000); + let mut buf: [u8; 256] = [0; 256]; + let mut bytes: [u8; 256] = [0; 256]; + let buf = ublox::FixedLinearBuffer::new(&mut buf[..]); + let mut parser = ublox::Parser::new(buf); + let mut msgs = parser.consume(&buf_clone); + while let Some(msg) = msgs.next() { + match msg { + Ok(msg) => match msg { + ublox::PacketRef::NavPosLlh(x) => { + let message_data = messages::sensor::NavPosLlh { + height_msl: x.height_msl(), + longitude: x.lon_degrees(), + latitude: x.lat_degrees(), + }; + // info!("GPS latitude: {:?}, longitude {:?}", x.lat_degrees(), x.lon_degrees()); + } + ublox::PacketRef::NavStatus(x) => { + info!("GPS fix stat: {:?}", x.fix_stat_raw()); + } + ublox::PacketRef::NavDop(x) => { + info!("GPS geometric drop: {:?}", x.geometric_dop()); + } + ublox::PacketRef::NavSat(x) => { + info!("GPS num sats used: {:?}", x.num_svs()); + } + ublox::PacketRef::NavVelNed(x) => { + info!("GPS velocity north: {:?}", x.vel_north()); + } + ublox::PacketRef::NavPvt(x) => { + info!("GPS nun sats PVT: {:?}", x.num_satellites()); + } + _ => { + info!("GPS Message not handled."); + } + }, + Err(e) => { + info!("GPS parse Error"); + } + } + } + } + }); +} diff --git a/boards/communication/src/main.rs b/boards/communication/src/main.rs index 67d12b3b..2b2e3bda 100644 --- a/boards/communication/src/main.rs +++ b/boards/communication/src/main.rs @@ -3,12 +3,14 @@ mod communication; mod data_manager; +mod gps_manager; mod types; use atsamd_hal as hal; use atsamd_hal::rtc::Count32Mode; use common_arm::*; use common_arm_atsame::mcan; +use gps_manager::{gps_dma, GpsManager}; // use panic_halt as _; use atsamd_hal::gpio::{Alternate, Output, PushPull, D, PA04, PA05, PA06, PA07}; use atsamd_hal::sercom::{ @@ -17,7 +19,7 @@ use atsamd_hal::sercom::{ Sercom0, }; use communication::Capacities; -use communication::{CanCommandManager, RadioDevice, RadioManager}; +use communication::{CanCommandManager, RadioManager}; use core::cell::RefCell; use cortex_m::interrupt::Mutex; use data_manager::DataManager; @@ -43,7 +45,7 @@ use types::*; #[inline(never)] #[defmt::panic_handler] fn panic() -> ! { - cortex_m::asm::udf() + atsamd_hal::pac::SCB::sys_reset() } /// Hardfault handler. @@ -56,6 +58,9 @@ unsafe fn HardFault(_frame: &cortex_m_rt::ExceptionFrame) -> ! { loop {} } +#[global_allocator] +static HEAP: embedded_alloc::Heap = embedded_alloc::Heap::empty(); + static RTC: Mutex>>> = Mutex::new(RefCell::new(None)); #[rtic::app(device = hal::pac, peripherals = true, dispatchers = [EVSYS_0, EVSYS_1, EVSYS_2])] @@ -71,7 +76,8 @@ mod app { struct Shared { em: ErrorManager, data_manager: DataManager, - radio_manager: RadioManager, + radio_manager: Option, + gps_manager: Option, can0: communication::CanDevice0, can_command_manager: CanCommandManager, } @@ -108,6 +114,12 @@ mod app { can_command_memory: SharedMemory = SharedMemory::new() ])] fn init(cx: init::Context) -> (Shared, Local, init::Monotonics) { + { + use core::mem::MaybeUninit; + const HEAP_SIZE: usize = 1024; + static mut HEAP_MEM: [MaybeUninit; HEAP_SIZE] = [MaybeUninit::uninit(); HEAP_SIZE]; + unsafe { HEAP.init(HEAP_MEM.as_ptr() as usize, HEAP_SIZE) } + } let mut peripherals = cx.device; let core = cx.core; let pins = Pins::new(peripherals.PORT); @@ -192,9 +204,9 @@ mod app { ) .enable(); - let radio = RadioDevice::new(uart); + // let radio = RadioDevice::new(uart); - let radio_manager = RadioManager::new(radio); + let radio_manager = RadioManager::new(uart); // /* SD config */ // let (mut gclk1, dfll) = @@ -215,16 +227,23 @@ mod app { let (pclk_gps, gclk0) = Pclk::enable(tokens.pclks.sercom4, gclk0); //info!("here"); - let pads = hal::sercom::uart::Pads::::default() - .rx(pins.pa12) - .tx(pins.pa13); - - let gps_uart = GpsUartConfig::new(&mclk, peripherals.SERCOM4, pads, pclk_gps.freq()) - .baud( - RateExtU32::Hz(9600), - uart::BaudMode::Fractional(uart::Oversampling::Bits16), - ) - .enable(); + // let pads = hal::sercom::uart::Pads::::default() + // .rx(pins.pa12) + // .tx(pins.pa13); + + // let gps_uart = GpsUartConfig::new(&mclk, peripherals.SERCOM4, pads, pclk_gps.freq()) + // .baud( + // RateExtU32::Hz(9600), + // uart::BaudMode::Fractional(uart::Oversampling::Bits16), + // ) + // .enable(); + + let gps_manager = GpsManager::new( + None, + pins.pb07.into_push_pull_output(), + pins.pb09.into_push_pull_output(), + ); + // gps_uart.enable_interrupts(hal::sercom::uart::Flags::RXC); /* Status LED */ @@ -248,11 +267,10 @@ mod app { }); let mut led_green = pins.pa03.into_push_pull_output(); let mut led_red = pins.pa02.into_push_pull_output(); - led_green.set_low(); - led_red.set_low(); + led_green.set_high(); + led_red.set_high(); blink::spawn().ok(); sensor_send::spawn().ok(); - generate_random_messages::spawn().ok(); // generate_random_command::spawn().ok(); let mono = Systick::new(core.SYST, gclk0.freq().to_Hz()); @@ -261,7 +279,8 @@ mod app { Shared { em: ErrorManager::new(), data_manager: DataManager::new(), - radio_manager, + radio_manager: Some(radio_manager), + gps_manager: Some(gps_manager), can0, can_command_manager, }, @@ -311,7 +330,7 @@ mod app { }); } /** - * Sends a message over CAN. + * Sends a mgps_txessage over CAN. */ #[task(capacity = 10, shared = [can0, &em])] fn send_internal(mut cx: send_internal::Context, m: Message) { @@ -347,24 +366,24 @@ mod app { COM_ID, d.into(), ); - - send_gs::spawn(message).ok(); + info!("Sending message {}", message); + // send_gs::spawn(message).ok(); } - /** - * 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) { - cx.shared.radio_manager.lock(|radio_manager| { - cx.shared.em.run(|| { - let mut buf = [0; 255]; - let data = postcard::to_slice(&m, &mut buf)?; - radio_manager.send_message(data)?; - Ok(()) - }) - }); - } + // /** + // * 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) { + // cx.shared.radio_manager.lock(|radio_manager| { + // cx.shared.em.run(|| { + // let mut buf = [0; 255]; + // let data = postcard::to_slice(&m, &mut buf)?; + // radio_manager.send_message(data)?; + // Ok(()) + // }) + // }); + // } // #[task(capacity = 10, local = [sd_manager], shared = [&em])] // fn sd_dump(cx: sd_dump::Context, m: Message) { @@ -386,27 +405,38 @@ mod app { /** * Sends information about the sensors. */ - #[task(shared = [data_manager, &em])] + #[task(shared = [data_manager, radio_manager, gps_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())); - cx.shared.em.run(|| { - for msg in sensors { - match msg { - Some(x) => { - spawn!(send_gs, x.clone())?; - // spawn!(sd_dump, x)?; - } - None => { - continue; - } + for msg in sensors { + match msg { + Some(x) => { + // spawn!(send_gs, x.clone())?; + cx.shared.radio_manager.lock(|radio_manager| { + cx.shared.em.run(|| { + let mut buf = [0; 255]; + let data = postcard::to_slice(&x, &mut buf)?; + let mut radio = radio_manager.take().unwrap(); + radio.send_message(data)?; + // let uart = radio.release(); + // cx.shared.gps_manager.lock(|gps| { + // let mut gps = gps.take().unwrap(); + // }); + Ok(()) + }); + }); + + // spawn!(sd_dump, x)?; + } + None => { + continue; } } - Ok(()) - }); + } match logging_rate { RadioRate::Fast => { spawn_after!(sensor_send, ExtU64::millis(100)).ok(); @@ -417,29 +447,53 @@ mod app { } } - #[task(shared = [&em])] - fn generate_random_messages(cx: generate_random_messages::Context) { - cx.shared.em.run(|| { - let message = Message::new( - cortex_m::interrupt::free(|cs| { - let mut rc = RTC.borrow(cs).borrow_mut(); - let rtc = rc.as_mut().unwrap(); - rtc.count32() - }), - COM_ID, - State::new(messages::state::StateData::Initializing), - ); - info!("Sending message {}", message.clone()); - // spawn!(send_gs, message.clone())?; - spawn!(send_gs, message)?; - Ok(()) - }); - spawn_after!(generate_random_messages, ExtU64::millis(200)).ok(); - } + // #[task(shared = [&em])] + // fn generate_random_messages(cx: generate_random_messages::Context) { + // cx.shared.em.run(|| { + // let message = Message::new( + // cortex_m::interrupt::free(|cs| { + // let mut rc = RTC.borrow(cs).borrow_mut(); + // let rtc = rc.as_mut().unwrap(); + // rtc.count32() + // }), + // COM_ID, + // State::new(messages::state::StateData::Initializing), + // ); + // info!("Sending message {}", message.clone()); + // // spawn!(send_gs, message.clone())?; + // spawn!(send_gs, message)?; + // Ok(()) + // }); + // spawn_after!(generate_random_messages, ExtU64::millis(200)).ok(); + // } - #[task(shared = [&em])] - fn generate_random_command(cx: generate_random_command::Context) { - cx.shared.em.run(|| { + // #[task(shared = [&em])] + // fn generate_random_command(cx: generate_random_command::Context) { + // cx.shared.em.run(|| { + // let message = Message::new( + // cortex_m::interrupt::free(|cs| { + // let mut rc = RTC.borrow(cs).borrow_mut(); + // let rtc = rc.as_mut().unwrap(); + // rtc.count32() + // }), + // COM_ID, + // Command::new(CommandData::PowerDown(command::PowerDown { + // board: Sender::BeaconBoard, + // })), + // ); + // spawn!(send_command, message)?; + // Ok(()) + // }); + // spawn!(generate_random_command).ok(); + // } + + #[task(shared = [data_manager, &em, radio_manager])] + fn state_send(mut cx: state_send::Context) { + let state_data = cx + .shared + .data_manager + .lock(|data_manager| data_manager.state.clone()); + if let Some(x) = state_data { let message = Message::new( cortex_m::interrupt::free(|cs| { let mut rc = RTC.borrow(cs).borrow_mut(); @@ -447,56 +501,41 @@ mod app { rtc.count32() }), COM_ID, - Command::new(CommandData::PowerDown(command::PowerDown { - board: Sender::BeaconBoard, - })), + State::new(x), ); - spawn!(send_command, message)?; - Ok(()) - }); - spawn!(generate_random_command).ok(); - } - - #[task(shared = [data_manager, &em])] - 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( - cortex_m::interrupt::free(|cs| { - let mut rc = RTC.borrow(cs).borrow_mut(); - let rtc = rc.as_mut().unwrap(); - rtc.count32() - }), - COM_ID, - State::new(x), - ); - // spawn!(send_gs, message)?; - } // if there is none we still return since we simply don't have data yet. - Ok(()) - }); + cx.shared.radio_manager.lock(|mut radio_manager| { + cx.shared.em.run(|| { + let mut buf = [0; 255]; + let data = postcard::to_slice(&message, &mut buf)?; + let mut radio = radio_manager.take().unwrap(); + radio.send_message(data)?; + *radio_manager = Some(radio); + Ok(()) + }); + }); + // spawn!(send_gs, message)?; + } // if there is none we still return since we simply don't have data yet. spawn_after!(state_send, ExtU64::secs(5)).ok(); } - /// Handles the radio interrupt. - /// This only needs to be called when the radio has data to read, this is why an interrupt handler is used above polling which would waste resources. - /// We use a critical section to ensure that we are not interrupted while servicing the mavlink message. - #[task(priority = 3, binds = SERCOM2_2, shared = [&em, radio_manager])] - fn radio_rx(mut cx: radio_rx::Context) { - cx.shared.radio_manager.lock(|radio_manager| { - cx.shared.em.run(|| { - cortex_m::interrupt::free(|cs| { - let m = radio_manager.receive_message()?; - info!("Received message {}", m.clone()); - spawn!(send_command, m) - })?; - Ok(()) - }); - }); - } + // /// Handles the radio interrupt. + // /// This only needs to be called when the radio has data to read, this is why an interrupt handler is used above polling which would waste resources. + // /// We use a critical section to ensure that we are not interrupted while servicing the mavlink message. + // #[task(priority = 3, binds = SERCOM2_2, shared = [&em, radio_manager])] + // fn radio_rx(mut cx: radio_rx::Context) { + // cx.shared.radio_manager.lock(|radio_manager| { + // cx.shared.em.run(|| { + // cortex_m::interrupt::free(|cs| { + // let mut radio = radio_manager.take(); + // let m = radio.receive_message()?; + // radio_manager = Some(radio); + // info!("Received message {}", m.clone()); + // spawn!(send_command, m) + // })?; + // Ok(()) + // }); + // }); + // } /** * Simple blink task to test the system. @@ -516,8 +555,8 @@ mod app { }); } - // extern "Rust" { - // #[task(binds = DMAC_0, shared=[&em], local=[radio_manager])] - // fn radio_dma(context: radio_dma::Context); - // } + extern "Rust" { + #[task(priority = 3, binds = DMAC_0, shared = [&em, gps_manager, data_manager, radio_manager])] + fn gps_dma(context: gps_dma::Context); + } } diff --git a/boards/communication/src/types.rs b/boards/communication/src/types.rs index cba8a5a6..2adf5d30 100644 --- a/boards/communication/src/types.rs +++ b/boards/communication/src/types.rs @@ -1,8 +1,11 @@ use atsamd_hal::gpio::*; use atsamd_hal::sercom::uart::EightBit; -use atsamd_hal::sercom::{uart, uart::Duplex, IoSet3, Sercom2, Sercom4}; +use atsamd_hal::sercom::{uart, uart::Duplex, IoSet3, Sercom2, IoSet1, Sercom4}; use messages::sender::Sender; use messages::sender::Sender::CommunicationBoard; +use atsamd_hal::{dmac, dmac::BufferPair}; +use atsamd_hal::sercom::uart::TxDuplex; + // ------- // Sender ID @@ -18,6 +21,12 @@ pub type GroundStationUartConfig = uart::Config; // ------- // GPS UART // ------- -pub type GpsPads = uart::PadsFromIds; +pub type GpsPads = uart::PadsFromIds; pub type GpsUartConfig = uart::Config; pub type GpsUart = uart::Uart; +pub type GpsUartTx = uart::Uart; +pub type GPSTransfer = dmac::Transfer< + dmac::Channel, + BufferPair, GPSBUFFER>, +>; +pub type GPSBUFFER = &'static mut [u8; 256]; \ No newline at end of file diff --git a/boards/gps/src/main.rs b/boards/gps/src/main.rs index 9e47d1f0..50928818 100644 --- a/boards/gps/src/main.rs +++ b/boards/gps/src/main.rs @@ -639,8 +639,5 @@ mod app { }); } - // extern "Rust" { - // #[task(binds = DMAC_0, shared=[&em], local=[radio_manager])] - // fn radio_dma(context: radio_dma::Context); - // } + } From bed3a92c1cefe5ca7910c0994c6b11911ba71e58 Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Mon, 19 Aug 2024 23:23:54 -0400 Subject: [PATCH 38/47] WIP: Weird link board. --- boards/communication/src/communication.rs | 20 ++- boards/communication/src/gps_manager.rs | 173 +++++++++++++++--- boards/communication/src/main.rs | 203 +++++++++++++++------- boards/gps/src/main.rs | 3 +- boards/link/src/main.rs | 106 +++++++---- boards/nav/src/communication.rs | 2 +- boards/nav/src/main.rs | 11 ++ boards/recovery/src/communication.rs | 2 +- boards/recovery/src/main.rs | 2 +- 9 files changed, 393 insertions(+), 129 deletions(-) diff --git a/boards/communication/src/communication.rs b/boards/communication/src/communication.rs index 16c7220e..a9fd57db 100644 --- a/boards/communication/src/communication.rs +++ b/boards/communication/src/communication.rs @@ -8,7 +8,7 @@ use atsamd_hal::clock::v2::pclk::Pclk; use atsamd_hal::clock::v2::types::{Can0, Can1}; use atsamd_hal::clock::v2::Source; use atsamd_hal::gpio::{ - Alternate, AlternateI, Pin, I, PA22, PA23, + Alternate, AlternateI, Pin, I, PA22, PA23, PA09, PA08, C, D }; use atsamd_hal::gpio::{AlternateH, H, PB14, PB15}; use atsamd_hal::pac::{CAN0, CAN1}; @@ -26,6 +26,8 @@ use defmt::error; use defmt::info; use heapless::HistoryBuffer; use heapless::Vec; +use atsamd_hal::time::Hertz; + use mavlink::peek_reader::PeekReader; use mcan::bus::Can; use mcan::embedded_can as ecan; @@ -397,17 +399,27 @@ impl CanCommandManager { pub struct RadioManager { buf: HistoryBuffer, - radio: Option>, + pub radio: Option>, + pub radio_pads: Option, + pub sercom: Option, mav_sequence: u8, + pub rx: Option>>, + pub tx: Option>>, + pub uart_clk_freq: Hertz, } impl RadioManager { - pub fn new(radio: atsamd_hal::sercom::uart::Uart) -> Self { + pub fn new(rx: Pin>, tx: Pin>, uart_clk_freq: Hertz) -> Self { let buf = HistoryBuffer::new(); RadioManager { buf, - radio: Some(radio), + radio: None, mav_sequence: 0, + radio_pads: None, + sercom: None, + uart_clk_freq, + rx: Some(rx), + tx: Some(tx), } } pub fn send_message(&mut self, payload: &[u8]) -> Result<(), HydraError> { diff --git a/boards/communication/src/gps_manager.rs b/boards/communication/src/gps_manager.rs index 6b044bda..d35c0a29 100644 --- a/boards/communication/src/gps_manager.rs +++ b/boards/communication/src/gps_manager.rs @@ -1,69 +1,188 @@ use crate::app::gps_dma; -use atsamd_hal::dmac; -use atsamd_hal::gpio::{Output, Pin, PushPull, PB07, PB09}; -use atsamd_hal::sercom::uart::Uart; -use defmt::info; use crate::types::{ - GPSTransfer, GpsUart, GpsUartConfig, GpsUartTx, GroundStationPads, GroundStationUartConfig, - GPSBUFFER, + GPSTransfer, GpsPads, GpsUart, GpsUartConfig, GpsUartTx, GroundStationPads, + GroundStationUartConfig, GPSBUFFER, }; +use atsamd_hal::dmac::Transfer; +use atsamd_hal::dmac::{self, transfer}; +use atsamd_hal::gpio::{Output, Pin, PushPull, PB07, PB09, Alternate, C, PA12, PA13}; +use atsamd_hal::sercom::uart; +use atsamd_hal::sercom::uart::Uart; +use atsamd_hal::sercom::Sercom; +use atsamd_hal::time::Hertz; +use defmt::info; use ublox::{ - CfgPrtUartBuilder, DataBits, InProtoMask, OutProtoMask, Parity, StopBits, UartMode, UartPortId, UbxPacketRequest + CfgMsgAllPortsBuilder, CfgPrtUartBuilder, DataBits, InProtoMask, NavPvt, OutProtoMask, Parity, + StopBits, UartMode, UartPortId, UbxPacketRequest, }; // use atsamd_hal::prelude::nb; -use rtic::Mutex; +use atsamd_hal::fugit::ExtU64; +use atsamd_hal::fugit::RateExtU32; use atsamd_hal::prelude::*; +use rtic::Mutex; pub static mut BUF_DST: GPSBUFFER = &mut [0; 256]; /// GPS needs reset only once on startup and gps enable otherwise they are unused. This should not be in the constructor incase we are /// We are using options right now because of a board issue and usage of two sercoms. pub struct GpsManager { - pub gps_tx: Option, + pub gps_uart: Option, + pub gps_uart_tx: Option, pub transfer: Option, gps_enable: Pin>, gps_reset: Pin>, + pub gps_pads: Option, + pub sercom: Option, + pub uart_clk_freq: Hertz, + pub rx: Option>>, + pub tx: Option>>, } impl GpsManager { pub fn new( - gps_tx: Option, - gps_reset: Pin>, - gps_enable: Pin>, + mut gps_uart: GpsUart, // cannot be an option in this constructor because we want to configure it before the radio. + gps_pads: Option, + sercom: Option, + mut gps_reset: Pin>, + mut gps_enable: Pin>, + mut dma_channel: dmac::Channel, + pclk_sercom2_freq: Hertz, ) -> Self { + gps_reset.set_low().ok(); + cortex_m::asm::delay(300_000); + gps_reset.set_high().ok(); + gps_enable.set_low().ok(); + + // let (mut gps_rx, mut gps_tx) = gps_uart.split(); + + let packet: [u8; 28] = CfgPrtUartBuilder { + portid: UartPortId::Uart1, + reserved0: 0, + tx_ready: 0, + mode: UartMode::new(DataBits::Eight, Parity::None, StopBits::One), + baud_rate: 9600, + in_proto_mask: InProtoMask::all(), + out_proto_mask: OutProtoMask::UBLOX, + flags: 0, + reserved5: 0, + } + .into_packet_bytes(); + + for byte in packet { + nb::block!(gps_uart.write(byte)).unwrap(); + } + + // cortex_m::asm::delay(300_000); + + // let packet_two = CfgMsgAllPortsBuilder::set_rate_for::([1, 0, 0, 0, 0, 0]).into_packet_bytes(); + // for byte in packet_two { + // nb::block!(gps_uart.write(byte)).unwrap(); + // } + + // cortex_m::asm::delay(300_000); + // let request = UbxPacketRequest::request_for::().into_packet_bytes(); + // for byte in request { + // nb::block!(gps_uart.write(byte)).unwrap(); + // } + + let (mut rx, mut tx) = gps_uart.split(); + dma_channel + .as_mut() + .enable_interrupts(dmac::InterruptFlags::new().with_tcmpl(true)); + let mut gps_dma_transfer = Transfer::new(dma_channel, rx, unsafe { &mut *BUF_DST }, false) + .expect("DMA err") + .begin( + atsamd_hal::sercom::Sercom2::DMA_RX_TRIGGER, + dmac::TriggerAction::BURST, + ); + + // let mut gps_dma_transfer = rx.receive_with_dma( unsafe { &mut *BUF_DST },dma_channel, |_| { + // info!("DMA Transfer Complete"); + // }); + + // gps_dma_transfer.wait(); + + loop { + if gps_dma_transfer.complete() { + info!("DMA Transfer Complete"); + break; + } + } + Self { - gps_tx, + gps_uart: None, + gps_uart_tx: Some(tx), gps_reset, gps_enable, transfer: None, + gps_pads, + sercom, + uart_clk_freq: pclk_sercom2_freq, + rx: None, // consumed by pads first + tx: None, } } } pub fn gps_dma(mut cx: crate::app::gps_dma::Context) { - cx.shared.gps_manager.lock(|gps_manager| { - let mut gps_manager = gps_manager.take().unwrap(); + info!("GPS DMA"); + cx.shared.gps_manager.lock(|mut gps_manager| { let mut gps_dma_transfer = gps_manager.transfer.take().unwrap(); - let mut gps_tx = gps_manager.gps_tx.take().unwrap(); + // let mut gps_tx = gps_manager.gps_tx.take().unwrap(); if gps_dma_transfer.complete() { - let (chan0, source, buf) = gps_dma_transfer.stop(); + let (chan0, mut gps_rx, buf) = gps_dma_transfer.stop(); + + // get the radio manager from the shared resources + // take the gps_tx from the gps_manager and gps_rx from the dma transfer and then join them to create the uart object. + // disable the gps uart giving back the config. + // free the config giving back the pads and sercom. + let mut gps_tx = gps_manager.gps_uart_tx.take().unwrap(); + let mut gps_uart = atsamd_hal::sercom::uart::Uart::join(gps_rx, gps_tx); + let mut config = gps_uart.disable(); + let (mut sercom, mut pads) = config.free(); + let (rx, tx, _, _) = pads.free(); + gps_manager.rx = Some(rx); + gps_manager.tx = Some(tx); + + cx.shared.radio_manager.lock(|mut radio_manager| { + let radio_uart = cortex_m::interrupt::free(|cs| { + let mut x = unsafe { crate::MCLK.borrow(cs).borrow_mut() }; + let mclk = x.as_mut().unwrap(); + let pads = atsamd_hal::sercom::uart::Pads::::default() + .rx(radio_manager.rx.take().unwrap()) + .tx(radio_manager.tx.take().unwrap()); + GroundStationUartConfig::new( + &mclk, + sercom, + pads, + radio_manager.uart_clk_freq, + ) + .baud( + RateExtU32::Hz(57600), + uart::BaudMode::Fractional(uart::Oversampling::Bits16), + ) + .enable() + }); + radio_manager.radio = Some(radio_uart); + }); + // gps_dma_transfer = dmac::Transfer::new(chan0, source, unsafe { &mut *BUF_DST }, false) // .unwrap() // .begin( // atsamd_hal::sercom::Sercom2::DMA_RX_TRIGGER, // dmac::TriggerAction::BURST, // ); - + let buf_clone = buf.clone(); - let mut uart = Uart::join(source, gps_tx); + // let mut uart = Uart::join(source, gps_tx); unsafe { BUF_DST.copy_from_slice(&[0; 256]) }; // gps_dma_transfer.block_transfer_interrupt(); - let request = UbxPacketRequest::request_for::().into_packet_bytes(); - for byte in request { - nb::block!(uart.write(byte)).unwrap(); - } - cortex_m::asm::delay(300_000); + // let request = UbxPacketRequest::request_for::().into_packet_bytes(); + // for byte in request { + // nb::block!(gps_uart.write(byte)).unwrap(); + // } + // cortex_m::asm::delay(300_000); let mut buf: [u8; 256] = [0; 256]; let mut bytes: [u8; 256] = [0; 256]; let buf = ublox::FixedLinearBuffer::new(&mut buf[..]); @@ -78,7 +197,11 @@ pub fn gps_dma(mut cx: crate::app::gps_dma::Context) { longitude: x.lon_degrees(), latitude: x.lat_degrees(), }; - // info!("GPS latitude: {:?}, longitude {:?}", x.lat_degrees(), x.lon_degrees()); + info!( + "GPS latitude: {:?}, longitude {:?}", + x.lat_degrees(), + x.lon_degrees() + ); } ublox::PacketRef::NavStatus(x) => { info!("GPS fix stat: {:?}", x.fix_stat_raw()); diff --git a/boards/communication/src/main.rs b/boards/communication/src/main.rs index 2b2e3bda..0bf01006 100644 --- a/boards/communication/src/main.rs +++ b/boards/communication/src/main.rs @@ -7,6 +7,7 @@ mod gps_manager; mod types; use atsamd_hal as hal; +use atsamd_hal::dmac; use atsamd_hal::rtc::Count32Mode; use common_arm::*; use common_arm_atsame::mcan; @@ -16,7 +17,7 @@ use atsamd_hal::gpio::{Alternate, Output, PushPull, D, PA04, PA05, PA06, PA07}; use atsamd_hal::sercom::{ spi, spi::{Config, Duplex, Pads, Spi}, - Sercom0, + IoSet1, Sercom0, }; use communication::Capacities; use communication::{CanCommandManager, RadioManager}; @@ -41,6 +42,10 @@ use messages::*; use panic_probe as _; use systick_monotonic::*; use types::*; +use ublox::{ + CfgPrtUartBuilder, DataBits, InProtoMask, OutProtoMask, Parity, StopBits, UartMode, UartPortId, + UbxPacketRequest, +}; #[inline(never)] #[defmt::panic_handler] @@ -63,9 +68,14 @@ static HEAP: embedded_alloc::Heap = embedded_alloc::Heap::empty(); static RTC: Mutex>>> = Mutex::new(RefCell::new(None)); +static mut MCLK: Mutex>> = Mutex::new(RefCell::new(None)); + #[rtic::app(device = hal::pac, peripherals = true, dispatchers = [EVSYS_0, EVSYS_1, EVSYS_2])] mod app { + use core::borrow::BorrowMut; + + use atsamd_hal::{clock::v2::pclk, sercom::dma}; use command::{Command, CommandData}; use sender::Sender; @@ -76,8 +86,8 @@ mod app { struct Shared { em: ErrorManager, data_manager: DataManager, - radio_manager: Option, - gps_manager: Option, + radio_manager: RadioManager, + gps_manager: GpsManager, can0: communication::CanDevice0, can_command_manager: CanCommandManager, } @@ -124,8 +134,8 @@ mod app { let core = cx.core; let pins = Pins::new(peripherals.PORT); - // let mut dmac = DmaController::init(peripherals.DMAC, &mut peripherals.PM); - // let dmaChannels = dmac.split(); + let mut dmac = dmac::DmaController::init(peripherals.DMAC, &mut peripherals.PM); + let dmaChannels = dmac.split(); /* Logging Setup */ HydraLogging::set_ground_station_callback(queue_gs_message); @@ -188,25 +198,65 @@ mod app { // let (mut gclk0, dfll) = // hal::clock::v2::gclk::Gclk::from_source(tokens.gclks.gclk2, xosc0); // let gclk2 = gclk2.div(gclk::GclkDiv8::Div(1)).enable(); - let (pclk_radio, gclk0) = Pclk::enable(tokens.pclks.sercom2, gclk0); + let (pclk_sercom2, gclk0) = Pclk::enable(tokens.pclks.sercom2, gclk0); /* Radio config */ - let rx: Pin> = pins.pa08.into_alternate(); - let tx: Pin> = pins.pa09.into_alternate(); - let pads = uart::Pads::::default() - .rx(rx) - .tx(tx); - let uart = - GroundStationUartConfig::new(&mclk, peripherals.SERCOM2, pads, pclk_radio.freq()) - .baud( - RateExtU32::Hz(57600), - uart::BaudMode::Fractional(uart::Oversampling::Bits16), - ) - .enable(); + let rx: Pin> = pins.pa08.into_alternate(); + let tx: Pin> = pins.pa09.into_alternate(); + // let pads = uart::Pads::::default() + // .rx(rx) + // .tx(tx); + // let uart = + // GroundStationUartConfig::new(&mclk, peripherals.SERCOM2, pads, pclk_sercom2.freq()) + // .baud( + // RateExtU32::Hz(57600), + // uart::BaudMode::Fractional(uart::Oversampling::Bits16), + // ) + // .enable(); // let radio = RadioDevice::new(uart); - let radio_manager = RadioManager::new(uart); + unsafe { + MCLK = Mutex::new(RefCell::new(Some(mclk))); + } + // let (pclk_gps, gclk0) = Pclk::enable(tokens.pclks.sercom2, gclk0); + let gps_rx: Pin> = pins.pa13.into_alternate(); + let gps_tx: Pin> = pins.pa12.into_alternate(); + let gps_pads = hal::sercom::uart::Pads::::default() + .rx(gps_rx) + .tx(gps_tx); + + let mut gps_uart_config = cortex_m::interrupt::free(|cs| { + let mut x = unsafe { MCLK.borrow(cs).borrow_mut() }; + let mclk = x.as_mut().unwrap(); + GpsUartConfig::new(&mclk, peripherals.SERCOM2, gps_pads, pclk_sercom2.freq()).baud( + RateExtU32::Hz(9600), + uart::BaudMode::Fractional(uart::Oversampling::Bits16), + ) + }); + gps_uart_config = gps_uart_config.immediate_overflow_notification(false); + + let mut gps_uart = gps_uart_config.enable(); + let mut dmaCh0 = dmaChannels.0.init(dmac::PriorityLevel::LVL3); + let gps_manager = cortex_m::interrupt::free(|cs| { + let mut x = unsafe { MCLK.borrow(cs).borrow_mut() }; + let mclk = x.as_mut().unwrap(); + GpsManager::new( + gps_uart, + None, // pads are already consumed by the uart. + None, + pins.pb07.into_push_pull_output(), + pins.pb09.into_push_pull_output(), + dmaCh0, + pclk_sercom2.freq(), + ) + }); + let radio_manager = cortex_m::interrupt::free(|cs| { + info!("Setting up radio"); + let mut x = unsafe { MCLK.borrow(cs).borrow_mut() }; + let mclk = x.as_mut().unwrap(); + RadioManager::new(rx, tx, pclk_sercom2.freq()) + }); // /* SD config */ // let (mut gclk1, dfll) = @@ -225,24 +275,7 @@ mod app { // .enable(); // let sd_manager = SdManager::new(sdmmc_spi, pins.pa06.into_push_pull_output()); - let (pclk_gps, gclk0) = Pclk::enable(tokens.pclks.sercom4, gclk0); - //info!("here"); - // let pads = hal::sercom::uart::Pads::::default() - // .rx(pins.pa12) - // .tx(pins.pa13); - - // let gps_uart = GpsUartConfig::new(&mclk, peripherals.SERCOM4, pads, pclk_gps.freq()) - // .baud( - // RateExtU32::Hz(9600), - // uart::BaudMode::Fractional(uart::Oversampling::Bits16), - // ) - // .enable(); - - let gps_manager = GpsManager::new( - None, - pins.pb07.into_push_pull_output(), - pins.pb09.into_push_pull_output(), - ); + // No need to reclock the peripheral sercom since it's shared with the radio. // gps_uart.enable_interrupts(hal::sercom::uart::Flags::RXC); @@ -255,11 +288,16 @@ mod app { /* Spawn tasks */ // sensor_send::spawn().ok(); // state_send::spawn().ok(); - let rtc = hal::rtc::Rtc::clock_mode( - peripherals.RTC, - atsamd_hal::fugit::RateExtU32::Hz(1024), - &mut mclk, - ); + let rtc = cortex_m::interrupt::free(|cs| { + let mut x = unsafe { MCLK.borrow(cs).borrow_mut() }; + let mut mclk = x.as_mut().unwrap(); + hal::rtc::Rtc::clock_mode( + peripherals.RTC, + atsamd_hal::fugit::RateExtU32::Hz(1024), + &mut mclk, + ) + }); + let mut rtc = rtc.into_count32_mode(); // hal bug this must be done rtc.set_count32(0); cortex_m::interrupt::free(|cs| { @@ -279,8 +317,8 @@ mod app { Shared { em: ErrorManager::new(), data_manager: DataManager::new(), - radio_manager: Some(radio_manager), - gps_manager: Some(gps_manager), + radio_manager: radio_manager, + gps_manager: gps_manager, can0, can_command_manager, }, @@ -411,32 +449,68 @@ mod app { .shared .data_manager .lock(|data_manager| (data_manager.take_sensors(), data_manager.get_logging_rate())); - - for msg in sensors { - match msg { - Some(x) => { - // spawn!(send_gs, x.clone())?; - cx.shared.radio_manager.lock(|radio_manager| { + cx.shared.radio_manager.lock(|mut radio_manager| { + for msg in sensors { + match msg { + Some(x) => { + // spawn!(send_gs, x.clone())?; cx.shared.em.run(|| { let mut buf = [0; 255]; let data = postcard::to_slice(&x, &mut buf)?; - let mut radio = radio_manager.take().unwrap(); - radio.send_message(data)?; - // let uart = radio.release(); - // cx.shared.gps_manager.lock(|gps| { - // let mut gps = gps.take().unwrap(); - // }); + radio_manager.send_message(data)?; + Ok(()) }); - }); - // spawn!(sd_dump, x)?; - } - None => { - continue; + // spawn!(sd_dump, x)?; + } + None => { + continue; + } } } - } + info!("before take"); + if let Some(radio) = radio_manager.radio.take() { + info!("after_take"); + let mut config = radio_manager.radio.take().unwrap().disable(); + + let (mut sercom, mut pads) = config.free(); + let (rx, tx, _, _) = pads.free(); + radio_manager.rx = Some(rx); + radio_manager.tx = Some(tx); + + cx.shared.gps_manager.lock(|mut gps_manager| { + let mut gps_uart = cortex_m::interrupt::free(|cs| { + let mut x = unsafe { crate::MCLK.borrow(cs).borrow_mut() }; + let mclk = x.as_mut().unwrap(); + let pads = atsamd_hal::sercom::uart::Pads::< + atsamd_hal::sercom::Sercom2, + atsamd_hal::sercom::IoSet1, + >::default() + .rx(gps_manager.rx.take().unwrap()) + .tx(gps_manager.tx.take().unwrap()); + let mut gps_uart_config = + GpsUartConfig::new(&mclk, sercom, pads, gps_manager.uart_clk_freq) + .baud( + RateExtU32::Hz(9600), + uart::BaudMode::Fractional(uart::Oversampling::Bits16), + ); + + gps_uart_config = gps_uart_config.immediate_overflow_notification(false); + gps_uart_config.enable() + }); + + let request = + UbxPacketRequest::request_for::().into_packet_bytes(); + for byte in request { + nb::block!(gps_uart.write(byte)).unwrap(); + } + cortex_m::asm::delay(300_000); + gps_manager.gps_uart = Some(gps_uart); + }); + } + }); + match logging_rate { RadioRate::Fast => { spawn_after!(sensor_send, ExtU64::millis(100)).ok(); @@ -507,9 +581,8 @@ mod app { cx.shared.em.run(|| { let mut buf = [0; 255]; let data = postcard::to_slice(&message, &mut buf)?; - let mut radio = radio_manager.take().unwrap(); - radio.send_message(data)?; - *radio_manager = Some(radio); + radio_manager.send_message(data)?; + // *radio_manager = Some(radio); Ok(()) }); }); diff --git a/boards/gps/src/main.rs b/boards/gps/src/main.rs index 50928818..97d957be 100644 --- a/boards/gps/src/main.rs +++ b/boards/gps/src/main.rs @@ -307,6 +307,7 @@ mod app { loop { if gps_dma_transfer.complete() { + info!("DMA transfer complete"); let (chan0, source, buf) = gps_dma_transfer.stop(); gps_dma_transfer = dmac::Transfer::new(chan0, source, unsafe { &mut *BUF_DST }, false) @@ -344,7 +345,7 @@ mod app { longitude: x.lon_degrees(), latitude: x.lat_degrees(), }; - // info!("GPS latitude: {:?}, longitude {:?}", x.lat_degrees(), x.lon_degrees()); + info!("GPS latitude: {:?}, longitude {:?}", x.lat_degrees(), x.lon_degrees()); } ublox::PacketRef::NavStatus(x) => { info!("GPS fix stat: {:?}", x.fix_stat_raw()); diff --git a/boards/link/src/main.rs b/boards/link/src/main.rs index d3e5cdd8..1539a72e 100644 --- a/boards/link/src/main.rs +++ b/boards/link/src/main.rs @@ -5,6 +5,7 @@ mod communication; mod data_manager; mod types; +use stm32h7xx_hal::nb; use chrono::{NaiveDate, NaiveDateTime, NaiveTime}; use common_arm::*; use communication::{CanCommandManager, CanDataManager}; @@ -60,10 +61,10 @@ mod app { struct SharedResources { data_manager: DataManager, em: ErrorManager, - sd_manager: SdManager< - stm32h7xx_hal::spi::Spi, - PA4>, - >, + // sd_manager: SdManager< + // stm32h7xx_hal::spi::Spi, + // PA4>, + // >, radio_manager: RadioManager, can_command_manager: CanCommandManager, can_data_manager: CanDataManager, @@ -148,7 +149,6 @@ mod app { // 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, @@ -172,14 +172,28 @@ mod app { 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::AllowFdCan); can_data.apply_config(config); - can_data.enable_interrupt(fdcan::interrupt::Interrupt::RxFifo0NewMsg); - can_data.enable_interrupt_line(fdcan::interrupt::InterruptLine::_0, true); let can_data_manager = CanDataManager::new(can_data.into_normal()); /// SAFETY: This is done as a result of a single memory mapped bit in hardware. Safe in this context. @@ -196,41 +210,53 @@ mod app { 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); - can_command.enable_interrupt(fdcan::interrupt::Interrupt::RxFifo0NewMsg); - - can_command.enable_interrupt_line(fdcan::interrupt::InterruptLine::_0, true); 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 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(); @@ -285,6 +311,7 @@ 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"); @@ -292,7 +319,7 @@ mod app { SharedResources { data_manager, em, - sd_manager, + // sd_manager, radio_manager, can_command_manager, can_data_manager, @@ -310,6 +337,23 @@ mod app { } } + // #[task(priority = 3, shared = [&em])] + // async fn generate_random_messages(cx: generate_random_messages::Context) { + // loop { + // cx.shared.em.run(|| { + // let message = Message::new( + // 0, + // 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])] async fn reset_reason_send(mut cx: reset_reason_send::Context) { let reason = cx diff --git a/boards/nav/src/communication.rs b/boards/nav/src/communication.rs index f29cc275..f679b18a 100644 --- a/boards/nav/src/communication.rs +++ b/boards/nav/src/communication.rs @@ -84,7 +84,7 @@ impl CanDataManager { 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, + bit_rate_switching: true, 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) diff --git a/boards/nav/src/main.rs b/boards/nav/src/main.rs index df16b637..d71ffb09 100644 --- a/boards/nav/src/main.rs +++ b/boards/nav/src/main.rs @@ -187,6 +187,17 @@ mod app { 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(), + ); + let config = can_data .get_config() .set_frame_transmit(fdcan::config::FrameTransmissionConfig::AllowFdCan); diff --git a/boards/recovery/src/communication.rs b/boards/recovery/src/communication.rs index 788b2607..f4cdbc81 100644 --- a/boards/recovery/src/communication.rs +++ b/boards/recovery/src/communication.rs @@ -161,7 +161,7 @@ impl CanDevice0 { for message in &mut self.can.rx_fifo_0 { match from_bytes::(message.data()) { Ok(data) => { - // info!("Sender: {:?}", data.clone()); + info!("Sender: {:?}", data.clone()); data_manager.handle_data(data)?; } Err(e) => { diff --git a/boards/recovery/src/main.rs b/boards/recovery/src/main.rs index be0accba..fffeb738 100644 --- a/boards/recovery/src/main.rs +++ b/boards/recovery/src/main.rs @@ -434,7 +434,7 @@ mod app { COM_ID, board_state, ); - spawn!(send_internal, message)?; + spawn!(send_command, message)?; spawn_after!(state_send, ExtU64::secs(2))?; // I think this is fine here. Ok(()) }); From 34f3cfc219a77fbf421d2c65bce16c146cea342a Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Tue, 20 Aug 2024 03:59:38 -0400 Subject: [PATCH 39/47] WIP: Fix baud rate. --- .cargo/config.toml | 4 +- boards/communication/src/main.rs | 63 +++++-- boards/gps/src/main.rs | 290 +++++++++++++++++-------------- boards/link/src/communication.rs | 8 +- boards/link/src/data_manager.rs | 2 +- boards/link/src/main.rs | 59 +++---- boards/nav/src/main.rs | 2 +- 7 files changed, 245 insertions(+), 183 deletions(-) diff --git a/.cargo/config.toml b/.cargo/config.toml index e3b9e473..4951091d 100644 --- a/.cargo/config.toml +++ b/.cargo/config.toml @@ -1,7 +1,7 @@ [target.'cfg(all(target_arch = "arm", target_os = "none"))'] # This runner needs to be parametric so we can pass in the chip name -#runner = "probe-rs run --chip STM32H733VGTx --protocol swd" -runner = "probe-rs run --chip ATSAME51J18A --protocol swd" +runner = "probe-rs run --chip STM32H733VGTx --protocol swd" +#runner = "probe-rs run --chip ATSAME51J18A --protocol swd" rustflags = [ "-C", "link-arg=-Tlink.x", "-C", "link-arg=-Tdefmt.x", diff --git a/boards/communication/src/main.rs b/boards/communication/src/main.rs index 0bf01006..d1fe714b 100644 --- a/boards/communication/src/main.rs +++ b/boards/communication/src/main.rs @@ -251,12 +251,53 @@ mod app { pclk_sercom2.freq(), ) }); + let message = Message::new( + cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + rtc.count32() + }), + COM_ID, + State::new(messages::state::StateData::Initializing), + ); + let radio_manager = cortex_m::interrupt::free(|cs| { info!("Setting up radio"); let mut x = unsafe { MCLK.borrow(cs).borrow_mut() }; let mclk = x.as_mut().unwrap(); RadioManager::new(rx, tx, pclk_sercom2.freq()) }); + let mut gps_tx = gps_manager.gps_uart_tx.take().unwrap(); + let mut gps_uart = atsamd_hal::sercom::uart::Uart::join(gps_rx, gps_tx); + let mut config = gps_uart.disable(); + let (mut sercom, mut pads) = config.free(); + let (rx, tx, _, _) = pads.free(); + let radio_uart = cortex_m::interrupt::free(|cs| { + let mut x = unsafe { crate::MCLK.borrow(cs).borrow_mut() }; + let mclk = x.as_mut().unwrap(); + let pads = atsamd_hal::sercom::uart::Pads::::default() + .rx(radio_manager.rx.take().unwrap()) + .tx(radio_manager.tx.take().unwrap()); + GroundStationUartConfig::new( + &mclk, + sercom, + pads, + radio_manager.uart_clk_freq, + ) + .baud( + RateExtU32::Hz(57600), + uart::BaudMode::Fractional(uart::Oversampling::Bits16), + ) + .enable() + }); + radio_manager.radio = Some(radio_uart); + + loop { + + let mut buf = [0; 255]; + let data = postcard::to_slice(&m, &mut buf).unwrap(); + radio_manager.send_message(data); + } // /* SD config */ // let (mut gclk1, dfll) = @@ -415,8 +456,8 @@ mod app { // fn send_gs(mut cx: send_gs::Context, m: Message) { // cx.shared.radio_manager.lock(|radio_manager| { // cx.shared.em.run(|| { - // let mut buf = [0; 255]; - // let data = postcard::to_slice(&m, &mut buf)?; + // let mut buf = [0; 255]; + // let data = postcard::to_slice(&m, &mut buf)?; // radio_manager.send_message(data)?; // Ok(()) // }) @@ -524,15 +565,15 @@ mod app { // #[task(shared = [&em])] // fn generate_random_messages(cx: generate_random_messages::Context) { // cx.shared.em.run(|| { - // let message = Message::new( - // cortex_m::interrupt::free(|cs| { - // let mut rc = RTC.borrow(cs).borrow_mut(); - // let rtc = rc.as_mut().unwrap(); - // rtc.count32() - // }), - // COM_ID, - // State::new(messages::state::StateData::Initializing), - // ); + // let message = Message::new( + // cortex_m::interrupt::free(|cs| { + // let mut rc = RTC.borrow(cs).borrow_mut(); + // let rtc = rc.as_mut().unwrap(); + // rtc.count32() + // }), + // COM_ID, + // State::new(messages::state::StateData::Initializing), + // ); // info!("Sending message {}", message.clone()); // // spawn!(send_gs, message.clone())?; // spawn!(send_gs, message)?; diff --git a/boards/gps/src/main.rs b/boards/gps/src/main.rs index 97d957be..9d46e4dc 100644 --- a/boards/gps/src/main.rs +++ b/boards/gps/src/main.rs @@ -20,7 +20,6 @@ use data_manager::DataManager; use defmt::info; use defmt_rtt as _; // global logger use fugit::ExtU64; -use types::GpsUartTx; use fugit::RateExtU32; use hal::clock::v2::pclk::Pclk; use hal::clock::v2::Source; @@ -35,6 +34,7 @@ use messages::*; use panic_probe as _; use systick_monotonic::*; use types::GPSTransfer; +use types::GpsUartTx; use types::GPSBUFFER; use types::*; use ublox::{ @@ -297,82 +297,76 @@ mod app { dmaCh0 .as_mut() .enable_interrupts(dmac::InterruptFlags::new().with_tcmpl(true)); - let mut gps_dma_transfer: GPSTransfer = - Transfer::new(dmaCh0, gps_rx, unsafe { &mut *BUF_DST }, false) - .expect("DMA err") - .begin( - atsamd_hal::sercom::Sercom2::DMA_RX_TRIGGER, - dmac::TriggerAction::BURST, - ); - - loop { - if gps_dma_transfer.complete() { - info!("DMA transfer complete"); - let (chan0, source, buf) = gps_dma_transfer.stop(); - gps_dma_transfer = - dmac::Transfer::new(chan0, source, unsafe { &mut *BUF_DST }, false) - .unwrap() - .begin( - atsamd_hal::sercom::Sercom2::DMA_RX_TRIGGER, - dmac::TriggerAction::BURST, - ); - let buf_clone = buf.clone(); - unsafe { BUF_DST.copy_from_slice(&[0; 256]) }; - gps_dma_transfer.block_transfer_interrupt(); - let request = - UbxPacketRequest::request_for::().into_packet_bytes(); - for byte in request { - nb::block!(gps_tx.write(byte)).unwrap(); - } - cortex_m::asm::delay(300_000); - let mut buf: [u8; 256] = [0; 256]; - let mut bytes: [u8; 256] = [0; 256]; - // for i in 0..buf.len() { - // let item = nb::block!(gps_uart.read()).unwrap(); - // // info!("Byte: {}", item); - // bytes[i] = item; - // } - let buf = ublox::FixedLinearBuffer::new(&mut buf[..]); - let mut parser = ublox::Parser::new(buf); - let mut msgs = parser.consume(&buf_clone); - while let Some(msg) = msgs.next() { - match msg { - Ok(msg) => match msg { - ublox::PacketRef::NavPosLlh(x) => { - let message_data = messages::sensor::NavPosLlh { - height_msl: x.height_msl(), - longitude: x.lon_degrees(), - latitude: x.lat_degrees(), - }; - info!("GPS latitude: {:?}, longitude {:?}", x.lat_degrees(), x.lon_degrees()); - } - ublox::PacketRef::NavStatus(x) => { - info!("GPS fix stat: {:?}", x.fix_stat_raw()); - } - ublox::PacketRef::NavDop(x) => { - info!("GPS geometric drop: {:?}", x.geometric_dop()); - } - ublox::PacketRef::NavSat(x) => { - info!("GPS num sats used: {:?}", x.num_svs()); - } - ublox::PacketRef::NavVelNed(x) => { - info!("GPS velocity north: {:?}", x.vel_north()); - } - ublox::PacketRef::NavPvt(x) => { - info!("GPS nun sats PVT: {:?}", x.num_satellites()); - } - _ => { - info!("GPS Message not handled."); - } - }, - Err(e) => { - info!("GPS parse Error"); - } - } - } - } - } + + // loop { + // if gps_dma_transfer.complete() { + // info!("DMA transfer complete"); + // let (chan0, source, buf) = gps_dma_transfer.stop(); + // gps_dma_transfer = + // dmac::Transfer::new(chan0, source, unsafe { &mut *BUF_DST }, false) + // .unwrap() + // .begin( + // atsamd_hal::sercom::Sercom2::DMA_RX_TRIGGER, + // dmac::TriggerAction::BURST, + // ); + // let buf_clone = buf.clone(); + + // unsafe { BUF_DST.copy_from_slice(&[0; 256]) }; + // gps_dma_transfer.block_transfer_interrupt(); + // let request = + // UbxPacketRequest::request_for::().into_packet_bytes(); + // for byte in request { + // nb::block!(gps_tx.write(byte)).unwrap(); + // } + // cortex_m::asm::delay(300_000); + // let mut buf: [u8; 256] = [0; 256]; + // let mut bytes: [u8; 256] = [0; 256]; + // // for i in 0..buf.len() { + // // let item = nb::block!(gps_uart.read()).unwrap(); + // // // info!("Byte: {}", item); + // // bytes[i] = item; + // // } + // let buf = ublox::FixedLinearBuffer::new(&mut buf[..]); + // let mut parser = ublox::Parser::new(buf); + // let mut msgs = parser.consume(&buf_clone); + // while let Some(msg) = msgs.next() { + // match msg { + // Ok(msg) => match msg { + // ublox::PacketRef::NavPosLlh(x) => { + // let message_data = messages::sensor::NavPosLlh { + // height_msl: x.height_msl(), + // longitude: x.lon_degrees(), + // latitude: x.lat_degrees(), + // }; + // info!("GPS latitude: {:?}, longitude {:?}", x.lat_degrees(), x.lon_degrees()); + // } + // ublox::PacketRef::NavStatus(x) => { + // info!("GPS fix stat: {:?}", x.fix_stat_raw()); + // } + // ublox::PacketRef::NavDop(x) => { + // info!("GPS geometric drop: {:?}", x.geometric_dop()); + // } + // ublox::PacketRef::NavSat(x) => { + // info!("GPS num sats used: {:?}", x.num_svs()); + // } + // ublox::PacketRef::NavVelNed(x) => { + // info!("GPS velocity north: {:?}", x.vel_north()); + // } + // ublox::PacketRef::NavPvt(x) => { + // info!("GPS nun sats PVT: {:?}", x.num_satellites()); + // } + // _ => { + // info!("GPS Message not handled."); + // } + // }, + // Err(e) => { + // info!("GPS parse Error"); + // } + // } + // } + // } + // } /* Spawn tasks */ // sensor_send::spawn().ok(); // state_send::spawn().ok(); @@ -401,6 +395,13 @@ mod app { }, ); send_command::spawn(message).ok(); + let mut gps_dma_transfer: GPSTransfer = + Transfer::new(dmaCh0, gps_rx, unsafe { &mut *BUF_DST }, false) + .expect("DMA err") + .begin( + atsamd_hal::sercom::Sercom2::DMA_RX_TRIGGER, + dmac::TriggerAction::BURST, + ); let mono = Systick::new(core.SYST, gclk0.freq().to_Hz()); ( @@ -414,7 +415,7 @@ mod app { led_green, led_red, gps_dma_transfer: Some(gps_dma_transfer), - gps_tx, + gps_tx, // sd_manager, }, init::Monotonics(mono), @@ -429,66 +430,89 @@ mod app { #[task(priority = 3, binds = DMAC_0, local = [gps_dma_transfer, gps_tx], shared = [&em, data_manager])] fn gps_dma(mut cx: gps_dma::Context) { - let mut gps_dma_transfer = cx.local.gps_dma_transfer.take().unwrap(); - let mut gps_tx = cx.local.gps_tx; - if gps_dma_transfer.complete() { - let (chan0, source, buf) = gps_dma_transfer.stop(); - gps_dma_transfer = dmac::Transfer::new(chan0, source, unsafe { &mut *BUF_DST }, false) - .unwrap() - .begin( - atsamd_hal::sercom::Sercom2::DMA_RX_TRIGGER, - dmac::TriggerAction::BURST, - ); - let buf_clone = buf.clone(); - - unsafe { BUF_DST.copy_from_slice(&[0; 256]) }; - gps_dma_transfer.block_transfer_interrupt(); - let request = UbxPacketRequest::request_for::().into_packet_bytes(); - for byte in request { - nb::block!(gps_tx.write(byte)).unwrap(); - } - cortex_m::asm::delay(300_000); - let mut buf: [u8; 256] = [0; 256]; - let mut bytes: [u8; 256] = [0; 256]; - let buf = ublox::FixedLinearBuffer::new(&mut buf[..]); - let mut parser = ublox::Parser::new(buf); - let mut msgs = parser.consume(&buf_clone); - while let Some(msg) = msgs.next() { - match msg { - Ok(msg) => match msg { - ublox::PacketRef::NavPosLlh(x) => { - let message_data = messages::sensor::NavPosLlh { - height_msl: x.height_msl(), - longitude: x.lon_degrees(), - latitude: x.lat_degrees(), - }; - // info!("GPS latitude: {:?}, longitude {:?}", x.lat_degrees(), x.lon_degrees()); - } - ublox::PacketRef::NavStatus(x) => { - info!("GPS fix stat: {:?}", x.fix_stat_raw()); - } - ublox::PacketRef::NavDop(x) => { - info!("GPS geometric drop: {:?}", x.geometric_dop()); - } - ublox::PacketRef::NavSat(x) => { - info!("GPS num sats used: {:?}", x.num_svs()); - } - ublox::PacketRef::NavVelNed(x) => { - info!("GPS velocity north: {:?}", x.vel_north()); - } - ublox::PacketRef::NavPvt(x) => { - info!("GPS nun sats PVT: {:?}", x.num_satellites()); - } - _ => { - info!("GPS Message not handled."); + let mut gps_dma_transfer = cx.local.gps_dma_transfer.take(); + match gps_dma_transfer { + Some(mut gps_dma_transfer) => {info!("DMA transfer availabe"); + let mut gps_tx = cx.local.gps_tx; + if gps_dma_transfer.complete() { + info!("DMA transfer complete"); + gps_dma_transfer.block_transfer_interrupt(); + let (chan0, source, buf) = gps_dma_transfer.stop(); + *cx.local.gps_dma_transfer = Some( + dmac::Transfer::new(chan0, source, unsafe { &mut *BUF_DST }, false) + .unwrap() + .begin( + atsamd_hal::sercom::Sercom2::DMA_RX_TRIGGER, + dmac::TriggerAction::BURST, + )); + let buf_clone = buf.clone(); + + unsafe { BUF_DST.copy_from_slice(&[0; 256]) }; + let request = + UbxPacketRequest::request_for::().into_packet_bytes(); + for byte in request { + nb::block!(gps_tx.write(byte)).unwrap(); + } + cortex_m::asm::delay(10_000); + let mut buf: [u8; 256] = [0; 256]; + let mut bytes: [u8; 256] = [0; 256]; + let buf = ublox::FixedLinearBuffer::new(&mut buf[..]); + let mut parser = ublox::Parser::new(buf); + let mut msgs = parser.consume(&buf_clone); + while let Some(msg) = msgs.next() { + match msg { + Ok(msg) => match msg { + ublox::PacketRef::NavPosLlh(x) => { + let message_data = messages::sensor::NavPosLlh { + height_msl: x.height_msl(), + longitude: x.lon_degrees(), + latitude: x.lat_degrees(), + }; + info!( + "GPS latitude: {:?}, longitude {:?}", + x.lat_degrees(), + x.lon_degrees() + ); + let message = Message::new( + cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + rtc.count32() + }), + COM_ID, + messages::sensor::Sensor::new(message_data), + ); + spawn!(send_internal, message).ok(); + // info!("GPS latitude: {:?}, longitude {:?}", x.lat_degrees(), x.lon_degrees()); + } + ublox::PacketRef::NavStatus(x) => { + info!("GPS fix stat: {:?}", x.fix_stat_raw()); + } + ublox::PacketRef::NavDop(x) => { + info!("GPS geometric drop: {:?}", x.geometric_dop()); + } + ublox::PacketRef::NavSat(x) => { + info!("GPS num sats used: {:?}", x.num_svs()); + } + ublox::PacketRef::NavVelNed(x) => { + info!("GPS velocity north: {:?}", x.vel_north()); + } + ublox::PacketRef::NavPvt(x) => { + info!("GPS nun sats PVT: {:?}", x.num_satellites()); + } + _ => { + info!("GPS Message not handled."); + } + }, + Err(e) => { + info!("GPS parse Error"); } - }, - Err(e) => { - info!("GPS parse Error"); } } } } + None => {} + } } /// Handles the CAN1 interrupt. @@ -639,6 +663,4 @@ mod app { Ok(()) }); } - - } diff --git a/boards/link/src/communication.rs b/boards/link/src/communication.rs index 34d0456d..b0bcadee 100644 --- a/boards/link/src/communication.rs +++ b/boards/link/src/communication.rs @@ -101,8 +101,8 @@ impl CanDataManager { for message in self.can.receive0(&mut buf) { match from_bytes::(&buf) { Ok(data) => { - info!("Received message {}", data.clone()); - data_manager.handle_data(data); + crate::app::send_gs::spawn(data).ok(); + // data_manager.handle_data(data); } Err(e) => { info!("Error: {:?}", e) @@ -192,6 +192,10 @@ impl RadioManager { info!("{}", command.command); return Ok(postcard::from_bytes::(&command.command)?); } + // mavlink::uorocketry::MavMessage::HEARTBEAT(heartbeat) => { + // info!("Heartbeat"); + // return + // } _ => { error!("Error, ErrorContext::UnkownPostcardMessage"); return Err(mavlink::error::MessageReadError::Io.into()); diff --git a/boards/link/src/data_manager.rs b/boards/link/src/data_manager.rs index 98e99414..2e2b02b0 100644 --- a/boards/link/src/data_manager.rs +++ b/boards/link/src/data_manager.rs @@ -3,7 +3,7 @@ use messages::state::StateData; use messages::Message; use common_arm::HydraError; use stm32h7xx_hal::rcc::ResetReason; - +use defmt::info; #[derive(Clone)] pub struct DataManager { pub air: Option, diff --git a/boards/link/src/main.rs b/boards/link/src/main.rs index 1539a72e..39947785 100644 --- a/boards/link/src/main.rs +++ b/boards/link/src/main.rs @@ -274,7 +274,7 @@ mod app { let uart_radio = ctx .device .UART4 - .serial((tx, rx), 115_200.bps(), ccdr.peripheral.UART4, &ccdr.clocks) + .serial((tx, rx), 57600.bps(), ccdr.peripheral.UART4, &ccdr.clocks) .unwrap(); // let mut sbg_manager = sbg_manager::SBGManager::new(uart_sbg, stream_tuple); @@ -312,7 +312,7 @@ mod app { reset_reason_send::spawn().ok(); state_send::spawn().ok(); // generate_random_messages::spawn().ok(); - sensor_send::spawn().ok(); + // sensor_send::spawn().ok(); info!("Online"); ( @@ -329,31 +329,23 @@ mod app { ) } - #[idle] - fn idle(ctx: idle::Context) -> ! { + #[task(priority = 3, shared = [&em])] + async fn generate_random_messages(cx: generate_random_messages::Context) { loop { - // info!("Idle"); - // cortex_m::asm::wfi(); // could case issue with CAN Bus. Was an issue with ATSAME51. + cx.shared.em.run(|| { + let message = Message::new( + 0, + 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 = [&em])] - // async fn generate_random_messages(cx: generate_random_messages::Context) { - // loop { - // cx.shared.em.run(|| { - // let message = Message::new( - // 0, - // 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])] async fn reset_reason_send(mut cx: reset_reason_send::Context) { let reason = cx @@ -391,7 +383,7 @@ mod app { } } - #[task(priority = 3, shared = [data_manager, &em])] + #[task(shared = [data_manager, &em])] async fn state_send(mut cx: state_send::Context) { let state_data = cx .shared @@ -415,7 +407,7 @@ mod app { /** * Sends information about the sensors. */ - #[task(priority = 2, shared = [data_manager, &em])] + #[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| { @@ -426,10 +418,12 @@ mod app { for msg in sensors { match msg { Some(x) => { - spawn!(send_gs, x.clone())?; + info!("Sending sensor data {}", x.clone()); + spawn!(send_gs, x)?; // spawn!(sd_dump, x)?; } None => { + info!("No sensor data to send"); continue; } } @@ -440,11 +434,9 @@ mod app { match logging_rate { RadioRate::Fast => { Mono::delay(100.millis()).await; - spawn!(sensor_send).ok(); } RadioRate::Slow => { Mono::delay(250.millis()).await; - spawn!(sensor_send).ok(); } } } @@ -462,9 +454,9 @@ mod app { // send_in::spawn(message).ok(); } - #[task(priority = 3, binds = FDCAN1_IT0, shared = [can_command_manager, data_manager, &em])] + #[task(priority = 2, binds = FDCAN1_IT0, shared = [can_command_manager, data_manager, &em])] fn can_command(mut cx: can_command::Context) { - info!("CAN Command"); + // info!("CAN Command"); cx.shared.can_command_manager.lock(|can| { cx.shared .data_manager @@ -487,8 +479,11 @@ mod app { */ #[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)?; @@ -499,6 +494,7 @@ mod app { #[task(priority = 3, binds = UART4, shared = [&em, radio_manager])] fn radio_rx(mut cx: radio_rx::Context) { + info!("Radio Rx"); cx.shared.radio_manager.lock(|radio_manager| { cx.shared.em.run(|| { cortex_m::interrupt::free(|cs| { @@ -511,8 +507,7 @@ mod app { }); } - // Might be the wrong interrupt - #[task(priority = 3, binds = FDCAN2_IT0, shared = [can_data_manager, data_manager])] + #[task( priority = 3, binds = FDCAN2_IT0, shared = [can_data_manager, data_manager])] fn can_data(mut cx: can_data::Context) { cx.shared.can_data_manager.lock(|can| { cx.shared diff --git a/boards/nav/src/main.rs b/boards/nav/src/main.rs index d71ffb09..3db75ecc 100644 --- a/boards/nav/src/main.rs +++ b/boards/nav/src/main.rs @@ -365,7 +365,7 @@ mod app { for msg in messages { sender.send(msg).await; } - Mono::delay(100.millis()).await; // what if there was no delay and we used chanenls to control the rate of messages. + Mono::delay(200.millis()).await; // what if there was no delay and we used chanenls to control the rate of messages. } } From 5f3492f9e545e562aa0a447bd2d5bb0b114ba881 Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Tue, 20 Aug 2024 04:55:37 -0400 Subject: [PATCH 40/47] Remove buggy message receiving and fix* timing of recovery algorithm. --- boards/link/src/communication.rs | 12 ++++----- boards/link/src/main.rs | 38 +++++++++++++++++------------ boards/nav/src/main.rs | 2 +- boards/recovery/src/data_manager.rs | 2 +- 4 files changed, 30 insertions(+), 24 deletions(-) diff --git a/boards/link/src/communication.rs b/boards/link/src/communication.rs index b0bcadee..ff72a311 100644 --- a/boards/link/src/communication.rs +++ b/boards/link/src/communication.rs @@ -117,7 +117,7 @@ impl CanDataManager { pub struct RadioDevice { transmitter: stm32h7xx_hal::serial::Tx, - receiver: PeekReader>, + pub receiver: PeekReader>, } impl RadioDevice { @@ -138,7 +138,7 @@ impl RadioDevice { } pub struct RadioManager { - radio: RadioDevice, + pub radio: RadioDevice, mav_sequence: u8, } @@ -192,10 +192,10 @@ impl RadioManager { info!("{}", command.command); return Ok(postcard::from_bytes::(&command.command)?); } - // mavlink::uorocketry::MavMessage::HEARTBEAT(heartbeat) => { - // info!("Heartbeat"); - // return - // } + mavlink::uorocketry::MavMessage::HEARTBEAT(heartbeat) => { + info!("Heartbeat"); + return Err(mavlink::error::MessageReadError::Io.into()); + } _ => { error!("Error, ErrorContext::UnkownPostcardMessage"); return Err(mavlink::error::MessageReadError::Io.into()); diff --git a/boards/link/src/main.rs b/boards/link/src/main.rs index 39947785..314c37a8 100644 --- a/boards/link/src/main.rs +++ b/boards/link/src/main.rs @@ -418,7 +418,7 @@ mod app { for msg in sensors { match msg { Some(x) => { - info!("Sending sensor data {}", x.clone()); + // info!("Sending sensor data {}", x.clone()); spawn!(send_gs, x)?; // spawn!(sd_dump, x)?; } @@ -483,7 +483,7 @@ mod app { cx.shared.radio_manager.lock(|radio_manager| { cx.shared.em.run(|| { - info!("Sending message {}", m); + // info!("Sending message {}", m); let mut buf = [0; 255]; let data = postcard::to_slice(&m, &mut buf)?; radio_manager.send_message(data)?; @@ -492,20 +492,26 @@ mod app { }); } - #[task(priority = 3, binds = UART4, shared = [&em, radio_manager])] - fn radio_rx(mut cx: radio_rx::Context) { - info!("Radio Rx"); - cx.shared.radio_manager.lock(|radio_manager| { - cx.shared.em.run(|| { - cortex_m::interrupt::free(|cs| { - let m = radio_manager.receive_message()?; - info!("Received message {}", m.clone()); - spawn!(send_command_internal, m) - })?; - Ok(()) - }); - }); - } + // #[task(priority = 3, binds = UART4, local = [just_fired: bool = false], shared = [&em, radio_manager])] + // fn radio_rx(mut cx: radio_rx::Context) { + // if *cx.local.just_fired { + // *cx.local.just_fired = false; + // return; + // } + // info!("Radio Rx"); + + // cx.shared.radio_manager.lock(|radio_manager| { + // cx.shared.em.run(|| { + // // cortex_m::interrupt::free(|cs| { + // let m = radio_manager.receive_message()?; + // *cx.local.just_fired = true; + // info!("Received message {}", m.clone()); + // spawn!(send_command_internal, m)?; + // // })?; + // Ok(()) + // }); + // }); + // } #[task( priority = 3, binds = FDCAN2_IT0, shared = [can_data_manager, data_manager])] fn can_data(mut cx: can_data::Context) { diff --git a/boards/nav/src/main.rs b/boards/nav/src/main.rs index 3db75ecc..2f7c0e70 100644 --- a/boards/nav/src/main.rs +++ b/boards/nav/src/main.rs @@ -41,7 +41,7 @@ use types::COM_ID; // global logger const DATA_CHANNEL_CAPACITY: usize = 10; -systick_monotonic!(Mono, 500); +systick_monotonic!(Mono, 500); // 2ms ticks #[inline(never)] #[defmt::panic_handler] diff --git a/boards/recovery/src/data_manager.rs b/boards/recovery/src/data_manager.rs index 34045dd4..67aad107 100644 --- a/boards/recovery/src/data_manager.rs +++ b/boards/recovery/src/data_manager.rs @@ -46,7 +46,7 @@ impl DataManager { let mut prev = last; for i in buf { - let time_diff: f32 = (i.1 - prev.1) as f32 / 100_000_0.0; + let time_diff: f32 = (i.1 - prev.1) as f32 * 0.002; // Each tick is 2ms, so multiply by 0.002 to get seconds info!("last: {:?}, timestamp: {}, time diff {}", last, i.1, time_diff); if time_diff == 0.0 { From f4bc78a6085015f96a244cf5aa6d2a03470d4895 Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Tue, 20 Aug 2024 22:40:57 -0400 Subject: [PATCH 41/47] WIP --- .cargo/config.toml | 4 +- boards/link/src/main.rs | 29 +++++++--- boards/nav/src/data_manager.rs | 54 +++++++++++++------ boards/nav/src/main.rs | 35 ++++++------ boards/nav/src/sbg_manager.rs | 9 +--- boards/recovery/src/data_manager.rs | 7 ++- boards/recovery/src/main.rs | 2 +- .../src/state_machine/states/ascent.rs | 2 - .../state_machine/states/wait_for_recovery.rs | 29 +++++----- libraries/sbg-rs/src/sbg.rs | 6 +-- 10 files changed, 99 insertions(+), 78 deletions(-) diff --git a/.cargo/config.toml b/.cargo/config.toml index 4951091d..e3b9e473 100644 --- a/.cargo/config.toml +++ b/.cargo/config.toml @@ -1,7 +1,7 @@ [target.'cfg(all(target_arch = "arm", target_os = "none"))'] # This runner needs to be parametric so we can pass in the chip name -runner = "probe-rs run --chip STM32H733VGTx --protocol swd" -#runner = "probe-rs run --chip ATSAME51J18A --protocol swd" +#runner = "probe-rs run --chip STM32H733VGTx --protocol swd" +runner = "probe-rs run --chip ATSAME51J18A --protocol swd" rustflags = [ "-C", "link-arg=-Tlink.x", "-C", "link-arg=-Tdefmt.x", diff --git a/boards/link/src/main.rs b/boards/link/src/main.rs index 314c37a8..2004700c 100644 --- a/boards/link/src/main.rs +++ b/boards/link/src/main.rs @@ -74,6 +74,7 @@ mod app { struct LocalResources { led_red: PA2>, led_green: PA3>, + buzzer: stm32h7xx_hal::pwm::Pwm } #[init] @@ -325,7 +326,7 @@ mod app { can_data_manager, sbg_power, }, - LocalResources { led_red, led_green }, + LocalResources { led_red, led_green, buzzer: c0 }, ) } @@ -545,18 +546,34 @@ mod app { // } } - #[task(priority = 1, local = [led_red, led_green], shared = [&em])] + #[task(priority = 1, local = [led_red, led_green, buzzer, buzzed: bool = false], shared = [&em])] async fn blink(cx: blink::Context) { loop { - cx.shared.em.run(|| { 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; + } - Ok(()) - }); - Mono::delay(1000.millis()).await; } } diff --git a/boards/nav/src/data_manager.rs b/boards/nav/src/data_manager.rs index 09a0ef2a..28f3e08c 100644 --- a/boards/nav/src/data_manager.rs +++ b/boards/nav/src/data_manager.rs @@ -4,6 +4,7 @@ use messages::sensor::{ Imu1, Imu2, SensorData, UtcTime, }; use messages::Message; +use heapless::Vec; #[derive(Clone)] pub struct DataManager { @@ -29,21 +30,44 @@ impl DataManager { } } - // TODO: stop cloning so much this is a waste of resources. - pub fn clone_sensors(&self) -> [Option; 11] { - [ - self.air.clone().map(|x| x.into()), - self.ekf_nav.clone().map(|x| x.0.into()), - self.ekf_nav.clone().map(|x| x.1.into()), - self.ekf_quat.clone().map(|x| x.into()), - self.imu.clone().map(|x| x.0.into()), - self.imu.clone().map(|x| x.1.into()), - self.utc_time.clone().map(|x| x.into()), - self.gps_vel.clone().map(|x| x.0.into()), - self.gps_vel.clone().map(|x| x.1.into()), - self.gps_pos.clone().map(|x| x.0.into()), - self.gps_pos.clone().map(|x| x.1.into()), - ] + pub fn take_sensors(&mut self) -> Vec { + let mut sensors = Vec::new(); + + if let Some(data) = self.air.take().map(|x| x.into()) { + sensors.push(data); + } + + if let Some((data1, data2, data3)) = self.ekf_nav.take().map(|x| (x.0.into(), x.1.into(), x.2.into())) { + sensors.push(data1); + sensors.push(data2); + sensors.push(data3); + } + + if let Some(data) = self.ekf_quat.take().map(|x| x.into()) { + sensors.push(data); + } + + if let Some((data1, data2)) = self.imu.take().map(|x| (x.0.into(), x.1.into())) { + sensors.push(data1); + sensors.push(data2); + } + + if let Some(data) = self.utc_time.take().map(|x| x.into()) { + sensors.push(data); + } + + if let Some((data1, data2)) = self.gps_vel.take().map(|x| (x.0.into(), x.1.into())) { + sensors.push(data1); + sensors.push(data2); + } + + if let Some((data1, data2, data3)) = self.gps_pos.take().map(|x| (x.0.into(), x.1.into(), x.2.into())) { + sensors.push(data1); + sensors.push(data2); + sensors.push(data3); + } + + sensors } pub fn handle_data(&mut self, data: Message) -> Result<(), HydraError> { diff --git a/boards/nav/src/main.rs b/boards/nav/src/main.rs index 2f7c0e70..e3a41468 100644 --- a/boards/nav/src/main.rs +++ b/boards/nav/src/main.rs @@ -41,7 +41,7 @@ use types::COM_ID; // global logger const DATA_CHANNEL_CAPACITY: usize = 10; -systick_monotonic!(Mono, 500); // 2ms ticks +systick_monotonic!(Mono, 500); // 2ms ticks #[inline(never)] #[defmt::panic_handler] @@ -178,7 +178,7 @@ mod app { can_data.set_nominal_bit_timing(btr); - // can_data.set_automatic_retransmit(false); // data can be dropped due to its volume. + can_data.set_automatic_retransmit(false); // data can be dropped due to its volume. // can_command.set_data_bit_timing(data_bit_timing); @@ -192,7 +192,6 @@ mod app { StandardFilter::accept_all_into_fifo0(), ); - can_data.set_standard_filter( StandardFilterSlot::_2, StandardFilter::accept_all_into_fifo0(), @@ -314,15 +313,17 @@ mod app { display_data::spawn(s).ok(); // sbg_power_on::spawn().ok(); let message = Message::new( - 0,// technically true time is not known yet. + 0, COM_ID, messages::command::Command { - data: messages::command::CommandData::Online(messages::command::Online { online: true }), + data: messages::command::CommandData::Online(messages::command::Online { + online: true, + }), }, ); - send_command_internal::spawn(r_command).ok(); + send_command_internal::spawn(r_command).ok(); async { - s_command.send(message).await; + s_command.send(message).await; }; ( @@ -356,14 +357,12 @@ mod app { let data = cx .shared .data_manager - .lock(|manager| manager.clone_sensors()); - info!("{:?}", data.clone()); - let messages = data - .into_iter() - .flatten() - .map(|x| Message::new(Mono::now().ticks(), COM_ID, Sensor::new(x))); - for msg in messages { - sender.send(msg).await; + .lock(|manager| manager.take_sensors()); + // info!("{:?}", data.clone()); + for msg in data { + sender + .send(Message::new(Mono::now().ticks(), COM_ID, Sensor::new(msg))) + .await; } Mono::delay(200.millis()).await; // what if there was no delay and we used chanenls to control the rate of messages. } @@ -372,11 +371,7 @@ mod app { /// Receives a log message from the custom logger so that it can be sent over the radio. pub fn queue_gs_message(d: impl Into) { info!("Queueing message"); - let message = messages::Message::new( - Mono::now().ticks(), - COM_ID, - d.into(), - ); + let message = messages::Message::new(Mono::now().ticks(), COM_ID, d.into()); info!("{:?}", message); // send_in::spawn(message).ok(); } diff --git a/boards/nav/src/sbg_manager.rs b/boards/nav/src/sbg_manager.rs index a2b7cf28..2ebe6d5a 100644 --- a/boards/nav/src/sbg_manager.rs +++ b/boards/nav/src/sbg_manager.rs @@ -161,7 +161,6 @@ pub fn sbg_get_time() -> u32 { } pub async fn sbg_handle_data(mut cx: sbg_handle_data::Context<'_>, data: CallbackData) { - cx.shared.data_manager.lock(|manager| match data { CallbackData::UtcTime(x) => manager.utc_time = Some(x), CallbackData::Air(x) => manager.air = Some(x), @@ -169,11 +168,7 @@ pub async fn sbg_handle_data(mut cx: sbg_handle_data::Context<'_>, data: Callbac CallbackData::EkfNav(x) => manager.ekf_nav = Some(x), CallbackData::Imu(x) => manager.imu = Some(x), CallbackData::GpsVel(x) => manager.gps_vel = Some(x), - CallbackData::GpsPos(x) => { - info!("{}", x.0.latitude.unwrap()); - panic!("fucl"); - manager.gps_pos = Some(x) - }, + CallbackData::GpsPos(x) => manager.gps_pos = Some(x), }); } @@ -213,7 +208,7 @@ pub fn sbg_dma(mut cx: crate::app::sbg_dma::Context) { // info!("{}", data); xfer.clear_transfer_complete_interrupt(); sbg.sbg_device.read_data(&data); - // crate::app::sbg_sd_task::spawn(data).ok(); + crate::app::sbg_sd_task::spawn(data).ok(); } } None => { diff --git a/boards/recovery/src/data_manager.rs b/boards/recovery/src/data_manager.rs index 67aad107..ace3196e 100644 --- a/boards/recovery/src/data_manager.rs +++ b/boards/recovery/src/data_manager.rs @@ -53,7 +53,7 @@ impl DataManager { continue; } let slope = (i.0 - prev.0) / time_diff; - if slope < -100.0 { + if slope > 100.0 { return false; } avg_sum += slope; @@ -62,7 +62,7 @@ impl DataManager { match avg_sum / 7.0 { // 7 because we have 8 points. // exclusive range - x if !(-100.0..=-5.0).contains(&x) => { + x if !(100.0..=5.0).contains(&x) => { return false; } _ => { @@ -140,9 +140,8 @@ impl DataManager { the alt is dropped, if the number is high switch to the on board barometer. */ - if let Some(alt) = air_data.altitude { - let tup_data: (f32, u32) = (alt, air_data.time_stamp); + let tup_data: (f32, u32) = (alt, data.timestamp); self.air = Some(air_data); if let Some(recent) = self.historical_barometer_altitude.recent() { if recent.1 != tup_data.1 { diff --git a/boards/recovery/src/main.rs b/boards/recovery/src/main.rs index fffeb738..8232f5ec 100644 --- a/boards/recovery/src/main.rs +++ b/boards/recovery/src/main.rs @@ -215,7 +215,7 @@ mod app { }, ); send_command::spawn(message).ok(); - // fire_main::spawn_after(ExtU64::secs(15)).ok(); + fire_main::spawn_after(ExtU64::secs(60)).ok(); // fire_drogue::spawn_after(ExtU64::secs(15)).ok(); /* Monotonic clock */ diff --git a/boards/recovery/src/state_machine/states/ascent.rs b/boards/recovery/src/state_machine/states/ascent.rs index 11a5a615..c1a84012 100644 --- a/boards/recovery/src/state_machine/states/ascent.rs +++ b/boards/recovery/src/state_machine/states/ascent.rs @@ -14,7 +14,6 @@ pub struct Ascent {} impl State for Ascent { fn enter(&self, context: &mut StateMachineContext) { - for _i in 0..10 { let radio_rate_change = RadioRateChange { rate: RadioRate::Fast, }; @@ -33,7 +32,6 @@ impl State for Ascent { Ok(()) }) }); - } } fn step(&mut self, context: &mut StateMachineContext) -> Option { context.shared_resources.data_manager.lock(|data| { diff --git a/boards/recovery/src/state_machine/states/wait_for_recovery.rs b/boards/recovery/src/state_machine/states/wait_for_recovery.rs index 6f689b4a..0c466174 100644 --- a/boards/recovery/src/state_machine/states/wait_for_recovery.rs +++ b/boards/recovery/src/state_machine/states/wait_for_recovery.rs @@ -15,22 +15,19 @@ pub struct WaitForRecovery {} impl State for WaitForRecovery { fn enter(&self, context: &mut StateMachineContext) { - // This should change to a ack and not be sent 10 times - // send a command over CAN to shut down non-critical systems for recovery. - for _i in 0..10 { - let sensor_power_down = PowerDown { board: SensorBoard }; + // let sensor_power_down = PowerDown { board: SensorBoard }; let radio_rate_change = RadioRateChange { rate: RadioRate::Slow, }; - let message = Message::new( - cortex_m::interrupt::free(|cs| { - let mut rc = RTC.borrow(cs).borrow_mut(); - let rtc = rc.as_mut().unwrap(); - rtc.count32() - }), - COM_ID, - Command::new(sensor_power_down), - ); + // let message = Message::new( + // cortex_m::interrupt::free(|cs| { + // let mut rc = RTC.borrow(cs).borrow_mut(); + // let rtc = rc.as_mut().unwrap(); + // rtc.count32() + // }), + // COM_ID, + // Command::new(sensor_power_down), + // ); let message_com = Message::new( cortex_m::interrupt::free(|cs| { let mut rc = RTC.borrow(cs).borrow_mut(); @@ -41,7 +38,7 @@ impl State for WaitForRecovery { Command::new(radio_rate_change), ); context.shared_resources.em.run(|| { - spawn!(send_command, message)?; + // spawn!(send_command, message)?; spawn!(send_command, message_com)?; Ok(()) }); @@ -53,7 +50,7 @@ impl State for WaitForRecovery { // Ok(()) // }) // }); - } + // } } fn step(&mut self, _context: &mut StateMachineContext) -> Option { no_transition!() // this is our final resting place. We should also powerdown this board. @@ -68,6 +65,6 @@ impl TransitionInto for TerminalDescent { impl Format for WaitForRecovery { fn format(&self, f: Formatter) { - write!(f, "Descent") + write!(f, "Wait For Recovery") } } diff --git a/libraries/sbg-rs/src/sbg.rs b/libraries/sbg-rs/src/sbg.rs index 6c436997..ec23c213 100644 --- a/libraries/sbg-rs/src/sbg.rs +++ b/libraries/sbg-rs/src/sbg.rs @@ -382,18 +382,14 @@ impl SBG { callback(CallbackData::EkfNav((*pLogData).ekfNavData.into())) } _SbgEComLog_SBG_ECOM_LOG_GPS1_POS => { - // panic!("GPS Data"); callback(CallbackData::GpsPos((*pLogData).gpsPosData.into())) } _SbgEComLog_SBG_ECOM_LOG_GPS1_VEL => { - // panic!("GPS Velocity Data"); callback(CallbackData::GpsVel((*pLogData).gpsVelData.into())) } _SbgEComLog_SBG_ECOM_LOG_GPS1_HDT => { - // info!("Heading Data"); } _ => { - panic!("unkown") }, } } @@ -488,7 +484,7 @@ pub unsafe extern "C" fn sbgPlatformDebugLogMsg( match logType { // silently handle errors - _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_ERROR => error!("SBG Error"), + // _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_ERROR => error!("SBG Error"), _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_WARNING => warn!("SBG Warning"), // _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_INFO => info!("SBG Info "), _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_DEBUG => debug!("SBG Debug "), From f31c25fb46f65ca08e578a14764a588e602b5cf4 Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Tue, 20 Aug 2024 23:54:14 -0400 Subject: [PATCH 42/47] Go over recovery algorithm --- .cargo/config.toml | 4 ++-- boards/link/src/main.rs | 2 +- boards/nav/src/main.rs | 3 +-- boards/recovery/src/data_manager.rs | 26 ++++++++++++-------------- 4 files changed, 16 insertions(+), 19 deletions(-) diff --git a/.cargo/config.toml b/.cargo/config.toml index e3b9e473..4951091d 100644 --- a/.cargo/config.toml +++ b/.cargo/config.toml @@ -1,7 +1,7 @@ [target.'cfg(all(target_arch = "arm", target_os = "none"))'] # This runner needs to be parametric so we can pass in the chip name -#runner = "probe-rs run --chip STM32H733VGTx --protocol swd" -runner = "probe-rs run --chip ATSAME51J18A --protocol swd" +runner = "probe-rs run --chip STM32H733VGTx --protocol swd" +#runner = "probe-rs run --chip ATSAME51J18A --protocol swd" rustflags = [ "-C", "link-arg=-Tlink.x", "-C", "link-arg=-Tdefmt.x", diff --git a/boards/link/src/main.rs b/boards/link/src/main.rs index 2004700c..b33a3d3f 100644 --- a/boards/link/src/main.rs +++ b/boards/link/src/main.rs @@ -144,7 +144,7 @@ mod app { c0.set_duty(c0.get_max_duty() / 4); // PWM outputs are disabled by default - // c0.enable(); + c0.enable(); info!("PWM enabled"); // assert_eq!(ccdr.clocks.pll1_q_ck().unwrap().raw(), 32_000_000); diff --git a/boards/nav/src/main.rs b/boards/nav/src/main.rs index e3a41468..9b3eee19 100644 --- a/boards/nav/src/main.rs +++ b/boards/nav/src/main.rs @@ -46,8 +46,7 @@ systick_monotonic!(Mono, 500); // 2ms ticks #[inline(never)] #[defmt::panic_handler] fn panic() -> ! { - // stm32h7xx_hal::pac::SCB::sys_reset() - cortex_m::asm::udf() + stm32h7xx_hal::pac::SCB::sys_reset() } static RTC: Mutex>> = Mutex::new(RefCell::new(None)); diff --git a/boards/recovery/src/data_manager.rs b/boards/recovery/src/data_manager.rs index ace3196e..2996fc7d 100644 --- a/boards/recovery/src/data_manager.rs +++ b/boards/recovery/src/data_manager.rs @@ -8,6 +8,10 @@ use messages::Message; const MAIN_HEIGHT: f32 = 876.0; // meters ASL const HEIGHT_MIN: f32 = 600.0; // meters ASL +const TICK_RATE: f32 = 0.002; // seconds +const ASCENT_LOCKOUT: f32 = 100.0; +const DATA_POINTS: usize = 8; +const VALID_DESCENT_RATE: f32 = -2.0; // meters per second pub struct DataManager { pub air: Option, @@ -16,7 +20,7 @@ pub struct DataManager { pub imu: (Option, Option), pub utc_time: Option, pub gps_vel: Option, - pub historical_barometer_altitude: HistoryBuffer<(f32, u32), 8>, + pub historical_barometer_altitude: HistoryBuffer<(f32, u32), DATA_POINTS>, pub current_state: Option, } @@ -46,28 +50,21 @@ impl DataManager { let mut prev = last; for i in buf { - let time_diff: f32 = (i.1 - prev.1) as f32 * 0.002; // Each tick is 2ms, so multiply by 0.002 to get seconds + let time_diff: f32 = (i.1 - prev.1) as f32 * TICK_RATE; // Each tick is 2ms, so multiply by 0.002 to get seconds info!("last: {:?}, timestamp: {}, time diff {}", last, i.1, time_diff); if time_diff == 0.0 { continue; } let slope = (i.0 - prev.0) / time_diff; - if slope > 100.0 { + if slope > ASCENT_LOCKOUT { return false; } avg_sum += slope; prev = i; } - match avg_sum / 7.0 { - // 7 because we have 8 points. - // exclusive range - x if !(100.0..=5.0).contains(&x) => { - return false; - } - _ => { - info!("avg: {}", avg_sum / 7.0); - } + if avg_sum / (DATA_POINTS as f32 - 1.0) > VALID_DESCENT_RATE { + return false; } } None => { @@ -76,6 +73,7 @@ impl DataManager { } true } + pub fn is_launched(&self) -> bool { match self.air.as_ref() { Some(air) => match air.altitude { @@ -95,7 +93,7 @@ impl DataManager { let mut avg_sum: f32 = 0.0; let mut prev = last; for i in buf { - let time_diff: f32 = (i.1 - prev.1) as f32 / 1_000_000.0; + let time_diff: f32 = (i.1 - prev.1) * TICK_RATE; if time_diff == 0.0 { continue; } @@ -104,7 +102,7 @@ impl DataManager { } match avg_sum / 7.0 { // inclusive range - x if (-0.25..=0.25).contains(&x) => { + x if (-0.5..=0.5).contains(&x) => { return true; } _ => { From 15e3b44a8dea802bd675c8c2992d1eff9e039180 Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Wed, 21 Aug 2024 12:48:51 -0400 Subject: [PATCH 43/47] Go over recovery algorithm --- .cargo/config.toml | 4 ++-- boards/recovery/src/data_manager.rs | 7 ++++--- boards/recovery/src/main.rs | 4 ++-- 3 files changed, 8 insertions(+), 7 deletions(-) diff --git a/.cargo/config.toml b/.cargo/config.toml index 4951091d..e3b9e473 100644 --- a/.cargo/config.toml +++ b/.cargo/config.toml @@ -1,7 +1,7 @@ [target.'cfg(all(target_arch = "arm", target_os = "none"))'] # This runner needs to be parametric so we can pass in the chip name -runner = "probe-rs run --chip STM32H733VGTx --protocol swd" -#runner = "probe-rs run --chip ATSAME51J18A --protocol swd" +#runner = "probe-rs run --chip STM32H733VGTx --protocol swd" +runner = "probe-rs run --chip ATSAME51J18A --protocol swd" rustflags = [ "-C", "link-arg=-Tlink.x", "-C", "link-arg=-Tdefmt.x", diff --git a/boards/recovery/src/data_manager.rs b/boards/recovery/src/data_manager.rs index 2996fc7d..b3e9c2f7 100644 --- a/boards/recovery/src/data_manager.rs +++ b/boards/recovery/src/data_manager.rs @@ -6,8 +6,9 @@ use heapless::HistoryBuffer; use messages::sensor::{Air, EkfNav1, EkfNav2, EkfQuat, GpsVel, Imu1, Imu2, UtcTime}; use messages::Message; -const MAIN_HEIGHT: f32 = 876.0; // meters ASL -const HEIGHT_MIN: f32 = 600.0; // meters ASL +const MAIN_HEIGHT: f32 = GROUND_HEIGHT + 500.0; // meters ASL +const HEIGHT_MIN: f32 = GROUND_HEIGHT + 300.0; // meters ASL +const GROUND_HEIGHT: f32 = 300.0; // meters ASL const TICK_RATE: f32 = 0.002; // seconds const ASCENT_LOCKOUT: f32 = 100.0; const DATA_POINTS: usize = 8; @@ -93,7 +94,7 @@ impl DataManager { let mut avg_sum: f32 = 0.0; let mut prev = last; for i in buf { - let time_diff: f32 = (i.1 - prev.1) * TICK_RATE; + let time_diff: f32 = (i.1 - prev.1) as f32 * TICK_RATE; if time_diff == 0.0 { continue; } diff --git a/boards/recovery/src/main.rs b/boards/recovery/src/main.rs index 8232f5ec..97d43232 100644 --- a/boards/recovery/src/main.rs +++ b/boards/recovery/src/main.rs @@ -215,8 +215,8 @@ mod app { }, ); send_command::spawn(message).ok(); - fire_main::spawn_after(ExtU64::secs(60)).ok(); - // fire_drogue::spawn_after(ExtU64::secs(15)).ok(); + // fire_main::spawn_after(ExtU64::secs(60)).ok(); + fire_drogue::spawn_after(ExtU64::secs(60)).ok(); /* Monotonic clock */ let mono = Systick::new(core.SYST, gclk0.freq().to_Hz()); From c08a2d27c39e1431370ef5cd8133efcf6dd34833 Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Wed, 21 Aug 2024 17:57:27 -0400 Subject: [PATCH 44/47] ? --- boards/gps/src/communication.rs | 8 ++-- boards/link/src/communication.rs | 1 + boards/link/src/main.rs | 4 +- boards/nav/src/communication.rs | 2 +- boards/nav/src/main.rs | 1 + boards/recovery/src/communication.rs | 6 +-- boards/recovery/src/data_manager.rs | 57 +++++++++++++++++++++++----- boards/recovery/src/main.rs | 7 ++-- 8 files changed, 63 insertions(+), 23 deletions(-) diff --git a/boards/gps/src/communication.rs b/boards/gps/src/communication.rs index bd81b73b..ad58f3fe 100644 --- a/boards/gps/src/communication.rs +++ b/boards/gps/src/communication.rs @@ -98,7 +98,7 @@ impl CanDevice0 { ) .unwrap(); can.config().mode = Mode::Fd { - allow_bit_rate_switching: false, + allow_bit_rate_switching: true, data_phase_timing: BitTiming::new(fugit::RateExtU32::kHz(500)), }; @@ -178,7 +178,7 @@ impl CanDevice0 { id: ecan::Id::Standard(ecan::StandardId::new(m.sender.into()).unwrap()), frame_type: tx::FrameType::FlexibleDatarate { payload: &payload[..], - bit_rate_switching: false, + bit_rate_switching: true, force_error_state_indicator: false, }, store_tx_event: None, @@ -262,7 +262,7 @@ impl CanCommandManager { ) .unwrap(); can.config().mode = Mode::Fd { - allow_bit_rate_switching: false, + allow_bit_rate_switching: true, data_phase_timing: BitTiming::new(fugit::RateExtU32::kHz(500)), }; @@ -342,7 +342,7 @@ impl CanCommandManager { id: ecan::Id::Standard(ecan::StandardId::new(m.sender.into()).unwrap()), frame_type: tx::FrameType::FlexibleDatarate { payload: &payload[..], - bit_rate_switching: false, + bit_rate_switching: true, force_error_state_indicator: false, }, store_tx_event: None, diff --git a/boards/link/src/communication.rs b/boards/link/src/communication.rs index ff72a311..cbf1c6d4 100644 --- a/boards/link/src/communication.rs +++ b/boards/link/src/communication.rs @@ -101,6 +101,7 @@ impl CanDataManager { for message in self.can.receive0(&mut buf) { match from_bytes::(&buf) { Ok(data) => { + info!("Received message {}", data.clone()); crate::app::send_gs::spawn(data).ok(); // data_manager.handle_data(data); } diff --git a/boards/link/src/main.rs b/boards/link/src/main.rs index b33a3d3f..2e492ce6 100644 --- a/boards/link/src/main.rs +++ b/boards/link/src/main.rs @@ -144,7 +144,7 @@ mod app { c0.set_duty(c0.get_max_duty() / 4); // PWM outputs are disabled by default - c0.enable(); + // c0.enable(); info!("PWM enabled"); // assert_eq!(ccdr.clocks.pll1_q_ck().unwrap().raw(), 32_000_000); @@ -191,7 +191,7 @@ mod app { let config = can_data .get_config() - .set_frame_transmit(fdcan::config::FrameTransmissionConfig::AllowFdCan); + .set_frame_transmit(fdcan::config::FrameTransmissionConfig::AllowFdCanAndBRS); can_data.apply_config(config); diff --git a/boards/nav/src/communication.rs b/boards/nav/src/communication.rs index f679b18a..67397e34 100644 --- a/boards/nav/src/communication.rs +++ b/boards/nav/src/communication.rs @@ -98,7 +98,7 @@ impl CanDataManager { for message in self.can.receive0(&mut buf) { match from_bytes::(&buf) { Ok(data) => { - info!("Received message {}", data.clone()); + // info!("Received message {}", data.clone()); data_manager.handle_data(data)?; } Err(e) => { diff --git a/boards/nav/src/main.rs b/boards/nav/src/main.rs index 9b3eee19..7b5f3ef6 100644 --- a/boards/nav/src/main.rs +++ b/boards/nav/src/main.rs @@ -359,6 +359,7 @@ mod app { .lock(|manager| manager.take_sensors()); // info!("{:?}", data.clone()); for msg in data { + info!("{:?}", msg); sender .send(Message::new(Mono::now().ticks(), COM_ID, Sensor::new(msg))) .await; diff --git a/boards/recovery/src/communication.rs b/boards/recovery/src/communication.rs index f4cdbc81..c6ae6607 100644 --- a/boards/recovery/src/communication.rs +++ b/boards/recovery/src/communication.rs @@ -79,7 +79,7 @@ impl CanDevice0 { let mut can = mcan::bus::CanConfigurable::new(200.kHz(), can_dependencies, can_memory).unwrap(); can.config().mode = Mode::Fd { - allow_bit_rate_switching: false, + allow_bit_rate_switching: true, data_phase_timing: BitTiming::new(500.kHz()), }; @@ -220,7 +220,7 @@ impl CanCommandManager { let mut can = mcan::bus::CanConfigurable::new(200.kHz(), can_dependencies, can_memory).unwrap(); can.config().mode = Mode::Fd { - allow_bit_rate_switching: false, + allow_bit_rate_switching: true, data_phase_timing: BitTiming::new(500.kHz()), }; @@ -300,7 +300,7 @@ impl CanCommandManager { id: ecan::Id::Standard(ecan::StandardId::new(m.sender.into()).unwrap()), frame_type: tx::FrameType::FlexibleDatarate { payload: &payload[..], - bit_rate_switching: false, + bit_rate_switching: true, force_error_state_indicator: false, }, store_tx_event: None, diff --git a/boards/recovery/src/data_manager.rs b/boards/recovery/src/data_manager.rs index b3e9c2f7..e11d258e 100644 --- a/boards/recovery/src/data_manager.rs +++ b/boards/recovery/src/data_manager.rs @@ -12,7 +12,7 @@ const GROUND_HEIGHT: f32 = 300.0; // meters ASL const TICK_RATE: f32 = 0.002; // seconds const ASCENT_LOCKOUT: f32 = 100.0; const DATA_POINTS: usize = 8; -const VALID_DESCENT_RATE: f32 = -2.0; // meters per second +const VALID_DESCENT_RATE: f32 = -1.0; // meters per second pub struct DataManager { pub air: Option, @@ -21,7 +21,7 @@ pub struct DataManager { pub imu: (Option, Option), pub utc_time: Option, pub gps_vel: Option, - pub historical_barometer_altitude: HistoryBuffer<(f32, u32), DATA_POINTS>, + pub historical_barometer_altitude: HistoryBuffer<(f32, u32), DATA_POINTS>, // (alt, timestamp) pub current_state: Option, } @@ -40,8 +40,45 @@ impl DataManager { } } /// Returns true if the rocket is descending + // pub fn is_falling(&self) -> bool { + // if self.historical_barometer_altitude.len() < 8 { + // info!("not enough data points"); + // return false; + // } + // let mut buf = self.historical_barometer_altitude.oldest_ordered(); + // match buf.next() { + // Some(last) => { + // let mut avg_sum: f32 = 0.0; + // let mut prev = last; + // for i in buf { + + // let time_diff: f32 = (i.1 - prev.1) as f32 * TICK_RATE; // Each tick is 2ms, so multiply by 0.002 to get seconds + // info!("prev alt: {:?}, new alt: {}, time diff {}", prev.0, i.0, time_diff); + + // if time_diff == 0.0 { + // continue; + // } + // let slope = (i.0 - prev.0) / time_diff; + // if slope > ASCENT_LOCKOUT { + // return false; + // } + // avg_sum += slope; + // prev = i; + // } + // if avg_sum / (DATA_POINTS as f32 - 1.0) > VALID_DESCENT_RATE { + // return false; + // } + // info!("avg_sum: {}", avg_sum / (DATA_POINTS as f32 - 1.0)); + // } + // None => { + // return false; + // } + // } + // true + // } pub fn is_falling(&self) -> bool { if self.historical_barometer_altitude.len() < 8 { + info!("not enough data points"); return false; } let mut buf = self.historical_barometer_altitude.oldest_ordered(); @@ -50,10 +87,9 @@ impl DataManager { let mut avg_sum: f32 = 0.0; let mut prev = last; for i in buf { - let time_diff: f32 = (i.1 - prev.1) as f32 * TICK_RATE; // Each tick is 2ms, so multiply by 0.002 to get seconds - info!("last: {:?}, timestamp: {}, time diff {}", last, i.1, time_diff); - + info!("prev alt: {:?}, new alt: {}, time diff {}", prev.0, i.0, time_diff); + if time_diff == 0.0 { continue; } @@ -63,16 +99,19 @@ impl DataManager { } avg_sum += slope; prev = i; - } - if avg_sum / (DATA_POINTS as f32 - 1.0) > VALID_DESCENT_RATE { - return false; + + // Check if the average descent rate is valid + if avg_sum / (DATA_POINTS as f32 - 1.0) <= VALID_DESCENT_RATE { + info!("avg_sum: {}", avg_sum / (DATA_POINTS as f32 - 1.0)); + return true; + } } } None => { return false; } } - true + false } pub fn is_launched(&self) -> bool { diff --git a/boards/recovery/src/main.rs b/boards/recovery/src/main.rs index 97d43232..ee7d3f52 100644 --- a/boards/recovery/src/main.rs +++ b/boards/recovery/src/main.rs @@ -216,7 +216,7 @@ mod app { ); send_command::spawn(message).ok(); // fire_main::spawn_after(ExtU64::secs(60)).ok(); - fire_drogue::spawn_after(ExtU64::secs(60)).ok(); + // fire_drogue::spawn_after(ExtU64::secs(60)).ok(); /* Monotonic clock */ let mono = Systick::new(core.SYST, gclk0.freq().to_Hz()); @@ -410,7 +410,7 @@ mod app { } /// Sends info about the current state of the system. - #[task(shared = [data_manager, &em])] + #[task(priority = 3, shared = [data_manager, &em])] fn state_send(mut cx: state_send::Context) { cx.shared.em.run(|| { let rocket_state = cx @@ -428,14 +428,13 @@ mod app { cortex_m::interrupt::free(|cs| { let mut rc = RTC.borrow(cs).borrow_mut(); let rtc = rc.as_mut().unwrap(); - // info!("RTC: {:?}", rtc.count32()); rtc.count32() }), COM_ID, board_state, ); spawn!(send_command, message)?; - spawn_after!(state_send, ExtU64::secs(2))?; // I think this is fine here. + spawn_after!(state_send, ExtU64::secs(5))?; // I think this is fine here. Ok(()) }); } From 8268798e33b26d8a2995d4e2cdc326ed9fee0ce7 Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Sun, 25 Aug 2024 00:01:49 -0400 Subject: [PATCH 45/47] Post comp update. --- .cargo/config.toml | 4 ++-- boards/nav/src/main.rs | 2 +- boards/nav/src/sbg_manager.rs | 2 +- boards/recovery/src/communication.rs | 4 ++-- boards/recovery/src/gpio_manager.rs | 12 +++++++++--- boards/recovery/src/main.rs | 18 ++++++++++++------ libraries/common-arm/src/sd_manager.rs | 2 +- 7 files changed, 28 insertions(+), 16 deletions(-) diff --git a/.cargo/config.toml b/.cargo/config.toml index e3b9e473..4951091d 100644 --- a/.cargo/config.toml +++ b/.cargo/config.toml @@ -1,7 +1,7 @@ [target.'cfg(all(target_arch = "arm", target_os = "none"))'] # This runner needs to be parametric so we can pass in the chip name -#runner = "probe-rs run --chip STM32H733VGTx --protocol swd" -runner = "probe-rs run --chip ATSAME51J18A --protocol swd" +runner = "probe-rs run --chip STM32H733VGTx --protocol swd" +#runner = "probe-rs run --chip ATSAME51J18A --protocol swd" rustflags = [ "-C", "link-arg=-Tlink.x", "-C", "link-arg=-Tdefmt.x", diff --git a/boards/nav/src/main.rs b/boards/nav/src/main.rs index 7b5f3ef6..aa1cdce0 100644 --- a/boards/nav/src/main.rs +++ b/boards/nav/src/main.rs @@ -156,7 +156,7 @@ mod app { c0.set_duty(c0.get_max_duty() / 4); // PWM outputs are disabled by default - // c0.enable(); + c0.enable(); info!("PWM enabled"); // assert_eq!(ccdr.clocks.pll1_q_ck().unwrap().raw(), 32_000_000); diff --git a/boards/nav/src/sbg_manager.rs b/boards/nav/src/sbg_manager.rs index 2ebe6d5a..4caef8a6 100644 --- a/boards/nav/src/sbg_manager.rs +++ b/boards/nav/src/sbg_manager.rs @@ -183,7 +183,7 @@ pub async fn sbg_sd_task( Ok(()) }); manager.file = Some(file); // give the file back after use - } else if let Ok(mut file) = manager.open_file("sbg.txt") { + } else if let Ok(mut file) = manager.open_file("lc24.txt") { cx.shared.em.run(|| { manager.write(&mut file, &data)?; Ok(()) diff --git a/boards/recovery/src/communication.rs b/boards/recovery/src/communication.rs index c6ae6607..5ffbb8af 100644 --- a/boards/recovery/src/communication.rs +++ b/boards/recovery/src/communication.rs @@ -79,7 +79,7 @@ impl CanDevice0 { let mut can = mcan::bus::CanConfigurable::new(200.kHz(), can_dependencies, can_memory).unwrap(); can.config().mode = Mode::Fd { - allow_bit_rate_switching: true, + allow_bit_rate_switching: false, data_phase_timing: BitTiming::new(500.kHz()), }; @@ -161,7 +161,7 @@ impl CanDevice0 { for message in &mut self.can.rx_fifo_0 { match from_bytes::(message.data()) { Ok(data) => { - info!("Sender: {:?}", data.clone()); + // info!("Sender: {:?}", data.clone()); data_manager.handle_data(data)?; } Err(e) => { diff --git a/boards/recovery/src/gpio_manager.rs b/boards/recovery/src/gpio_manager.rs index ee871fbe..6e8cb61a 100644 --- a/boards/recovery/src/gpio_manager.rs +++ b/boards/recovery/src/gpio_manager.rs @@ -1,6 +1,6 @@ use atsamd_hal::gpio::{Pin, PushPullOutput, PB11, PB12}; use atsamd_hal::prelude::*; - +use defmt::info; pub struct GPIOManager { main_ematch: Pin, drogue_ematch: Pin, @@ -11,8 +11,14 @@ impl GPIOManager { mut main_ematch: Pin, mut drogue_ematch: Pin, ) -> Self { - // drogue_ematch.set_low().ok(); - // main_ematch.set_low().ok(); + drogue_ematch.set_low().ok(); + match drogue_ematch.set_low() { + Ok(_) => {} + Err(_) => { + info!("Cannot set low"); + } + } + main_ematch.set_low().ok(); Self { main_ematch, drogue_ematch, diff --git a/boards/recovery/src/main.rs b/boards/recovery/src/main.rs index ee7d3f52..04fb58d0 100644 --- a/boards/recovery/src/main.rs +++ b/boards/recovery/src/main.rs @@ -56,6 +56,7 @@ unsafe fn HardFault(_frame: &cortex_m_rt::ExceptionFrame) -> ! { mod app { use atsamd_hal::gpio::{B, PA03, PB04, PB06, PB07, PB08, PB09}; + use embedded_hal::digital::v2::StatefulOutputPin; use super::*; @@ -192,14 +193,19 @@ mod app { // info!("RTC done"); // ADC sensing setup - let (pclk_adc, gclk0) = Pclk::enable(tokens.pclks.adc1, gclk0); - let adc1 = hal::adc::Adc::adc1(peripherals.ADC1, &mut mclk); - let drogue_current_sense = pins.pb06.into_alternate(); - let main_current_sense = pins.pb07.into_alternate(); - let drogue_sense = pins.pb08.into_alternate(); - let main_sense = pins.pb09.into_alternate(); + // let (pclk_adc, gclk0) = Pclk::enable(tokens.pclks.adc1, gclk0); + // let adc1 = hal::adc::Adc::adc1(peripherals.ADC1, &mut mclk); + // let drogue_current_sense = pins.pb06.into_alternate(); + // let main_current_sense = pins.pb07.into_alternate(); + // let drogue_sense = pins.pb08.into_alternate(); + // let main_sense = pins.pb09.into_alternate(); // let data = adc1.read(&mut drogue_sense); + let mut drogue_current_sense = pins.pb06.into_push_pull_output(); + let mut main_current_sense = pins.pb07.into_push_pull_output(); + drogue_current_sense.set_low().ok(); + main_current_sense.set_low().ok(); + /* Spawn tasks */ run_sm::spawn().ok(); // read_barometer::spawn().ok(); diff --git a/libraries/common-arm/src/sd_manager.rs b/libraries/common-arm/src/sd_manager.rs index f11c9728..9e087b7e 100644 --- a/libraries/common-arm/src/sd_manager.rs +++ b/libraries/common-arm/src/sd_manager.rs @@ -81,7 +81,7 @@ where let file = sd_cont.open_file_in_dir( &mut volume, &root_directory, - "log.txt", + "lc24.txt", sd::Mode::ReadWriteCreateOrTruncate, ); let file = match file { From d275eea3b3fb8845dba81a8044bcdb82fb82db5a Mon Sep 17 00:00:00 2001 From: NoahSprenger Date: Sun, 25 Aug 2024 00:07:29 -0400 Subject: [PATCH 46/47] Remove unused boards. --- boards/beacon/Cargo.toml | 17 - boards/beacon/src/communication.rs | 1 - boards/beacon/src/data_manager.rs | 140 ----- boards/beacon/src/main.rs | 121 ---- boards/beacon/src/types.rs | 7 - boards/communication/Cargo.toml | 26 - boards/communication/src/communication.rs | 483 ---------------- boards/communication/src/data_manager.rs | 165 ------ boards/communication/src/gps_manager.rs | 232 -------- boards/communication/src/main.rs | 676 ---------------------- boards/communication/src/types.rs | 32 - boards/power/Cargo.toml | 21 - boards/power/src/communication.rs | 174 ------ boards/power/src/data_manager.rs | 19 - boards/power/src/main.rs | 169 ------ boards/power/src/types.rs | 5 - boards/sensor/Cargo.toml | 23 - boards/sensor/src/communication.rs | 197 ------- boards/sensor/src/data_manager.rs | 73 --- boards/sensor/src/main.rs | 261 --------- boards/sensor/src/sbg_manager.rs | 207 ------- boards/sensor/src/types.rs | 42 -- 22 files changed, 3091 deletions(-) delete mode 100644 boards/beacon/Cargo.toml delete mode 100644 boards/beacon/src/communication.rs delete mode 100644 boards/beacon/src/data_manager.rs delete mode 100644 boards/beacon/src/main.rs delete mode 100644 boards/beacon/src/types.rs delete mode 100644 boards/communication/Cargo.toml delete mode 100644 boards/communication/src/communication.rs delete mode 100644 boards/communication/src/data_manager.rs delete mode 100644 boards/communication/src/gps_manager.rs delete mode 100644 boards/communication/src/main.rs delete mode 100644 boards/communication/src/types.rs delete mode 100644 boards/power/Cargo.toml delete mode 100644 boards/power/src/communication.rs delete mode 100644 boards/power/src/data_manager.rs delete mode 100644 boards/power/src/main.rs delete mode 100644 boards/power/src/types.rs delete mode 100644 boards/sensor/Cargo.toml delete mode 100644 boards/sensor/src/communication.rs delete mode 100644 boards/sensor/src/data_manager.rs delete mode 100644 boards/sensor/src/main.rs delete mode 100644 boards/sensor/src/sbg_manager.rs delete mode 100644 boards/sensor/src/types.rs diff --git a/boards/beacon/Cargo.toml b/boards/beacon/Cargo.toml deleted file mode 100644 index 064ce6b8..00000000 --- a/boards/beacon/Cargo.toml +++ /dev/null @@ -1,17 +0,0 @@ -[package] -name = "beacon" -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 = "^0.7.0" -cortex-m-rtic = "1.1.3" -common-arm-stm32l0 = {path = "../../libraries/common-arm-stm32l0"} -common-arm = {path = "../../libraries/common-arm"} -stm32l0xx-hal = { workspace = true } -postcard = "1.0.2" -messages = { path = "../../libraries/messages" } -systick-monotonic = "1.0.1" \ No newline at end of file diff --git a/boards/beacon/src/communication.rs b/boards/beacon/src/communication.rs deleted file mode 100644 index 765597b7..00000000 --- a/boards/beacon/src/communication.rs +++ /dev/null @@ -1 +0,0 @@ -//! Interface the SPI-based CAN device here. diff --git a/boards/beacon/src/data_manager.rs b/boards/beacon/src/data_manager.rs deleted file mode 100644 index 6d600691..00000000 --- a/boards/beacon/src/data_manager.rs +++ /dev/null @@ -1,140 +0,0 @@ -use messages::command::RadioRate; -use messages::state::StateData; -use messages::Message; - -#[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 logging_rate: Option, -} - -impl DataManager { - pub fn new() -> Self { - Self { - air: None, - ekf_nav_1: None, - ekf_nav_2: None, - ekf_nav_acc: None, - ekf_quat: None, - imu_1: None, - imu_2: None, - utc_time: None, - gps_vel: None, - gps_vel_acc: None, - gps_pos_1: None, - gps_pos_2: None, - gps_pos_acc: None, - state: None, - logging_rate: Some(RadioRate::Slow), // start slow. - } - } - - 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); - return RadioRate::Slow; - } - - /// Do not clone instead take to reduce CPU load. - pub fn take_sensors(&mut self) -> [Option; 13] { - [ - self.air.take(), - self.ekf_nav_1.take(), - self.ekf_nav_2.take(), - self.ekf_nav_acc.take(), - self.ekf_quat.take(), - self.imu_1.take(), - self.imu_2.take(), - self.utc_time.take(), - self.gps_vel.take(), - self.gps_vel_acc.take(), - self.gps_pos_1.take(), - self.gps_pos_2.take(), - self.gps_pos_acc.take(), - ] - } - - pub fn clone_states(&self) -> [Option; 1] { - [self.state.clone()] - } - pub fn handle_data(&mut self, data: Message) { - match data.data { - messages::Data::Sensor(ref sensor) => match sensor.data { - messages::sensor::SensorData::EkfNavAcc(_) => { - self.ekf_nav_acc = Some(data); - } - messages::sensor::SensorData::GpsPosAcc(_) => { - self.gps_pos_acc = Some(data); - } - messages::sensor::SensorData::Air(_) => { - self.air = Some(data); - } - messages::sensor::SensorData::EkfNav1(_) => { - self.ekf_nav_1 = Some(data); - } - messages::sensor::SensorData::EkfNav2(_) => { - self.ekf_nav_2 = Some(data); - } - messages::sensor::SensorData::EkfQuat(_) => { - self.ekf_quat = Some(data); - } - messages::sensor::SensorData::GpsVel(_) => { - self.gps_vel = Some(data); - } - messages::sensor::SensorData::GpsVelAcc(_) => { - self.gps_vel_acc = Some(data); - } - messages::sensor::SensorData::Imu1(_) => { - self.imu_1 = Some(data); - } - messages::sensor::SensorData::Imu2(_) => { - self.imu_2 = Some(data); - } - messages::sensor::SensorData::UtcTime(_) => { - self.utc_time = Some(data); - } - messages::sensor::SensorData::GpsPos1(_) => { - self.gps_pos_1 = Some(data); - } - messages::sensor::SensorData::GpsPos2(_) => { - self.gps_pos_2 = Some(data); - } - }, - messages::Data::State(state) => { - self.state = Some(state.data); - } - messages::Data::Command(command) => match command.data { - messages::command::CommandData::RadioRateChange(command_data) => { - self.logging_rate = Some(command_data.rate); - } - 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/beacon/src/main.rs b/boards/beacon/src/main.rs deleted file mode 100644 index fbf26102..00000000 --- a/boards/beacon/src/main.rs +++ /dev/null @@ -1,121 +0,0 @@ -#![no_std] -#![no_main] - -mod communication; -mod data_manager; -mod types; - -use common_arm::SdManager; -use common_arm::*; -use data_manager::DataManager; -// use defmt::info; -use hal::{ - gpio::*, - gpio::{ - gpioa::{PA10, PA9}, - Output, PushPull, - }, - prelude::*, - rcc::Config, -}; -use messages::sensor::Sensor; -use messages::*; -use stm32l0xx_hal as hal; - -use systick_monotonic::*; - -// use https://github.com/lora-rs/lora-rs.git - -/// Custom panic handler. -/// Reset the system if a panic occurs. -#[panic_handler] -fn panic(_info: &core::panic::PanicInfo) -> ! { - stm32l0xx_hal::pac::SCB::sys_reset(); -} - -// Add dispatchers -#[rtic::app(device = hal::pac, peripherals = true, dispatchers = [EXTI0_1, EXTI2_3, EXTI4_15])] -mod app { - - use super::*; - - #[shared] - struct Shared { - em: ErrorManager, - data_manager: DataManager, - } - - #[local] - struct Local { - // add leds - green_led: PA9>, - red_led: PA10>, - } - - #[monotonic(binds = SysTick, default = true)] - type SysMono = Systick<100>; // 100 Hz / 10 ms granularity - - #[init] - fn init(cx: init::Context) -> (Shared, Local, init::Monotonics) { - let device = cx.device; - let core = cx.core; - - // configure the clock - let mut rcc = device.RCC.freeze(Config::hse(64_000_000u32.Hz())); - - // configure leds - let gpioa = device.GPIOA.split(&mut rcc); - let mut green_led = gpioa.pa9.into_push_pull_output(); - let mut red_led = gpioa.pa10.into_push_pull_output(); - - /* Monotonic clock */ - // implement the monotonic clock - let mono = Systick::new(core.SYST, rcc.clocks.sys_clk().0); - - ( - Shared { - em: ErrorManager::new(), - data_manager: DataManager::new(), - }, - Local { green_led, red_led }, - init::Monotonics(mono), - ) - } - - /// Idle task for when no other tasks are running. - #[idle] - fn idle(_: idle::Context) -> ! { - loop {} - } - - /** - * Sends a message over CAN. - */ - #[task(capacity = 10, local = [counter: u16 = 0], shared = [&em])] - fn send_internal(cx: send_internal::Context, m: Message) { - todo!("Send messages over CAN"); - } - - #[task(capacity = 5)] - fn send_external(cx: send_external::Context, m: Message) { - todo!("Send messages over LORA"); - } - - /** - * Simple blink task to test the system. - * Acts as a heartbeat for the system. - */ - #[task(local = [green_led, red_led], shared = [&em])] - fn blink(cx: blink::Context) { - cx.shared.em.run(|| { - if cx.shared.em.has_error() { - cx.local.red_led.toggle().ok(); - spawn_after!(blink, ExtU64::millis(200))?; - } else { - cx.local.green_led.toggle().ok(); // doesn't matter if an LED fails to blink - spawn_after!(blink, ExtU64::secs(1))?; - } - Ok(()) - }); - } -} diff --git a/boards/beacon/src/types.rs b/boards/beacon/src/types.rs deleted file mode 100644 index c83ce98d..00000000 --- a/boards/beacon/src/types.rs +++ /dev/null @@ -1,7 +0,0 @@ -use messages::sender::Sender; -use messages::sender::Sender::BeaconBoard; - -// ------- -// Sender ID -// ------- -pub static COM_ID: Sender = BeaconBoard; diff --git a/boards/communication/Cargo.toml b/boards/communication/Cargo.toml deleted file mode 100644 index 2297846c..00000000 --- a/boards/communication/Cargo.toml +++ /dev/null @@ -1,26 +0,0 @@ -[package] -name = "communication" -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 = "^0.7.0" -cortex-m-rtic = "1.1.3" -systick-monotonic = "1.0.1" -postcard = "1.0.2" -heapless = "0.7.16" -common-arm-atsame = { path = "../../libraries/common-arm-atsame" } -common-arm = { path = "../../libraries/common-arm" } -atsamd-hal = { workspace = true } -messages = { path = "../../libraries/messages" } -typenum = "1.16.0" -embedded-sdmmc = "0.8.0" -#panic-halt = "0.2.0" -defmt = "0.3" -defmt-rtt = "0.4" -panic-probe = { version = "0.3", features = ["print-defmt"] } -ublox = {version = "0.4.5", features = ['serde', 'alloc'], default-features = false} -embedded-alloc = "0.5.0" diff --git a/boards/communication/src/communication.rs b/boards/communication/src/communication.rs deleted file mode 100644 index a9fd57db..00000000 --- a/boards/communication/src/communication.rs +++ /dev/null @@ -1,483 +0,0 @@ - -use crate::data_manager::DataManager; -use crate::types::*; -use atsamd_hal::can::Dependencies; -use atsamd_hal::clock::v2::ahb::AhbClk; -use atsamd_hal::clock::v2::gclk::{Gclk0Id}; -use atsamd_hal::clock::v2::pclk::Pclk; -use atsamd_hal::clock::v2::types::{Can0, Can1}; -use atsamd_hal::clock::v2::Source; -use atsamd_hal::gpio::{ - Alternate, AlternateI, Pin, I, PA22, PA23, PA09, PA08, C, D -}; -use atsamd_hal::gpio::{AlternateH, H, PB14, PB15}; -use atsamd_hal::pac::{CAN0, CAN1}; -use atsamd_hal::prelude::*; -use atsamd_hal::sercom::spi::Duplex; -use atsamd_hal::sercom::uart; -use atsamd_hal::sercom::uart::Uart; -use atsamd_hal::sercom::uart::{RxDuplex, TxDuplex}; -use atsamd_hal::typelevel::Increment; -use common_arm::HydraError; -use common_arm_atsame::mcan; -use common_arm_atsame::mcan::message::{rx, Raw}; -use common_arm_atsame::mcan::tx_buffers::DynTx; -use defmt::error; -use defmt::info; -use heapless::HistoryBuffer; -use heapless::Vec; -use atsamd_hal::time::Hertz; - -use mavlink::peek_reader::PeekReader; -use mcan::bus::Can; -use mcan::embedded_can as ecan; -use mcan::interrupt::state::EnabledLine0; -use mcan::interrupt::{Interrupt, OwnedInterruptSet}; -use mcan::message::tx; -use mcan::messageram::SharedMemory; -use mcan::{ - config::{BitTiming, Mode}, - filter::{Action, Filter}, -}; -use messages::mavlink::uorocketry::MavMessage; -use messages::mavlink::{self}; -use messages::Message; -use postcard::from_bytes; -use systick_monotonic::*; -use typenum::{U0, U128, U32, U64}; -pub struct Capacities; - -impl mcan::messageram::Capacities for Capacities { - type StandardFilters = U128; - type ExtendedFilters = U64; - type RxBufferMessage = rx::Message<64>; - type DedicatedRxBuffers = U64; - type RxFifo0Message = rx::Message<64>; - type RxFifo0 = U64; - type RxFifo1Message = rx::Message<64>; - type RxFifo1 = U64; - type TxMessage = tx::Message<64>; - type TxBuffers = U32; - type DedicatedTxBuffers = U0; - type TxEventFifo = U32; -} - -// macro_rules! create_filter { -// ($can:expr, $action:expr, $sender:expr) => { -// $can.filters_standard() -// .push(Filter::Classic { -// action: $action, -// filter: ecan::StandardId::new($sender.into()).unwrap(), -// mask: ecan::StandardId::ZERO, -// }) -// .unwrap_or_else(|_| panic!("Filter Error")); -// }; -// } - -pub struct CanDevice0 { - pub can: Can< - 'static, - Can0, - Dependencies>, Pin>, CAN0>, - Capacities, - >, - line_interrupts: OwnedInterruptSet, -} - -impl CanDevice0 { - pub fn new( - can_rx: Pin, - can_tx: Pin, - pclk_can: Pclk, - ahb_clock: AhbClk, - peripheral: CAN0, - gclk0: S, - can_memory: &'static mut SharedMemory, - loopback: bool, - ) -> (Self, S::Inc) - where - S: Source + Increment, - { - let (can_dependencies, gclk0) = - Dependencies::new(gclk0, pclk_can, ahb_clock, can_rx, can_tx, peripheral); - - let mut can = mcan::bus::CanConfigurable::new( - 200.kHz(), - can_dependencies, - can_memory, - ) - .unwrap(); - can.config().mode = Mode::Fd { - allow_bit_rate_switching: false, - data_phase_timing: BitTiming::new(fugit::RateExtU32::kHz(500)), - }; - - if loopback { - can.config().loopback = true; - } - - let interrupts_to_be_enabled = can - .interrupts() - .split( - [ - Interrupt::RxFifo0NewMessage, - Interrupt::RxFifo0Full, - Interrupt::RxFifo0MessageLost, - Interrupt::RxFifo1NewMessage, - Interrupt::RxFifo1Full, - Interrupt::RxFifo1MessageLost, - ] - .into_iter() - .collect(), - ) - .unwrap(); - - // Line 0 and 1 are connected to the same interrupt line - let line_interrupts = can - .interrupt_configuration() - .enable_line_0(interrupts_to_be_enabled); - - can.filters_standard() - .push(Filter::Classic { - action: Action::StoreFifo0, - filter: ecan::StandardId::new(messages::sender::Sender::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::sender::Sender::SensorBoard.into()) - .unwrap(), - mask: ecan::StandardId::ZERO, - }) - .unwrap_or_else(|_| panic!("Sensor filter")); - - can.filters_standard() - .push(Filter::Classic { - action: Action::StoreFifo0, - filter: ecan::StandardId::new(messages::sender::Sender::PowerBoard.into()).unwrap(), - mask: ecan::StandardId::ZERO, - }) - .unwrap_or_else(|_| panic!("Power filter")); - - can.filters_standard() - .push(Filter::Classic { - action: Action::StoreFifo0, - filter: ecan::StandardId::new(messages::sender::Sender::GroundStation.into()) - .unwrap(), - mask: ecan::StandardId::ZERO, - }) - .unwrap_or_else(|_| panic!("Ground Station filter")); - - let can = can.finalize().unwrap(); - ( - CanDevice0 { - can, - line_interrupts, - }, - gclk0, - ) - } - pub fn send_message(&mut self, m: Message) -> Result<(), HydraError> { - let payload: Vec = postcard::to_vec(&m)?; - self.can.tx.transmit_queued( - tx::MessageBuilder { - id: ecan::Id::Standard(ecan::StandardId::new(m.sender.into()).unwrap()), - frame_type: tx::FrameType::FlexibleDatarate { - payload: &payload[..], - bit_rate_switching: false, - force_error_state_indicator: false, - }, - store_tx_event: None, - } - .build()?, - )?; - Ok(()) - } - pub fn process_data(&mut self, data_manager: &mut DataManager) { - let line_interrupts = &self.line_interrupts; - for interrupt in line_interrupts.iter_flagged() { - match interrupt { - Interrupt::RxFifo0NewMessage => { - for message in &mut self.can.rx_fifo_0 { - match from_bytes::(message.data()) { - Ok(data) => { - // info!("Received message {}", data.clone()); - - data_manager.handle_data(data); - } - Err(_) => { - // error!("Error, ErrorContext::UnkownCanMessage"); - } - } - } - } - Interrupt::RxFifo1NewMessage => { - for message in &mut self.can.rx_fifo_1 { - match from_bytes::(message.data()) { - Ok(data) => { - info!("Received message {}", data.clone()); - - data_manager.handle_data(data); - } - Err(_) => { - // error!("Error, ErrorContext::UnkownCanMessage"); - } - } - } - } - _ => (), - } - } - } -} - -// So I really am not a fan of this can device 0 and can device 1, I think it would be better to have a single generic can manager -// that can also take a list of filters and apply them. - -pub struct CanCommandManager { - pub can: Can< - 'static, - Can1, - Dependencies>, Pin>, CAN1>, - Capacities, - >, - line_interrupts: OwnedInterruptSet, -} - -impl CanCommandManager { - pub fn new( - can_rx: Pin, - can_tx: Pin, - pclk_can: Pclk, - ahb_clock: AhbClk, - peripheral: CAN1, - gclk0: S, - can_memory: &'static mut SharedMemory, - loopback: bool, - ) -> (Self, S::Inc) - where - S: Source + Increment, - { - let (can_dependencies, gclk0) = - Dependencies::new(gclk0, pclk_can, ahb_clock, can_rx, can_tx, peripheral); - - let mut can = mcan::bus::CanConfigurable::new( - 200.kHz(), // needs a prescaler of 6 to be changed in the mcan source because mcan is meh. - can_dependencies, - can_memory, - ) - .unwrap(); - can.config().mode = Mode::Fd { - allow_bit_rate_switching: false, - data_phase_timing: BitTiming::new(fugit::RateExtU32::kHz(500)), - }; - - if loopback { - can.config().loopback = true; - } - - let interrupts_to_be_enabled = can - .interrupts() - .split( - [ - Interrupt::RxFifo0NewMessage, - Interrupt::RxFifo0Full, - // Interrupt::RxFifo0MessageLost, - Interrupt::RxFifo1NewMessage, - Interrupt::RxFifo1Full, - // Interrupt::RxFifo1MessageLost, - ] - .into_iter() - .collect(), - ) - .unwrap(); - - // Line 0 and 1 are connected to the same interrupt line - let line_interrupts = can - .interrupt_configuration() - .enable_line_0(interrupts_to_be_enabled); - - can.filters_standard() - .push(Filter::Classic { - action: Action::StoreFifo0, - filter: ecan::StandardId::new(messages::sender::Sender::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::sender::Sender::SensorBoard.into()) - .unwrap(), - mask: ecan::StandardId::ZERO, - }) - .unwrap_or_else(|_| panic!("Sensor filter")); - - can.filters_standard() - .push(Filter::Classic { - action: Action::StoreFifo0, - filter: ecan::StandardId::new(messages::sender::Sender::PowerBoard.into()).unwrap(), - mask: ecan::StandardId::ZERO, - }) - .unwrap_or_else(|_| panic!("Power filter")); - - can.filters_standard() - .push(Filter::Classic { - action: Action::StoreFifo0, - filter: ecan::StandardId::new(messages::sender::Sender::GroundStation.into()) - .unwrap(), - mask: ecan::StandardId::ZERO, - }) - .unwrap_or_else(|_| panic!("Ground Station filter")); - - let can = can.finalize().unwrap(); - ( - CanCommandManager { - can, - line_interrupts, - }, - gclk0, - ) - } - pub fn send_message(&mut self, m: Message) -> Result<(), HydraError> { - let payload: Vec = postcard::to_vec(&m)?; - self.can.tx.transmit_queued( - tx::MessageBuilder { - id: ecan::Id::Standard(ecan::StandardId::new(m.sender.into()).unwrap()), - frame_type: tx::FrameType::FlexibleDatarate { - payload: &payload[..], - bit_rate_switching: false, - force_error_state_indicator: false, - }, - store_tx_event: None, - } - .build()?, - )?; - Ok(()) - } - pub fn process_data(&mut self, data_manager: &mut DataManager) { - let line_interrupts = &self.line_interrupts; - for interrupt in line_interrupts.iter_flagged() { - match interrupt { - Interrupt::RxFifo0NewMessage => { - for message in &mut self.can.rx_fifo_0 { - match from_bytes::(message.data()) { - Ok(data) => { - info!("Received message {}", data.clone()); - data_manager.handle_command(data); - } - Err(_) => { - error!("Error, ErrorContext::UnkownCanMessage"); - } - } - } - } - Interrupt::RxFifo1NewMessage => { - for message in &mut self.can.rx_fifo_1 { - match from_bytes::(message.data()) { - Ok(data) => { - info!("Received message {}", data.clone()); - data_manager.handle_command(data); - } - Err(_) => { - error!("Error, ErrorContext::UnkownCanMessage"); - } - } - } - } - _ => (), - } - } - } -} - -pub struct RadioManager { - buf: HistoryBuffer, - pub radio: Option>, - pub radio_pads: Option, - pub sercom: Option, - mav_sequence: u8, - pub rx: Option>>, - pub tx: Option>>, - pub uart_clk_freq: Hertz, -} - -impl RadioManager { - pub fn new(rx: Pin>, tx: Pin>, uart_clk_freq: Hertz) -> Self { - let buf = HistoryBuffer::new(); - RadioManager { - buf, - radio: None, - mav_sequence: 0, - radio_pads: None, - sercom: None, - uart_clk_freq, - rx: Some(rx), - tx: Some(tx), - } - } - 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, - }, - ); - let mut radio = self.radio.take().unwrap(); - mavlink::write_versioned_msg( - &mut radio, - 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) => { - // return Ok(postcard::from_bytes::(&msg.message)?); - // // weird Ok syntax to coerce to hydra error type. - // } - // mavlink::uorocketry::MavMessage::COMMAND_MESSAGE(command) => { - // info!("{}", command.command); - // return Ok(postcard::from_bytes::(&command.command)?); - // } - // _ => { - // error!("Error, ErrorContext::UnkownPostcardMessage"); - // return Err(mavlink::error::MessageReadError::Io.into()); - // } - // } - // } -} - -// impl Read for RadioManager { -// fn read_exact(&mut self, buf: &mut [u8]) -> Result<(), mavlink::error::MessageReadError> { -// let len = buf.len().min(self.buf.len()); -// buf[..len].copy_from_slice(&self.buf[..len]); -// Ok(()) -// } -// } diff --git a/boards/communication/src/data_manager.rs b/boards/communication/src/data_manager.rs deleted file mode 100644 index 5f093c02..00000000 --- a/boards/communication/src/data_manager.rs +++ /dev/null @@ -1,165 +0,0 @@ -use defmt::{info, error}; -use messages::command::RadioRate; -use messages::state::StateData; -use messages::Message; - -#[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 logging_rate: Option, - pub recovery_sensing: 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, - logging_rate: Some(RadioRate::Slow), // start slow. - recovery_sensing: 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); - return RadioRate::Slow; - } - - /// Do not clone instead take to reduce CPU load. - pub fn take_sensors(&mut self) -> [Option; 14] { - [ - 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.recovery_sensing.take(), - ] - } - - pub fn clone_states(&self) -> [Option; 1] { - [self.state.clone()] - } - pub fn handle_command(&mut self, command: Message) { - info!("Handling command"); - match command.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(_) => {} - messages::command::CommandData::Online(_) => {} - }, - _ => {} - } - } - pub fn handle_data(&mut self, data: Message) { - 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::sensor::SensorData::RecoverySensing(_) => { - self.recovery_sensing = Some(data); - } - _ => { - error!("Unknown sensor data type"); - } - }, - 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/communication/src/gps_manager.rs b/boards/communication/src/gps_manager.rs deleted file mode 100644 index d35c0a29..00000000 --- a/boards/communication/src/gps_manager.rs +++ /dev/null @@ -1,232 +0,0 @@ -use crate::app::gps_dma; -use crate::types::{ - GPSTransfer, GpsPads, GpsUart, GpsUartConfig, GpsUartTx, GroundStationPads, - GroundStationUartConfig, GPSBUFFER, -}; -use atsamd_hal::dmac::Transfer; -use atsamd_hal::dmac::{self, transfer}; -use atsamd_hal::gpio::{Output, Pin, PushPull, PB07, PB09, Alternate, C, PA12, PA13}; -use atsamd_hal::sercom::uart; -use atsamd_hal::sercom::uart::Uart; -use atsamd_hal::sercom::Sercom; -use atsamd_hal::time::Hertz; -use defmt::info; -use ublox::{ - CfgMsgAllPortsBuilder, CfgPrtUartBuilder, DataBits, InProtoMask, NavPvt, OutProtoMask, Parity, - StopBits, UartMode, UartPortId, UbxPacketRequest, -}; -// use atsamd_hal::prelude::nb; -use atsamd_hal::fugit::ExtU64; -use atsamd_hal::fugit::RateExtU32; -use atsamd_hal::prelude::*; -use rtic::Mutex; - -pub static mut BUF_DST: GPSBUFFER = &mut [0; 256]; - -/// GPS needs reset only once on startup and gps enable otherwise they are unused. This should not be in the constructor incase we are -/// We are using options right now because of a board issue and usage of two sercoms. -pub struct GpsManager { - pub gps_uart: Option, - pub gps_uart_tx: Option, - pub transfer: Option, - gps_enable: Pin>, - gps_reset: Pin>, - pub gps_pads: Option, - pub sercom: Option, - pub uart_clk_freq: Hertz, - pub rx: Option>>, - pub tx: Option>>, -} - -impl GpsManager { - pub fn new( - mut gps_uart: GpsUart, // cannot be an option in this constructor because we want to configure it before the radio. - gps_pads: Option, - sercom: Option, - mut gps_reset: Pin>, - mut gps_enable: Pin>, - mut dma_channel: dmac::Channel, - pclk_sercom2_freq: Hertz, - ) -> Self { - gps_reset.set_low().ok(); - cortex_m::asm::delay(300_000); - gps_reset.set_high().ok(); - gps_enable.set_low().ok(); - - // let (mut gps_rx, mut gps_tx) = gps_uart.split(); - - let packet: [u8; 28] = CfgPrtUartBuilder { - portid: UartPortId::Uart1, - reserved0: 0, - tx_ready: 0, - mode: UartMode::new(DataBits::Eight, Parity::None, StopBits::One), - baud_rate: 9600, - in_proto_mask: InProtoMask::all(), - out_proto_mask: OutProtoMask::UBLOX, - flags: 0, - reserved5: 0, - } - .into_packet_bytes(); - - for byte in packet { - nb::block!(gps_uart.write(byte)).unwrap(); - } - - // cortex_m::asm::delay(300_000); - - // let packet_two = CfgMsgAllPortsBuilder::set_rate_for::([1, 0, 0, 0, 0, 0]).into_packet_bytes(); - // for byte in packet_two { - // nb::block!(gps_uart.write(byte)).unwrap(); - // } - - // cortex_m::asm::delay(300_000); - // let request = UbxPacketRequest::request_for::().into_packet_bytes(); - // for byte in request { - // nb::block!(gps_uart.write(byte)).unwrap(); - // } - - let (mut rx, mut tx) = gps_uart.split(); - dma_channel - .as_mut() - .enable_interrupts(dmac::InterruptFlags::new().with_tcmpl(true)); - let mut gps_dma_transfer = Transfer::new(dma_channel, rx, unsafe { &mut *BUF_DST }, false) - .expect("DMA err") - .begin( - atsamd_hal::sercom::Sercom2::DMA_RX_TRIGGER, - dmac::TriggerAction::BURST, - ); - - // let mut gps_dma_transfer = rx.receive_with_dma( unsafe { &mut *BUF_DST },dma_channel, |_| { - // info!("DMA Transfer Complete"); - // }); - - // gps_dma_transfer.wait(); - - loop { - if gps_dma_transfer.complete() { - info!("DMA Transfer Complete"); - break; - } - } - - Self { - gps_uart: None, - gps_uart_tx: Some(tx), - gps_reset, - gps_enable, - transfer: None, - gps_pads, - sercom, - uart_clk_freq: pclk_sercom2_freq, - rx: None, // consumed by pads first - tx: None, - } - } -} - -pub fn gps_dma(mut cx: crate::app::gps_dma::Context) { - info!("GPS DMA"); - cx.shared.gps_manager.lock(|mut gps_manager| { - let mut gps_dma_transfer = gps_manager.transfer.take().unwrap(); - // let mut gps_tx = gps_manager.gps_tx.take().unwrap(); - if gps_dma_transfer.complete() { - let (chan0, mut gps_rx, buf) = gps_dma_transfer.stop(); - - // get the radio manager from the shared resources - // take the gps_tx from the gps_manager and gps_rx from the dma transfer and then join them to create the uart object. - // disable the gps uart giving back the config. - // free the config giving back the pads and sercom. - let mut gps_tx = gps_manager.gps_uart_tx.take().unwrap(); - let mut gps_uart = atsamd_hal::sercom::uart::Uart::join(gps_rx, gps_tx); - let mut config = gps_uart.disable(); - let (mut sercom, mut pads) = config.free(); - let (rx, tx, _, _) = pads.free(); - gps_manager.rx = Some(rx); - gps_manager.tx = Some(tx); - - cx.shared.radio_manager.lock(|mut radio_manager| { - let radio_uart = cortex_m::interrupt::free(|cs| { - let mut x = unsafe { crate::MCLK.borrow(cs).borrow_mut() }; - let mclk = x.as_mut().unwrap(); - let pads = atsamd_hal::sercom::uart::Pads::::default() - .rx(radio_manager.rx.take().unwrap()) - .tx(radio_manager.tx.take().unwrap()); - GroundStationUartConfig::new( - &mclk, - sercom, - pads, - radio_manager.uart_clk_freq, - ) - .baud( - RateExtU32::Hz(57600), - uart::BaudMode::Fractional(uart::Oversampling::Bits16), - ) - .enable() - }); - radio_manager.radio = Some(radio_uart); - }); - - // gps_dma_transfer = dmac::Transfer::new(chan0, source, unsafe { &mut *BUF_DST }, false) - // .unwrap() - // .begin( - // atsamd_hal::sercom::Sercom2::DMA_RX_TRIGGER, - // dmac::TriggerAction::BURST, - // ); - - let buf_clone = buf.clone(); - // let mut uart = Uart::join(source, gps_tx); - unsafe { BUF_DST.copy_from_slice(&[0; 256]) }; - // gps_dma_transfer.block_transfer_interrupt(); - - // let request = UbxPacketRequest::request_for::().into_packet_bytes(); - // for byte in request { - // nb::block!(gps_uart.write(byte)).unwrap(); - // } - // cortex_m::asm::delay(300_000); - let mut buf: [u8; 256] = [0; 256]; - let mut bytes: [u8; 256] = [0; 256]; - let buf = ublox::FixedLinearBuffer::new(&mut buf[..]); - let mut parser = ublox::Parser::new(buf); - let mut msgs = parser.consume(&buf_clone); - while let Some(msg) = msgs.next() { - match msg { - Ok(msg) => match msg { - ublox::PacketRef::NavPosLlh(x) => { - let message_data = messages::sensor::NavPosLlh { - height_msl: x.height_msl(), - longitude: x.lon_degrees(), - latitude: x.lat_degrees(), - }; - info!( - "GPS latitude: {:?}, longitude {:?}", - x.lat_degrees(), - x.lon_degrees() - ); - } - ublox::PacketRef::NavStatus(x) => { - info!("GPS fix stat: {:?}", x.fix_stat_raw()); - } - ublox::PacketRef::NavDop(x) => { - info!("GPS geometric drop: {:?}", x.geometric_dop()); - } - ublox::PacketRef::NavSat(x) => { - info!("GPS num sats used: {:?}", x.num_svs()); - } - ublox::PacketRef::NavVelNed(x) => { - info!("GPS velocity north: {:?}", x.vel_north()); - } - ublox::PacketRef::NavPvt(x) => { - info!("GPS nun sats PVT: {:?}", x.num_satellites()); - } - _ => { - info!("GPS Message not handled."); - } - }, - Err(e) => { - info!("GPS parse Error"); - } - } - } - } - }); -} diff --git a/boards/communication/src/main.rs b/boards/communication/src/main.rs deleted file mode 100644 index d1fe714b..00000000 --- a/boards/communication/src/main.rs +++ /dev/null @@ -1,676 +0,0 @@ -#![no_std] -#![no_main] - -mod communication; -mod data_manager; -mod gps_manager; -mod types; - -use atsamd_hal as hal; -use atsamd_hal::dmac; -use atsamd_hal::rtc::Count32Mode; -use common_arm::*; -use common_arm_atsame::mcan; -use gps_manager::{gps_dma, GpsManager}; -// use panic_halt as _; -use atsamd_hal::gpio::{Alternate, Output, PushPull, D, PA04, PA05, PA06, PA07}; -use atsamd_hal::sercom::{ - spi, - spi::{Config, Duplex, Pads, Spi}, - IoSet1, Sercom0, -}; -use communication::Capacities; -use communication::{CanCommandManager, RadioManager}; -use core::cell::RefCell; -use cortex_m::interrupt::Mutex; -use data_manager::DataManager; -use defmt::info; -use defmt_rtt as _; // global logger -use fugit::ExtU64; -use fugit::RateExtU32; -use hal::clock::v2::pclk::Pclk; -use hal::clock::v2::Source; -use hal::gpio::Pins; -use hal::gpio::{Pin, PushPullOutput, C, PA02, PA03, PA08, PA09}; -use hal::prelude::*; -use hal::sercom::uart; -use hal::sercom::IoSet3; -use mcan::messageram::SharedMemory; -use messages::command::RadioRate; -use messages::state::State; -use messages::*; -use panic_probe as _; -use systick_monotonic::*; -use types::*; -use ublox::{ - CfgPrtUartBuilder, DataBits, InProtoMask, OutProtoMask, Parity, StopBits, UartMode, UartPortId, - UbxPacketRequest, -}; - -#[inline(never)] -#[defmt::panic_handler] -fn panic() -> ! { - atsamd_hal::pac::SCB::sys_reset() -} - -/// Hardfault handler. -/// -/// Terminates the application and makes a semihosting-capable debug tool exit -/// with an error. This seems better than the default, which is to spin in a -/// loop. -#[cortex_m_rt::exception] -unsafe fn HardFault(_frame: &cortex_m_rt::ExceptionFrame) -> ! { - loop {} -} - -#[global_allocator] -static HEAP: embedded_alloc::Heap = embedded_alloc::Heap::empty(); - -static RTC: Mutex>>> = Mutex::new(RefCell::new(None)); - -static mut MCLK: Mutex>> = Mutex::new(RefCell::new(None)); - -#[rtic::app(device = hal::pac, peripherals = true, dispatchers = [EVSYS_0, EVSYS_1, EVSYS_2])] -mod app { - - use core::borrow::BorrowMut; - - use atsamd_hal::{clock::v2::pclk, sercom::dma}; - use command::{Command, CommandData}; - - use sender::Sender; - - use super::*; - - #[shared] - struct Shared { - em: ErrorManager, - data_manager: DataManager, - radio_manager: RadioManager, - gps_manager: GpsManager, - can0: communication::CanDevice0, - can_command_manager: CanCommandManager, - } - - #[local] - struct Local { - led_green: Pin, - led_red: Pin, - // gps_uart: GpsUart, - // sd_manager: SdManager< - // Spi< - // Config< - // Pads< - // hal::pac::SERCOM0, - // IoSet3, - // Pin>, - // Pin>, - // Pin>, - // >, - // >, - // Duplex, - // >, - // Pin>, - // >, - } - - #[monotonic(binds = SysTick, default = true)] - type MyMono = Systick<100>; // 100 Hz / 10 ms granularity - - #[init(local = [ - #[link_section = ".can"] - can_memory: SharedMemory = SharedMemory::new(), - #[link_section = ".can_command"] - can_command_memory: SharedMemory = SharedMemory::new() - ])] - fn init(cx: init::Context) -> (Shared, Local, init::Monotonics) { - { - use core::mem::MaybeUninit; - const HEAP_SIZE: usize = 1024; - static mut HEAP_MEM: [MaybeUninit; HEAP_SIZE] = [MaybeUninit::uninit(); HEAP_SIZE]; - unsafe { HEAP.init(HEAP_MEM.as_ptr() as usize, HEAP_SIZE) } - } - let mut peripherals = cx.device; - let core = cx.core; - let pins = Pins::new(peripherals.PORT); - - let mut dmac = dmac::DmaController::init(peripherals.DMAC, &mut peripherals.PM); - let dmaChannels = dmac.split(); - - /* Logging Setup */ - HydraLogging::set_ground_station_callback(queue_gs_message); - - /* Clock setup */ - let (_, clocks, tokens) = atsamd_hal::clock::v2::clock_system_at_reset( - peripherals.OSCCTRL, - peripherals.OSC32KCTRL, - peripherals.GCLK, - peripherals.MCLK, - &mut peripherals.NVMCTRL, - ); - - let gclk0 = clocks.gclk0; - - // SAFETY: Misusing the PAC API can break the system. - // This is safe because we only steal the MCLK. - let (_, _, _, mut mclk) = unsafe { clocks.pac.steal() }; - - /* CAN config */ - - let (pclk_can, gclk0) = Pclk::enable(tokens.pclks.can0, gclk0); - let (can0, gclk0) = communication::CanDevice0::new( - pins.pa23.into_mode(), - pins.pa22.into_mode(), - pclk_can, - clocks.ahbs.can0, - peripherals.CAN0, - gclk0, - cx.local.can_memory, - false, - ); - - let (pclk_can_command, gclk0) = Pclk::enable(tokens.pclks.can1, gclk0); - let (can_command_manager, gclk0) = CanCommandManager::new( - pins.pb15.into_mode(), - pins.pb14.into_mode(), - pclk_can_command, - clocks.ahbs.can1, - peripherals.CAN1, - gclk0, - cx.local.can_command_memory, - false, - ); - - // setup external osc - // let xosc0 = atsamd_hal::clock::v2::xosc::Xosc::from_crystal( - // tokens.xosc0, - // pins.pa14, - // pins.pa15, - // 48_000_000.Hz(), - // ).current(CrystalCurrent::Medium) - // .loop_control(true) - // .low_buf_gain(true) - // .start_up_delay(StartUpDelay::Delay488us).enable(); - // while !xosc0.is_ready() { - // info!("Waiting for XOSC0 to stabilize"); - // } - - // let (mut gclk0, dfll) = - // hal::clock::v2::gclk::Gclk::from_source(tokens.gclks.gclk2, xosc0); - // let gclk2 = gclk2.div(gclk::GclkDiv8::Div(1)).enable(); - let (pclk_sercom2, gclk0) = Pclk::enable(tokens.pclks.sercom2, gclk0); - /* Radio config */ - - let rx: Pin> = pins.pa08.into_alternate(); - let tx: Pin> = pins.pa09.into_alternate(); - // let pads = uart::Pads::::default() - // .rx(rx) - // .tx(tx); - // let uart = - // GroundStationUartConfig::new(&mclk, peripherals.SERCOM2, pads, pclk_sercom2.freq()) - // .baud( - // RateExtU32::Hz(57600), - // uart::BaudMode::Fractional(uart::Oversampling::Bits16), - // ) - // .enable(); - - // let radio = RadioDevice::new(uart); - - unsafe { - MCLK = Mutex::new(RefCell::new(Some(mclk))); - } - // let (pclk_gps, gclk0) = Pclk::enable(tokens.pclks.sercom2, gclk0); - let gps_rx: Pin> = pins.pa13.into_alternate(); - let gps_tx: Pin> = pins.pa12.into_alternate(); - let gps_pads = hal::sercom::uart::Pads::::default() - .rx(gps_rx) - .tx(gps_tx); - - let mut gps_uart_config = cortex_m::interrupt::free(|cs| { - let mut x = unsafe { MCLK.borrow(cs).borrow_mut() }; - let mclk = x.as_mut().unwrap(); - GpsUartConfig::new(&mclk, peripherals.SERCOM2, gps_pads, pclk_sercom2.freq()).baud( - RateExtU32::Hz(9600), - uart::BaudMode::Fractional(uart::Oversampling::Bits16), - ) - }); - gps_uart_config = gps_uart_config.immediate_overflow_notification(false); - - let mut gps_uart = gps_uart_config.enable(); - let mut dmaCh0 = dmaChannels.0.init(dmac::PriorityLevel::LVL3); - let gps_manager = cortex_m::interrupt::free(|cs| { - let mut x = unsafe { MCLK.borrow(cs).borrow_mut() }; - let mclk = x.as_mut().unwrap(); - GpsManager::new( - gps_uart, - None, // pads are already consumed by the uart. - None, - pins.pb07.into_push_pull_output(), - pins.pb09.into_push_pull_output(), - dmaCh0, - pclk_sercom2.freq(), - ) - }); - let message = Message::new( - cortex_m::interrupt::free(|cs| { - let mut rc = RTC.borrow(cs).borrow_mut(); - let rtc = rc.as_mut().unwrap(); - rtc.count32() - }), - COM_ID, - State::new(messages::state::StateData::Initializing), - ); - - let radio_manager = cortex_m::interrupt::free(|cs| { - info!("Setting up radio"); - let mut x = unsafe { MCLK.borrow(cs).borrow_mut() }; - let mclk = x.as_mut().unwrap(); - RadioManager::new(rx, tx, pclk_sercom2.freq()) - }); - let mut gps_tx = gps_manager.gps_uart_tx.take().unwrap(); - let mut gps_uart = atsamd_hal::sercom::uart::Uart::join(gps_rx, gps_tx); - let mut config = gps_uart.disable(); - let (mut sercom, mut pads) = config.free(); - let (rx, tx, _, _) = pads.free(); - let radio_uart = cortex_m::interrupt::free(|cs| { - let mut x = unsafe { crate::MCLK.borrow(cs).borrow_mut() }; - let mclk = x.as_mut().unwrap(); - let pads = atsamd_hal::sercom::uart::Pads::::default() - .rx(radio_manager.rx.take().unwrap()) - .tx(radio_manager.tx.take().unwrap()); - GroundStationUartConfig::new( - &mclk, - sercom, - pads, - radio_manager.uart_clk_freq, - ) - .baud( - RateExtU32::Hz(57600), - uart::BaudMode::Fractional(uart::Oversampling::Bits16), - ) - .enable() - }); - radio_manager.radio = Some(radio_uart); - - loop { - - let mut buf = [0; 255]; - let data = postcard::to_slice(&m, &mut buf).unwrap(); - radio_manager.send_message(data); - } - - // /* SD config */ - // let (mut gclk1, dfll) = - // hal::clock::v2::gclk::Gclk::from_source(tokens.gclks.gclk1, clocks.dfll); - // let gclk1 = gclk1.div(gclk::GclkDiv16::Div(3)).enable(); // 48 / 3 = 16 MHzs - // let (pclk_sd, gclk0) = Pclk::enable(tokens.pclks.sercom0, gclk0); - - // let pads_spi = spi::Pads::::default() - // .sclk(pins.pa05) - // .data_in(pins.pa07) - // .data_out(pins.pa04); - // let sdmmc_spi = spi::Config::new(&mclk, peripherals.SERCOM0, pads_spi, pclk_sd.freq()) - // .length::() - // .bit_order(spi::BitOrder::MsbFirst) - // .spi_mode(spi::MODE_0) - // .enable(); - // let sd_manager = SdManager::new(sdmmc_spi, pins.pa06.into_push_pull_output()); - - // No need to reclock the peripheral sercom since it's shared with the radio. - - // gps_uart.enable_interrupts(hal::sercom::uart::Flags::RXC); - - /* Status LED */ - // info!("Setting up LED"); - // let led = pins.pa02.into_push_pull_output(); - // let mut gps_enable = pins.pb09.into_push_pull_output(); - // //gps_enable.set_high().ok(); - // gps_enable.set_low().ok(); - /* Spawn tasks */ - // sensor_send::spawn().ok(); - // state_send::spawn().ok(); - let rtc = cortex_m::interrupt::free(|cs| { - let mut x = unsafe { MCLK.borrow(cs).borrow_mut() }; - let mut mclk = x.as_mut().unwrap(); - hal::rtc::Rtc::clock_mode( - peripherals.RTC, - atsamd_hal::fugit::RateExtU32::Hz(1024), - &mut mclk, - ) - }); - - let mut rtc = rtc.into_count32_mode(); // hal bug this must be done - rtc.set_count32(0); - cortex_m::interrupt::free(|cs| { - RTC.borrow(cs).replace(Some(rtc)); - }); - let mut led_green = pins.pa03.into_push_pull_output(); - let mut led_red = pins.pa02.into_push_pull_output(); - led_green.set_high(); - led_red.set_high(); - blink::spawn().ok(); - sensor_send::spawn().ok(); - // generate_random_command::spawn().ok(); - let mono = Systick::new(core.SYST, gclk0.freq().to_Hz()); - - // info!("Init done"); - ( - Shared { - em: ErrorManager::new(), - data_manager: DataManager::new(), - radio_manager: radio_manager, - gps_manager: gps_manager, - can0, - can_command_manager, - }, - Local { - led_green, - led_red, - // sd_manager, - }, - init::Monotonics(mono), - ) - } - - /// Idle task for when no other tasks are running. - #[idle] - fn idle(_: idle::Context) -> ! { - loop {} - } - - // #[task(binds = SERCOM2_2, local = [gps_uart], shared = [&em])] - // fn gps_rx(mut cx: gps_rx::Context) { - // cx.shared.em.run(|| { - // let byte = cx.local.gps_uart.read().unwrap(); - // info!("GPS: {}", byte); - // Ok(()) - // }); - // } - - /// Handles the CAN1 interrupt. - #[task(priority = 3, binds = CAN1, shared = [can_command_manager, data_manager])] - fn can_command(mut cx: can_command::Context) { - info!("CAN1 interrupt"); - cx.shared.can_command_manager.lock(|can| { - cx.shared - .data_manager - .lock(|data_manager| can.process_data(data_manager)); - }); - } - - /// Handles the CAN0 interrupt. - #[task(priority = 3, binds = CAN0, shared = [can0, data_manager])] - fn can0(mut cx: can0::Context) { - info!("CAN0 interrupt"); - cx.shared.can0.lock(|can| { - cx.shared - .data_manager - .lock(|data_manager| can.process_data(data_manager)); - }); - } - /** - * Sends a mgps_txessage over CAN. - */ - #[task(capacity = 10, shared = [can0, &em])] - fn send_internal(mut cx: send_internal::Context, m: Message) { - // info!("{}", m.clone()); - cx.shared.em.run(|| { - cx.shared.can0.lock(|can| can.send_message(m))?; - Ok(()) - }); - } - - /** - * Sends a message over CAN. - */ - #[task(priority = 3, capacity = 5, shared = [can_command_manager, &em])] - fn send_command(mut cx: send_command::Context, m: Message) { - info!("{}", m.clone()); - cx.shared.em.run(|| { - cx.shared - .can_command_manager - .lock(|can| can.send_message(m))?; - 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) { - let message = Message::new( - cortex_m::interrupt::free(|cs| { - let mut rc = RTC.borrow(cs).borrow_mut(); - let rtc = rc.as_mut().unwrap(); - rtc.count32() - }), - COM_ID, - d.into(), - ); - info!("Sending message {}", message); - // send_gs::spawn(message).ok(); - } - - // /** - // * 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) { - // cx.shared.radio_manager.lock(|radio_manager| { - // cx.shared.em.run(|| { - // let mut buf = [0; 255]; - // let data = postcard::to_slice(&m, &mut buf)?; - // radio_manager.send_message(data)?; - // Ok(()) - // }) - // }); - // } - - // #[task(capacity = 10, local = [sd_manager], shared = [&em])] - // fn sd_dump(cx: sd_dump::Context, m: Message) { - // let manager = cx.local.sd_manager; - // cx.shared.em.run(|| { - // let mut buf: [u8; 255] = [0; 255]; - // let msg_ser = postcard::to_slice_cobs(&m, &mut buf)?; - // if let Some(mut file) = manager.file.take() { - // manager.write(&mut file, &msg_ser)?; - // manager.file = Some(file); - // } else if let Ok(mut file) = manager.open_file("log.txt") { - // manager.write(&mut file, &msg_ser)?; - // manager.file = Some(file); - // } - // Ok(()) - // }); - // } - - /** - * Sends information about the sensors. - */ - #[task(shared = [data_manager, radio_manager, gps_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())); - cx.shared.radio_manager.lock(|mut radio_manager| { - for msg in sensors { - match msg { - Some(x) => { - // spawn!(send_gs, x.clone())?; - cx.shared.em.run(|| { - let mut buf = [0; 255]; - let data = postcard::to_slice(&x, &mut buf)?; - radio_manager.send_message(data)?; - - Ok(()) - }); - - // spawn!(sd_dump, x)?; - } - None => { - continue; - } - } - } - info!("before take"); - if let Some(radio) = radio_manager.radio.take() { - info!("after_take"); - let mut config = radio_manager.radio.take().unwrap().disable(); - - let (mut sercom, mut pads) = config.free(); - let (rx, tx, _, _) = pads.free(); - radio_manager.rx = Some(rx); - radio_manager.tx = Some(tx); - - cx.shared.gps_manager.lock(|mut gps_manager| { - let mut gps_uart = cortex_m::interrupt::free(|cs| { - let mut x = unsafe { crate::MCLK.borrow(cs).borrow_mut() }; - let mclk = x.as_mut().unwrap(); - let pads = atsamd_hal::sercom::uart::Pads::< - atsamd_hal::sercom::Sercom2, - atsamd_hal::sercom::IoSet1, - >::default() - .rx(gps_manager.rx.take().unwrap()) - .tx(gps_manager.tx.take().unwrap()); - let mut gps_uart_config = - GpsUartConfig::new(&mclk, sercom, pads, gps_manager.uart_clk_freq) - .baud( - RateExtU32::Hz(9600), - uart::BaudMode::Fractional(uart::Oversampling::Bits16), - ); - - gps_uart_config = gps_uart_config.immediate_overflow_notification(false); - gps_uart_config.enable() - }); - - let request = - UbxPacketRequest::request_for::().into_packet_bytes(); - for byte in request { - nb::block!(gps_uart.write(byte)).unwrap(); - } - cortex_m::asm::delay(300_000); - gps_manager.gps_uart = Some(gps_uart); - }); - } - }); - - match logging_rate { - RadioRate::Fast => { - spawn_after!(sensor_send, ExtU64::millis(100)).ok(); - } - RadioRate::Slow => { - spawn_after!(sensor_send, ExtU64::millis(250)).ok(); - } - } - } - - // #[task(shared = [&em])] - // fn generate_random_messages(cx: generate_random_messages::Context) { - // cx.shared.em.run(|| { - // let message = Message::new( - // cortex_m::interrupt::free(|cs| { - // let mut rc = RTC.borrow(cs).borrow_mut(); - // let rtc = rc.as_mut().unwrap(); - // rtc.count32() - // }), - // COM_ID, - // State::new(messages::state::StateData::Initializing), - // ); - // info!("Sending message {}", message.clone()); - // // spawn!(send_gs, message.clone())?; - // spawn!(send_gs, message)?; - // Ok(()) - // }); - // spawn_after!(generate_random_messages, ExtU64::millis(200)).ok(); - // } - - // #[task(shared = [&em])] - // fn generate_random_command(cx: generate_random_command::Context) { - // cx.shared.em.run(|| { - // let message = Message::new( - // cortex_m::interrupt::free(|cs| { - // let mut rc = RTC.borrow(cs).borrow_mut(); - // let rtc = rc.as_mut().unwrap(); - // rtc.count32() - // }), - // COM_ID, - // Command::new(CommandData::PowerDown(command::PowerDown { - // board: Sender::BeaconBoard, - // })), - // ); - // spawn!(send_command, message)?; - // Ok(()) - // }); - // spawn!(generate_random_command).ok(); - // } - - #[task(shared = [data_manager, &em, radio_manager])] - fn state_send(mut cx: state_send::Context) { - let state_data = cx - .shared - .data_manager - .lock(|data_manager| data_manager.state.clone()); - if let Some(x) = state_data { - let message = Message::new( - cortex_m::interrupt::free(|cs| { - let mut rc = RTC.borrow(cs).borrow_mut(); - let rtc = rc.as_mut().unwrap(); - rtc.count32() - }), - COM_ID, - State::new(x), - ); - cx.shared.radio_manager.lock(|mut radio_manager| { - cx.shared.em.run(|| { - let mut buf = [0; 255]; - let data = postcard::to_slice(&message, &mut buf)?; - radio_manager.send_message(data)?; - // *radio_manager = Some(radio); - Ok(()) - }); - }); - // spawn!(send_gs, message)?; - } // if there is none we still return since we simply don't have data yet. - spawn_after!(state_send, ExtU64::secs(5)).ok(); - } - - // /// Handles the radio interrupt. - // /// This only needs to be called when the radio has data to read, this is why an interrupt handler is used above polling which would waste resources. - // /// We use a critical section to ensure that we are not interrupted while servicing the mavlink message. - // #[task(priority = 3, binds = SERCOM2_2, shared = [&em, radio_manager])] - // fn radio_rx(mut cx: radio_rx::Context) { - // cx.shared.radio_manager.lock(|radio_manager| { - // cx.shared.em.run(|| { - // cortex_m::interrupt::free(|cs| { - // let mut radio = radio_manager.take(); - // let m = radio.receive_message()?; - // radio_manager = Some(radio); - // info!("Received message {}", m.clone()); - // spawn!(send_command, m) - // })?; - // Ok(()) - // }); - // }); - // } - - /** - * Simple blink task to test the system. - * Acts as a heartbeat for the system. - */ - #[task(local = [led_green, led_red], shared = [&em])] - fn blink(cx: blink::Context) { - cx.shared.em.run(|| { - if cx.shared.em.has_error() { - cx.local.led_red.toggle()?; - spawn_after!(blink, ExtU64::millis(200))?; - } else { - cx.local.led_green.toggle()?; - spawn_after!(blink, ExtU64::secs(1))?; - } - Ok(()) - }); - } - - extern "Rust" { - #[task(priority = 3, binds = DMAC_0, shared = [&em, gps_manager, data_manager, radio_manager])] - fn gps_dma(context: gps_dma::Context); - } -} diff --git a/boards/communication/src/types.rs b/boards/communication/src/types.rs deleted file mode 100644 index 2adf5d30..00000000 --- a/boards/communication/src/types.rs +++ /dev/null @@ -1,32 +0,0 @@ -use atsamd_hal::gpio::*; -use atsamd_hal::sercom::uart::EightBit; -use atsamd_hal::sercom::{uart, uart::Duplex, IoSet3, Sercom2, IoSet1, Sercom4}; -use messages::sender::Sender; -use messages::sender::Sender::CommunicationBoard; -use atsamd_hal::{dmac, dmac::BufferPair}; -use atsamd_hal::sercom::uart::TxDuplex; - - -// ------- -// Sender ID -// ------- -pub static COM_ID: Sender = CommunicationBoard; - -// ------- -// Ground Station -// ------- -pub type GroundStationPads = uart::PadsFromIds; -pub type GroundStationUartConfig = uart::Config; - -// ------- -// GPS UART -// ------- -pub type GpsPads = uart::PadsFromIds; -pub type GpsUartConfig = uart::Config; -pub type GpsUart = uart::Uart; -pub type GpsUartTx = uart::Uart; -pub type GPSTransfer = dmac::Transfer< - dmac::Channel, - BufferPair, GPSBUFFER>, ->; -pub type GPSBUFFER = &'static mut [u8; 256]; \ No newline at end of file diff --git a/boards/power/Cargo.toml b/boards/power/Cargo.toml deleted file mode 100644 index 56ae7a12..00000000 --- a/boards/power/Cargo.toml +++ /dev/null @@ -1,21 +0,0 @@ -[package] -name = "power" -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 = "^0.7.0" -cortex-m-rtic = "1.1.3" -systick-monotonic = "1.0.1" -defmt = "0.3.2" -postcard = "1.0.2" -heapless = "0.7.16" -common-arm-atsame = { path = "../../libraries/common-arm-atsame" } -common-arm = { path = "../../libraries/common-arm" } -atsamd-hal = { workspace = true } -messages = { path = "../../libraries/messages" } -typenum = "1.16.0" -enum_dispatch = "0.3.11" \ No newline at end of file diff --git a/boards/power/src/communication.rs b/boards/power/src/communication.rs deleted file mode 100644 index 532bb563..00000000 --- a/boards/power/src/communication.rs +++ /dev/null @@ -1,174 +0,0 @@ -use crate::data_manager::DataManager; -use atsamd_hal as hal; -use common_arm::HydraError; -use common_arm_atsame::mcan; -use defmt::info; -use hal::can::Dependencies; -use hal::clock::v2::ahb::AhbClk; -use hal::clock::v2::gclk::Gclk0Id; -use hal::clock::v2::pclk::Pclk; -use hal::clock::v2::types::Can0; -use hal::clock::v2::Source; -use hal::gpio::{Alternate, AlternateI, Pin, I, PA22, PA23}; -use hal::pac::CAN0; -use hal::typelevel::Increment; -use heapless::Vec; -use mcan::bus::Can; -use mcan::embedded_can as ecan; -use mcan::interrupt::state::EnabledLine0; // line 0 and 1 are connected -use mcan::interrupt::{Interrupt, OwnedInterruptSet}; -use mcan::message::tx; -use mcan::message::{rx, Raw}; -use mcan::messageram::SharedMemory; -use mcan::tx_buffers::DynTx; -use mcan::{ - config::{BitTiming, Mode}, - filter::{Action, Filter}, -}; -use messages::Message; -use postcard::from_bytes; -use systick_monotonic::fugit::RateExtU32; -use typenum::{U0, U128, U32, U64}; -pub struct Capacities; - -impl mcan::messageram::Capacities for Capacities { - type StandardFilters = U128; - type ExtendedFilters = U64; - type RxBufferMessage = rx::Message<64>; - type DedicatedRxBuffers = U64; - type RxFifo0Message = rx::Message<64>; - type RxFifo0 = U64; - type RxFifo1Message = rx::Message<64>; - type RxFifo1 = U64; - type TxMessage = tx::Message<64>; - type TxBuffers = U32; - type DedicatedTxBuffers = U0; - type TxEventFifo = U32; -} - -pub struct CanDevice0 { - pub can: Can< - 'static, - Can0, - Dependencies>, Pin>, CAN0>, - Capacities, - >, - line_interrupts: OwnedInterruptSet, -} - -impl CanDevice0 { - pub fn new( - can_rx: Pin, - can_tx: Pin, - pclk_can: Pclk, - ahb_clock: AhbClk, - peripheral: CAN0, - gclk0: S, - can_memory: &'static mut SharedMemory, - loopback: bool, - ) -> (Self, S::Inc) - where - S: Source + Increment, - { - let (can_dependencies, gclk0) = - Dependencies::new(gclk0, pclk_can, ahb_clock, can_rx, can_tx, peripheral); - - let mut can = - mcan::bus::CanConfigurable::new(200.kHz(), can_dependencies, can_memory).unwrap(); - can.config().mode = Mode::Fd { - allow_bit_rate_switching: false, - data_phase_timing: BitTiming::new(500.kHz()), - }; - - if loopback { - can.config().loopback = true; - } - - let interrupts_to_be_enabled = can - .interrupts() - .split( - [ - Interrupt::RxFifo0NewMessage, - Interrupt::RxFifo0Full, - Interrupt::RxFifo0MessageLost, - Interrupt::RxFifo1NewMessage, - Interrupt::RxFifo1Full, - Interrupt::RxFifo1MessageLost, - ] - .into_iter() - .collect(), - ) - .unwrap(); - - // Line 0 and 1 are connected to the same interrupt line - let line_interrupts = can - .interrupt_configuration() - .enable_line_0(interrupts_to_be_enabled); - - can.filters_standard() - .push(Filter::Classic { - action: Action::StoreFifo1, - filter: ecan::StandardId::new(messages::sender::Sender::CommunicationBoard.into()) - .unwrap(), - mask: ecan::StandardId::ZERO, - }) - .unwrap_or_else(|_| panic!("Ground Station filter")); - - let can = can.finalize().unwrap(); - ( - CanDevice0 { - can, - line_interrupts, - }, - gclk0, - ) - } - pub fn _send_message(&mut self, m: Message) -> Result<(), HydraError> { - let payload: Vec = postcard::to_vec(&m)?; - self.can.tx.transmit_queued( - tx::MessageBuilder { - id: ecan::Id::Standard(ecan::StandardId::new(m.sender.into()).unwrap()), - frame_type: tx::FrameType::FlexibleDatarate { - payload: &payload[..], - bit_rate_switching: false, - force_error_state_indicator: false, - }, - store_tx_event: None, - } - .build()?, - )?; - Ok(()) - } - pub fn _process_data(&mut self, data_manager: &mut DataManager) { - let line_interrupts = &self.line_interrupts; - for interrupt in line_interrupts.iter_flagged() { - match interrupt { - Interrupt::RxFifo0NewMessage => { - for message in &mut self.can.rx_fifo_0 { - match from_bytes::(message.data()) { - Ok(data) => { - data_manager.handle_data(data); - } - Err(e) => { - info!("Error: {:?}", e) - } - } - } - } - Interrupt::RxFifo1NewMessage => { - for message in &mut self.can.rx_fifo_1 { - match from_bytes::(message.data()) { - Ok(data) => { - data_manager.handle_data(data); - } - Err(e) => { - info!("Error: {:?}", e) - } - } - } - } - _ => (), - } - } - } -} diff --git a/boards/power/src/data_manager.rs b/boards/power/src/data_manager.rs deleted file mode 100644 index 0b20b216..00000000 --- a/boards/power/src/data_manager.rs +++ /dev/null @@ -1,19 +0,0 @@ -// import messages -use messages::Message; - -pub struct DataManager {} - -impl DataManager { - pub fn new() -> Self { - Self {} - } - pub fn handle_data(&mut self, _data: Message) { - // to do - } -} - -impl Default for DataManager { - fn default() -> Self { - Self::new() - } -} diff --git a/boards/power/src/main.rs b/boards/power/src/main.rs deleted file mode 100644 index f8d6fc6d..00000000 --- a/boards/power/src/main.rs +++ /dev/null @@ -1,169 +0,0 @@ -#![no_std] -#![no_main] - -mod communication; -mod data_manager; -mod types; - -use crate::data_manager::DataManager; -use atsamd_hal as hal; -use common_arm::*; -use common_arm_atsame::mcan; -use communication::CanDevice0; -use communication::Capacities; -use defmt::info; -use hal::clock::v2::pclk::Pclk; -use hal::clock::v2::Source; -use hal::gpio::{Pin, Pins, PushPullOutput, PA08, PB16, PB17}; -use hal::prelude::*; -use mcan::messageram::SharedMemory; -use systick_monotonic::*; - -/// Custom panic handler. -/// Reset the system if a panic occurs. -#[panic_handler] -fn panic(_info: &core::panic::PanicInfo) -> ! { - atsamd_hal::pac::SCB::sys_reset(); -} - -#[rtic::app(device = hal::pac, peripherals = true, dispatchers = [EVSYS_0, EVSYS_1, EVSYS_2])] -mod app { - use super::*; - - #[shared] - struct Shared { - em: ErrorManager, - data_manager: DataManager, - can0: CanDevice0, - } - - #[local] - struct Local { - led_green: Pin, - led_red: Pin, - reg_5v: Pin, - } - - #[monotonic(binds = SysTick, default = true)] - type SysMono = Systick<100>; // 100 Hz / 10 ms granularity - - #[init(local = [ - #[link_section = ".can"] - can_memory: SharedMemory = SharedMemory::new() - ])] - fn init(cx: init::Context) -> (Shared, Local, init::Monotonics) { - info!("Starting up"); - let mut peripherals = cx.device; - let core = cx.core; - let pins = Pins::new(peripherals.PORT); - - /* Clock setup */ - let (_, clocks, tokens) = hal::clock::v2::clock_system_at_reset( - peripherals.OSCCTRL, - peripherals.OSC32KCTRL, - peripherals.GCLK, - peripherals.MCLK, - &mut peripherals.NVMCTRL, - ); - let gclk0 = clocks.gclk0; - - /* CAN config */ - let (pclk_can, gclk0) = Pclk::enable(tokens.pclks.can0, gclk0); - let (can0, gclk0) = communication::CanDevice0::new( - pins.pa23.into_mode(), - pins.pa22.into_mode(), - pclk_can, - clocks.ahbs.can0, - peripherals.CAN0, - gclk0, - cx.local.can_memory, - false, - ); - - // let (pclk_adc0, gclk0) = Pclk::enable(tokens.pclks.adc0, gclk0); - - // // SAFETY: Misusing the PAC API can break the system. - // // This is safe because we only steal the MCLK. - // let (_, _, gclk, mut mclk) = unsafe { clocks.pac.steal() }; - - // // setup ADC - // let mut adc_test = hal::adc::Adc::adc0(peripherals.ADC0, &mut mclk); - - // LEDs - let led_green = pins.pb16.into_push_pull_output(); - let led_red = pins.pb17.into_push_pull_output(); - let reg_5v = pins.pa08.into_push_pull_output(); - - blink::spawn().ok(); - reg_5v::spawn().ok(); - // adc::spawn().ok(); - - /* Monotonic clock */ - let mono = Systick::new(core.SYST, gclk0.freq().to_Hz()); - - ( - Shared { - em: ErrorManager::new(), - data_manager: DataManager::new(), - can0, - }, - Local { - led_green, - led_red, - reg_5v, - }, - init::Monotonics(mono), - ) - } - - /// Idle task for when no other tasks are running. - #[idle] - fn idle(_: idle::Context) -> ! { - loop {} - } - - #[task(local = [reg_5v], shared = [&em])] - fn reg_5v(cx: reg_5v::Context) { - cx.shared.em.run(|| { - info!("Enable 5V regulator"); - cx.local.reg_5v.set_high()?; - Ok(()) - }); - } - - // #[task(local = [adc_test, adc_pin], shared = [&em])] - // fn adc(cx: adc::Context) { - // // test adc for 5v PWR sense - // info!("try adc"); - // cx.shared.em.run(||{ - // let val = cx.local.adc_test.read(cx.local.adc_pin); - // let x: u16 = match val { - // Ok(v) => { - // v - // } - // Err(_) => { - // 0 - // } - // }; - // info!("{}", x); - // Ok(()) - // }); - // spawn_after!(adc, ExtU64::millis(500)).ok(); - // } - - /// Simple blink task to test the system. - /// Acts as a heartbeat for the system. - #[task(local = [led_green, led_red], shared = [&em])] - fn blink(cx: blink::Context) { - cx.shared.em.run(|| { - if cx.shared.em.has_error() { - cx.local.led_red.toggle()?; - spawn_after!(blink, ExtU64::millis(200))?; - } else { - cx.local.led_green.toggle()?; - spawn_after!(blink, ExtU64::secs(1))?; - } - Ok(()) - }); - } -} diff --git a/boards/power/src/types.rs b/boards/power/src/types.rs deleted file mode 100644 index 196af97b..00000000 --- a/boards/power/src/types.rs +++ /dev/null @@ -1,5 +0,0 @@ -use messages::sender::{Sender, Sender::PowerBoard}; -// ------- -// Sender ID -// ------- -pub static _COM_ID: Sender = PowerBoard; diff --git a/boards/sensor/Cargo.toml b/boards/sensor/Cargo.toml deleted file mode 100644 index 13142f56..00000000 --- a/boards/sensor/Cargo.toml +++ /dev/null @@ -1,23 +0,0 @@ -[package] -name = "sensor" -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 = "^0.7.0" -cortex-m-rtic = "1.1.3" -systick-monotonic = "1.0.1" -defmt = "0.3.2" -postcard = "1.0.2" -heapless = "0.7.16" -common-arm-atsame = { path = "../../libraries/common-arm-atsame" } -common-arm = { path = "../../libraries/common-arm" } -atsamd-hal = { workspace = true } -messages = { path = "../../libraries/messages" } -sbg-rs = {path = "../../libraries/sbg-rs"} -embedded-sdmmc = "0.3.0" # port to v5 -typenum = "1.16.0" -embedded-alloc = "0.5.0" diff --git a/boards/sensor/src/communication.rs b/boards/sensor/src/communication.rs deleted file mode 100644 index c2e059a2..00000000 --- a/boards/sensor/src/communication.rs +++ /dev/null @@ -1,197 +0,0 @@ -use atsamd_hal::can::Dependencies; -use atsamd_hal::clock::v2::ahb::AhbClk; -use atsamd_hal::clock::v2::gclk::Gclk0Id; -use atsamd_hal::clock::v2::pclk::Pclk; - -use atsamd_hal::clock::v2::types::Can0; -use atsamd_hal::clock::v2::Source; -use atsamd_hal::gpio::{Alternate, AlternateI, Pin, I, PA22, PA23}; -use atsamd_hal::pac::CAN0; - -use atsamd_hal::typelevel::Increment; -use common_arm::HydraError; -use common_arm_atsame::mcan; -use defmt::info; -use heapless::Vec; -use mcan::bus::Can; -use mcan::embedded_can as ecan; -use mcan::interrupt::state::EnabledLine0; -use mcan::interrupt::{Interrupt, OwnedInterruptSet}; -use mcan::message::tx; -use mcan::message::{rx, Raw}; -use mcan::messageram::SharedMemory; -use mcan::tx_buffers::DynTx; -use mcan::{ - config::{BitTiming, Mode}, - filter::{Action, Filter}, -}; - -use messages::Message; -use postcard::from_bytes; -use systick_monotonic::fugit::RateExtU32; -use typenum::{U0, U128, U32, U64}; - -use crate::data_manager::DataManager; - -pub struct Capacities; - -impl mcan::messageram::Capacities for Capacities { - type StandardFilters = U128; - type ExtendedFilters = U64; - type RxBufferMessage = rx::Message<64>; - type DedicatedRxBuffers = U64; - type RxFifo0Message = rx::Message<64>; - type RxFifo0 = U64; - type RxFifo1Message = rx::Message<64>; - type RxFifo1 = U64; - type TxMessage = tx::Message<64>; - type TxBuffers = U32; - type DedicatedTxBuffers = U0; - type TxEventFifo = U32; -} - -pub struct CanDevice0 { - pub can: Can< - 'static, - Can0, - Dependencies>, Pin>, CAN0>, - Capacities, - >, - line_interrupts: OwnedInterruptSet, -} - -impl CanDevice0 { - pub fn new( - can_rx: Pin, - can_tx: Pin, - pclk_can: Pclk, - ahb_clock: AhbClk, - peripheral: CAN0, - gclk0: S, - can_memory: &'static mut SharedMemory, - loopback: bool, - ) -> (Self, S::Inc) - where - S: Source + Increment, - { - let (can_dependencies, gclk0) = - Dependencies::new(gclk0, pclk_can, ahb_clock, can_rx, can_tx, peripheral); - - let mut can = - mcan::bus::CanConfigurable::new(200.kHz(), can_dependencies, can_memory).unwrap(); - can.config().mode = Mode::Fd { - allow_bit_rate_switching: false, - data_phase_timing: BitTiming::new(500.kHz()), - }; - - if loopback { - can.config().loopback = true; - } - - let interrupts_to_be_enabled = can - .interrupts() - .split( - [ - Interrupt::RxFifo0NewMessage, - Interrupt::RxFifo0Full, - Interrupt::RxFifo0MessageLost, - Interrupt::RxFifo1NewMessage, - Interrupt::RxFifo1Full, - Interrupt::RxFifo1MessageLost, - ] - .into_iter() - .collect(), - ) - .unwrap(); - - // Line 0 and 1 are connected to the same interrupt line - let line_interrupts = can - .interrupt_configuration() - .enable_line_0(interrupts_to_be_enabled); - - can.filters_standard() - .push(Filter::Classic { - action: Action::StoreFifo0, - filter: ecan::StandardId::new(messages::sender::Sender::CommunicationBoard.into()) - .unwrap(), - mask: ecan::StandardId::ZERO, - }) - .unwrap_or_else(|_| panic!("Communication filter")); - - can.filters_standard() - .push(Filter::Classic { - action: Action::StoreFifo0, - filter: ecan::StandardId::new(messages::sender::Sender::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::sender::Sender::GroundStation.into()) - .unwrap(), - mask: ecan::StandardId::ZERO, - }) - .unwrap_or_else(|_| panic!("Ground Station filter")); - - let can = can.finalize().unwrap(); - ( - CanDevice0 { - can, - line_interrupts, - }, - gclk0, - ) - } - pub fn send_message(&mut self, m: Message) -> Result<(), HydraError> { - let payload: Vec = postcard::to_vec(&m)?; - self.can.tx.transmit_queued( - tx::MessageBuilder { - id: ecan::Id::Standard(ecan::StandardId::new(m.sender.into()).unwrap()), - frame_type: tx::FrameType::FlexibleDatarate { - payload: &payload[..], - bit_rate_switching: false, - force_error_state_indicator: false, - }, - store_tx_event: None, - } - .build()?, - )?; - Ok(()) - } - pub fn process_data(&mut self, data_manager: &mut DataManager) -> Result<(), HydraError> { - let line_interrupts = &self.line_interrupts; - for interrupt in line_interrupts.iter_flagged() { - match interrupt { - Interrupt::RxFifo0NewMessage => { - for message in &mut self.can.rx_fifo_0 { - match from_bytes::(message.data()) { - Ok(data) => { - data_manager.handle_data(data)?; - } - Err(e) => { - info!("Error: {:?}", e) - } - } - } - } - Interrupt::RxFifo1NewMessage => { - for message in &mut self.can.rx_fifo_1 { - match from_bytes::(message.data()) { - Ok(data) => { - data_manager.handle_data(data)?; - } - Err(e) => { - info!("Error: {:?}", e) - } - } - } - } - _ => (), - } - } - Ok(()) - } -} diff --git a/boards/sensor/src/data_manager.rs b/boards/sensor/src/data_manager.rs deleted file mode 100644 index 5691b5cd..00000000 --- a/boards/sensor/src/data_manager.rs +++ /dev/null @@ -1,73 +0,0 @@ -use crate::app::sleep_system; -use common_arm::{spawn, HydraError}; -use messages::sensor::{ - Air, EkfNav1, EkfNav2, EkfNavAcc, EkfQuat, GpsPos1, GpsPos2, GpsPosAcc, GpsVel, GpsVelAcc, - Imu1, Imu2, SensorData, UtcTime, -}; -use messages::Message; - -#[derive(Clone)] -pub struct DataManager { - pub air: Option, - pub ekf_nav: Option<(EkfNav1, EkfNav2, EkfNavAcc)>, - pub ekf_quat: Option, - pub imu: Option<(Imu1, Imu2)>, - pub utc_time: Option, - pub gps_vel: Option<(GpsVel, GpsVelAcc)>, - pub gps_pos: Option<(GpsPos1, GpsPos2, GpsPosAcc)>, -} - -impl DataManager { - pub fn new() -> Self { - Self { - air: None, - ekf_nav: None, - ekf_quat: None, - imu: None, - utc_time: None, - gps_vel: None, - gps_pos: None, - } - } - - pub fn clone_sensors(&self) -> [Option; 13] { - [ - self.air.clone().map(|x| x.into()), - self.ekf_nav.clone().map(|x| x.0.into()), - self.ekf_nav.clone().map(|x| x.1.into()), - self.ekf_nav.clone().map(|x| x.2.into()), - self.ekf_quat.clone().map(|x| x.into()), - self.imu.clone().map(|x| x.0.into()), - self.imu.clone().map(|x| x.1.into()), - self.utc_time.clone().map(|x| x.into()), - self.gps_vel.clone().map(|x| x.0.into()), - self.gps_vel.clone().map(|x| x.1.into()), - self.gps_pos.clone().map(|x| x.0.into()), - self.gps_pos.clone().map(|x| x.1.into()), - self.gps_pos.clone().map(|x| x.2.into()), - ] - } - - pub fn handle_data(&mut self, data: Message) -> Result<(), HydraError> { - match data.data { - messages::Data::Command(command) => match command.data { - messages::command::CommandData::PowerDown(_) => { - spawn!(sleep_system)?; // need proper error handling. could just expect, but that is mal practice. - } - _ => { - // We don't care atm about these other commands. - } - }, - _ => { - // we can disregard all other messages for now. - } - } - Ok(()) - } -} - -impl Default for DataManager { - fn default() -> Self { - Self::new() - } -} diff --git a/boards/sensor/src/main.rs b/boards/sensor/src/main.rs deleted file mode 100644 index 163822ee..00000000 --- a/boards/sensor/src/main.rs +++ /dev/null @@ -1,261 +0,0 @@ -#![no_std] -#![no_main] - -mod communication; -mod data_manager; -mod sbg_manager; -mod types; - -use atsamd_hal as hal; -use atsamd_hal::clock::v2::pclk::Pclk; -use atsamd_hal::clock::v2::Source; -use atsamd_hal::dmac::DmaController; -use common_arm::SdManager; -use common_arm::*; -use common_arm_atsame::mcan; -use communication::Capacities; -use data_manager::DataManager; -use defmt::info; -use hal::dmac; -use hal::gpio::Output; -use hal::gpio::Pins; -use hal::gpio::{Pin, PushPullOutput}; -use hal::gpio::{PushPull, PB01, PB10, PB16, PB17}; -use hal::prelude::*; -use hal::sercom::{spi, IoSet2, Sercom4}; -use mcan::messageram::SharedMemory; -use messages::sensor::Sensor; -use messages::*; -use sbg_manager::{sbg_dma, sbg_handle_data, sbg_sd_task, SBGManager}; -//use sbg_manager::{sbg_dma, sbg_handle_data, SBGManager}; -use sbg_rs::sbg::{CallbackData, SBG_BUFFER_SIZE}; -use systick_monotonic::*; -use types::*; - -/// Custom panic handler. -/// Reset the system if a panic occurs. -#[panic_handler] -fn panic(_info: &core::panic::PanicInfo) -> ! { - atsamd_hal::pac::SCB::sys_reset(); -} - -#[rtic::app(device = hal::pac, peripherals = true, dispatchers = [EVSYS_0, EVSYS_1, EVSYS_2])] -mod app { - use super::*; - - #[shared] - struct Shared { - em: ErrorManager, - data_manager: DataManager, - can: communication::CanDevice0, - sd_manager: SdManager>>, - } - - #[local] - struct Local { - led_green: Pin, - led_red: Pin, - sbg_manager: SBGManager, - sbg_power_pin: Pin, // this is here so we do not need to lock sbg_manager.! put into a gpio controller with leds. - } - - #[monotonic(binds = SysTick, default = true)] - type SysMono = Systick<100>; // 100 Hz / 10 ms granularity - - #[init(local = [ - #[link_section = ".can"] - can_memory: SharedMemory = SharedMemory::new() - ])] - fn init(cx: init::Context) -> (Shared, Local, init::Monotonics) { - let mut peripherals = cx.device; - let core = cx.core; - let pins = Pins::new(peripherals.PORT); - - let mut sbg_power_pin = pins.pb01.into_push_pull_output(); - sbg_power_pin.set_high().unwrap(); - - let mut dmac = DmaController::init(peripherals.DMAC, &mut peripherals.PM); - let dmaChannels = dmac.split(); - - /* Clock setup */ - let (_, clocks, tokens) = atsamd_hal::clock::v2::clock_system_at_reset( - peripherals.OSCCTRL, - peripherals.OSC32KCTRL, - peripherals.GCLK, - peripherals.MCLK, - &mut peripherals.NVMCTRL, - ); - let gclk0 = clocks.gclk0; - - // SAFETY: Misusing the PAC API can break the system. - // This is safe because we only steal the MCLK. - let (_, _, _, mut mclk) = unsafe { clocks.pac.steal() }; - - /* CAN config */ - let (pclk_can, gclk0) = Pclk::enable(tokens.pclks.can0, gclk0); - let (can, gclk0) = communication::CanDevice0::new( - pins.pa23.into_mode(), - pins.pa22.into_mode(), - pclk_can, - clocks.ahbs.can0, - peripherals.CAN0, - gclk0, - cx.local.can_memory, - false, - ); - - /* SD config */ - let (pclk_sd, gclk0) = Pclk::enable(tokens.pclks.sercom4, gclk0); - let pads_spi = spi::Pads::::default() - .sclk(pins.pb09) - .data_in(pins.pb11) - .data_out(pins.pb08); - - let sdmmc_spi = spi::Config::new(&mclk, peripherals.SERCOM4, pads_spi, pclk_sd.freq()) - .length::() - .bit_order(spi::BitOrder::MsbFirst) - .spi_mode(spi::MODE_0) - .enable(); - let sd_manager = SdManager::new(sdmmc_spi, pins.pb10.into_push_pull_output()); - - /* SBG config */ - let (pclk_sbg, gclk0) = Pclk::enable(tokens.pclks.sercom5, gclk0); - let dmaCh0 = dmaChannels.0.init(dmac::PriorityLevel::LVL3); - let sbg_manager = SBGManager::new( - pins.pb03, - pins.pb02, - pclk_sbg, - &mut mclk, - peripherals.SERCOM5, - peripherals.RTC, - dmaCh0, - ); - - // Buzzer should go here. There is complexity using the new clock system with the atsamdhal pwm implementation. - - /* Status LED */ - let led_green = pins.pb16.into_push_pull_output(); - let led_red = pins.pb17.into_push_pull_output(); - - /* Spawn tasks */ - sensor_send::spawn().ok(); - blink::spawn().ok(); - - /* Monotonic clock */ - let mono = Systick::new(core.SYST, gclk0.freq().to_Hz()); - - ( - Shared { - em: ErrorManager::new(), - data_manager: DataManager::new(), - can, - sd_manager, - }, - Local { - led_green, - led_red, - sbg_manager, - sbg_power_pin, - }, - init::Monotonics(mono), - ) - } - - /// Idle task for when no other tasks are running. - #[idle] - fn idle(_: idle::Context) -> ! { - loop {} - } - - #[task(local = [sbg_power_pin], shared = [sd_manager, &em])] - fn sleep_system(mut cx: sleep_system::Context) { - info!("Power Down"); - // close out sd files. - cx.shared.sd_manager.lock(|sd| { - cx.shared.em.run(|| { - sd.close_current_file()?; - // sd.close(); // we can also close the root directory and volume. - // power down sbg - cx.local.sbg_power_pin.set_low()?; // define hydra error for this error type. - // Call core.SCB.set_deepsleep for even less power consumption. - Ok(()) - }); - }); - } - - #[task(priority = 3, binds = CAN0, shared = [can, data_manager, &em])] - fn can0(mut cx: can0::Context) { - cx.shared.can.lock(|can| { - cx.shared.data_manager.lock(|manager| { - cx.shared.em.run(|| { - can.process_data(manager)?; - Ok(()) - }); - }); - }); - } - - /** - * Sends a message over CAN. - */ - #[task(capacity = 10, local = [counter: u16 = 0], shared = [can, &em])] - fn send_internal(mut cx: send_internal::Context, m: Message) { - cx.shared.em.run(|| { - cx.shared.can.lock(|can| can.send_message(m))?; - Ok(()) - }); - } - - /** - * Sends information about the sensors. - */ - #[task(shared = [data_manager, &em])] - fn sensor_send(mut cx: sensor_send::Context) { - let sensor_data = cx - .shared - .data_manager - .lock(|data_manager| data_manager.clone_sensors()); - - let messages = sensor_data - .into_iter() - .flatten() - .map(|x| Message::new(&monotonics::now(), COM_ID, Sensor::new(x))); - - cx.shared.em.run(|| { - for msg in messages { - spawn!(send_internal, msg)?; - } - Ok(()) - }); - spawn_after!(sensor_send, ExtU64::millis(100)).ok(); - } - - /** - * Simple blink task to test the system. - * Acts as a heartbeat for the system. - */ - #[task(local = [led_green, led_red], shared = [&em])] - fn blink(cx: blink::Context) { - cx.shared.em.run(|| { - if cx.shared.em.has_error() { - cx.local.led_red.toggle()?; - spawn_after!(blink, ExtU64::millis(200))?; - } else { - cx.local.led_green.toggle()?; - spawn_after!(blink, ExtU64::secs(1))?; - } - Ok(()) - }); - } - - extern "Rust" { - #[task(capacity = 3, shared = [&em, sd_manager])] - fn sbg_sd_task(context: sbg_sd_task::Context, data: [u8; SBG_BUFFER_SIZE]); - - #[task(binds = DMAC_0, shared = [&em], local = [sbg_manager])] - fn sbg_dma(context: sbg_dma::Context); - - #[task(capacity = 20, shared = [data_manager])] - fn sbg_handle_data(context: sbg_handle_data::Context, data: CallbackData); - } -} diff --git a/boards/sensor/src/sbg_manager.rs b/boards/sensor/src/sbg_manager.rs deleted file mode 100644 index 25b8e259..00000000 --- a/boards/sensor/src/sbg_manager.rs +++ /dev/null @@ -1,207 +0,0 @@ -use crate::types::{ConfigSBG, SBGBuffer, SBGTransfer}; -use atsamd_hal::clock::v2::gclk::Gclk0Id; -use atsamd_hal::clock::v2::pclk::Pclk; -use atsamd_hal::dmac; -use atsamd_hal::dmac::Transfer; -use atsamd_hal::gpio::{Pin, Reset, PB02, PB03}; -use atsamd_hal::pac::{MCLK, RTC}; -use atsamd_hal::sercom::IoSet6; -// use atsamd_hal::prelude::_atsamd21_hal_time_U32Ext; -use atsamd_hal::rtc::Rtc; -use core::alloc::{GlobalAlloc, Layout}; -use core::ffi::c_void; -use core::mem::size_of; -use core::ptr; -// use atsamd_hal::time::*; -use crate::app::sbg_handle_data; -use crate::app::sbg_sd_task as sbg_sd; -use atsamd_hal::prelude::*; -use atsamd_hal::sercom::{uart, Sercom, Sercom5}; -use common_arm::spawn; -use defmt::info; -use embedded_alloc::Heap; -use rtic::Mutex; -use sbg_rs::sbg; -use sbg_rs::sbg::{CallbackData, SBG, SBG_BUFFER_SIZE}; - -pub static mut BUF_DST: SBGBuffer = &mut [0; SBG_BUFFER_SIZE]; - -// Simple heap required by the SBG library -static HEAP: Heap = Heap::empty(); - -pub struct SBGManager { - sbg_device: SBG, - xfer: Option, -} - -impl SBGManager { - pub fn new( - rx: Pin, - tx: Pin, - pclk_sercom5: Pclk, - mclk: &mut MCLK, - sercom5: Sercom5, - rtc: RTC, - mut dma_channel: dmac::Channel, - ) -> Self { - /* Initialize the Heap */ - { - use core::mem::MaybeUninit; - const HEAP_SIZE: usize = 1024; - static mut HEAP_MEM: [MaybeUninit; HEAP_SIZE] = [MaybeUninit::uninit(); HEAP_SIZE]; - unsafe { HEAP.init(HEAP_MEM.as_ptr() as usize, HEAP_SIZE) } - } - - let pads_sbg = uart::Pads::::default().rx(rx).tx(tx); - let uart_sbg = ConfigSBG::new(mclk, sercom5, pads_sbg, pclk_sercom5.freq()) - .baud( - 115200.Hz(), - uart::BaudMode::Fractional(uart::Oversampling::Bits8), - ) - .enable(); - - let (sbg_rx, sbg_tx) = uart_sbg.split(); - - /* DMAC config */ - dma_channel - .as_mut() - .enable_interrupts(dmac::InterruptFlags::new().with_tcmpl(true)); - let xfer = Transfer::new(dma_channel, sbg_rx, unsafe { &mut *BUF_DST }, false) - .expect("DMA err") - .begin(Sercom5::DMA_RX_TRIGGER, dmac::TriggerAction::BURST); - - // There is a bug within the HAL that improperly configures the RTC - // in count32 mode. This is circumvented by first using clock mode then - // converting to count32 mode. - let rtc_temp = Rtc::clock_mode(rtc, 1024.Hz(), mclk); - let mut rtc = rtc_temp.into_count32_mode(); - rtc.set_count32(0); - - let sbg: sbg::SBG = sbg::SBG::new(sbg_tx, rtc, |data| { - sbg_handle_data::spawn(data).ok(); - }); - - SBGManager { - sbg_device: sbg, - xfer: Some(xfer), - } - } -} - -pub fn sbg_handle_data(mut cx: sbg_handle_data::Context, data: CallbackData) { - cx.shared.data_manager.lock(|manager| match data { - CallbackData::UtcTime(x) => manager.utc_time = Some(x), - CallbackData::Air(x) => manager.air = Some(x), - CallbackData::EkfQuat(x) => manager.ekf_quat = Some(x), - CallbackData::EkfNav(x) => manager.ekf_nav = Some(x), - CallbackData::Imu(x) => manager.imu = Some(x), - CallbackData::GpsVel(x) => manager.gps_vel = Some(x), - CallbackData::GpsPos(x) => manager.gps_pos = Some(x), - }); -} - -pub fn sbg_sd_task(mut cx: crate::app::sbg_sd_task::Context, data: [u8; SBG_BUFFER_SIZE]) { - cx.shared.sd_manager.lock(|manager| { - if let Some(mut file) = manager.file.take() { - cx.shared.em.run(|| { - manager.write(&mut file, &data)?; - Ok(()) - }); - manager.file = Some(file); // give the file back after use - } else if let Ok(mut file) = manager.open_file("sbg.txt") { - cx.shared.em.run(|| { - manager.write(&mut file, &data)?; - Ok(()) - }); - manager.file = Some(file); - } - }); -} -/** - * Handles the DMA interrupt. - * Handles the SBG data. - */ -pub fn sbg_dma(cx: crate::app::sbg_dma::Context) { - let sbg = cx.local.sbg_manager; - - match &mut sbg.xfer { - Some(xfer) => { - if xfer.complete() { - let (chan0, source, buf) = sbg.xfer.take().unwrap().stop(); - let mut xfer = dmac::Transfer::new(chan0, source, unsafe { &mut *BUF_DST }, false) - .unwrap() - .begin(Sercom5::DMA_RX_TRIGGER, dmac::TriggerAction::BURST); - let buf_clone = buf.clone(); - sbg.sbg_device.read_data(buf); - unsafe { BUF_DST.copy_from_slice(&[0; SBG_BUFFER_SIZE]) }; - xfer.block_transfer_interrupt(); - sbg.xfer = Some(xfer); - cx.shared.em.run(|| { - spawn!(sbg_sd, buf_clone)?; // this warning isn't right but it's fine - Ok(()) - }); - } - } - None => { - // it should be impossible to reach here. - info!("None"); - } - } -} - -/// Stored right before an allocation. Stores information that is needed to deallocate memory. -#[derive(Copy, Clone)] -struct AllocInfo { - layout: Layout, - ptr: *mut u8, -} - -/// Custom malloc for the SBG library. This uses the HEAP object initialized at the start of the -/// [`SBGManager`]. The [`Layout`] of the allocation is stored right before the returned pointed, -/// which makes it possible to implement [`free`] without any other data structures. -#[no_mangle] -pub extern "C" fn malloc(size: usize) -> *mut c_void { - if size == 0 { - return ptr::null_mut(); - } - - // Get a layout for both the requested size - let header_layout = Layout::new::(); - let requested_layout = Layout::from_size_align(size, 8).unwrap(); - let (layout, offset) = header_layout.extend(requested_layout).unwrap(); - - // Ask the allocator for memory - let orig_ptr = unsafe { HEAP.alloc(layout) }; - if orig_ptr.is_null() { - return orig_ptr as *mut c_void; - } - - // Compute the pointer that we will return - let result_ptr = unsafe { orig_ptr.add(offset) }; - - // Store the allocation information right before the returned pointer - let info_ptr = unsafe { result_ptr.sub(size_of::()) as *mut AllocInfo }; - unsafe { - info_ptr.write_unaligned(AllocInfo { - layout, - ptr: orig_ptr, - }); - } - - result_ptr as *mut c_void -} - -/// Custom free implementation for the SBG library. This uses the stored allocation information -/// right before the pointer to free up the resources. -/// -/// SAFETY: The value passed to ptr must have been obtained from a previous call to [`malloc`]. -#[no_mangle] -pub unsafe extern "C" fn free(ptr: *mut c_void) { - assert!(!ptr.is_null()); - - let info_ptr = unsafe { ptr.sub(size_of::()) as *const AllocInfo }; - let info = unsafe { info_ptr.read_unaligned() }; - unsafe { - HEAP.dealloc(info.ptr, info.layout); - } -} diff --git a/boards/sensor/src/types.rs b/boards/sensor/src/types.rs deleted file mode 100644 index fe0877ab..00000000 --- a/boards/sensor/src/types.rs +++ /dev/null @@ -1,42 +0,0 @@ -use atsamd_hal as hal; -use atsamd_hal::gpio::*; -use atsamd_hal::sercom::uart::EightBit; -use atsamd_hal::sercom::uart::Uart; -use atsamd_hal::sercom::{spi, uart, IoSet6, Sercom5}; -use hal::dmac; -use hal::dmac::BufferPair; -use hal::sercom::IoSet2; - -use hal::sercom::Sercom4; -use messages::sender::Sender; -use messages::sender::Sender::SensorBoard; -use sbg_rs::sbg::SBG_BUFFER_SIZE; - -// ------- -// Sender ID -// ------- -pub static COM_ID: Sender = SensorBoard; - -// ------- -// SBG -// ------- -pub type PadsSBG = uart::PadsFromIds; -pub type ConfigSBG = uart::Config; -pub type SBGTransfer = dmac::Transfer< - dmac::Channel, - BufferPair, SBGBuffer>, ->; -pub type SBGBuffer = &'static mut [u8; SBG_BUFFER_SIZE]; - -// ------- -// SD Card -// ------- -pub type SdPads = spi::Pads< - Sercom4, - IoSet2, - Pin>, - Pin>, - Pin>, ->; - -pub type SdSpi = spi::Spi, spi::Duplex>; From edabe7ede246abbbd33cf2d5da5d771fd7f09c00 Mon Sep 17 00:00:00 2001 From: Noah Sprenger Date: Sat, 14 Sep 2024 12:20:50 -0400 Subject: [PATCH 47/47] Update lock --- Cargo.lock | 187 ----------------------------------------------------- 1 file changed, 187 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 65ee7864..7e24f1d1 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -20,15 +20,6 @@ dependencies = [ "opaque-debug", ] -[[package]] -name = "as-slice" -version = "0.2.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "516b6b4f0e40d50dcda9365d53964ec74560ad4284da2e7fc97122cd83174516" -dependencies = [ - "stable_deref_trait", -] - [[package]] name = "atomic-polyfill" version = "1.0.3" @@ -97,21 +88,6 @@ version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "f8fe8f5a8a398345e52358e18ff07cc17a568fbca5c6f73873d3a62056309603" -[[package]] -name = "beacon" -version = "0.1.0" -dependencies = [ - "common-arm", - "common-arm-stm32l0", - "cortex-m", - "cortex-m-rt", - "cortex-m-rtic", - "messages", - "postcard", - "stm32l0xx-hal", - "systick-monotonic", -] - [[package]] name = "bit-set" version = "0.5.3" @@ -290,29 +266,6 @@ dependencies = [ "panic-probe", ] -[[package]] -name = "communication" -version = "0.1.0" -dependencies = [ - "atsamd-hal", - "common-arm", - "common-arm-atsame", - "cortex-m", - "cortex-m-rt", - "cortex-m-rtic", - "defmt", - "defmt-rtt", - "embedded-alloc", - "embedded-sdmmc 0.8.0", - "heapless 0.7.17", - "messages", - "panic-probe", - "postcard", - "systick-monotonic", - "typenum", - "ublox", -] - [[package]] name = "convert_case" version = "0.4.0" @@ -588,15 +541,6 @@ version = "0.3.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a21dea9854beb860f3062d10228ce9b976da520a73474aed3171ec276bc0c032" -[[package]] -name = "embedded-time" -version = "0.12.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d7a4b4d10ac48d08bfe3db7688c402baadb244721f30a77ce360bd24c3dffe58" -dependencies = [ - "num", -] - [[package]] name = "enum_dispatch" version = "0.3.13" @@ -1053,28 +997,6 @@ version = "1.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8d5439c4ad607c3c23abf66de8c8bf57ba8adcd1f129e699851a6e43935d339d" -[[package]] -name = "num" -version = "0.3.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8b7a8e9be5e039e2ff869df49155f1c06bd01ade2117ec783e56ab0932b67a8f" -dependencies = [ - "num-complex", - "num-integer", - "num-iter", - "num-rational", - "num-traits", -] - -[[package]] -name = "num-complex" -version = "0.3.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "747d632c0c558b87dbabbe6a82f3b4ae03720d0646ac5b7b4dae89394be5f2c5" -dependencies = [ - "num-traits", -] - [[package]] name = "num-derive" version = "0.3.3" @@ -1086,37 +1008,6 @@ dependencies = [ "syn 1.0.109", ] -[[package]] -name = "num-integer" -version = "0.1.46" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7969661fd2958a5cb096e56c8e1ad0444ac2bbcd0061bd28660485a44879858f" -dependencies = [ - "num-traits", -] - -[[package]] -name = "num-iter" -version = "0.1.45" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1429034a0490724d0075ebb2bc9e875d6503c3cf69e235a8941aa757d83ef5bf" -dependencies = [ - "autocfg", - "num-integer", - "num-traits", -] - -[[package]] -name = "num-rational" -version = "0.3.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "12ac428b1cb17fce6f731001d307d351ec70a6d202fc2e60f7d4c5e42d8f4f07" -dependencies = [ - "autocfg", - "num-integer", - "num-traits", -] - [[package]] name = "num-traits" version = "0.2.19" @@ -1192,25 +1083,6 @@ dependencies = [ "serde", ] -[[package]] -name = "power" -version = "0.1.0" -dependencies = [ - "atsamd-hal", - "common-arm", - "common-arm-atsame", - "cortex-m", - "cortex-m-rt", - "cortex-m-rtic", - "defmt", - "enum_dispatch", - "heapless 0.7.17", - "messages", - "postcard", - "systick-monotonic", - "typenum", -] - [[package]] name = "ppv-lite86" version = "0.2.20" @@ -1394,15 +1266,6 @@ version = "0.8.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "7a66a03ae7c801facd77a29370b4faec201768915ac14a721ba36f20bc9c209b" -[[package]] -name = "rtcc" -version = "0.3.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "95973c3a0274adc4f3c5b70d2b5b85618d6de9559a6737d3293ecae9a2fc0839" -dependencies = [ - "chrono", -] - [[package]] name = "rtic" version = "2.1.1" @@ -1615,27 +1478,6 @@ version = "0.7.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "388a1df253eca08550bef6c72392cfe7c30914bf41df5269b68cbd6ff8f570a3" -[[package]] -name = "sensor" -version = "0.1.0" -dependencies = [ - "atsamd-hal", - "common-arm", - "common-arm-atsame", - "cortex-m", - "cortex-m-rt", - "cortex-m-rtic", - "defmt", - "embedded-alloc", - "embedded-sdmmc 0.3.0", - "heapless 0.7.17", - "messages", - "postcard", - "sbg-rs", - "systick-monotonic", - "typenum", -] - [[package]] name = "seq-macro" version = "0.3.5" @@ -1803,35 +1645,6 @@ dependencies = [ "void", ] -[[package]] -name = "stm32l0" -version = "0.15.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "37c35ac5e2c330137c257b7254eb4593e3e8312104dedc1880447654cb8dd50f" -dependencies = [ - "bare-metal 1.0.0", - "cortex-m", - "cortex-m-rt", - "vcell", -] - -[[package]] -name = "stm32l0xx-hal" -version = "0.10.0" -source = "git+https://github.com/stm32-rs/stm32l0xx-hal#7132578d4c1d92e8c7afa777c0ac2e29855a2bdd" -dependencies = [ - "as-slice", - "cast", - "cortex-m", - "cortex-m-rt", - "embedded-hal 0.2.7", - "embedded-time", - "nb 1.1.0", - "rtcc", - "stm32l0", - "void", -] - [[package]] name = "syn" version = "0.15.44"