From fa9ac741f945e173eed9b8df54e2ab14d9517eb6 Mon Sep 17 00:00:00 2001
From: Samuel Tardieu <sam@rfc1149.net>
Date: Tue, 16 Jul 2024 20:29:06 +0200
Subject: [PATCH] Use queue to handle timeout
---
controller/src/logic.rs | 47 +++++++++++++++++++----------------------
1 file changed, 22 insertions(+), 25 deletions(-)
diff --git a/controller/src/logic.rs b/controller/src/logic.rs
index c45ee77..c8fdeeb 100644
--- a/controller/src/logic.rs
+++ b/controller/src/logic.rs
@@ -2,16 +2,19 @@ use crate::{
encoders::Encoders,
tb6612fng::{Movement, Tb6612fng},
};
-use core::sync::atomic::{AtomicU32, Ordering};
+use core::sync::atomic::{AtomicU8, Ordering};
use embassy_stm32::{
pac::{self, wwdg::vals::Wdgtb},
peripherals::WWDG,
time::hz,
};
+use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, signal::Signal};
use embassy_time::{Duration, Instant, Ticker};
use heapless::Deque;
use i2c2_target::{I2C2, MESSAGE_SIZE};
+static WATCHDOG_TICKS: AtomicU8 = AtomicU8::new(5);
+
struct State {
i2c: I2C2,
motors: Tb6612fng<'static>,
@@ -19,8 +22,6 @@ struct State {
max_motor_percentage: u8,
motor_speed: [u8; 2],
standby: bool,
- /// Number of 1/10th of seconds
- watchdog_ticks: u8,
}
impl State {
@@ -32,7 +33,6 @@ impl State {
max_motor_percentage: 100,
motor_speed: [0, 0],
standby: true,
- watchdog_ticks: 5,
}
}
@@ -87,17 +87,6 @@ impl State {
static mut STATE: Option<State> = None;
-/// Date of the next watchdog expiration. A u32 value can hold up to
-/// 49 days which is more than enough. It is not stored in the state
-/// to be able to be checked outside any critical section.
-static WATCHDOG_EXPIRATION: AtomicU32 = AtomicU32::new(0);
-
-/// Number of milliseconds since boot
-#[allow(clippy::cast_possible_truncation)]
-fn current_millis() -> u32 {
- Instant::now().as_millis() as u32
-}
-
fn wwdg_configure() {
// Enable WWDG clock
pac::RCC.apb1enr().modify(|w| w.set_wwdgen(true));
@@ -133,16 +122,25 @@ pub async fn start_i2c_target(
STATE = Some(State::new(i2c2, motors, encoders));
STATE.as_mut().unwrap().i2c.start(&i2c_callback);
}
+ watchdog().await;
+}
+
+static WATCHDOG_PING: Signal<CriticalSectionRawMutex, Instant> = Signal::new();
+
+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 current_millis() >= WATCHDOG_EXPIRATION.load(Ordering::Relaxed) {
+ if let Some(ping) = WATCHDOG_PING.try_take() {
+ watchdog_expiration = ping
+ + Duration::from_millis(100 * u64::from(WATCHDOG_TICKS.load(Ordering::Relaxed)));
+ }
+ if Instant::now() >= watchdog_expiration {
State::with_critical_section(|state| {
- if state.is_moving()
- && current_millis() >= WATCHDOG_EXPIRATION.load(Ordering::Relaxed)
- {
+ if state.is_moving() {
defmt::debug!("stopping motors after watchdog has expired");
state.standby();
}
@@ -155,10 +153,7 @@ fn i2c_callback(command: &[u8], response: &mut Deque<u8, MESSAGE_SIZE>) {
#[allow(static_mut_refs)]
let state = unsafe { STATE.as_mut().unwrap() };
if process_command(state, command, response) {
- WATCHDOG_EXPIRATION.store(
- current_millis() + 100 * u32::from(state.watchdog_ticks),
- Ordering::Relaxed,
- );
+ WATCHDOG_PING.signal(Instant::now());
} else {
state.standby();
}
@@ -225,14 +220,16 @@ fn process_command(
}
&[MOTOR_SHUTDOWN_TIMEOUT, ticks] => {
if (1..=100).contains(&ticks) {
- state.watchdog_ticks = ticks;
+ WATCHDOG_TICKS.store(ticks, Ordering::Relaxed);
} else {
defmt::warn!("ticks out of range: {}", ticks);
return false;
}
}
[MOTOR_SHUTDOWN_TIMEOUT] => {
- response.push_back(state.watchdog_ticks).unwrap();
+ response
+ .push_back(WATCHDOG_TICKS.load(Ordering::Relaxed))
+ .unwrap();
}
[ENCODER_TICKS] => {
let (left, right) = state.encoders.ticks();
--
GitLab