Implement watchdog command

Cargo.lock

@@ -133,6 +133,7 @@ version = "0.1.0"
 dependencies = [
  "cortex-m",
  "cortex-m-rt",
+ "critical-section",
  "defmt",
  "defmt-rtt",
  "embassy-executor",

README.org

@@ -77,9 +77,10 @@ Values are exchanged in little endian format.
 - In automatic motor distance, time in 100th of seconds to reach the maximum speed
   or to decrease the speed from the maximum to 0.
 
-** 0x28 Motor shutdown timeout (R/W)
+** [IMPLEMENTED] 0x28 Motor shutdown timeout (R/W)
 
-- Number of tenths of seconds in before the motor shut down
+- Number of tenths of seconds in before the motor shut down. The minimum is 1
+  (0.1 seconds) and the maximum is 100 (10 seconds).
 
 ** [IMPLEMENTED] 0x30-0x31 Motor speed (R/W)
 
@@ -92,7 +93,7 @@ Values are exchanged in little endian format.
 
 - Left and right encoder counters since last time, 2 complement, 2 bytes each
 
-** 0x36 Status (R)
+** [IMPLEMENTED] 0x36 Status (R)
 
 - Bit 0: moving (1) or idle (0)
 

controller/Cargo.toml

@@ -7,6 +7,7 @@ edition = "2021"
 [dependencies]
 cortex-m = { version = "0.7.7", features = ["critical-section-single-core", "inline-asm"] }
 cortex-m-rt = "0.7.3"
+critical-section = "1.1.2"
 defmt = "0.3.5"
 defmt-rtt = "0.4.0"
 embassy-executor = { git = "https://github.com/embassy-rs/embassy", features = ["arch-cortex-m", "executor-thread", "defmt", "integrated-timers"] }

controller/python/controller.py

@@ -9,6 +9,7 @@ class Controller:
 
     WHO_AM_I = 0x0F
     MAX_MOTOR_PERCENTAGE = 0x11
+    MOTOR_SHUTDOWN_TIMEOUT = 0x28
     MOTOR_SPEED = 0x30
     ENCODER_TICKS = 0x32
     STATUS = 0x36
@@ -96,3 +97,16 @@ class Controller:
         - "moving": True if at least one motor is moving, False otherwise"""
         (status,) = self._read(self.STATUS, 1, "?")
         return {"moving": (status & 1) != 0}
+
+    def set_motor_shutdown_timeout(self, duration: float):
+        """Set the duration in seconds after which the motors will shut down if no valid
+        command is received. The minimum is 0.1 seconds, the maximum is 10 seconds.
+        """
+        if duration < 0.1 or duration > 10.0:
+            raise ValueError
+        self._write(self.MOTOR_SHUTDOWN_TIMEOUT, [round(duration * 10)])
+
+    def get_motor_shutdown_timeout(self) -> float:
+        """Get the duration in seconds after which the motors will shut down if no valid
+        command is received."""
+        return self._read(self.MOTOR_SHUTDOWN_TIMEOUT, 1, "B")[0] / 10

controller/src/logic.rs

@@ -2,6 +2,8 @@ use crate::{
     encoders::Encoders,
     tb6612fng::{Movement, Tb6612fng},
 };
+use core::sync::atomic::{AtomicU32, Ordering};
+use embassy_time::{Instant, Timer};
 use heapless::Deque;
 use i2c2_target::{I2C2, MESSAGE_SIZE};
 
@@ -12,6 +14,8 @@ struct State {
     max_motor_percentage: u8,
     motor_speed: [u8; 2],
     standby: bool,
+    /// Number of 1/10th of seconds
+    watchdog_ticks: u8,
 }
 
 impl State {
@@ -23,12 +27,65 @@ impl State {
             max_motor_percentage: 100,
             motor_speed: [0, 0],
             standby: true,
+            watchdog_ticks: 5,
         }
     }
+
+    fn set_motor_speed(&mut self, ms: [u8; 2]) {
+        let (max_pwm, mmp) = (
+            self.motors.max_pwm() as i32,
+            i32::from(self.max_motor_percentage),
+        );
+        let scaled_speed = |speed| match speed as i8 {
+            -128 => None,
+            speed @ (-100..=100) => Some(i32::from(speed) * max_pwm * mmp / (100 * 100)),
+            speed => {
+                defmt::warn!("speed {} out of range [-100, 100]", speed);
+                None
+            }
+        };
+        let (speed_left, speed_right) = (scaled_speed(ms[0]), scaled_speed(ms[1]));
+        if (speed_left, speed_right) == (None, None) {
+            self.motors.standby_enter();
+            self.standby = true;
+            self.motor_speed = [0, 0];
+        } else {
+            if self.standby {
+                self.motors.standby_leave();
+                self.standby = false;
+            }
+            if let Some(speed_left) = speed_left {
+                self.motors.move_left(Movement::Advance(speed_left));
+                self.motor_speed[0] = ms[0];
+            }
+            if let Some(speed_right) = speed_right {
+                self.motors.move_right(Movement::Advance(speed_right));
+                self.motor_speed[1] = ms[1];
+            }
+        }
+    }
+
+    fn standby(&mut self) {
+        self.set_motor_speed([0x80, 0x80]);
+    }
+
+    fn is_moving(&self) -> bool {
+        self.motor_speed != [0, 0]
+    }
+
+    fn with_critical_section<R, F: FnMut(&mut State) -> R>(mut f: F) -> R {
+        let state = unsafe { STATE.as_mut().unwrap() };
+        critical_section::with(|_| f(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);
+
 #[embassy_executor::task]
 pub async fn start_i2c_target(
     i2c2: I2C2,
@@ -38,21 +95,48 @@ pub async fn start_i2c_target(
     motors.standby_enter();
     unsafe {
         STATE = Some(State::new(i2c2, motors, encoders));
-        STATE.as_mut().unwrap().i2c.start(CALLBACK);
+        STATE.as_mut().unwrap().i2c.start(&i2c_callback);
+    }
+    loop {
+        Timer::after_millis(100).await;
+        if Instant::now().as_millis() as u32 >= WATCHDOG_EXPIRATION.load(Ordering::Relaxed) {
+            State::with_critical_section(|state| {
+                if state.is_moving()
+                    && Instant::now().as_millis() as u32
+                        >= WATCHDOG_EXPIRATION.load(Ordering::Relaxed)
+                {
+                    state.standby();
+                }
+            });
+        }
     }
 }
 
-const CALLBACK: i2c2_target::Callback = &handle_command;
+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(
+            Instant::now().as_millis() as u32 + 100 * state.watchdog_ticks as u32,
+            Ordering::Relaxed,
+        );
+    } else {
+        state.standby();
+    }
+}
 
 const WHO_AM_I: u8 = 0x0f;
 const MAX_MOTOR_PERCENTAGE: u8 = 0x11;
+const MOTOR_SHUTDOWN_TIMEOUT: u8 = 0x28;
 const MOTOR_SPEED: u8 = 0x30;
 const ENCODER_TICKS: u8 = 0x32;
 const STATUS: u8 = 0x36;
 
-fn handle_command(command: &[u8], response: &mut Deque<u8, MESSAGE_SIZE>) {
-    #[allow(static_mut_refs)]
-    let state = unsafe { STATE.as_mut().unwrap() };
+fn process_command(
+    state: &mut State,
+    command: &[u8],
+    response: &mut Deque<u8, MESSAGE_SIZE>,
+) -> bool {
     defmt::trace!("Processing command {:?}", command);
     match command {
         [WHO_AM_I, ..] => {
@@ -63,48 +147,30 @@ fn handle_command(command: &[u8], response: &mut Deque<u8, MESSAGE_SIZE>) {
                 state.max_motor_percentage = p;
             } else {
                 defmt::warn!("Incorrect max percentage {}", p);
+                return false;
             }
         }
         [MAX_MOTOR_PERCENTAGE] => {
             response.push_back(state.max_motor_percentage).unwrap();
         }
         [MOTOR_SPEED, ms @ ..] if ms.len() == 2 => {
-            let (max_pwm, mmp) = (
-                state.motors.max_pwm() as i32,
-                i32::from(state.max_motor_percentage),
-            );
-            let scaled_speed = |speed| match speed as i8 {
-                -128 => None,
-                speed @ (-100..=100) => Some(i32::from(speed) * max_pwm * mmp / (100 * 100)),
-                speed => {
-                    defmt::warn!("speed {} out of range [-100, 100]", speed);
-                    None
-                }
-            };
-            let (speed_left, speed_right) = (scaled_speed(ms[0]), scaled_speed(ms[1]));
-            if (speed_left, speed_right) == (None, None) {
-                state.motors.standby_enter();
-                state.standby = true;
-                state.motor_speed = [0, 0];
-            } else {
-                if state.standby {
-                    state.motors.standby_leave();
-                    state.standby = false;
-                }
-                if let Some(speed_left) = speed_left {
-                    state.motors.move_left(Movement::Advance(speed_left));
-                    state.motor_speed[0] = ms[0];
-                }
-                if let Some(speed_right) = speed_right {
-                    state.motors.move_right(Movement::Advance(speed_right));
-                    state.motor_speed[1] = ms[1];
-                }
-            }
+            state.set_motor_speed([ms[0], ms[1]]);
         }
         [MOTOR_SPEED] => {
             response.push_back(state.motor_speed[0]).unwrap();
             response.push_back(state.motor_speed[1]).unwrap();
         }
+        &[MOTOR_SHUTDOWN_TIMEOUT, ticks] => {
+            if (1..=100).contains(&ticks) {
+                state.watchdog_ticks = ticks;
+            } else {
+                defmt::warn!("ticks out of range: {}", ticks);
+                return false;
+            }
+        }
+        [MOTOR_SHUTDOWN_TIMEOUT] => {
+            response.push_back(state.watchdog_ticks).unwrap();
+        }
         [ENCODER_TICKS] => {
             let (left, right) = state.encoders.ticks();
             let (left, right) = (left.to_le_bytes(), right.to_le_bytes());
@@ -114,14 +180,12 @@ fn handle_command(command: &[u8], response: &mut Deque<u8, MESSAGE_SIZE>) {
             response.push_back(right[1]).unwrap();
         }
         [STATUS] => {
-            let moving = (state.motor_speed != [0, 0]) as u8;
-            response.push_back(moving << 0).unwrap();
+            response.push_back(state.is_moving() as u8).unwrap();
         }
-        &[_, ..] => {
+        _ => {
             defmt::warn!("unknown command or args {:#04x}", command);
-        }
-        &[] => {
-            defmt::warn!("received empty command");
+            return false;
         }
     }
+    true
 }