watchdog.rs

use super::motor_control;
use core::sync::atomic::{AtomicU8, Ordering};
use embassy_stm32::pac::{self, wwdg::vals::Wdgtb};
use embassy_sync::{blocking_mutex::raw::ThreadModeRawMutex, signal::Signal};
use embassy_time::{Duration, Instant, Ticker};

static WATCHDOG_PING: Signal<ThreadModeRawMutex, Instant> = Signal::new();
static WATCHDOG_TICKS: AtomicU8 = AtomicU8::new(5);

pub fn ping() {
    WATCHDOG_PING.signal(Instant::now());
}

pub fn set_shutdown_timeout(ticks: u8) {
    WATCHDOG_TICKS.store(ticks, Ordering::Relaxed);
}

pub fn get_shutdown_timeout() -> u8 {
    WATCHDOG_TICKS.load(Ordering::Relaxed)
}

#[embassy_executor::task]
pub async fn watchdog() {
    wwdg_configure();
    let mut ticker = Ticker::every(Duration::from_millis(50));
    let mut watchdog_expiration: Instant = Instant::now();
    loop {
        ticker.next().await;
        wwdg_pet();
        if let Some(ping) = WATCHDOG_PING.try_take() {
            watchdog_expiration = ping
                + Duration::from_millis(100 * u64::from(WATCHDOG_TICKS.load(Ordering::Relaxed)));
        }
        if motor_control::is_moving() && Instant::now() >= watchdog_expiration {
            defmt::debug!("stopping motors after watchdog has expired");
            motor_control::standby().await;
        }
    }
}

fn wwdg_configure() {
    // Enable WWDG clock
    pac::RCC.apb1enr().modify(|w| w.set_wwdgen(true));
    // With PCLK1 at 36MHz, 63 as a timeout value with wdgtb at /8
    // means ~58ms.  Comparing with a value of 45 means that at
    // least ~42ms must have passed before the watchdog can be
    // petted, so we have 8ms on each side.
    pac::WWDG.cfr().write(|w| {
        w.set_wdgtb(Wdgtb::DIV8);
        w.set_w((1 << 6) | 31);
    });
    wwdg_pet();
}

fn wwdg_pet() {
    // Reset watchdog and start it if not started yet
    pac::WWDG.cr().write(|w| {
        w.set_t((1 << 6) | 63);
        w.set_wdga(true);
    });
}