From 282be3cf3780d5718fa270c8851dc73c1ac9dd0b Mon Sep 17 00:00:00 2001
From: Ingmar Jager <ingmarjager@gmail.com>
Date: Mon, 2 Dec 2019 15:17:25 +0100
Subject: [PATCH] Add sysreset command to reset entire mcu.

Use rtfm scheduler api to run reset command in the future so that the
sysreset command has time to be sent to the next board in the chain.
---
 app/comports.py           |  6 ++++--
 firmware-rtfm/src/main.rs | 37 ++++++++++++++++---------------------
 2 files changed, 20 insertions(+), 23 deletions(-)

diff --git a/app/comports.py b/app/comports.py
index c69a9b6..d34e0f4 100644
--- a/app/comports.py
+++ b/app/comports.py
@@ -59,8 +59,9 @@ def refresh(self):
 
 
     def sendCmd(self, cmd: str):
-        line = cmd + '\n'
-        self.serial.write(line.encode('utf-8'))
+        if self.serial:
+            line = cmd + '\n'
+            self.serial.write(line.encode('utf-8'))
 
     def change_port(self, port: serial.Serial):
 
@@ -68,6 +69,7 @@ def change_port(self, port: serial.Serial):
             # reader thread needs to be shut down
             self._stop_reader()
         self.serial = serial.Serial(port.device, BAUDRATE, timeout=10)
+        self.sendCmd('reset')
         print('open port: ', self.serial, self.serial.port)
         status.set_status('open port: {}'.format(self.serial.port))
         self._start_reader()
diff --git a/firmware-rtfm/src/main.rs b/firmware-rtfm/src/main.rs
index d28b3ce..57f8b2c 100644
--- a/firmware-rtfm/src/main.rs
+++ b/firmware-rtfm/src/main.rs
@@ -10,6 +10,8 @@ use embedded_hal::digital::v2::OutputPin;
 
 use arrayvec::ArrayString;
 
+use rtfm::cyccnt::{U32Ext as _};
+
 use stm32f1xx_hal::{
     prelude::*,
     pac::{self, EXTI},
@@ -83,7 +85,7 @@ const BAUDRATE: u32 = 57600;
 const ENCODER_PREFIX: &str = "KEY ";
 const ENCODER_SUFFIX: &str = "END\n";
 
-#[rtfm::app(device = stm32f1xx_hal::pac, peripherals = true)]
+#[rtfm::app(device = stm32f1xx_hal::pac, peripherals = true, monotonic = rtfm::cyccnt::CYCCNT)]
 const APP: () = {
 
     struct Resources {
@@ -119,7 +121,10 @@ const APP: () = {
         let (p, c) = Q.split();
 
         // Cortex-M peripherals
-        let _core: cortex_m::Peripherals = cx.core;
+        let mut core: rtfm::Peripherals = cx.core;
+        // Initialize (enable) the monotonic timer (CYCCNT)
+        core.DCB.enable_trace();
+        core.DWT.enable_cycle_counter();
 
         // Device specific peripherals
         let _device = cx.device;
@@ -355,6 +360,11 @@ const APP: () = {
 
     }
 
+    #[task]
+    fn system_reset(_cx: system_reset::Context) {
+        cortex_m::peripheral::SCB::sys_reset();
+    }
+
     #[task(resources=[encoder1, encoder2, encoder3, encoder4, encoder5, p], spawn=[send])]
     fn encoder_positions(cx: encoder_positions::Context) {
         let encoder_positions::Resources {
@@ -380,25 +390,7 @@ const APP: () = {
         cx.spawn.send().ok();
     }
 
-
-    // #[task(resources=[p], spawn=[send])]
-    // fn unknown_command(cx: unknown_command::Context, cmd: str) {
-    //     let unknown_command::Resources {
-    //         mut p,
-    //     } = cx.resources;
-
-
-    //     // let mut string: ArrayString::<[u8; 40]> = ArrayString::new();
-    //     p.lock(|p| {
-    //         write_string_to_queue(p, "unknown cmd \"");
-    //         write_string_to_queue(p, cmd);
-    //         write_string_to_queue(p, "\"\n");
-    //     });
-
-    //     cx.spawn.send().ok();
-    // }
-
-    #[task(binds=USART2, priority = 1, resources=[rx2, tx3, cmd_buffer, p], spawn=[reset_encoders, encoder_positions, send])]
+    #[task(binds=USART2, priority = 1, resources=[rx2, tx3, cmd_buffer, p], spawn=[reset_encoders, encoder_positions, send], schedule=[system_reset])]
     fn serial_cmd(cx: serial_cmd::Context) {
 
         let serial_cmd::Resources {
@@ -425,6 +417,9 @@ const APP: () = {
                         cx.spawn.reset_encoders().ok();
                     } else if cmd_buffer.starts_with("pos") {
                         cx.spawn.encoder_positions().ok();
+                    } else if cmd_buffer.starts_with("sysreset") {
+                        let now = cx.start;
+                        cx.schedule.system_reset(now + 8_000_000.cycles()).ok();
                     } else {
                         // cx.spawn.unknown_command().ok();
                         p.lock(|p| {