From abef723b0afe7711b0d8f40642468833cd4b2239 Mon Sep 17 00:00:00 2001
From: Nhat Ha Pham <547508@student.saxion.nl>
Date: Wed, 1 Apr 2026 15:07:31 +0200
Subject: [PATCH 1/3] Modify the checkWatchdog. The device can move to OP
 state. And move to SAFEOP ERROR. Testing both happy scenario and unhappy
 scenario by tuning the SM watchdog and Multiplier in TwinCAT.

---
 lib/slave/src/ESC/EmulatedESC.cc | 37 +++++++++++++++++---------------
 1 file changed, 20 insertions(+), 17 deletions(-)

diff --git a/lib/slave/src/ESC/EmulatedESC.cc b/lib/slave/src/ESC/EmulatedESC.cc
index cbc2495d..0719d825 100644
--- a/lib/slave/src/ESC/EmulatedESC.cc
+++ b/lib/slave/src/ESC/EmulatedESC.cc
@@ -586,28 +586,31 @@ namespace kickcat
 
     void EmulatedESC::checkWatchdog()
     {
-        // Warning: PDI watchdog not handled yet!
-
         nanoseconds delay = pdoWatchdog();
-        if (delay == 0ns)
-        {
-            return; // watchdog deactivated
-        }
-
-        if ((lastLogicalWrite_ + delay) >= since_epoch())
-        {
-            // Watchdog OK
-            memory_.watchdog_status_process_data = 1;
+        if (delay == 0ns) //Handle the watchdog disable case
+        {  
+            memory_.watchdog_status_process_data = 1; // Consider watchdog is OK if disable
             return;
         }
-
-        if (memory_.watchdog_status_process_data == 1)
+        
+        auto now = since_epoch(); // Create the current time
+        if (now < (lastLogicalWrite_ + delay)) // If the current time is before the last valid PDO write plus the delay
         {
-            // Front detection (1 to 0)
-            memory_.watchdog_status_process_data = 0;
-            if (memory_.watchdog_counter_process_data < 0xFF)
+            memory_.watchdog_status_process_data = 1; // Watchdog is healthy
+        } 
+        else
+        {  // Watchdog EXPIRED
+            if (memory_.watchdog_status_process_data == 1) // Checks if the watchdog was previously OK
             {
-                memory_.watchdog_counter_process_data++;
+                memory_.watchdog_status_process_data = 0; // Watchdog expired
+                if (memory_.watchdog_counter_process_data < 0xFF)
+                {
+                    memory_.watchdog_counter_process_data++; // Counter of how many times the watchdog has expired
+                }
+                // If the watchdog expires in OP, we must tell the master and potentially drop the status ourselves
+                memory_.al_control = (memory_.al_status & 0xFFF0) | State::SAFE_OP;
+                memory_.al_status |= 0x0010; // Set Error Bit
+                memory_.al_status_code = 0x001B; // SM Watchdog code
             }
         }
     }

From fff17e68043abce87475d1930fad32c1aaa0e149 Mon Sep 17 00:00:00 2001
From: trns1997 <trns1997@gmail.com>
Date: Mon, 6 Apr 2026 16:39:04 +0200
Subject: [PATCH 2/3] apply suggestions

---
 lib/slave/src/ESC/EmulatedESC.cc | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/lib/slave/src/ESC/EmulatedESC.cc b/lib/slave/src/ESC/EmulatedESC.cc
index 0719d825..e8361252 100644
--- a/lib/slave/src/ESC/EmulatedESC.cc
+++ b/lib/slave/src/ESC/EmulatedESC.cc
@@ -30,6 +30,9 @@ namespace kickcat
         // eeprom never busy because the data are processed in sync with the request.
         memory_.eeprom_control &= ~0x8000;
 
+        // Disabled watchdog = OK at power-on
+        memory_.watchdog_status_process_data = 1;
+
         // Set default values in registers (TODO: a lot of them are left uninitialized)
         std::memset(memory_.sync_manager, 0, sizeof(memory_.sync_manager));
     }
@@ -589,7 +592,6 @@ namespace kickcat
         nanoseconds delay = pdoWatchdog();
         if (delay == 0ns) //Handle the watchdog disable case
         {  
-            memory_.watchdog_status_process_data = 1; // Consider watchdog is OK if disable
             return;
         }
         
@@ -609,8 +611,8 @@ namespace kickcat
                 }
                 // If the watchdog expires in OP, we must tell the master and potentially drop the status ourselves
                 memory_.al_control = (memory_.al_status & 0xFFF0) | State::SAFE_OP;
-                memory_.al_status |= 0x0010; // Set Error Bit
-                memory_.al_status_code = 0x001B; // SM Watchdog code
+                memory_.al_status |= AL_STATUS_ERR_IND; // Set Error Bit
+                memory_.al_status_code = SYNC_MANAGER_WATCHDOG; // SM Watchdog code
             }
         }
     }

From 56f897cf93128fdffa2e0f91657f46b58fe09c01 Mon Sep 17 00:00:00 2001
From: trns1997 <trns1997@gmail.com>
Date: Mon, 6 Apr 2026 16:40:44 +0200
Subject: [PATCH 3/3] revert certain lines

---
 lib/slave/src/ESC/EmulatedESC.cc | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/lib/slave/src/ESC/EmulatedESC.cc b/lib/slave/src/ESC/EmulatedESC.cc
index e8361252..e91983d7 100644
--- a/lib/slave/src/ESC/EmulatedESC.cc
+++ b/lib/slave/src/ESC/EmulatedESC.cc
@@ -590,9 +590,9 @@ namespace kickcat
     void EmulatedESC::checkWatchdog()
     {
         nanoseconds delay = pdoWatchdog();
-        if (delay == 0ns) //Handle the watchdog disable case
-        {  
-            return;
+        if (delay == 0ns)
+        {
+            return; // watchdog deactivated
         }
         
         auto now = since_epoch(); // Create the current time