diff --git a/boards/recovery/src/data_manager.rs b/boards/recovery/src/data_manager.rs index 293ca0c..63b1d70 100644 --- a/boards/recovery/src/data_manager.rs +++ b/boards/recovery/src/data_manager.rs @@ -8,6 +8,9 @@ use messages::Message; const MAIN_HEIGHT: f32 = 876.0; // meters ASL const HEIGHT_MIN: f32 = 600.0; // meters ASL +const RECOVERY_DATA_POINTS: u8 = 8; // number of barometric altitude readings held by the recovery + // algorithm +const RECOVERY_TIMER_TIMEOUT: u8 = 15; // minutes pub struct DataManager { pub air: Option, @@ -16,8 +19,11 @@ 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), 8>, // RECOVERY_DATA_POINTS (issue + // when putting as const) pub current_state: Option, + // each tick represents a minute that passed + pub recovery_counter: u8, } impl DataManager { @@ -32,11 +38,12 @@ impl DataManager { gps_vel: None, historical_barometer_altitude, current_state: None, + recovery_counter: 0, } } /// Returns true if the rocket is descending pub fn is_falling(&self) -> bool { - if self.historical_barometer_altitude.len() < 8 { + if (self.historical_barometer_altitude.len() as u8) < RECOVERY_DATA_POINTS { return false; } let mut buf = self.historical_barometer_altitude.oldest_ordered(); @@ -56,14 +63,14 @@ impl DataManager { avg_sum += slope; prev = i; } - match avg_sum / 7.0 { + match avg_sum / (RECOVERY_DATA_POINTS as f32 - 1.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); + info!("avg: {}", avg_sum / (RECOVERY_DATA_POINTS as f32 - 1.0)); } } } @@ -79,8 +86,8 @@ impl DataManager { None => false, } } - pub fn is_landed(&self) -> bool { - if self.historical_barometer_altitude.len() < 8 { + pub fn is_landed(&mut self) -> bool { + if self.historical_barometer_altitude.len() < RECOVERY_DATA_POINTS.into() { return false; } let mut buf = self.historical_barometer_altitude.oldest_ordered(); @@ -96,13 +103,15 @@ impl DataManager { avg_sum += (i.0 - prev.0) / time_diff; prev = i; } - match avg_sum / 7.0 { + match avg_sum / (RECOVERY_DATA_POINTS as f32 - 1.0) { // inclusive range x if (-0.25..=0.25).contains(&x) => { - return true; + if self.recovery_counter >= RECOVERY_TIMER_TIMEOUT { + return true; + } } _ => { - // continue + self.recovery_counter = 0; } } } diff --git a/boards/recovery/src/main.rs b/boards/recovery/src/main.rs index bd08c8f..c96fbbc 100644 --- a/boards/recovery/src/main.rs +++ b/boards/recovery/src/main.rs @@ -19,6 +19,7 @@ use data_manager::DataManager; use gpio_manager::GPIOManager; use hal::gpio::{Pin, Pins, PushPullOutput, PB16, PB17}; use hal::prelude::*; +use hal::timer::TimerCounter2; use mcan::messageram::SharedMemory; use messages::*; use state_machine::{StateMachine, StateMachineContext}; @@ -42,6 +43,7 @@ mod app { data_manager: DataManager, can0: communication::CanDevice0, gpio: GPIOManager, + recovery_timer: TimerCounter2, } #[local] @@ -78,7 +80,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); @@ -104,6 +106,11 @@ mod app { /* State Machine config */ let state_machine = StateMachine::new(); + /* Recovery Timer config */ + let (pclk_tc2tc3, gclk0) = Pclk::enable(tokens.pclks.tc2_tc3, gclk0); + let timerclk: hal::clock::v1::Tc2Tc3Clock = pclk_tc2tc3.into(); + let recovery_timer = hal::timer::TimerCounter2::tc2_(&timerclk, peripherals.TC2, &mut mclk); + /* Spawn tasks */ run_sm::spawn().ok(); state_send::spawn().ok(); @@ -120,6 +127,7 @@ mod app { data_manager: DataManager::new(), can0, gpio, + recovery_timer, }, Local { led_green, @@ -136,6 +144,25 @@ mod app { loop {} } + // interrupt handler for recovery counter + #[task(binds=TC2, shared=[data_manager, recovery_timer])] + fn recovery_counter_tick(mut cx: recovery_counter_tick::Context) { + cx.shared.recovery_timer.lock(|timer| { + if timer.wait().is_ok() { + cx.shared.data_manager.lock(|data| { + data.recovery_counter += 1; + }); + // restart timer after interrupt + let duration_mins = atsamd_hal::fugit::MinutesDurationU32::minutes(1); + // timer requires specific duration format + let timer_duration: atsamd_hal::fugit::Duration = + duration_mins.convert(); + timer.start(timer_duration); + } + timer.enable_interrupt(); // clear interrupt + }); + } + #[task(priority = 3, local = [fired: bool = false], shared=[gpio, &em])] fn fire_drogue(mut cx: fire_drogue::Context) { cx.shared.em.run(|| { @@ -175,7 +202,7 @@ mod app { /// Runs the state machine. /// This takes control of the shared resources. - #[task(priority = 3, local = [state_machine], shared = [can0, gpio, data_manager, &em])] + #[task(priority = 3, local = [state_machine], shared = [can0, gpio, data_manager, &em, recovery_timer])] fn run_sm(mut cx: run_sm::Context) { cx.local.state_machine.run(&mut StateMachineContext { shared_resources: &mut cx.shared, diff --git a/boards/recovery/src/state_machine/mod.rs b/boards/recovery/src/state_machine/mod.rs index 7b13913..3972b68 100644 --- a/boards/recovery/src/state_machine/mod.rs +++ b/boards/recovery/src/state_machine/mod.rs @@ -5,6 +5,7 @@ use crate::communication::CanDevice0; use crate::data_manager::DataManager; use crate::gpio_manager::GPIOManager; use crate::state_machine::states::*; +use atsamd_hal::timer::TimerCounter2; pub use black_magic::*; use core::fmt::Debug; use defmt::Format; @@ -17,6 +18,7 @@ pub trait StateMachineSharedResources { fn lock_can(&mut self, f: &dyn Fn(&mut CanDevice0)); fn lock_data_manager(&mut self, f: &dyn Fn(&mut DataManager)); fn lock_gpio(&mut self, f: &dyn Fn(&mut GPIOManager)); + fn lock_recovery_timer(&mut self, f: &dyn Fn(&mut TimerCounter2)); } impl<'a> StateMachineSharedResources for crate::app::__rtic_internal_run_smSharedResources<'a> { @@ -29,6 +31,9 @@ impl<'a> StateMachineSharedResources for crate::app::__rtic_internal_run_smShare fn lock_gpio(&mut self, fun: &dyn Fn(&mut GPIOManager)) { self.gpio.lock(fun) } + fn lock_recovery_timer(&mut self, fun: &dyn Fn(&mut TimerCounter2)) { + self.recovery_timer.lock(fun) + } } pub struct StateMachineContext<'a, 'b> { @@ -103,9 +108,9 @@ impl From for RocketStates { } } // Linter: an implementation of From is preferred since it gives you Into<_> for free where the reverse isn't true -impl Into for RocketStates { - fn into(self) -> state::StateData { - match self { +impl From for state::StateData { + fn from(val: RocketStates) -> Self { + match val { RocketStates::Initializing(_) => state::StateData::Initializing, RocketStates::WaitForTakeoff(_) => state::StateData::WaitForTakeoff, RocketStates::Ascent(_) => state::StateData::Ascent, diff --git a/boards/recovery/src/state_machine/states/terminal_descent.rs b/boards/recovery/src/state_machine/states/terminal_descent.rs index 104e6cc..b95b0a5 100644 --- a/boards/recovery/src/state_machine/states/terminal_descent.rs +++ b/boards/recovery/src/state_machine/states/terminal_descent.rs @@ -4,6 +4,8 @@ use crate::state_machine::{ RocketStates, State, StateMachineContext, TransitionInto, WaitForRecovery, }; use crate::{no_transition, transition}; +use atsamd_hal::prelude::_embedded_hal_timer_CountDown; +use atsamd_hal::timer_traits::InterruptDrivenTimer; use common_arm::spawn; use defmt::{write, Format, Formatter}; use rtic::mutex::Mutex; @@ -17,6 +19,14 @@ impl State for TerminalDescent { spawn!(fire_main)?; Ok(()) }); + context.shared_resources.recovery_timer.lock(|timer| { + timer.enable_interrupt(); + let duration_mins = atsamd_hal::fugit::MinutesDurationU32::minutes(1); + // timer requires specific duration format + let timer_duration: atsamd_hal::fugit::Duration = + duration_mins.convert(); + timer.start(timer_duration); + }); } 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 cd404c8..302c39d 100644 --- a/boards/recovery/src/state_machine/states/wait_for_recovery.rs +++ b/boards/recovery/src/state_machine/states/wait_for_recovery.rs @@ -3,6 +3,7 @@ use crate::app::monotonics; use crate::no_transition; use crate::state_machine::{RocketStates, State, StateMachineContext, TransitionInto}; use crate::types::COM_ID; +use atsamd_hal::timer_traits::InterruptDrivenTimer; use defmt::{write, Format, Formatter}; use messages::command::{Command, PowerDown, RadioRate, RadioRateChange}; use messages::sender::Sender::SensorBoard; @@ -32,6 +33,9 @@ impl State for WaitForRecovery { }) }); } + context.shared_resources.recovery_timer.lock(|timer| { + timer.disable_interrupt(); + }) } fn step(&mut self, _context: &mut StateMachineContext) -> Option { no_transition!() // this is our final resting place. We should also powerdown this board.