Skip to content

Commit

Permalink
Add Recovery Timeout (#101)
Browse files Browse the repository at this point in the history 
* create recovery timer mechanism

* fix build

* clear interrupts

* WIP: Timer Update

* fix build errors

* optimize

* fix linting

* Fix: Cippy warnings

* Fix: remove magic number

* Fix: remove magic number

* Fix: Clippy

* Fix: Import

---------

Co-authored-by: Eilay Katsnelson <[email protected]>
Co-authored-by: Noah Sprenger <[email protected]>
  • Loading branch information
@eilayk @NoahSprenger
3 people authored Jun 8, 2024
1 parent a05f34a commit 0251afa
Show file tree
Hide file tree
Showing 5 changed files with 69 additions and 14 deletions.
27 changes: 18 additions & 9 deletions boards/recovery/src/data_manager.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<Air>,
Expand All @@ -16,8 +19,11 @@ pub struct DataManager {
pub imu: (Option<Imu1>, Option<Imu2>),
pub utc_time: Option<UtcTime>,
pub gps_vel: Option<GpsVel>,
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<RocketStates>,
// each tick represents a minute that passed
pub recovery_counter: u8,
}

impl DataManager {
Expand All @@ -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();
Expand All @@ -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));
}
}
}
Expand All @@ -82,8 +89,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();
Expand All @@ -99,13 +106,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;
}
}
}
Expand Down
31 changes: 29 additions & 2 deletions boards/recovery/src/main.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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};
Expand All @@ -42,6 +43,7 @@ mod app {
data_manager: DataManager,
can0: communication::CanDevice0,
gpio: GPIOManager,
recovery_timer: TimerCounter2,
}

#[local]
Expand Down Expand Up @@ -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);
Expand All @@ -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();
Expand All @@ -120,6 +127,7 @@ mod app {
data_manager: DataManager::new(),
can0,
gpio,
recovery_timer,
},
Local {
led_green,
Expand All @@ -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<u32, 1, 1000000000> =
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(|| {
Expand Down Expand Up @@ -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,
Expand Down
11 changes: 8 additions & 3 deletions boards/recovery/src/state_machine/mod.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -17,6 +18,7 @@ pub trait StateMachineSharedResources {
fn lock_can(&mut self, f: &dyn Fn(&mut CanDevice0));

Check warning on line 18 in boards/recovery/src/state_machine/mod.rs

View workflow job for this annotation

GitHub Actions / All 

methods `lock_can`, `lock_data_manager`, `lock_gpio`, and `lock_recovery_timer` are never used
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> {
Expand All @@ -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> {
Expand Down Expand Up @@ -103,9 +108,9 @@ impl From<state::StateData> for RocketStates {
}
}
// Linter: an implementation of From is preferred since it gives you Into<_> for free where the reverse isn't true
impl Into<state::StateData> for RocketStates {
fn into(self) -> state::StateData {
match self {
impl From<RocketStates> 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,
Expand Down
10 changes: 10 additions & 0 deletions boards/recovery/src/state_machine/states/terminal_descent.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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<u32, 1, 1000000000> =
duration_mins.convert();
timer.start(timer_duration);
});
}
fn step(&mut self, context: &mut StateMachineContext) -> Option<RocketStates> {
context.shared_resources.data_manager.lock(|data| {
Expand Down
4 changes: 4 additions & 0 deletions boards/recovery/src/state_machine/states/wait_for_recovery.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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::node::Node::SensorBoard;
Expand Down Expand Up @@ -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<RocketStates> {
no_transition!() // this is our final resting place. We should also powerdown this board.
Expand Down

0 comments on commit 0251afa

Please sign in to comment.